Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
Diff: handle/mavcommands.h
- Revision:
- 61:aa32e17f6801
- Parent:
- 60:bf851bafc807
- Child:
- 63:6e4afaa03d07
--- a/handle/mavcommands.h Sat Apr 26 23:01:45 2014 +0000 +++ b/handle/mavcommands.h Sun Apr 27 02:31:00 2014 +0000 @@ -1,6 +1,7 @@ #include "mbed.h" #include "mavcontrol.h" #include "adapt/usb.h" +#include "packet.h" #include <algorithm> #include"handle/dataLocation.h" typedef struct MAV_REQUEST_LIST_S{ @@ -62,13 +63,15 @@ } MAV_DATA_STREAM; typedef struct MAV_LOCDATA_S{ - int32_t timestamp; + int32_t alt;//timestamp; int32_t lat; int32_t lon; - int32_t alt; - uint16_t x; - uint16_t y; - uint16_t z; + int32_t altB; + //int32_t ralt; + //int16_t x; + //int16_t y; + //int16_t z; + //uint16_t hdg; }MAV_LOCDATA; class MavCmd{ @@ -101,7 +104,27 @@ void setup(){if(!initialized){setupCmds();initialized=true;}} void run(){ setup(); - for(int i=0;i<100;i++){readState=0;handleNextCmd();wait_ms(1);} + readState=0; + while(Mav::getSerial().readable()>0){char input=Mav::getSerial().getc();} + if(hasMoved()){ + USB::getSerial().printf("USING NEW WAYPOINT!!!\n"); + // Issue count to init waypoint entry + Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT)); + }else{ + USB::getSerial().printf("Grabbing Stream\n"); + Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM)); + } + //wait_ms(10); + for(int i=0;i<1000;i++){ + if(getPS().rx_ready_with_timeout(&Mav::getSerial(),0,1000)==1){ + }else{ + readState=0; + } + handleNextCmd(); + wait_ms(1); + USB::getSerial().printf("."); + } + USB::getSerial().printf("\n"); } static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;} };