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