File content as of revision 66:5d43988d100c:

#include "mbed.h"
#include "mavcontrol.h"
#include "adapt/usb.h"
#include "packet.h"
#include <algorithm>
#include"handle/dataLocation.h"
#include"handle/handleCompass.h"

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;

typedef struct MAV_DATA_STREAM_S{
    uint16_t rate; // Hz
    uint8_t targSys;
    uint8_t targComp;
    uint8_t streamID;
    uint8_t start; // Set to 1 to start and 0 to stop
} MAV_DATA_STREAM;

typedef struct MAV_LOCDATA_S{
    int32_t alt;//timestamp;
    int32_t lat;
    int32_t lon;
    int32_t altB;
    //int32_t ralt;
    //int16_t x;
    //int16_t y;
    //int16_t z;
    //uint16_t hdg;
}MAV_LOCDATA;

class MavCmd{
private:
    MAV_REQUEST_LIST req;
    MAV_APM issueArm;
    MAV_APM issueDisArm;
    MAV_COUNT issueCount;
    MAV_MISSION_ITEM issueItem;
    MAV_MISSION_ITEM issueStart;
    MAV_MISSION_ITEM issueTakeOff;
    MAV_DATA_STREAM issueStreamReq;
    
    // 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;
    double cLat,cLon,cAlt;
    bool hasMoved();
    bool moveValid;
    double calibAlt;
public:
    MavCmd():initialized(false){}
    char* getNextCmd();
    void handleNextCmd();
    void setupCmds();
    void setup(){if(!initialized){setupCmds();initialized=true;calibAlt=0.0f;}}
    void run(){
        setup();
        readState=0;
        while(Mav::getSerial().readable()>0){char input=Mav::getSerial().getc();}
        //&&DH::Locs().getC(LHType_locs,DH::Locs().getI()).getLat()!=0&&DH::Locs().getC(LHType_locs,DH::Locs().getI()).getLon()!=0
        if(1==2&&DH::Locs().getI(LHType_targ,LHIType_size)>0&&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("\n");
    }
    static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;}
};