Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

handle/mavcommands.cpp@52:b4dddb28dffa, 2014-04-26 (annotated)

Committer:
dylanembed123
Date:
Sat Apr 26 03:35:53 2014 +0000
Revision:
52:b4dddb28dffa
Parent:
39:1acea80563cf
Child:
53:d785a6f6abdd
Need to pull;

Who changed what in which revision?

UserRevisionLine numberNew 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 52:b4dddb28dffa 23 // Issue location request
dylanembed123 52:b4dddb28dffa 24 issueStreamReq.targSys=1;issueStreamReq.targComp=1;issueStreamReq.streamID=6;issueStreamReq.rate=5;issueStreamReq.start=1;
dylanembed123 52:b4dddb28dffa 25
dylanembed123 24:e65416d6de22 26 // Local variables
dylanembed123 24:e65416d6de22 27 startSetup=true;// Set to true to initiate startup sequence
dylanembed123 24:e65416d6de22 28 readState=0; // Read State
dylanembed123 24:e65416d6de22 29 realLen=0; // How many more bytes need to be read
dylanembed123 24:e65416d6de22 30 readIndex=0; // Current index in next cmd (also the size)
dylanembed123 24:e65416d6de22 31 }
dylanembed123 24:e65416d6de22 32
dylanembed123 24:e65416d6de22 33 char* MavCmd::getNextCmd(){
dylanembed123 24:e65416d6de22 34 for(int readcnt=0;readcnt<MAX_READ_LOOP;readcnt++){
dylanembed123 24:e65416d6de22 35 if(Mav::getSerial().readable()<=0){
dylanembed123 24:e65416d6de22 36 // Nothing to run, wait until next time
dylanembed123 24:e65416d6de22 37 return NULL;
dylanembed123 24:e65416d6de22 38 }else{
dylanembed123 24:e65416d6de22 39 char input=Mav::getSerial().getc();
dylanembed123 52:b4dddb28dffa 40 USB::getSerial().printf(">%x ",input);
dylanembed123 24:e65416d6de22 41 if(readState==0&&input==0xFE){
dylanembed123 24:e65416d6de22 42 readState=1;
dylanembed123 24:e65416d6de22 43 readIndex=1;
dylanembed123 24:e65416d6de22 44 }else if(readState!=0){
dylanembed123 24:e65416d6de22 45 nextCmd[std::min(readIndex++,512)]=input;
dylanembed123 24:e65416d6de22 46 if(readState==1){
dylanembed123 24:e65416d6de22 47 realLen=input+5-1;
dylanembed123 24:e65416d6de22 48 readState=2;
dylanembed123 24:e65416d6de22 49 }
dylanembed123 24:e65416d6de22 50 if(readState==2){
dylanembed123 24:e65416d6de22 51 realLen--;
dylanembed123 24:e65416d6de22 52 if(realLen==0){
dylanembed123 52:b4dddb28dffa 53 USB::getSerial().printf("\n");
dylanembed123 24:e65416d6de22 54 readState=0;
dylanembed123 24:e65416d6de22 55 char* output=new char[readIndex];
dylanembed123 24:e65416d6de22 56 for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];}
dylanembed123 24:e65416d6de22 57 return output;
dylanembed123 24:e65416d6de22 58 }
dylanembed123 24:e65416d6de22 59 }
dylanembed123 24:e65416d6de22 60 }
dylanembed123 24:e65416d6de22 61 }
dylanembed123 24:e65416d6de22 62 }
dylanembed123 24:e65416d6de22 63 }
dylanembed123 24:e65416d6de22 64
dylanembed123 24:e65416d6de22 65 // Handle the next command (if one is available)
dylanembed123 24:e65416d6de22 66 void MavCmd::handleNextCmd(){
dylanembed123 24:e65416d6de22 67 char* myCmd=getNextCmd();
dylanembed123 24:e65416d6de22 68 // Output debug info
dylanembed123 24:e65416d6de22 69 if(myCmd!=NULL){
dylanembed123 24:e65416d6de22 70 USB::getSerial().printf("Got CMD len %d messageid %d \n",myCmd[1],myCmd[5]);
dylanembed123 24:e65416d6de22 71 if(myCmd[5]==0){
dylanembed123 24:e65416d6de22 72 if(startSetup){
dylanembed123 24:e65416d6de22 73 startSetup=false;
dylanembed123 24:e65416d6de22 74 USB::getSerial().printf("Issue Command\n",myCmd[1],myCmd[5]);
dylanembed123 24:e65416d6de22 75 // Issue count to init waypoint entry
dylanembed123 52:b4dddb28dffa 76 //Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
dylanembed123 52:b4dddb28dffa 77 // Start sending location data
dylanembed123 52:b4dddb28dffa 78 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
dylanembed123 52:b4dddb28dffa 79 }//else{
dylanembed123 52:b4dddb28dffa 80 // Start sending location data
dylanembed123 52:b4dddb28dffa 81 Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
dylanembed123 52:b4dddb28dffa 82 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
dylanembed123 52:b4dddb28dffa 83 //}
dylanembed123 24:e65416d6de22 84 }
dylanembed123 24:e65416d6de22 85 // Check for mavlink message request
dylanembed123 24:e65416d6de22 86 if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
dylanembed123 24:e65416d6de22 87 USB::getSerial().printf("Issue Item\n");
dylanembed123 24:e65416d6de22 88 // Set sequence to requested sequence
dylanembed123 24:e65416d6de22 89 issueItem.seq=myCmd[6];
dylanembed123 39:1acea80563cf 90 issueItem.lon=(float)issueItem.seq;
dylanembed123 24:e65416d6de22 91 // Issue mission item
dylanembed123 24:e65416d6de22 92 Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
dylanembed123 24:e65416d6de22 93 }
dylanembed123 24:e65416d6de22 94 // Check for mission accepted
dylanembed123 24:e65416d6de22 95 if(myCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){
dylanembed123 33:ad63e7013801 96 // Start
dylanembed123 33:ad63e7013801 97 Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueStart,sizeof(MAV_MISSION_ITEM));
dylanembed123 33:ad63e7013801 98 wait(1);
dylanembed123 33:ad63e7013801 99 // Take off
dylanembed123 33:ad63e7013801 100 Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM));
dylanembed123 33:ad63e7013801 101 wait(1);
dylanembed123 52:b4dddb28dffa 102 // Start sending location data
dylanembed123 52:b4dddb28dffa 103 Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
dylanembed123 52:b4dddb28dffa 104 wait(1);
dylanembed123 24:e65416d6de22 105 // Arm motors
dylanembed123 24:e65416d6de22 106 Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
dylanembed123 24:e65416d6de22 107 }
dylanembed123 52:b4dddb28dffa 108 // Check for GPS
dylanembed123 52:b4dddb28dffa 109 if(myCmd[5]==33){
dylanembed123 52:b4dddb28dffa 110 for(int i=0;i<18;i++){USB::getSerial().printf("Got loc %d/18 %d\n",i,myCmd[6+i]);}
dylanembed123 52:b4dddb28dffa 111 MAV_LOCDATA* input=(MAV_LOCDATA*)&myCmd[6];
dylanembed123 52:b4dddb28dffa 112 USB::getSerial().printf("Got lat %d\n",input->lat);// %d %d %d %d %d,input->lon,input->alt,input->x,input->y,input->z);
dylanembed123 52:b4dddb28dffa 113 USB::getSerial().printf("Got lon %d\n",input->lon);
dylanembed123 52:b4dddb28dffa 114 USB::getSerial().printf("Got alt %d\n",input->alt);
dylanembed123 52:b4dddb28dffa 115 }
dylanembed123 39:1acea80563cf 116 // Check for waypoint count
dylanembed123 39:1acea80563cf 117 if(myCmd[5]==44){
dylanembed123 39:1acea80563cf 118 USB::getSerial().printf("Waypoints tsys %d tcomp %d cnt %d\n",myCmd[8],myCmd[7],myCmd[6]);
dylanembed123 39:1acea80563cf 119 }
dylanembed123 24:e65416d6de22 120 delete myCmd;
dylanembed123 24:e65416d6de22 121 }
dylanembed123 24:e65416d6de22 122 }