File content as of revision 58:e8426e488925:

#include "mbed.h"
#include "PinDetect.h"
#include "Point.h"
#include <vector>
#include "Line.h"
#include "stringUtils.h"
#include "base.h"
#include "Config.h"
#include "imu_functions.h"
#include "atoh.h"
#include "string_utilities.h"
#include "checksum.h"
#include <string.h>
#include "gps.h"
#include "autosteer.h"
#include "utilities.h"
#include "angle_error.h"

char *version="FreePilot V2.11 Jtan 20, 2015\r\n";
long lastsend_version=0;
Timer vTimer; //this timer is int based! Max is 30 minutes

//int checksumm;
double distance_from_line;
static double cm_per_deg_lon;
static double cm_per_deg_lat;
//all timing objects
Timer gps_connecting;
Timer autosteer_time;
Timer autosteer_timeout; //timeout work near as timer, but they don't give timing. they just trigger an interrupt once the time we assigned it is passed.
Ticker accelerometerTicker;
Ticker gyroscopeTicker;
Ticker filterTicker;
static Ticker  angle_print;

//Motor
PinDetect  motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
DigitalOut enable_motor(p7);

PwmOut pwm1(p21);
PwmOut pwm2(p22);

//equipment switches
PinDetect  boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect  boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect  boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect  boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.

char boom18; //1 byte
char lastboom18; //1 byte
char boomstate[8]= {'$','F','B','S',0,13,10,0 };

//double filterg = 100;

Point looked_ahead;
Point line_start;
Point line_end;
Point tilt_compensated_position;
Point yaw_compensated_position;

extern int gyro_pos;
double distance_to_line;

//FreePilot variables
int timer_enabled;
double motorspeed;
int enable_time;
char* motor_enable_state = 0;
int motor_enable = 0;
int lastmotor_enable = 1;
double pwm1_speed;
double pwm2_speed;
long lastsend_motorstate=0;
Timer motTimer; //this timer is int based! Max is 30 minutes
Timer btTimer; //measure time for Bluetooth communication
long lastgetBT=0;
char msg[256]; //GPS line buffer
char msg2[256];//PC line buffer
char* result;
int printing;
float longitude;
float latitude;
char ns, ew;
int lock;

int connect_time = 10000; //variable to change the time that the serial output all the strings in order to verify if the command was right.
int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
int angle_send = 0;
int correct_rmc = 1;
char* degminsec;
double m_per_deg_lon;
double m_per_deg_lat;
double look_ahead_lon;
double look_ahead_lat;
int active_AB = 0;
char output[256];

double yaw;
double pitch;
double roll;
double a_x;
double a_y;
double a_z;
double w_x;
double w_y;
double w_z;

int readings[3];
double Freepilot_bearing;
int time_till_stop = 200;
static double track_arr[20] = {0};
static double z_arr[25] = {0};

void autosteer_done()
{
    enable_motor = 0;
}

Point compensation;

void yaw_compensate()
{
    yaw = get_yaw();
}

void pitch_and_roll(double real_bearing)
{
    pitch = get_pitch();
    roll = get_roll();
    compensation.SetX(antennaheight * tan(roll) * sin(ToRad(real_bearing))-antennaheight * tan(pitch) * cos(ToRad(real_bearing)));
    compensation.SetY(antennaheight * tan(roll) * cos(ToRad(real_bearing))-antennaheight * tan(pitch) * sin(ToRad(real_bearing)));
}

