Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
handle/mavcommands.cpp@39:1acea80563cf, 2014-04-23 (annotated)
- Committer:
- dylanembed123
- Date:
- Wed Apr 23 04:00:32 2014 +0000
- Revision:
- 39:1acea80563cf
- Parent:
- 33:ad63e7013801
- Child:
- 47:fd373c4ea831
- Child:
- 52:b4dddb28dffa
Waypoints now work;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dylanembed123 | 24:e65416d6de22 | 1 | #include "mavcommands.h" |
dylanembed123 | 24:e65416d6de22 | 2 | |
dylanembed123 | 24:e65416d6de22 | 3 | // Maximium number of charictors to read without giving up the processor |
dylanembed123 | 24:e65416d6de22 | 4 | #define MAX_READ_LOOP 256 |
dylanembed123 | 24:e65416d6de22 | 5 | |
dylanembed123 | 24:e65416d6de22 | 6 | MavCmd* MavCmd::mavcmd=NULL; |
dylanembed123 | 24:e65416d6de22 | 7 | |
dylanembed123 | 24:e65416d6de22 | 8 | void MavCmd::setupCmds(){ |
dylanembed123 | 24:e65416d6de22 | 9 | // Request item list |
dylanembed123 | 24:e65416d6de22 | 10 | req.targSys=1;req.targComp=1; |
dylanembed123 | 24:e65416d6de22 | 11 | // Issue arm motors |
dylanembed123 | 24:e65416d6de22 | 12 | issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f; |
dylanembed123 | 24:e65416d6de22 | 13 | // Issue disarm motors |
dylanembed123 | 24:e65416d6de22 | 14 | issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f; |
dylanembed123 | 24:e65416d6de22 | 15 | // Issue a mission count |
dylanembed123 | 39:1acea80563cf | 16 | issueCount.targSys=1;issueCount.targComp=1;issueCount.count=5; |
dylanembed123 | 24:e65416d6de22 | 17 | // Issue a mission item |
dylanembed123 | 39:1acea80563cf | 18 | 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; |
dylanembed123 | 24:e65416d6de22 | 19 | // Issue a take off item |
dylanembed123 | 24:e65416d6de22 | 20 | 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; |
dylanembed123 | 33:ad63e7013801 | 21 | // Issue start |
dylanembed123 | 33:ad63e7013801 | 22 | 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; |
dylanembed123 | 24:e65416d6de22 | 23 | |
dylanembed123 | 24:e65416d6de22 | 24 | // Local variables |
dylanembed123 | 24:e65416d6de22 | 25 | startSetup=true;// Set to true to initiate startup sequence |
dylanembed123 | 24:e65416d6de22 | 26 | readState=0; // Read State |
dylanembed123 | 24:e65416d6de22 | 27 | realLen=0; // How many more bytes need to be read |
dylanembed123 | 24:e65416d6de22 | 28 | readIndex=0; // Current index in next cmd (also the size) |
dylanembed123 | 24:e65416d6de22 | 29 | } |
dylanembed123 | 24:e65416d6de22 | 30 | |
dylanembed123 | 24:e65416d6de22 | 31 | char* MavCmd::getNextCmd(){ |
dylanembed123 | 24:e65416d6de22 | 32 | for(int readcnt=0;readcnt<MAX_READ_LOOP;readcnt++){ |
dylanembed123 | 24:e65416d6de22 | 33 | if(Mav::getSerial().readable()<=0){ |
dylanembed123 | 24:e65416d6de22 | 34 | // Nothing to run, wait until next time |
dylanembed123 | 24:e65416d6de22 | 35 | return NULL; |
dylanembed123 | 24:e65416d6de22 | 36 | }else{ |
dylanembed123 | 24:e65416d6de22 | 37 | char input=Mav::getSerial().getc(); |
dylanembed123 | 24:e65416d6de22 | 38 | USB::getSerial().printf("> %x %d %d\n",input,readState,realLen); |
dylanembed123 | 24:e65416d6de22 | 39 | if(readState==0&&input==0xFE){ |
dylanembed123 | 24:e65416d6de22 | 40 | readState=1; |
dylanembed123 | 24:e65416d6de22 | 41 | readIndex=1; |
dylanembed123 | 24:e65416d6de22 | 42 | }else if(readState!=0){ |
dylanembed123 | 24:e65416d6de22 | 43 | nextCmd[std::min(readIndex++,512)]=input; |
dylanembed123 | 24:e65416d6de22 | 44 | if(readState==1){ |
dylanembed123 | 24:e65416d6de22 | 45 | realLen=input+5-1; |
dylanembed123 | 24:e65416d6de22 | 46 | readState=2; |
dylanembed123 | 24:e65416d6de22 | 47 | } |
dylanembed123 | 24:e65416d6de22 | 48 | if(readState==2){ |
dylanembed123 | 24:e65416d6de22 | 49 | realLen--; |
dylanembed123 | 24:e65416d6de22 | 50 | if(realLen==0){ |
dylanembed123 | 24:e65416d6de22 | 51 | readState=0; |
dylanembed123 | 24:e65416d6de22 | 52 | char* output=new char[readIndex]; |
dylanembed123 | 24:e65416d6de22 | 53 | for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];} |
dylanembed123 | 24:e65416d6de22 | 54 | return output; |
dylanembed123 | 24:e65416d6de22 | 55 | } |
dylanembed123 | 24:e65416d6de22 | 56 | } |
dylanembed123 | 24:e65416d6de22 | 57 | } |
dylanembed123 | 24:e65416d6de22 | 58 | } |
dylanembed123 | 24:e65416d6de22 | 59 | } |
dylanembed123 | 24:e65416d6de22 | 60 | } |
dylanembed123 | 24:e65416d6de22 | 61 | |
dylanembed123 | 24:e65416d6de22 | 62 | // Handle the next command (if one is available) |
dylanembed123 | 24:e65416d6de22 | 63 | void MavCmd::handleNextCmd(){ |
dylanembed123 | 24:e65416d6de22 | 64 | char* myCmd=getNextCmd(); |
dylanembed123 | 24:e65416d6de22 | 65 | // Output debug info |
dylanembed123 | 24:e65416d6de22 | 66 | if(myCmd!=NULL){ |
dylanembed123 | 24:e65416d6de22 | 67 | USB::getSerial().printf("Got CMD len %d messageid %d \n",myCmd[1],myCmd[5]); |
dylanembed123 | 24:e65416d6de22 | 68 | if(myCmd[5]==0){ |
dylanembed123 | 24:e65416d6de22 | 69 | if(startSetup){ |
dylanembed123 | 24:e65416d6de22 | 70 | startSetup=false; |
dylanembed123 | 24:e65416d6de22 | 71 | USB::getSerial().printf("Issue Command\n",myCmd[1],myCmd[5]); |
dylanembed123 | 24:e65416d6de22 | 72 | // Issue count to init waypoint entry |
dylanembed123 | 24:e65416d6de22 | 73 | Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT)); |
dylanembed123 | 39:1acea80563cf | 74 | }else{ |
dylanembed123 | 39:1acea80563cf | 75 | Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST)); |
dylanembed123 | 24:e65416d6de22 | 76 | } |
dylanembed123 | 24:e65416d6de22 | 77 | } |
dylanembed123 | 24:e65416d6de22 | 78 | // Check for mavlink message request |
dylanembed123 | 24:e65416d6de22 | 79 | if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){ |
dylanembed123 | 24:e65416d6de22 | 80 | USB::getSerial().printf("Issue Item\n"); |
dylanembed123 | 24:e65416d6de22 | 81 | // Set sequence to requested sequence |
dylanembed123 | 24:e65416d6de22 | 82 | issueItem.seq=myCmd[6]; |
dylanembed123 | 39:1acea80563cf | 83 | issueItem.lon=(float)issueItem.seq; |
dylanembed123 | 24:e65416d6de22 | 84 | // Issue mission item |
dylanembed123 | 24:e65416d6de22 | 85 | Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM)); |
dylanembed123 | 24:e65416d6de22 | 86 | } |
dylanembed123 | 24:e65416d6de22 | 87 | // Check for mission accepted |
dylanembed123 | 24:e65416d6de22 | 88 | if(myCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){ |
dylanembed123 | 33:ad63e7013801 | 89 | // Start |
dylanembed123 | 33:ad63e7013801 | 90 | Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueStart,sizeof(MAV_MISSION_ITEM)); |
dylanembed123 | 33:ad63e7013801 | 91 | wait(1); |
dylanembed123 | 33:ad63e7013801 | 92 | // Take off |
dylanembed123 | 33:ad63e7013801 | 93 | Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM)); |
dylanembed123 | 33:ad63e7013801 | 94 | wait(1); |
dylanembed123 | 24:e65416d6de22 | 95 | // Arm motors |
dylanembed123 | 24:e65416d6de22 | 96 | Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM)); |
dylanembed123 | 24:e65416d6de22 | 97 | } |
dylanembed123 | 39:1acea80563cf | 98 | // Check for waypoint count |
dylanembed123 | 39:1acea80563cf | 99 | if(myCmd[5]==44){ |
dylanembed123 | 39:1acea80563cf | 100 | USB::getSerial().printf("Waypoints tsys %d tcomp %d cnt %d\n",myCmd[8],myCmd[7],myCmd[6]); |
dylanembed123 | 39:1acea80563cf | 101 | } |
dylanembed123 | 24:e65416d6de22 | 102 | delete myCmd; |
dylanembed123 | 24:e65416d6de22 | 103 | } |
dylanembed123 | 24:e65416d6de22 | 104 | } |