File content as of revision 52:b4dddb28dffa:

#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;

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{
    uint32_t lat;
    uint32_t lon;
    uint32_t alt;
    uint16_t x;
    uint16_t y;
    uint16_t z;
}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;
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;}
};