void process_GPSHEIGHT(char* height_string)
{
    char *token;
    int  token_counter = 0;
    char *height  = (char *)NULL;
    token = strtok(height_string, ",");
    while (token) {
        switch (token_counter) {
            case 1:
                height = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( height ) {
        antennaheight = atof(height);
        Config_Save();
    }
}

void process_ASTEER(char* asteer, bool frompc)
{
    char *token;
    int  token_counter = 0;
    char *asteer_speed  = (char *)NULL;
    char *asteer_time = (char *)NULL;
    token = strtok(asteer, ",");
    while (token) {
        switch (token_counter) {
            case 1:
                asteer_speed  = token;
                break;
            case 2:
                asteer_time = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( asteer_speed && asteer_time ) {
        motorspeed = atof(asteer_speed);
        if ( frompc) {
            motorspeed = 255.0 - motorspeed;
        }
        if ( motorspeed != 127 ) {
            enable_time = atof(asteer_time);
            autosteer_timeout.reset();
            time_till_stop = atoi(asteer_time);
            if ( motor_enable == 0 ) {
            } else {
                if ( motorspeed > 127.0 ) {
                    pwm2_speed = 0.0;
                    pwm1_speed = ((double)motorspeed - (double)127.0) / 128.0;

                } else if ( motorspeed < 127.0 ) {
                    pwm1_speed = 0.0;
                    pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
                } else {
                    pwm1_speed = 0;
                    pwm2_speed = 0;
                    enable_motor = 0;
                }
                if(Authenticated) {
                    pwm1 = pwm1_speed;
                    pwm2 = pwm2_speed;
                    enable_motor = 1;
                }
            }
        }
    }
}

void line_inverted()
{
    double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
    double diff_angle = Freepilot_bearing - angle;
    diff_angle = ((int)diff_angle + 180) % 360 - 180;
    if ( abs(diff_angle) > 90 ) {
        if ( (abs(360 - diff_angle)) > 90 ) {
            Point temp = line_end;
            line_end = line_start;
            line_start = temp;
            Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
        }
    }
}

void action_on_rmc()
{
    line_inverted();

    double track_avg = new_bearing(track, avgpos, track_arr,z_arr);
    
    
    pitch_and_roll(track_avg);// as to be real bearing

    cm_per_deg_lat = 11054000;
    cm_per_deg_lon = 11132000 * cos(decimal_latitude);
    compensation.SetY(compensation.GetY() / cm_per_deg_lon);
    compensation.SetX(compensation.GetX() / cm_per_deg_lat);

    position = point_add(position,compensation);
    double lookaheaddistance = lookaheadtime * speed_m_s;
    get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,track_avg,look_ahead_lon,look_ahead_lat);

    looked_ahead.SetX(look_ahead_lat);
    looked_ahead.SetY(look_ahead_lon);
    double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(ToRad(decimal_latitude))*111111.0 * cos(ToRad(decimal_latitude)));
    distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;/////////////////////////////////////////////////


    /* SetDigitalFilter(phaseadv,tcenter, 0 );
     string steering = ControlSteerFilter(225, distance_to_line, speed_km , filterg, 20.0, 75.0, scale*-1);
     char command[128];
     sprintf(command,"%s\r\n\0",steering.c_str());    //(int)((((val/2)-127)/scale)+127),500);
     pc.puts("NPC");
     pc.puts(command);
     process_ASTEER(command,false);
    */

    double bearing_error = bearing_to_line(z_arr, track_arr, lookaheaddistance, position, line_start, line_end, lookaheadtime, scale * -1,avgpos, looked_ahead);





    string rmc = "";
    rmc = "$GPRMC,";
    rmc +=  not_equal(string(time_s));
    rmc += not_equal(string(stat));
    rmc += string(To_DMS(position.GetX())) + "," + not_equal(string(lat_dir));
    rmc += (string(To_DMS_lon(position.GetY())) + "," + not_equal(string(lon_dir)));
    rmc += not_equal(string(vel));
    rmc += not_equal(string(trk));
    rmc += not_equal(string(date));
    rmc += not_equal(string(magv));
    rmc += not_equal(string(magd)) + "W";

    char test[256];
    sprintf(test,"%s*\0",rmc.c_str());
    sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));
    pc.puts(output);
    bluetooth.puts("\r\n");
    bluetooth.puts(output);
}

