Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
handle/mavcommands.h
- Committer:
- krobertson
- Date:
- 2014-04-26
- Revision:
- 60:bf851bafc807
- Parent:
- 58:ea73523cf04b
- Child:
- 61:aa32e17f6801
File content as of revision 60:bf851bafc807:
#include "mbed.h" #include "mavcontrol.h" #include "adapt/usb.h" #include <algorithm> #include"handle/dataLocation.h" typedef struct MAV_REQUEST_LIST_S{ char targSys; char targComp; }MAV_REQUEST_LIST; typedef struct MAV_COUNT_S{ uint16_t count; char targSys; char targComp; }MAV_COUNT; typedef struct MAV_REQ_S{ char targSys; char targComp; char other; char count; }MAV_REQ; typedef struct MAV_APM_S{ float parm1; float parm2; float parm3; float parm4; float parm5; float parm6; float parm7; uint16_t cmd; uint8_t targSys; uint8_t targComp; uint8_t confirm; } MAV_APM; typedef struct MAV_MISSION_ITEM_S{ float parm1; float parm2; float parm3; float parm4; float lat; float lon; float alt; uint16_t seq; uint16_t cmd; uint8_t targSys; uint8_t targComp; uint8_t frame; uint8_t current; // set to true/1 to make current uint8_t autoContinue;// set to true/1 to auto continue uint8_t confirm; } MAV_MISSION_ITEM; typedef struct MAV_DATA_STREAM_S{ uint16_t rate; // Hz uint8_t targSys; uint8_t targComp; uint8_t streamID; uint8_t start; // Set to 1 to start and 0 to stop } MAV_DATA_STREAM; typedef struct MAV_LOCDATA_S{ int32_t timestamp; int32_t lat; int32_t lon; int32_t alt; uint16_t x; uint16_t y; uint16_t z; }MAV_LOCDATA; class MavCmd{ private: MAV_REQUEST_LIST req; MAV_APM issueArm; MAV_APM issueDisArm; MAV_COUNT issueCount; MAV_MISSION_ITEM issueItem; MAV_MISSION_ITEM issueStart; MAV_MISSION_ITEM issueTakeOff; MAV_DATA_STREAM issueStreamReq; // Local variables bool startSetup; // Set to true to initiate startup sequence int readState; // Read State int realLen; // How many more bytes need to be read char nextCmd[512+1]; // Temperary storage of next command int readIndex; // Current index in next cmd (also the size) static MavCmd* mavcmd; bool initialized; double cLat,cLon,cAlt; bool hasMoved(); bool moveValid; public: MavCmd():initialized(false){} char* getNextCmd(); void handleNextCmd(); void setupCmds(); void setup(){if(!initialized){setupCmds();initialized=true;}} void run(){ setup(); for(int i=0;i<100;i++){readState=0;handleNextCmd();wait_ms(1);} } static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;} };