Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

handle/mavcommands.cpp@66:5d43988d100c, 2014-05-05 (annotated)

Committer:
dylanembed123
Date:
Mon May 05 13:20:35 2014 +0000
Revision:
66:5d43988d100c
Parent:
64:d4818fb7813c 
Final Project;

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
krobertson 60:bf851bafc807 4 #define MAX_READ_LOOP 2560
dylanembed123 24:e65416d6de22 5
dylanembed123 54:fc7c8b5d4d41 6 #define ENABLE_MOTORS false
dylanembed123 54:fc7c8b5d4d41 7
dylanembed123 24:e65416d6de22 8 MavCmd* MavCmd::mavcmd=NULL;
dylanembed123 24:e65416d6de22 9
dylanembed123 54:fc7c8b5d4d41 10 static void fixLatLonAlt(double& latitude,double& longitude,double& altitude){
dylanembed123 54:fc7c8b5d4d41 11 latitude/=10000000.0f;
dylanembed123 54:fc7c8b5d4d41 12 longitude/=10000000.0f;
dylanembed123 66:5d43988d100c 13 //altitude/=10000000.0f;
dylanembed123 66:5d43988d100c 14 //USB::getSerial().printf("Got2 lat %f\n",latitude);USB::getSerial().printf("Got lon2 %f\n",longitude);USB::getSerial().printf("Got alt2 %f\n",altitude);
krobertson 60:bf851bafc807 15 return;
dylanembed123 47:fd373c4ea831 16 int degrees;
dylanembed123 47:fd373c4ea831 17 double minutes;
dylanembed123 47:fd373c4ea831 18 degrees = (int)(latitude/100);
dylanembed123 47:fd373c4ea831 19 minutes = latitude - degrees*100;
dylanembed123 47:fd373c4ea831 20 latitude = degrees + minutes/60;
dylanembed123 47:fd373c4ea831 21 degrees = (int)(longitude/100);
dylanembed123 47:fd373c4ea831 22 minutes = longitude - degrees*100;
dylanembed123 47:fd373c4ea831 23 longitude = degrees + minutes/60;
dylanembed123 47:fd373c4ea831 24 }
dylanembed123 47:fd373c4ea831 25
dylanembed123 54:fc7c8b5d4d41 26 bool MavCmd::hasMoved(){
dylanembed123 57:0b725b24f0c7 27 double nLat=DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ)).getLat();
dylanembed123 57:0b725b24f0c7 28 double nLon=DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ)).getLon();
dylanembed123 57:0b725b24f0c7 29 double nAlt=DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ)).getAlt();
dylanembed123 64:d4818fb7813c 30 USB::getSerial().printf("HM Lat %f\n",nLat);
dylanembed123 64:d4818fb7813c 31 USB::getSerial().printf("HM Lon %f\n",nLon);
dylanembed123 64:d4818fb7813c 32 USB::getSerial().printf("HM Alt %f\n",nAlt);
dylanembed123 66:5d43988d100c 33 //if(moveValid&&cLat==nLat&&cLon==nLon&&cAlt==nAlt)return false;
dylanembed123 54:fc7c8b5d4d41 34 moveValid=false;
dylanembed123 54:fc7c8b5d4d41 35 cLat=nLat;
dylanembed123 54:fc7c8b5d4d41 36 cLon=nLon;
dylanembed123 54:fc7c8b5d4d41 37 cAlt=nAlt;
dylanembed123 54:fc7c8b5d4d41 38 return true;
dylanembed123 54:fc7c8b5d4d41 39 }
dylanembed123 54:fc7c8b5d4d41 40
dylanembed123 24:e65416d6de22 41 void MavCmd::setupCmds(){
dylanembed123 24:e65416d6de22 42 // Request item list
dylanembed123 24:e65416d6de22 43 req.targSys=1;req.targComp=1;
dylanembed123 24:e65416d6de22 44 // Issue arm motors
dylanembed123 24:e65416d6de22 45 issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f;
dylanembed123 24:e65416d6de22 46 // Issue disarm motors
dylanembed123 24:e65416d6de22 47 issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
dylanembed123 24:e65416d6de22 48 // Issue a mission count
dylanembed123 66:5d43988d100c 49 issueCount.targSys=1;issueCount.targComp=1;issueCount.count=5;
dylanembed123 24:e65416d6de22 50 // Issue a mission item
dylanembed123 39:1acea80563cf 51 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 52 // Issue a take off item
dylanembed123 24:e65416d6de22 53 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 54 // Issue start
dylanembed123 33:ad63e7013801 55 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 56 // Issue location request
dylanembed123 52:b4dddb28dffa 57 issueStreamReq.targSys=1;issueStreamReq.targComp=1;issueStreamReq.streamID=6;issueStreamReq.rate=5;issueStreamReq.start=1;
dylanembed123 52:b4dddb28dffa 58
dylanembed123 24:e65416d6de22 59 // Local variables
dylanembed123 24:e65416d6de22 60 startSetup=true;// Set to true to initiate startup sequence
dylanembed123 24:e65416d6de22 61 readState=0; // Read State
dylanembed123 24:e65416d6de22 62 realLen=0; // How many more bytes need to be read
dylanembed123 24:e65416d6de22 63 readIndex=0; // Current index in next cmd (also the size)
dylanembed123 54:fc7c8b5d4d41 64 cLat=cLon=cAlt=0.0f;
dylanembed123 24:e65416d6de22 65 }
dylanembed123 24:e65416d6de22 66
dylanembed123 24:e65416d6de22 67 char* MavCmd::getNextCmd(){
dylanembed123 61:aa32e17f6801 68 if(Mav::getSerial().readable()>0){
dylanembed123 24:e65416d6de22 69 char input=Mav::getSerial().getc();
dylanembed123 61:aa32e17f6801 70 bool start=false;
dylanembed123 24:e65416d6de22 71 if(readState==0&&input==0xFE){
dylanembed123 61:aa32e17f6801 72 USB::getSerial().printf("\nS: ");
dylanembed123 24:e65416d6de22 73 readState=1;
dylanembed123 24:e65416d6de22 74 readIndex=1;
dylanembed123 61:aa32e17f6801 75 start=true;
dylanembed123 61:aa32e17f6801 76 }
dylanembed123 61:aa32e17f6801 77 USB::getSerial().printf(">%x~",input);
dylanembed123 61:aa32e17f6801 78 if(!start&&readState!=0){
dylanembed123 24:e65416d6de22 79 nextCmd[std::min(readIndex++,512)]=input;
dylanembed123 24:e65416d6de22 80 if(readState==1){
dylanembed123 24:e65416d6de22 81 realLen=input+5-1;
dylanembed123 24:e65416d6de22 82 readState=2;
dylanembed123 24:e65416d6de22 83 }
dylanembed123 24:e65416d6de22 84 if(readState==2){
dylanembed123 24:e65416d6de22 85 realLen--;
dylanembed123 61:aa32e17f6801 86 if(readIndex-1==5&&nextCmd[5]==33){realLen=12;}
dylanembed123 24:e65416d6de22 87 if(realLen==0){
dylanembed123 61:aa32e17f6801 88 USB::getSerial().printf(" E\n");
dylanembed123 24:e65416d6de22 89 readState=0;
dylanembed123 24:e65416d6de22 90 char* output=new char[readIndex];
dylanembed123 24:e65416d6de22 91 for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];}
dylanembed123 24:e65416d6de22 92 return output;
dylanembed123 24:e65416d6de22 93 }
dylanembed123 24:e65416d6de22 94 }
dylanembed123 24:e65416d6de22 95 }
dylanembed123 24:e65416d6de22 96 }
dylanembed123 61:aa32e17f6801 97 return NULL;
dylanembed123 24:e65416d6de22 98 }
dylanembed123 24:e65416d6de22 99
dylanembed123 24:e65416d6de22 100 // Handle the next command (if one is available)
dylanembed123 24:e65416d6de22 101 void MavCmd::handleNextCmd(){
dylanembed123 24:e65416d6de22 102 char* myCmd=getNextCmd();
dylanembed123 24:e65416d6de22 103 // Output debug info
dylanembed123 24:e65416d6de22 104 if(myCmd!=NULL){
dylanembed123 24:e65416d6de22 105 USB::getSerial().printf("Got CMD len %d messageid %d \n",myCmd[1],myCmd[5]);
dylanembed123 24:e65416d6de22 106 if(myCmd[5]==0){
dylanembed123 61:aa32e17f6801 107 //USB::getSerial().printf("Issue Command\n",myCmd[1],myCmd[5]);
dylanembed123 54:fc7c8b5d4d41 108 // Issue count to init waypoint entry
dylanembed123 54:fc7c8b5d4d41 109 //Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
dylanembed123 54:fc7c8b5d4d41 110 // Start sending location data
dylanembed123 54:fc7c8b5d4d41 111 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
dylanembed123 54:fc7c8b5d4d41 112 //else{
dylanembed123 52:b4dddb28dffa 113 // Start sending location data
dylanembed123 52:b4dddb28dffa 114 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
dylanembed123 52:b4dddb28dffa 115 //}
dylanembed123 24:e65416d6de22 116 }
dylanembed123 24:e65416d6de22 117 // Check for mavlink message request
dylanembed123 24:e65416d6de22 118 if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
dylanembed123 66:5d43988d100c 119 hasMoved();
dylanembed123 54:fc7c8b5d4d41 120 USB::getSerial().printf("Issue Item %f %f %f\n",cLat,cLon,cAlt);
dylanembed123 24:e65416d6de22 121 // Set sequence to requested sequence
dylanembed123 24:e65416d6de22 122 issueItem.seq=myCmd[6];
dylanembed123 54:fc7c8b5d4d41 123 issueItem.lat=(float)cLat;
dylanembed123 54:fc7c8b5d4d41 124 issueItem.lon=(float)cLon;
dylanembed123 54:fc7c8b5d4d41 125 issueItem.alt=(float)cAlt;
dylanembed123 24:e65416d6de22 126 // Issue mission item
dylanembed123 24:e65416d6de22 127 Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
dylanembed123 24:e65416d6de22 128 }
dylanembed123 24:e65416d6de22 129 // Check for mission accepted
dylanembed123 24:e65416d6de22 130 if(myCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){
dylanembed123 64:d4818fb7813c 131 USB::getSerial().printf("Ack\n");
dylanembed123 54:fc7c8b5d4d41 132 moveValid=true;
dylanembed123 54:fc7c8b5d4d41 133 if(startSetup&&ENABLE_MOTORS){
dylanembed123 54:fc7c8b5d4d41 134 startSetup=false;
dylanembed123 54:fc7c8b5d4d41 135 // Start
dylanembed123 54:fc7c8b5d4d41 136 Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueStart,sizeof(MAV_MISSION_ITEM));
dylanembed123 54:fc7c8b5d4d41 137 wait(1);
dylanembed123 54:fc7c8b5d4d41 138 // Take off
dylanembed123 54:fc7c8b5d4d41 139 Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM));
dylanembed123 54:fc7c8b5d4d41 140 wait(1);
dylanembed123 54:fc7c8b5d4d41 141 // Start sending location data
dylanembed123 54:fc7c8b5d4d41 142 Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
dylanembed123 54:fc7c8b5d4d41 143 wait(1);
dylanembed123 54:fc7c8b5d4d41 144 // Arm motors
dylanembed123 54:fc7c8b5d4d41 145 Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
dylanembed123 54:fc7c8b5d4d41 146 }
dylanembed123 24:e65416d6de22 147 }
dylanembed123 52:b4dddb28dffa 148 // Check for GPS
dylanembed123 52:b4dddb28dffa 149 if(myCmd[5]==33){
dylanembed123 61:aa32e17f6801 150 //for(int i=0;i<18;i++){USB::getSerial().printf("Got loc %d/18 %d\n",i,myCmd[6+i]);}
dylanembed123 52:b4dddb28dffa 151 MAV_LOCDATA* input=(MAV_LOCDATA*)&myCmd[6];
krobertson 60:bf851bafc807 152
dylanembed123 66:5d43988d100c 153 //USB::getSerial().printf("Got lat %d\n",input->lat);USB::getSerial().printf("Got lon %d\n",input->lon);USB::getSerial().printf("Got alt %d\n",input->alt);
dylanembed123 66:5d43988d100c 154 double lat=(double)*((int32_t*)&myCmd[10]);double lon=(double)*((int32_t*)&myCmd[10+4]);
dylanembed123 66:5d43988d100c 155 double alt=(double)*((int32_t*)&myCmd[10-4]);
dylanembed123 54:fc7c8b5d4d41 156 fixLatLonAlt(lat,lon,alt);
dylanembed123 66:5d43988d100c 157 if(calibAlt==0){
dylanembed123 66:5d43988d100c 158 calibAlt=alt;
dylanembed123 66:5d43988d100c 159 }
dylanembed123 66:5d43988d100c 160 USB::getSerial().printf("Got lat %f\n",lat);USB::getSerial().printf("Got lon %f\n",lon);USB::getSerial().printf("Got alt %f\n",alt-calibAlt);USB::getSerial().printf("Got aalt %f\n",alt);
dylanembed123 66:5d43988d100c 161 USB::getSerial().printf("Got heading %f\n",compassHandle::getCompassHand().heading);
dylanembed123 66:5d43988d100c 162 DH::Locs().add(LHType_locs,DataLocation(lat,lon,alt-calibAlt,0,compassHandle::getCompassHand().heading));
dylanembed123 66:5d43988d100c 163 USB::getSerial().printf("USING NEW WAYPOINT!!!\n");
dylanembed123 66:5d43988d100c 164 // Issue count to init waypoint entry
dylanembed123 66:5d43988d100c 165 Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
dylanembed123 52:b4dddb28dffa 166 }
dylanembed123 39:1acea80563cf 167 // Check for waypoint count
dylanembed123 39:1acea80563cf 168 if(myCmd[5]==44){
dylanembed123 39:1acea80563cf 169 USB::getSerial().printf("Waypoints tsys %d tcomp %d cnt %d\n",myCmd[8],myCmd[7],myCmd[6]);
dylanembed123 39:1acea80563cf 170 }
dylanembed123 24:e65416d6de22 171 delete myCmd;
dylanembed123 24:e65416d6de22 172 }
dylanembed123 24:e65416d6de22 173 }