Revision 21:c546eab07e28, committed 2014-04-21

Comitter:

dylanembed123

Date:

Mon Apr 21 13:54:55 2014 +0000

Parent:

16:4f5d20b87dc3

Child:

22:9880a26886db

Commit message:

Issue command to flight controller.

Changed in this revision

--- a/main.cpp	Sat Apr 19 14:39:19 2014 +0000
+++ b/main.cpp	Mon Apr 21 13:54:55 2014 +0000
@@ -4,6 +4,7 @@
 #include "adapt/usb.h"
 #include "handle/handleCamera.h"
 #include "handle/handleGPS.h"
+#include "mavcontrol.h"
 
 Serial pc(USBTX,USBRX);
 Serial xbee(p9,p10);//tx, rx
@@ -100,9 +101,102 @@
 //    }
 //}
 
+typedef struct MAV_REQUEST_LIST_S{
+    char targSys;
+    char targComp;
+}MAV_REQUEST_LIST;
+
+typedef struct MAV_COUNT_S{
+    char targSys;
+    char targComp;
+    uint16_t count; 
+}MAV_COUNT;
+
+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;
 
 int main()
 {
+    char input[9];
+    input[0]=0x00;
+    input[1]=0x00;
+    input[2]=0x00;
+    input[3]=0x00;
+    input[4]=0x02;
+    input[5]=0x03;
+    input[6]=0x51;
+    input[7]=0x04;
+    input[8]=0x03;
+    /*
+    while(true){
+        char input[9];
+        input[0]=0x00;
+        input[1]=0x00;
+        input[2]=0x00;
+        input[3]=0x00;
+        input[4]=0x02;
+        input[5]=0x03;
+        input[6]=0x51;
+        input[7]=0x04;
+        input[8]=0x03;
+        Mav::sendOutput(0,input,9);
+        wait_ms(50);
+    }
+    */
+    //Mav::sendOutput(MAV_CMD_NAV_TAKEOFF,NULL,0);
+    //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,NULL,0);
+    //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,NULL,0);
+    //Mav::sendOutput(MAV_CMD_NAV_LAND,NULL,0);
+    //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,NULL,0);
+    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;
+    while(true){
+        if(Mav::getSerial().readable()>0){
+            char input=Mav::getSerial().getc();
+            USB::getSerial().printf("> %x\n",input);
+            if(input==0xFE){
+                reading=true;
+                readIndex=1;
+            }else if(reading){
+                nextCmd[std::min(readIndex++,512)]=input;
+            }
+        }
+        // Output debug info
+        if(readIndex==1&&reading){
+            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_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
+            }
+        }
+    }
+    
+    int outLength;
+    char* output=Mav::generatePacket(MAVLINK_MSG_ID_REQUEST_LIST,NULL,0,&outLength);
+    for(int i=0;i<outLength;i++){
+        Mav::getSerial().putc(output[i]);
+    }
+    while(1){}
+    
     //getPS().sendPacket(0,NULL,0,PT_EMPTY);getPS().sendPacket(55,NULL,0,PT_IMAGE);
     //char output[256];for(int i=0;i<256;i++){output[i]=i;}getPS().sendPacket(0,NULL,0,PT_IMAGE);getPS().sendPacket(55,output,256);getPS().sendPacket(55,output,5);getPS().sendPacket(55,NULL,0,PT_END);
     //while(true){}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mavcontrol.cpp	Mon Apr 21 13:54:55 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/mavcontrol.h	Mon Apr 21 13:54:55 2014 +0000
@@ -0,0 +1,30 @@
+#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_REQUEST_LIST 43
+#define MAVLINK_MSG_ID_COUNT 44
+#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