Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
Revision 23:497f8faa908e, committed 2014-04-22
- Comitter:
- dylanembed123
- Date:
- Tue Apr 22 13:29:31 2014 +0000
- Parent:
- 22:9880a26886db
- Child:
- 24:e65416d6de22
- Commit message:
- Arm motors and send take off command
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mavcontrol.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Apr 22 00:10:16 2014 +0000 +++ b/main.cpp Tue Apr 22 13:29:31 2014 +0000 @@ -107,9 +107,9 @@ }MAV_REQUEST_LIST; typedef struct MAV_COUNT_S{ + uint16_t count; char targSys; char targComp; - uint16_t count; }MAV_COUNT; typedef struct MAV_REQ_S{ @@ -154,6 +154,9 @@ int main() { + USB::getSerial().printf("Wait 20\n"); + wait(20); + /* char input[9]; input[0]=0x00; input[1]=0x00; @@ -190,9 +193,17 @@ int readIndex=0; 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; + // Issue arm motors + MAV_APM issueArm;issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f; + // Issue disarm motors + MAV_APM issueDisArm;issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f; + // Issue a mission count MAV_COUNT issueCount;issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1; + // Issue a mission item 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; + // Issue a take off item + MAV_MISSION_ITEM issueTakeOff;issueTakeOff.targSys=1;issueTakeOff.targComp=1;issueTakeOff.lat=5.0f;issueTakeOff.lon=6.0f;issueTakeOff.alt=7.0f;issueTakeOff.cmd=22;issueTakeOff.seq=0;issueTakeOff.current=1; + bool firstRun=true; bool gotNewRead=false; int readState=0; @@ -225,7 +236,7 @@ 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_LONG,(char*)&issueArm,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]); @@ -242,6 +253,12 @@ 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)); } + if(nextCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){ + // Arm motors + Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM)); + // Take off + Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM)); + } } }
--- a/mavcontrol.h Tue Apr 22 00:10:16 2014 +0000 +++ b/mavcontrol.h Tue Apr 22 13:29:31 2014 +0000 @@ -1,11 +1,12 @@ #define MAVLINK_MSG_ID_PING 4 -#define MAVLINK_MSG_ID_LOITERU 76 #define MAVLINK_MSG_ID_REQUEST_PLIST 21 #define MAVLINK_MSG_ID_ATTITUDE 30 #define MAVLINK_MSG_ID_ITEM 39 #define MAVLINK_MSG_ID_MISSION_REQUEST 40 #define MAVLINK_MSG_ID_REQUEST_LIST 43 #define MAVLINK_MSG_ID_COUNT 44 +#define MAVLINK_MSG_ID_MISSION_ACK 47 +#define MAVLINK_MSG_ID_LONG 76 #include "mbed.h"