File content as of revision 66:5d43988d100c:

#include "handleGPS.h"

//Serial gps(p28,p27);

GPSHandle* GPSHandle::hand = NULL;

void testVar(){
}
void GPSHandle::setup(){
    //gpsGPS().getSerial().baud(57600);
    sendGpsCommand("PMTK301,1");
    //GPS::getSerial().attach(&GPSHandle::handleUpdate,Serial::RxIrq);
    //GPS::getSerial().attach(this,&GPSHandle::handleUpdate,Serial::RxIrq);
    //cs: Send other standard init commands? Not strictly speaking necessary,
    //    but forces "up to date documentation" in the form of always knowing
    //    gps config.
}

char GPSHandle::readWaypoints(){
    //USB::getSerial().printf("getting waypoitns\r\n");
    PacketStruct pack;
    char rx_status = getPS().receivePacket(&pack);
    if(rx_status != 1){
        return rx_status;
    }
    DH::Locs().getI(LHType_targ,LHIType_size) = 0;//reset size to 0 (clear out old waypoints
    Point* points = (Point*)pack.data;
    unsigned int num_points = pack.size;
    for(int i=0;i<num_points;i++){
        //USB::getSerial().printf("Adding Waypoint: %f, %f\r\n",points[i].lat,points[i].lon);
        if(i==num_points-1){
            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,75.0f));
            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,25.0f));
            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,10.0f));
            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,5.0f));
            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,0.0f));
        }else{
            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,150.0f));
        }
    }
    //USB::getSerial().printf("Waypoints size: %d\r\n",DH::Locs().getI(LHType_targ,LHIType_size));
    for(int i=0;i<DH::Locs().getI(LHType_targ,LHIType_size);i++){
        DataLocation thisData=DH::Locs().getC(LHType_targ,i);
        USB::getSerial().printf("Waypoint %d: %f,%f\r\n",i,thisData.getLat(),thisData.getLon());
    }
    DH::Locs().inc(LHType_targ,0,true);
    return 1;
}

void GPSHandle::next_waypoint(){
    DH::Locs().inc(LHType_targ);
    USB::getSerial().printf("Sending Quadcopter to next waypoint\r\n");
    //code to command quad to go to next waypoint goes here
}

void GPSHandle::sendLoc(){
    wait_us(100000);
    getPS().openConnection();
    wait_us(100000);
    unsigned int sID=getPS().getSuperID();
    getPS().sendPacket(0,NULL,0,PT_EMPTY);
    getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
    DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)).getLon() *= -1;
    getPS().sendPacket(sID,(char*)(&DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs))),sizeof(DataLocation));
    DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)).getLon() *= -1;
    getPS().sendPacket(sID,NULL,0,PT_END);
    wait_us(100000);
    getPS().closeConnection();
    wait_us(100000);
}

bool GPSHandle::if_image_location(){
    double lon_thresh = 0.00030;
    double lat_thresh = 0.00018;
    //USB::getSerial().printf("Checking if at waypoint\r\n");
    DataLocation current_loc = DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs));
    DataLocation next_waypoint = DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ));
    USB::getSerial().printf("current: %f,%f ... waypoint: %f,%f \r\n",current_loc.getLat(),current_loc.getLon(),next_waypoint.getLat(),next_waypoint.getLon());
    double lat_diff = (current_loc.getLat()>next_waypoint.getLat()) ? current_loc.getLat() - next_waypoint.getLat() : next_waypoint.getLat() - current_loc.getLat();
    double lon_diff = (current_loc.getLon()>next_waypoint.getLon()) ? current_loc.getLon() - next_waypoint.getLon() : next_waypoint.getLon() - current_loc.getLon();
    USB::getSerial().printf("lat diff: %f, lon dif %f \r\n",lat_diff,lon_diff);
    return (lat_diff < lat_thresh && lon_diff < lon_thresh);
}