void process_FGPSAB(char* ab)
{
    char *token;
    int  token_counter = 0;
    char *line_lat  = (char *)NULL;
    char *line_lon  = (char *)NULL;
    char *line_lat1  = (char *)NULL;
    char *line_lon1  = (char *)NULL;
    //char *bearing  = (char *)NULL;
    string bearing = "";
    token = strtok(ab, ",");
    while (token) {
        switch (token_counter) {
            case 1:
                line_lat = token;
                break;
            case 2:
                line_lon = token;
                break;
            case 3:
                line_lat1 = token;
                break;
            case 4:
                line_lon1 = token;
                break;
            case 5:
                bearing = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    double Freepilot_lon  = atof(line_lon);
    double Freepilot_lat = atof(line_lat);
    double Freepilot_lon1  = atof(line_lon1);
    double Freepilot_lat1 = atof(line_lat1);
    Freepilot_bearing = atof(bearing.c_str()) + 360;
    Freepilot_bearing = ((int)Freepilot_bearing+ 180) % 360 - 180;
    line_start.SetX(Freepilot_lat);
    line_start.SetY(Freepilot_lon);
    line_end.SetX(Freepilot_lat1);
    line_end.SetY(Freepilot_lon1);

    active_AB = 1;
}

void process_FGPSAUTO(char* FGPSAUTO)
{
    char *token;
    int  token_counter = 0;
    char *ahead  = (char *)NULL;
    char *center  = (char *)NULL;
    char *phase  = (char *)NULL;
    char *scl  = (char *)NULL;
    char *avg  = (char *)NULL;
    char *_kp = (char *)NULL;
    char *_ki  = (char *)NULL;
    char *fg  = (char *)NULL;
    char *_kd = (char *)NULL;

    token = strtok(FGPSAUTO, ",");
    while (token) {
        switch (token_counter) {
            case 1:
                phase = token;
                break;
            case 2:
                center = token;
                break;
            case 3:
                fg = token;
                break;
            case 4:
                scl = token;
                break;
            case 5:
                ahead = token;
                break;
            case 6:
                avg = token;
                break;
            case 7:
                _kp = token;
                break;
            case 8:
                _ki = token;
                break;
            case 9:
                _kd = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( _kp && _ki && _kd ) {
        kp = atof(_kp);
        ki = atof(_ki);
        kd = atof(_kd);
    }
    if ( phase && center && scl && avg && ahead ) {
        lookaheadtime = atof(ahead);
        scale = atof(scl);
        phaseadv = atof(phase);
        avgpos = atof(avg);
        tcenter = atof(center);
        filterg = atof(fg);
        SetDigitalFilter(phaseadv,tcenter, 0 );
    }
}

void pc_analyse(char* pc_string)
{
    string temp(pc_string);
    pc.puts(temp.c_str());
    while ( temp.find ("\r\n") != string::npos ) {
        temp.erase ( temp.find ("\r\n"), 2 );
    }
    sprintf(pc_string,"%s",temp.c_str());
    if (!strncmp(pc_string, "$ASTEER", 7)) {
        process_ASTEER(pc_string,true);
    } else if (!strncmp(pc_string, "$BANY",5)) {
        _ID = Config_GetID();
        Config_Save();
    } else if (!strncmp(pc_string, "$FGPS-BAUD",10)) {
        pc.puts(pc_string);
        process_GPSBAUD(pc_string);
        Config_Save();
    } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
        process_FGPSAUTO(pc_string);
        sprintf(output,"%s\r\n",pc_string);
        bluetooth.puts(output);
        Config_Save();
    } else if (!strncmp(pc_string, "$FGPS,",6)) {
        pc.puts(pc_string);
        int i=5;
        char c=pc_string[i];
        while (c!=0) {
            i++;
            if (i>255) break; //protect msg buffer!
            c=pc_string[i];
            gps.putc(c);
        }
    } else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
        process_GPSHEIGHT(pc_string);
        sprintf(output,"%s\r\n",pc_string);
        bluetooth.puts(output);
        Config_Save();
    } else if (!strncmp(pc_string, "$FGPSAB",7)) {
        process_FGPSAB(pc_string);
    } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
        calibrateGyroscope();
        calibrateAccelerometer();
        //  Config_Save();
    } else if (!strncmp(pc_string, "$POSITION",9)) {
        char* pointer;
        char* Data[5];
        int index = 0;
        //Split data at commas
        pointer = strtok(pc_string, ",");
        if(pointer == NULL)
            Data[0] = pc_string;
        while(pointer != NULL) {
            Data[index] = pointer;
            pointer = strtok(NULL, ",");
            index++;
        }
        gyro_pos = atoi(Data[1]);
        Config_Save();
    } else {
        pc.puts(pc_string);
    }
}

void gps_analyse(char* gps_string)
{
    string temp(gps_string);
    pc.puts(temp.c_str());
    while ( temp.find ("\r\n") != string::npos ) {
        temp.erase ( temp.find ("\r\n"), 2 );
    }
    sprintf(gps_string,"%s",temp.c_str());

    if (!strncmp(gps_string, "$GPRMC", 6) || !strncmp(gps_string, "$GNRMC", 6) || !strncmp(gps_string, "$GLRMC", 6)) {
        if (nmea_rmc(gps_string)) {
            action_on_rmc();
        } //analyse and decompose the rmc string
        else {
            char test[256];
            sprintf(test,"%s*",temp.c_str());
            sprintf(output,"%s*%02X\r\n\0",temp.c_str(),getCheckSum(test));
            bluetooth.puts(output);
            // pc.puts(output);
        }
    } else {
        char test[256];
        sprintf(test,"%s*\0",gps_string);
        sprintf(output,"%s*%02X\r\n\0",gps_string,getCheckSum(test));
        bluetooth.puts(output);
        //pc.puts(output);
    }
}

static int i2 = 0;
static bool end2 = false;
static bool start2 = false;

bool getline2()
{
    int gotstring=false;
    while (1) {
        if( !bluetooth.readable() ) {
            break;
        }
        char c = bluetooth.getc();
        if (c == 36 ) {
            start2=true;
            end2 = false;
            i2 = 0;
        }
        if ((start2) && (c == 10)) {
            end2=true;
            start2 = false;
        }
        if (start2) {
            msg2[i2]=c;
            i2++;
            if (i2>255) break; //protect msg buffer!
        }
        if (end2) {
            msg2[i2]=c;
            msg2[i2+1] = 0;
            start2 = false;
            gotstring = true;
            end2=false;
            i2=0;
            break;
        }
    }
    return gotstring;
}

static int i=0;
static bool start=false;
static bool end=false;

bool getline(bool forward)
{
    while (1) {
        //   gps.putc('\0');
        if( !gps.readable() ) {
            break;
        }
        char c = gps.getc();
        if (forward) { //simply forward all to Bluetooth
            pc.putc(c);
        }
        if (c == 36 ) {
            start=true;
            end = false;
            i = 0;
        }
        if ((start) && (c == 10)) {
            end=true;
            start = false;
        }
        if (start) {
            msg[i]=c;
            i++;
            if (i>255) break; //protect msg buffer!
        }
        if (end) {
            msg[i]=c;
            msg[i+1] = 0;
            i=0;
            start = false;
            end = true;
            break;
        }
    }
    return end;
}

void keyPressedHeld( void )
{
    motor_enable_state = "$ENABLE,0\r\n";
    motor_enable = 0;
    pwm1 = 0.0;
    pwm2 = 0.0;
    ledGREEN=1; //show green for being ready to steer
}

void keyReleasedHeld( void )
{
    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    motor_enable_state = "$ENABLE,1\r\n";
    motor_enable = 1;
    pwm1 = 0.0;
    pwm2 = 0.0;
    ledGREEN=0;
}

void boom1PressedHeld( void )
{
    // ledGREEN=1;
    boom18=boom18 & 0xFE;
}

void boom1ReleasedHeld( void )
{
    //ledGREEN=0;
    boom18=boom18 | 0x01;
}

void boom2PressedHeld( void )
{
    boom18=boom18 & 0xFD;
}

void boom2ReleasedHeld( void )
{
    boom18=boom18 | 0x02;
}
void boom3PressedHeld( void )
{
    boom18=boom18 & 0xFB;
}

void boom3ReleasedHeld( void )
{
    boom18=boom18 | 0x04;
}

void boom4PressedHeld( void )
{
    boom18=boom18 & 0xF7;
}

void boom4ReleasedHeld( void )
{
    boom18=boom18 | 0x08;
}

void toprint()
{
    angle_send = 1;
}

int main()
{
    bluetooth.baud(115200);
    gps.baud(38400);
    pc.baud(38400);

//JH prepare and send version info
    vTimer.start();
    vTimer.reset();

    motTimer.start();
    motTimer.reset();
    lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
    motor_enable_state = "$ENABLE,1\r\n";

    btTimer.start();
    btTimer.reset();
    lastgetBT= btTimer.read_ms();

    bluetooth.puts(version);

    lastsend_version=vTimer.read_ms()-18000;
    //  pc.puts("test\r\n");
    Config_Startup();
    //_ID = Config_GetID();
    Config_Save();

    boom1.attach_asserted_held( &boom1PressedHeld );
    boom1.attach_deasserted_held( &boom1ReleasedHeld );
    boom1.setSampleFrequency(); //default = 20 ms
    boom1.setSamplesTillAssert(5);
    boom1.setSamplesTillHeld(5);
    boom2.attach_asserted_held( &boom2PressedHeld );
    boom2.attach_deasserted_held( &boom2ReleasedHeld );
    boom2.setSamplesTillAssert(5);
    boom2.setSamplesTillHeld(5);
    boom2.setSampleFrequency();
    boom3.attach_asserted_held( &boom3PressedHeld );
    boom3.attach_deasserted_held( &boom3ReleasedHeld );
    boom3.setSamplesTillAssert(5);
    boom3.setSamplesTillHeld(5);
    boom3.setSampleFrequency();
    boom4.attach_asserted_held( &boom4PressedHeld );
    boom4.attach_deasserted_held( &boom4ReleasedHeld );
    boom4.setSamplesTillAssert(5);
    boom4.setSamplesTillHeld(5);
    boom4.setSampleFrequency();

    motor_switch.setSamplesTillAssert(5);
    motor_switch.setSamplesTillHeld(5);
    motor_switch.setSampleFrequency();
    motor_switch.attach_asserted_held( &keyPressedHeld );
    motor_switch.attach_deasserted_held( &keyReleasedHeld );

    initializeAccelerometer();
    calibrateAccelerometer();
    initializeGyroscope();
    calibrateGyroscope();

    accelerometerTicker.attach(&sampleAccelerometer, 0.005);
    gyroscopeTicker.attach(&sampleGyroscope, 0.005);
    filterTicker.attach(&filter, FILTER_RATE);
    angle_print.attach(&toprint,0.2);
    activate_antenna();
    autosteer_timeout.start();

    SetDigitalFilter(phaseadv,tcenter, 0);  //for FGPS guidance

    while(1) {
        //JH send version information every 10 seconds to keep Bluetooth alive
        if ((vTimer.read_ms()-lastsend_version)>25000) {
            // pc.puts(version);
            bluetooth.puts(version);
            vTimer.reset();
            lastsend_version=vTimer.read_ms();
        }

        if ( autosteer_timeout.read_ms() > 5000 || autosteer_timeout.read_ms() > time_till_stop ) {
            autosteer_timeout.reset();
            enable_motor = 0;
        }
        if ( antenna_active == 1 && gps.readable()) {
            if (getline(false)) {
                if ( validate_checksum(msg,result)) {
                    // pc.puts(msg);
                    gps_analyse(result);
                } else {
                    pc.puts(result);
                }
            }
        }
        if ( bluetooth.readable()) {
            if (getline2()) {
                if ( validate_checksum(msg2,result)) {
                    btTimer.reset();
                    lastgetBT=  btTimer.read_ms();

                    pc.puts(msg2);
                    pc_analyse(result);
                }
            }
        }
        if (  btTimer.read_ms()-lastgetBT>1000) {
            //we did not get any commands over BT
            ledRED=1; //turn red
        } else ledRED=0;

        if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) {
            bluetooth.puts(motor_enable_state);
            motTimer.reset();
            lastsend_motorstate=motTimer.read_ms();
            lastmotor_enable=motor_enable;
        }
        if (boom18!=lastboom18) {
            boomstate[4]=boom18 | 0x80; //
            bluetooth.puts(boomstate);
            lastboom18=boom18;
        }
        if ( print_euler == 1 && angle_send == 1 ) {
            update_euler(z_arr,output, angle_send);
            pc.puts(output);
        }
    }
}