Revision 26:06f1c9d70e9f, committed 2014-04-22

Comitter:

dylanembed123

Date:

Tue Apr 22 14:21:01 2014 +0000

Parent:

25:b7f861fc8ddd

Child:

27:db73f8ac6c75

Commit message:

Move mav into handlers

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/mavcommands.cpp	Tue Apr 22 14:21:01 2014 +0000
@@ -0,0 +1,91 @@
+#include "mavcommands.h"
+
+// Maximium number of charictors to read without giving up the processor
+#define MAX_READ_LOOP 256
+
+MavCmd* MavCmd::mavcmd=NULL;
+
+void MavCmd::setupCmds(){
+    // Request item list
+    req.targSys=1;req.targComp=1;
+    // Issue arm motors
+    issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f;
+    // Issue disarm motors
+    issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
+    // Issue a mission count
+    issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
+    // Issue a mission item
+    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
+    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;
+
+    // Local variables
+    startSetup=true;// Set to true to initiate startup sequence
+    readState=0;     // Read State
+    realLen=0;       // How many more bytes need to be read
+    readIndex=0;     // Current index in next cmd (also the size)
+}
+
+char* MavCmd::getNextCmd(){
+    for(int readcnt=0;readcnt<MAX_READ_LOOP;readcnt++){
+        if(Mav::getSerial().readable()<=0){
+            // Nothing to run, wait until next time
+            return NULL;
+        }else{
+            char input=Mav::getSerial().getc();
+            USB::getSerial().printf("> %x %d %d\n",input,readState,realLen);
+            if(readState==0&&input==0xFE){
+                readState=1;
+                readIndex=1;
+            }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;
+                        char* output=new char[readIndex];
+                        for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];}
+                        return output;
+                    }
+                }
+            }
+        }
+    }
+}
+
+// Handle the next command (if one is available)
+void MavCmd::handleNextCmd(){
+    char* myCmd=getNextCmd();
+    // Output debug info
+    if(myCmd!=NULL){
+        USB::getSerial().printf("Got CMD len %d messageid %d \n",myCmd[1],myCmd[5]);
+        if(myCmd[5]==0){
+            if(startSetup){
+                startSetup=false;
+                USB::getSerial().printf("Issue Command\n",myCmd[1],myCmd[5]);
+                // Issue count to init waypoint entry
+                Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
+            }
+        }
+        // Check for mavlink message request
+        if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
+            USB::getSerial().printf("Issue Item\n");
+            // Set sequence to requested sequence
+            issueItem.seq=myCmd[6];
+            // Issue mission item
+            Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
+        }
+        // Check for mission accepted
+        if(myCmd[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));
+        }
+        delete myCmd;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/mavcommands.h	Tue Apr 22 14:21:01 2014 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include "mavcontrol.h"
+#include "adapt/usb.h"
+#include <algorithm>
+
+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;
+
+class MavCmd{
+private:
+    MAV_REQUEST_LIST req;
+    MAV_APM issueArm;
+    MAV_APM issueDisArm;
+    MAV_COUNT issueCount;
+    MAV_MISSION_ITEM issueItem;
+    MAV_MISSION_ITEM issueTakeOff;
+    
+    // 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;
+public:
+    MavCmd():initialized(false){}
+    char* getNextCmd();
+    void handleNextCmd();
+    void setupCmds();
+    void setup(){if(!initialized){setupCmds();initialized=true;}}
+    void run(){setup();handleNextCmd();}
+    static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;}
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/mavcontrol.cpp	Tue Apr 22 14:21:01 2014 +0000
@@ -0,0 +1,70 @@
+
+#define X25_INIT_CRC 0xffff
+#define X25_VALIDATE_CRC 0xf0b8
+#include "mavcontrol.h"
+#include "usb.h"
+#define MAVLINK_CRC_EXTRA 1
+
+Serial* Mav::mav=NULL;
+Serial& Mav::getSerial(){
+    if(mav==NULL){
+        // Init Serial USB
+        mav=new Serial(MAVPINTX,MAVPINRX);
+        mav->baud(MAVBAUD);
+    }
+    return *mav;
+}
+
+static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum){
+        /*Accumulate one byte of data into the CRC*/
+        uint8_t tmp;
+
+        tmp = data ^ (uint8_t)(*crcAccum &0xff);
+        tmp ^= (tmp<<4);
+        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
+}
+
+static inline void crc_init(uint16_t* crcAccum){
+        *crcAccum = X25_INIT_CRC;
+}
+
+static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length){
+        uint16_t crcTmp;
+        crc_init(&crcTmp);
+    while (length--) {
+                crc_accumulate(*pBuffer++, &crcTmp);
+        }
+        return crcTmp;
+        //return 5;
+}
+
+static int seqcount=0;
+int crcExtraCount=50;
+char* Mav::generatePacket(int messageID,char* payload,int length,int* outLength){
+    char* output=new char[MAVMAXSIZE+1];
+    output[0]=0xFE;
+    output[1]=(0xFF&length);
+    output[2]=(0xFF&(seqcount++));
+    output[3]=2;// Sending system ID
+    output[4]=1;// Sending system component ID
+    output[5]=0xFF&messageID;
+    for(int i=0;i<length;i++){output[6+i]=payload[i];}
+    int ofs=6+length;
+    // Determine checksum
+    uint16_t checksum = crc_calculate((uint8_t*)&output[1], length + 5);
+    crc_accumulate(crcExtraCount, &checksum);
+    output[ofs+0]=(0xFF&checksum);
+    output[ofs+1]=(0xFF&(checksum>>0x08));
+    if(outLength!=NULL){*outLength=ofs+2;}
+    return output;
+}
+
+void Mav::sendOutput(int messageID,char* payload,int length){
+    int outLength;
+    char* output=generatePacket(messageID,payload,length,&outLength);
+    for(int i=0;i<outLength;i++){
+        getSerial().putc(output[i]);
+        USB::getSerial().printf("+ %x\n",output[i]);
+    }
+    delete output;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/mavcontrol.h	Tue Apr 22 14:21:01 2014 +0000
@@ -0,0 +1,32 @@
+#define MAVLINK_MSG_ID_PING 4
+#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"
+
+
+/// Define Pinout
+#define MAVPINTX p28
+#define MAVPINRX p27
+
+/// Define Baud
+#define MAVBAUD 57600
+
+#define MAVMAXSIZE 512
+
+typedef unsigned char uint8_t;
+typedef unsigned short uint16_t;
+
+class Mav{
+private:
+    static Serial* mav;
+public:
+    static Serial& getSerial();
+    static char* generatePacket(int messageID,char* payload=NULL,int length=0,int* outLength=NULL);
+    static void sendOutput(int messageID,char* payload=NULL,int length=0);
+};
\ No newline at end of file

--- a/main.cpp	Tue Apr 22 14:18:30 2014 +0000
+++ b/main.cpp	Tue Apr 22 14:21:01 2014 +0000
@@ -5,7 +5,7 @@
 #include "handle/handleCamera.h"
 #include "handle/handleGPS.h"
 #include "handle/handleCommand.h"
-#include "mavcommands.h"
+#include "handle/mavcommands.h"
 
 Serial pc(USBTX,USBRX);
 Serial xbee(p9,p10);//tx, rx

--- a/mavcommands.cpp	Tue Apr 22 14:18:30 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,91 +0,0 @@
-#include "mavcommands.h"
-
-// Maximium number of charictors to read without giving up the processor
-#define MAX_READ_LOOP 256
-
-MavCmd* MavCmd::mavcmd=NULL;
-
-void MavCmd::setupCmds(){
-    // Request item list
-    req.targSys=1;req.targComp=1;
-    // Issue arm motors
-    issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f;
-    // Issue disarm motors
-    issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
-    // Issue a mission count
-    issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
-    // Issue a mission item
-    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
-    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;
-
-    // Local variables
-    startSetup=true;// Set to true to initiate startup sequence
-    readState=0;     // Read State
-    realLen=0;       // How many more bytes need to be read
-    readIndex=0;     // Current index in next cmd (also the size)
-}
-
-char* MavCmd::getNextCmd(){
-    for(int readcnt=0;readcnt<MAX_READ_LOOP;readcnt++){
-        if(Mav::getSerial().readable()<=0){
-            // Nothing to run, wait until next time
-            return NULL;
-        }else{
-            char input=Mav::getSerial().getc();
-            USB::getSerial().printf("> %x %d %d\n",input,readState,realLen);
-            if(readState==0&&input==0xFE){
-                readState=1;
-                readIndex=1;
-            }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;
-                        char* output=new char[readIndex];
-                        for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];}
-                        return output;
-                    }
-                }
-            }
-        }
-    }
-}
-
-// Handle the next command (if one is available)
-void MavCmd::handleNextCmd(){
-    char* myCmd=getNextCmd();
-    // Output debug info
-    if(myCmd!=NULL){
-        USB::getSerial().printf("Got CMD len %d messageid %d \n",myCmd[1],myCmd[5]);
-        if(myCmd[5]==0){
-            if(startSetup){
-                startSetup=false;
-                USB::getSerial().printf("Issue Command\n",myCmd[1],myCmd[5]);
-                // Issue count to init waypoint entry
-                Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
-            }
-        }
-        // Check for mavlink message request
-        if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
-            USB::getSerial().printf("Issue Item\n");
-            // Set sequence to requested sequence
-            issueItem.seq=myCmd[6];
-            // Issue mission item
-            Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
-        }
-        // Check for mission accepted
-        if(myCmd[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));
-        }
-        delete myCmd;
-    }
-}
\ No newline at end of file

