Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

handle/mavcommands.cpp@47:fd373c4ea831, 2014-04-26 (annotated)

Committer:
dylanembed123
Date:
Sat Apr 26 04:01:13 2014 +0000
Revision:
47:fd373c4ea831
Parent:
39:1acea80563cf
Child:
53:d785a6f6abdd
Fix add command for being called more times

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