static bool reading = false;
//static std::stringstream line;
static char line[MAXREADIN+10];
static int line_i=0;
char* getNext(char*& field){
    char* output=new char[MAXREADIN+1];
    int i;
    for(i=0;i<MAXREADIN;i++){
        if(field[i]=='\0'||field[i]==',')break;
    }
    for(int a=0;a<i;a++){
        output[a]=field[a];
    }
    output[i]='\0';
    field=&field[i+1];
    return output;
}
    
void GPSHandle::handleUpdate(){
    char c;
    reading = false;
    while(getPS().rx_ready_with_timeout(&GPS::getSerial(),0,10000)){
        c = GPS::getSerial().getc();
        //USB::getSerial().printf("%c",c);
        if (reading) {
            if(line_i>=MAXREADIN){reading=false;return;}
            if (c == '*') { //sentence buffer complete; we're ignoring the checksum
                char* field=line;
                char* op;
                op=getNext(field);delete op; //GPGGA
                if(op[0]=='G'||op[1]=='P'||op[2]=='G'||op[3]=='G'||op[4]=='A'){
                    op=getNext(field);double timeS = atof(op);delete op; //time
                    op=getNext(field);double latitude = atof(op);delete op; //latitude
                    op=getNext(field);delete op; //N or S
                    op=getNext(field);double longitude = atof(op);delete op; //longitude
                    op=getNext(field);delete op; //E or W
                    op=getNext(field);delete op; //skip
                    op=getNext(field);delete op; //skip
                    op=getNext(field);delete op; //skip
                    op=getNext(field);delete op; //altitude
                    double altitude = atof(op);
                    if(timeS>0.5f){
                        int degrees = (int)(latitude/100);
                        double minutes = latitude - degrees*100;
                        latitude = degrees + minutes/60;
                        degrees = (int)(longitude/100);
                        minutes = longitude - degrees*100;
                        longitude = -1*(degrees + minutes/60);
                        USB::getSerial().printf("\nMy GPS data: Lat: %f, Lon: %f, Alt: %f, Time:%f\r\n",latitude,longitude,altitude,timeS);
                        DH::Locs().add(LHType_locs,DataLocation(latitude,longitude,altitude,timeS));
                       // USB::getSerial().printf("Current Time:%f\r\n",DH::Locs().getC().getTime());
                        return;
                    }
                }
                //update whatever needs updating when gps updates
                //pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
                //gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
                //);
                reading = false;
            } else {
                line[line_i]=c;
                line_i=(line_i+1)%MAXREADIN;
            }
        }else if (c == '$') {
            reading = true;
            line_i=0;
        }
    }
    return;
}

//sends: "$<command>*<checksum>\r\l"
void GPSHandle::sendGpsCommand(std::string command){
    uint8_t checksum = 0;
    //pc.printf("Sending command to gps: ");
    GPS::getSerial().putc('$');
    //pc.putc('$');
    char c;
    for (int i = 0; i < command.length(); i++) {
        c = command[i];
        checksum ^= c;
        GPS::getSerial().putc(c);
        //pc.putc(c);
    }
    GPS::getSerial().putc('*');
    //pc.putc('*');
    string checkSumString;
    while (checksum > 0) {
        uint8_t checksumChar = checksum & 0x0F;
        if (checksumChar >= 10) {
            checksumChar -= 10;
            checksumChar += 'A';
        } else {
            checksumChar += '0';
        }
        checkSumString.push_back((char) checksumChar);
        checksum = checksum >> 4;
    }

    for (int i = checkSumString.length() - 1; i >= 0; i--) {
        GPS::getSerial().putc(checkSumString[i]);
        //pc.putc(checkSumString[i]);
    }
    GPS::getSerial().putc('\r');
    //pc.putc('\r');
    GPS::getSerial().putc('\n');
    //pc.putc('\n');
}

int stringToDecimal(string s)
{
    int mult = 1;
    int result = 0;
    for (int i = s.length() - 1; i >=0; i--) {
        if (s[i] != '.') {
            result += (s[i] - '0') * mult;
            mult *= 10;
        }
    }
    return result;
}

bool GPSHandle::check(){
    return true;
}
void GPSHandle::run(){
    if(!initialized){initialized=true;setup();}
    handleUpdate();
}