--- a/mavcommands.h	Tue Apr 22 14:18:30 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,81 +0,0 @@
-#include "mbed.h"
-#include "mavcontrol.h"
-#include "adapt/usb.h"
-#include <algorithm>
-
-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;
-
-class MavCmd{
-private:
-    MAV_REQUEST_LIST req;
-    MAV_APM issueArm;
-    MAV_APM issueDisArm;
-    MAV_COUNT issueCount;
-    MAV_MISSION_ITEM issueItem;
-    MAV_MISSION_ITEM issueTakeOff;
-    
-    // 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;
-public:
-    MavCmd():initialized(false){}
-    char* getNextCmd();
-    void handleNextCmd();
-    void setupCmds();
-    void setup(){if(!initialized){setupCmds();initialized=true;}}
-    void run(){setup();handleNextCmd();}
-    static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;}
-};

--- a/mavcontrol.cpp	Tue Apr 22 14:18:30 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,70 +0,0 @@
-
-#define X25_INIT_CRC 0xffff
-#define X25_VALIDATE_CRC 0xf0b8
-#include "mavcontrol.h"
-#include "usb.h"
-#define MAVLINK_CRC_EXTRA 1
-
-Serial* Mav::mav=NULL;
-Serial& Mav::getSerial(){
-    if(mav==NULL){
-        // Init Serial USB
-        mav=new Serial(MAVPINTX,MAVPINRX);
-        mav->baud(MAVBAUD);
-    }
-    return *mav;
-}
-
-static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum){
-        /*Accumulate one byte of data into the CRC*/
-        uint8_t tmp;
-
-        tmp = data ^ (uint8_t)(*crcAccum &0xff);
-        tmp ^= (tmp<<4);
-        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
-}
-
-static inline void crc_init(uint16_t* crcAccum){
-        *crcAccum = X25_INIT_CRC;
-}
-
-static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length){
-        uint16_t crcTmp;
-        crc_init(&crcTmp);
-    while (length--) {
-                crc_accumulate(*pBuffer++, &crcTmp);
-        }
-        return crcTmp;
-        //return 5;
-}
-
-static int seqcount=0;
-int crcExtraCount=50;
-char* Mav::generatePacket(int messageID,char* payload,int length,int* outLength){
-    char* output=new char[MAVMAXSIZE+1];
-    output[0]=0xFE;
-    output[1]=(0xFF&length);
-    output[2]=(0xFF&(seqcount++));
-    output[3]=2;// Sending system ID
-    output[4]=1;// Sending system component ID
-    output[5]=0xFF&messageID;
-    for(int i=0;i<length;i++){output[6+i]=payload[i];}
-    int ofs=6+length;
-    // Determine checksum
-    uint16_t checksum = crc_calculate((uint8_t*)&output[1], length + 5);
-    crc_accumulate(crcExtraCount, &checksum);
-    output[ofs+0]=(0xFF&checksum);
-    output[ofs+1]=(0xFF&(checksum>>0x08));
-    if(outLength!=NULL){*outLength=ofs+2;}
-    return output;
-}
-
-void Mav::sendOutput(int messageID,char* payload,int length){
-    int outLength;
-    char* output=generatePacket(messageID,payload,length,&outLength);
-    for(int i=0;i<outLength;i++){
-        getSerial().putc(output[i]);
-        USB::getSerial().printf("+ %x\n",output[i]);
-    }
-    delete output;
-}
\ No newline at end of file

--- a/mavcontrol.h	Tue Apr 22 14:18:30 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,32 +0,0 @@
-#define MAVLINK_MSG_ID_PING 4
-#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"
-
-
-/// Define Pinout
-#define MAVPINTX p28
-#define MAVPINRX p27
-
-/// Define Baud
-#define MAVBAUD 57600
-
-#define MAVMAXSIZE 512
-
-typedef unsigned char uint8_t;
-typedef unsigned short uint16_t;
-
-class Mav{
-private:
-    static Serial* mav;
-public:
-    static Serial& getSerial();
-    static char* generatePacket(int messageID,char* payload=NULL,int length=0,int* outLength=NULL);
-    static void sendOutput(int messageID,char* payload=NULL,int length=0);
-};
\ No newline at end of file