File content as of revision 66:5d43988d100c:

#include "mavcommands.h"

// Maximium number of charictors to read without giving up the processor
#define MAX_READ_LOOP 2560

#define ENABLE_MOTORS false

MavCmd* MavCmd::mavcmd=NULL;

static void fixLatLonAlt(double& latitude,double& longitude,double& altitude){
    latitude/=10000000.0f;
    longitude/=10000000.0f;
    //altitude/=10000000.0f;
    //USB::getSerial().printf("Got2 lat %f\n",latitude);USB::getSerial().printf("Got lon2 %f\n",longitude);USB::getSerial().printf("Got alt2 %f\n",altitude);
    return;
    int degrees;
    double minutes;
    degrees = (int)(latitude/100);
    minutes = latitude - degrees*100;
    latitude = degrees + minutes/60;
    degrees = (int)(longitude/100);
    minutes = longitude - degrees*100;
    longitude = degrees + minutes/60;    
}

bool MavCmd::hasMoved(){
    double nLat=DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ)).getLat();
    double nLon=DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ)).getLon();
    double nAlt=DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ)).getAlt();
    USB::getSerial().printf("HM Lat %f\n",nLat);
    USB::getSerial().printf("HM Lon %f\n",nLon);
    USB::getSerial().printf("HM Alt %f\n",nAlt);
    //if(moveValid&&cLat==nLat&&cLon==nLon&&cAlt==nAlt)return false;
    moveValid=false;
    cLat=nLat;
    cLon=nLon;
    cAlt=nAlt;
    return true;
}

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=5;
    // 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=0;issueItem.frame=0;issueItem.confirm=0;
    // 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;
    // Issue start
    issueStart.targSys=1;issueStart.targComp=1;issueStart.lat=5.0f;issueStart.lon=6.0f;issueStart.alt=7.0f;issueStart.cmd=22;issueStart.seq=0;issueStart.current=1;
    // Issue location request
    issueStreamReq.targSys=1;issueStreamReq.targComp=1;issueStreamReq.streamID=6;issueStreamReq.rate=5;issueStreamReq.start=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)
    cLat=cLon=cAlt=0.0f;
}

char* MavCmd::getNextCmd(){
        if(Mav::getSerial().readable()>0){
            char input=Mav::getSerial().getc();
            bool start=false;
            if(readState==0&&input==0xFE){
                USB::getSerial().printf("\nS: ");
                readState=1;
                readIndex=1;
                start=true;
            }
            USB::getSerial().printf(">%x~",input);
            if(!start&&readState!=0){
                nextCmd[std::min(readIndex++,512)]=input;
                if(readState==1){
                    realLen=input+5-1;
                    readState=2;
                }
                if(readState==2){
                    realLen--;
                    if(readIndex-1==5&&nextCmd[5]==33){realLen=12;}
                    if(realLen==0){
                        USB::getSerial().printf(" E\n");
                        readState=0;
                        char* output=new char[readIndex];
                        for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];}
                        return output;
                    }
                }
            }
        }
        return NULL;
}

// 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){
            //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));
            // Start sending location data
            //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
            //else{
            // Start sending location data
                //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
            //}
        }
        // Check for mavlink message request
        if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
            hasMoved();
            USB::getSerial().printf("Issue Item %f %f %f\n",cLat,cLon,cAlt);
            // Set sequence to requested sequence
            issueItem.seq=myCmd[6];
            issueItem.lat=(float)cLat;
            issueItem.lon=(float)cLon;
            issueItem.alt=(float)cAlt;
            // 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){
            USB::getSerial().printf("Ack\n");
            moveValid=true;
            if(startSetup&&ENABLE_MOTORS){
                startSetup=false;
                // Start
                Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueStart,sizeof(MAV_MISSION_ITEM));
                wait(1);
                // Take off
                Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM));
                wait(1);
                // Start sending location data
                Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
                wait(1);
                // Arm motors
                Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
            }
        }
        // Check for GPS
        if(myCmd[5]==33){
            //for(int i=0;i<18;i++){USB::getSerial().printf("Got loc %d/18 %d\n",i,myCmd[6+i]);}
            MAV_LOCDATA* input=(MAV_LOCDATA*)&myCmd[6];
            
            //USB::getSerial().printf("Got lat %d\n",input->lat);USB::getSerial().printf("Got lon %d\n",input->lon);USB::getSerial().printf("Got alt %d\n",input->alt);
            double lat=(double)*((int32_t*)&myCmd[10]);double lon=(double)*((int32_t*)&myCmd[10+4]);
            double alt=(double)*((int32_t*)&myCmd[10-4]);
            fixLatLonAlt(lat,lon,alt);
            if(calibAlt==0){
                calibAlt=alt;
            }
            USB::getSerial().printf("Got lat %f\n",lat);USB::getSerial().printf("Got lon %f\n",lon);USB::getSerial().printf("Got alt %f\n",alt-calibAlt);USB::getSerial().printf("Got aalt %f\n",alt);
            USB::getSerial().printf("Got heading %f\n",compassHandle::getCompassHand().heading);
            DH::Locs().add(LHType_locs,DataLocation(lat,lon,alt-calibAlt,0,compassHandle::getCompassHand().heading));
            USB::getSerial().printf("USING NEW WAYPOINT!!!\n");
            // Issue count to init waypoint entry
            Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
        }
        // Check for waypoint count
        if(myCmd[5]==44){
            USB::getSerial().printf("Waypoints tsys %d tcomp %d cnt %d\n",myCmd[8],myCmd[7],myCmd[6]);
        }
        delete myCmd;
    }
}