Diff: handle/mavcommands.cpp

Revision:

26:06f1c9d70e9f

Parent:

24:e65416d6de22

Child:

33:ad63e7013801

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/mavcommands.cpp	Tue Apr 22 14:21:01 2014 +0000
@@ -0,0 +1,91 @@
+#include "mavcommands.h"
+
+// Maximium number of charictors to read without giving up the processor
+#define MAX_READ_LOOP 256
+
+MavCmd* MavCmd::mavcmd=NULL;
+
+void MavCmd::setupCmds(){
+    // Request item list
+    req.targSys=1;req.targComp=1;
+    // Issue arm motors
+    issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f;
+    // Issue disarm motors
+    issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
+    // Issue a mission count
+    issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
+    // Issue a mission item
+    issueItem.targSys=1;issueItem.targComp=1;issueItem.lat=5.0f;issueItem.lon=6.0f;issueItem.alt=7.0f;issueItem.cmd=16;issueItem.seq=0;issueItem.current=1;
+    // Issue a take off item
+    issueTakeOff.targSys=1;issueTakeOff.targComp=1;issueTakeOff.lat=5.0f;issueTakeOff.lon=6.0f;issueTakeOff.alt=7.0f;issueTakeOff.cmd=22;issueTakeOff.seq=0;issueTakeOff.current=1;
+
+    // Local variables
+    startSetup=true;// Set to true to initiate startup sequence
+    readState=0;     // Read State
+    realLen=0;       // How many more bytes need to be read
+    readIndex=0;     // Current index in next cmd (also the size)
+}
+
+char* MavCmd::getNextCmd(){
+    for(int readcnt=0;readcnt<MAX_READ_LOOP;readcnt++){
+        if(Mav::getSerial().readable()<=0){
+            // Nothing to run, wait until next time
+            return NULL;
+        }else{
+            char input=Mav::getSerial().getc();
+            USB::getSerial().printf("> %x %d %d\n",input,readState,realLen);
+            if(readState==0&&input==0xFE){
+                readState=1;
+                readIndex=1;
+            }else if(readState!=0){
+                nextCmd[std::min(readIndex++,512)]=input;
+                if(readState==1){
+                    realLen=input+5-1;
+                    readState=2;
+                }
+                if(readState==2){
+                    realLen--;
+                    if(realLen==0){
+                        readState=0;
+                        char* output=new char[readIndex];
+                        for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];}
+                        return output;
+                    }
+                }
+            }
+        }
+    }
+}
+
+// Handle the next command (if one is available)
+void MavCmd::handleNextCmd(){
+    char* myCmd=getNextCmd();
+    // Output debug info
+    if(myCmd!=NULL){
+        USB::getSerial().printf("Got CMD len %d messageid %d \n",myCmd[1],myCmd[5]);
+        if(myCmd[5]==0){
+            if(startSetup){
+                startSetup=false;
+                USB::getSerial().printf("Issue Command\n",myCmd[1],myCmd[5]);
+                // Issue count to init waypoint entry
+                Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
+            }
+        }
+        // Check for mavlink message request
+        if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
+            USB::getSerial().printf("Issue Item\n");
+            // Set sequence to requested sequence
+            issueItem.seq=myCmd[6];
+            // Issue mission item
+            Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
+        }
+        // Check for mission accepted
+        if(myCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){
+            // Arm motors
+            Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
+            // Take off
+            Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM));
+        }
+        delete myCmd;
+    }
+}
\ No newline at end of file