Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

handle/mavcommands.h@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 "mbed.h"
dylanembed123 24:e65416d6de22 2 #include "mavcontrol.h"
dylanembed123 24:e65416d6de22 3 #include "adapt/usb.h"
dylanembed123 61:aa32e17f6801 4 #include "packet.h"
dylanembed123 24:e65416d6de22 5 #include <algorithm>
dylanembed123 54:fc7c8b5d4d41 6 #include"handle/dataLocation.h"
dylanembed123 66:5d43988d100c 7 #include"handle/handleCompass.h"
dylanembed123 66:5d43988d100c 8
dylanembed123 24:e65416d6de22 9 typedef struct MAV_REQUEST_LIST_S{
dylanembed123 24:e65416d6de22 10 char targSys;
dylanembed123 24:e65416d6de22 11 char targComp;
dylanembed123 24:e65416d6de22 12 }MAV_REQUEST_LIST;
dylanembed123 24:e65416d6de22 13
dylanembed123 24:e65416d6de22 14 typedef struct MAV_COUNT_S{
dylanembed123 24:e65416d6de22 15 uint16_t count;
dylanembed123 24:e65416d6de22 16 char targSys;
dylanembed123 24:e65416d6de22 17 char targComp;
dylanembed123 24:e65416d6de22 18 }MAV_COUNT;
dylanembed123 24:e65416d6de22 19
dylanembed123 24:e65416d6de22 20 typedef struct MAV_REQ_S{
dylanembed123 24:e65416d6de22 21 char targSys;
dylanembed123 24:e65416d6de22 22 char targComp;
dylanembed123 24:e65416d6de22 23 char other;
dylanembed123 24:e65416d6de22 24 char count;
dylanembed123 24:e65416d6de22 25 }MAV_REQ;
dylanembed123 24:e65416d6de22 26
dylanembed123 24:e65416d6de22 27 typedef struct MAV_APM_S{
dylanembed123 24:e65416d6de22 28 float parm1;
dylanembed123 24:e65416d6de22 29 float parm2;
dylanembed123 24:e65416d6de22 30 float parm3;
dylanembed123 24:e65416d6de22 31 float parm4;
dylanembed123 24:e65416d6de22 32 float parm5;
dylanembed123 24:e65416d6de22 33 float parm6;
dylanembed123 24:e65416d6de22 34 float parm7;
dylanembed123 24:e65416d6de22 35 uint16_t cmd;
dylanembed123 24:e65416d6de22 36 uint8_t targSys;
dylanembed123 24:e65416d6de22 37 uint8_t targComp;
dylanembed123 24:e65416d6de22 38 uint8_t confirm;
dylanembed123 24:e65416d6de22 39 } MAV_APM;
dylanembed123 24:e65416d6de22 40
dylanembed123 24:e65416d6de22 41 typedef struct MAV_MISSION_ITEM_S{
dylanembed123 24:e65416d6de22 42 float parm1;
dylanembed123 24:e65416d6de22 43 float parm2;
dylanembed123 24:e65416d6de22 44 float parm3;
dylanembed123 24:e65416d6de22 45 float parm4;
dylanembed123 24:e65416d6de22 46 float lat;
dylanembed123 24:e65416d6de22 47 float lon;
dylanembed123 24:e65416d6de22 48 float alt;
dylanembed123 24:e65416d6de22 49 uint16_t seq;
dylanembed123 24:e65416d6de22 50 uint16_t cmd;
dylanembed123 24:e65416d6de22 51 uint8_t targSys;
dylanembed123 24:e65416d6de22 52 uint8_t targComp;
dylanembed123 24:e65416d6de22 53 uint8_t frame;
dylanembed123 24:e65416d6de22 54 uint8_t current; // set to true/1 to make current
dylanembed123 24:e65416d6de22 55 uint8_t autoContinue;// set to true/1 to auto continue
dylanembed123 24:e65416d6de22 56 uint8_t confirm;
dylanembed123 24:e65416d6de22 57 } MAV_MISSION_ITEM;
dylanembed123 24:e65416d6de22 58
dylanembed123 39:1acea80563cf 59 typedef struct MAV_DATA_STREAM_S{
dylanembed123 52:b4dddb28dffa 60 uint16_t rate; // Hz
dylanembed123 39:1acea80563cf 61 uint8_t targSys;
dylanembed123 39:1acea80563cf 62 uint8_t targComp;
dylanembed123 39:1acea80563cf 63 uint8_t streamID;
dylanembed123 39:1acea80563cf 64 uint8_t start; // Set to 1 to start and 0 to stop
dylanembed123 39:1acea80563cf 65 } MAV_DATA_STREAM;
dylanembed123 39:1acea80563cf 66
dylanembed123 52:b4dddb28dffa 67 typedef struct MAV_LOCDATA_S{
dylanembed123 61:aa32e17f6801 68 int32_t alt;//timestamp;
krobertson 60:bf851bafc807 69 int32_t lat;
krobertson 60:bf851bafc807 70 int32_t lon;
dylanembed123 61:aa32e17f6801 71 int32_t altB;
dylanembed123 61:aa32e17f6801 72 //int32_t ralt;
dylanembed123 61:aa32e17f6801 73 //int16_t x;
dylanembed123 61:aa32e17f6801 74 //int16_t y;
dylanembed123 61:aa32e17f6801 75 //int16_t z;
dylanembed123 61:aa32e17f6801 76 //uint16_t hdg;
dylanembed123 52:b4dddb28dffa 77 }MAV_LOCDATA;
dylanembed123 52:b4dddb28dffa 78
dylanembed123 24:e65416d6de22 79 class MavCmd{
dylanembed123 24:e65416d6de22 80 private:
dylanembed123 24:e65416d6de22 81 MAV_REQUEST_LIST req;
dylanembed123 24:e65416d6de22 82 MAV_APM issueArm;
dylanembed123 24:e65416d6de22 83 MAV_APM issueDisArm;
dylanembed123 24:e65416d6de22 84 MAV_COUNT issueCount;
dylanembed123 24:e65416d6de22 85 MAV_MISSION_ITEM issueItem;
dylanembed123 33:ad63e7013801 86 MAV_MISSION_ITEM issueStart;
dylanembed123 24:e65416d6de22 87 MAV_MISSION_ITEM issueTakeOff;
dylanembed123 52:b4dddb28dffa 88 MAV_DATA_STREAM issueStreamReq;
dylanembed123 24:e65416d6de22 89
dylanembed123 24:e65416d6de22 90 // Local variables
dylanembed123 24:e65416d6de22 91 bool startSetup; // Set to true to initiate startup sequence
dylanembed123 24:e65416d6de22 92 int readState; // Read State
dylanembed123 24:e65416d6de22 93 int realLen; // How many more bytes need to be read
dylanembed123 24:e65416d6de22 94 char nextCmd[512+1]; // Temperary storage of next command
dylanembed123 24:e65416d6de22 95 int readIndex; // Current index in next cmd (also the size)
dylanembed123 24:e65416d6de22 96 static MavCmd* mavcmd;
dylanembed123 24:e65416d6de22 97 bool initialized;
dylanembed123 54:fc7c8b5d4d41 98 double cLat,cLon,cAlt;
dylanembed123 54:fc7c8b5d4d41 99 bool hasMoved();
dylanembed123 54:fc7c8b5d4d41 100 bool moveValid;
dylanembed123 66:5d43988d100c 101 double calibAlt;
dylanembed123 24:e65416d6de22 102 public:
dylanembed123 24:e65416d6de22 103 MavCmd():initialized(false){}
dylanembed123 24:e65416d6de22 104 char* getNextCmd();
dylanembed123 24:e65416d6de22 105 void handleNextCmd();
dylanembed123 24:e65416d6de22 106 void setupCmds();
dylanembed123 66:5d43988d100c 107 void setup(){if(!initialized){setupCmds();initialized=true;calibAlt=0.0f;}}
krobertson 58:ea73523cf04b 108 void run(){
krobertson 58:ea73523cf04b 109 setup();
dylanembed123 61:aa32e17f6801 110 readState=0;
dylanembed123 61:aa32e17f6801 111 while(Mav::getSerial().readable()>0){char input=Mav::getSerial().getc();}
dylanembed123 66:5d43988d100c 112 //&&DH::Locs().getC(LHType_locs,DH::Locs().getI()).getLat()!=0&&DH::Locs().getC(LHType_locs,DH::Locs().getI()).getLon()!=0
dylanembed123 66:5d43988d100c 113 if(1==2&&DH::Locs().getI(LHType_targ,LHIType_size)>0&&hasMoved()){
dylanembed123 61:aa32e17f6801 114 USB::getSerial().printf("USING NEW WAYPOINT!!!\n");
dylanembed123 61:aa32e17f6801 115 // Issue count to init waypoint entry
dylanembed123 61:aa32e17f6801 116 Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
dylanembed123 61:aa32e17f6801 117 }else{
dylanembed123 61:aa32e17f6801 118 USB::getSerial().printf("Grabbing Stream\n");
dylanembed123 61:aa32e17f6801 119 Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
dylanembed123 61:aa32e17f6801 120 }
dylanembed123 61:aa32e17f6801 121 //wait_ms(10);
dylanembed123 61:aa32e17f6801 122 for(int i=0;i<1000;i++){
dylanembed123 61:aa32e17f6801 123 if(getPS().rx_ready_with_timeout(&Mav::getSerial(),0,1000)==1){
dylanembed123 61:aa32e17f6801 124 }else{
dylanembed123 61:aa32e17f6801 125 readState=0;
dylanembed123 61:aa32e17f6801 126 }
dylanembed123 61:aa32e17f6801 127 handleNextCmd();
dylanembed123 61:aa32e17f6801 128 wait_ms(1);
dylanembed123 61:aa32e17f6801 129 }
dylanembed123 61:aa32e17f6801 130 USB::getSerial().printf("\n");
krobertson 58:ea73523cf04b 131 }
dylanembed123 24:e65416d6de22 132 static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;}
dylanembed123 24:e65416d6de22 133 };