Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
handle/mavcommands.cpp
- Committer:
- dylanembed123
- Date:
- 2014-04-26
- Revision:
- 52:b4dddb28dffa
- Parent:
- 39:1acea80563cf
- Child:
- 53:d785a6f6abdd
File content as of revision 52:b4dddb28dffa:
#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=5; // 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=0;issueItem.frame=0;issueItem.confirm=0; // 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; // Issue start issueStart.targSys=1;issueStart.targComp=1;issueStart.lat=5.0f;issueStart.lon=6.0f;issueStart.alt=7.0f;issueStart.cmd=22;issueStart.seq=0;issueStart.current=1; // Issue location request issueStreamReq.targSys=1;issueStreamReq.targComp=1;issueStreamReq.streamID=6;issueStreamReq.rate=5;issueStreamReq.start=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 ",input); 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){ USB::getSerial().printf("\n"); 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)); // Start sending location data //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM)); }//else{ // Start sending location data Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM)); //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST)); //} } // 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]; issueItem.lon=(float)issueItem.seq; // 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){ // Start Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueStart,sizeof(MAV_MISSION_ITEM)); wait(1); // Take off Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM)); wait(1); // Start sending location data Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM)); wait(1); // Arm motors Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM)); } // Check for GPS if(myCmd[5]==33){ for(int i=0;i<18;i++){USB::getSerial().printf("Got loc %d/18 %d\n",i,myCmd[6+i]);} MAV_LOCDATA* input=(MAV_LOCDATA*)&myCmd[6]; USB::getSerial().printf("Got lat %d\n",input->lat);// %d %d %d %d %d,input->lon,input->alt,input->x,input->y,input->z); USB::getSerial().printf("Got lon %d\n",input->lon); USB::getSerial().printf("Got alt %d\n",input->alt); } // Check for waypoint count if(myCmd[5]==44){ USB::getSerial().printf("Waypoints tsys %d tcomp %d cnt %d\n",myCmd[8],myCmd[7],myCmd[6]); } delete myCmd; } }