Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
Diff: main.cpp
- Revision:
- 22:9880a26886db
- Parent:
- 21:c546eab07e28
- Child:
- 23:497f8faa908e
--- a/main.cpp Mon Apr 21 13:54:55 2014 +0000 +++ b/main.cpp Tue Apr 22 00:10:16 2014 +0000 @@ -112,7 +112,14 @@ uint16_t count; }MAV_COUNT; -typedef struct mav_APM_S{ +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; @@ -124,7 +131,26 @@ uint8_t targSys; uint8_t targComp; uint8_t confirm; -} mav_APM; +} 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; + int main() { @@ -162,30 +188,59 @@ USB::getSerial().printf("System startup...\n"); char nextCmd[512+1]; int readIndex=0; - bool reading=false; MAV_REQUEST_LIST req;req.targSys=1;req.targComp=1; - mav_APM issueCmd; - issueCmd.targSys=1;issueCmd.targComp=1;issueCmd.cmd=17;issueCmd.parm1=76;issueCmd.parm2=5.0f;issueCmd.parm2=6.0f;issueCmd.parm3=7.0f; + MAV_APM issueCmd;issueCmd.targSys=1;issueCmd.targComp=1;issueCmd.cmd=17;issueCmd.parm1=76;issueCmd.parm2=5.0f;issueCmd.parm2=6.0f;issueCmd.parm3=7.0f; + MAV_COUNT issueCount;issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1; + MAV_MISSION_ITEM issueItem;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=1; + bool firstRun=true; + bool gotNewRead=false; + int readState=0; + int realLen=0; while(true){ + gotNewRead=false; if(Mav::getSerial().readable()>0){ char input=Mav::getSerial().getc(); - USB::getSerial().printf("> %x\n",input); - if(input==0xFE){ - reading=true; + USB::getSerial().printf("> %x %d %d\n",input,readState,realLen); + if(readState==0&&input==0xFE){ + readState=1; readIndex=1; - }else if(reading){ + }else if(readState!=0){ nextCmd[std::min(readIndex++,512)]=input; + if(readState==1){ + realLen=input+5-1; + readState=2; + } + if(readState==2){ + realLen--; + if(realLen==0){ + readState=0; + gotNewRead=true; + } + } } } // Output debug info - if(readIndex==1&&reading){ + if(gotNewRead){ USB::getSerial().printf("Got CMD len %d messageid %d \n",nextCmd[1],nextCmd[5]); if(nextCmd[5]==0){ //Mav::sendOutput(0,input,9); - //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,(char*)&issueCmd,sizeof(mav_APM)); - Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST)); + //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,(char*)&issueCmd,sizeof(MAV_APM)); //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST)); + if(firstRun){ + USB::getSerial().printf("Issue Command\n",nextCmd[1],nextCmd[5]); + Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT)); + //Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM)); + //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST)); + } + firstRun=false; + } + if(nextCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){ + USB::getSerial().printf("Issue Item\n"); + MAV_REQ* incnt=(MAV_REQ*)(&(nextCmd[6])); + issueItem.seq=nextCmd[6];//incnt->count; + USB::getSerial().printf("Got request for %d %d\n",nextCmd[9],incnt->count); + Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM)); } } }