Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
Diff: mavcommands.cpp
- Revision:
- 26:06f1c9d70e9f
- Parent:
- 25:b7f861fc8ddd
- Child:
- 27:db73f8ac6c75
--- a/mavcommands.cpp Tue Apr 22 14:18:30 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,91 +0,0 @@ -#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