File content as of revision 0:f8fd381a5b8a:

#include "mbed.h"

#include "GPSINT.h"
#include "BNO055.h"
#include "nav_ekf.h"
#include "ServoIn.h"
#include "ServoOut.h"
#include "NeoStrip.h"

#define MAX_MESSAGE_SIZE 64
#define IMU_RATE 100.0
#define GPS_RATE 1.0
#define LOOP_RATE 300.0
#define CMD_TIMEOUT 1.0
#define GEAR_RATIO (1/2.75)
// Reference origin is at entrance to hospital point monument
#define REF_LAT 38.986534
#define REF_LON -76.489914
#define REF_ALT 1.8
#define NUM_LED 28
#define LED_CLUSTERS 4
#define LED_PER_CLUSTER 12

#define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
#define COURSE_MODE 1 //Commands map to heading and speed

//I2C    i2c(p9, p10); // SDA, SCL
BNO055 imu(p9, p10);
GPSINT gps(p28,p27);

vector <int> str_buf;
int left;

// Function Prototypes
float saturateCmd(float cmd);
void menuFunction(Serial *port);
float wrapToPi(float ang);
int readMessage(Serial *port, char * buffer);
void parseMessage(char * msg);
void setLED(int *colors, float brightness);


// LED Definitions
DigitalOut rc_LED(LED1);
DigitalOut armed_LED(LED2);
DigitalOut auto_LED(LED3);
DigitalOut imu_LED(LED4);

NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);

Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
Serial xbee(p13, p14); // tx, rx for Xbee
ServoIn CH1(p15);
ServoIn CH2(p16);

//InterruptIn he_sensor(p11);

ServoOut Steer(p22);
ServoOut Throttle(p21);
Timer t; // create timer instance
Ticker log_tick;
Ticker heartbeat;
float t_imu,t_gps,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd;
float psi_err,spd_err, psi_err_i,spd_err_i;
float t_hall, dt_hall,t_run,t_stop,t_log;
bool armed, auto_ctrl,auto_ctrl_old,rc_conn;
float wheel_spd;
float arm_clock,auto_clock;
bool str_cond,thr_cond,run_ctrl,log_data;
bool log_imu,log_bno,log_odo,log_mag = false;
int cmd_mode;
float Kp_psi, Kp_spd, Ki_psi, Ki_spd;
int led1,led2,led3,led4;

int main()
{
    nav_EKF ekf;

    pc.baud(115200);
    xbee.baud(115200);
    Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
    Throttle.write(1500);   //Set Throttle to Low
    
    led1=led2=led3=led4 =WHITE;
    left = 0;
    str_cmd = 0;
    str=0;
    thr=0;
    thr_cmd = 0;
    des_psi = 0;
    des_spd = 0;
    psi_err = 0;
    spd_err = 0;
    spd_err_i = 0;
    arm_clock = 0;
    auto_clock = 0;
    Kp_psi = 1;
    Kp_spd = 0.3;
    Ki_spd = 0.05;
    str_cond = false;
    thr_cond = false;
    armed = false;
    rc_conn = false;
    auto_ctrl = false;
    auto_ctrl_old = false;
    run_ctrl = false;
    log_data = false;

    t.start();
    t_imu = t.read();
    t_gps = t.read();
    t_cmd = 0;

    leds.setBrightness(0.5);
    
    rc_LED = 0;
    imu_LED = 0;
    gps.setRefPoint(REF_LAT,REF_LON,REF_ALT);
    if(imu.check()) {
        pc.printf("BNO055 connected\r\n");
        imu.setmode(OPERATION_MODE_CONFIG);
        imu.SetExternalCrystal(1);
        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
        imu.set_angle_units(RADIANS);
        imu.set_accel_units(MPERSPERS);
        imu.setoutputformat(WINDOWS);
        imu.set_mapping(2);

        while(int(imu.calib) < 0xCF) {
            pc.printf("Calibration = %x.\n\r",imu.calib);
            imu.get_calib();
            wait(0.5);
            imu_LED = !imu_LED;
        }
        imu_LED = 1;

    } else {
        pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
        rc_LED = 1;
        armed_LED = 1;
        imu_LED = 1;
        auto_LED = 1;
        while(1) {
            rc_LED = !rc_LED;
            armed_LED = !armed_LED;
            imu_LED = !imu_LED;
            auto_LED = !auto_LED;
            
            wait(0.5);
        }
    }
  //  int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};

    pc.printf("Emaxx Navigation Program\r\n");
        
    while(1) {

        if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
            rc_conn = false;
        } else {
            rc_conn = true;
        }
        
        rc_LED = rc_conn;
        auto_LED = auto_ctrl;
        armed_LED = armed;
        
        str_cond = (CH1.servoPulse > 1800); // If steering is full right
        thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero
        
        if(t.read()-auto_clock > 3){ //Auto control timeout if no commands recevied after 5 seconds
            auto_ctrl = false;     

        }
        

   
        if(xbee.readable()) {
            char buf[MAX_MESSAGE_SIZE];
            int n = readMessage(&xbee,buf); 
            parseMessage(buf);   

        }

        if(!rc_conn) { // Is System Armed
           // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);                
            if(auto_ctrl) {
                 
                 switch (cmd_mode){
                    case DIRECT_MODE: {
                        str = str_cmd;
                        thr = thr_cmd;
                      //  printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr); 

                        break;
                    }
                    case COURSE_MODE: {
                        
                        psi_err = wrapToPi(des_psi-imu.euler.yaw);
                        spd_err = des_spd - ekf.getSpd();
                        spd_err_i += spd_err;
                        str = -Kp_psi*psi_err;
                        
                        thr =  Kp_spd*spd_err + Ki_spd*spd_err_i;
                        
                        if (thr >= 1.0){
                            thr = 1.0;
                            spd_err_i = 0; // Reset Integral If Saturated
                        }
                        if (thr < 0){
                            thr = 0;
                            spd_err_i = 0;  // Reset Integral If Saturated                          
                        }

                        //printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr); 
                        break;

                    }
                    default:{
                        break;
                    }                 
                
                }
                Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
                 
            } else {
                
                Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                Throttle.write(1500); //Write Throttle Pulse
                
            }

        } else {
            auto_ctrl = false;
            armed_LED = 0;          //Turn off armed LED indicator
            str = ((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
            thr = ((CH2.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
            Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
            Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
                     
        }/// end  else armed

        imu.get_angles();
        imu.get_accel();
        imu.get_gyro();
        imu.get_lia();
        float dt = t.read()-t_imu;
        if(dt > (1/IMU_RATE)) {

            float tic = t.read();
            ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
            ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);

            xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
            xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));

            t_imu = t.read();
        }

        if(t.read()-t_gps >(1/GPS_RATE)) {
            //printf("Kp_psi: %.2f Kp_spd: %.2f Ki_spd: %.2f\r\n", Kp_psi,Kp_spd,Ki_spd); 
 

            float tic2 = t.read();
            ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
           // xbee.printf("$GPS,%.8f,%.8f,%.3f,%.3f,%.3f\r\n",gps.dec_latitude,gps.dec_longitude,gps.msl_altitude,gps.course_d,KNOTS_2_MPS*gps.speed_k);
           // xbee.printf("$STA,%d,%d,%d,%d\r\n",);

            t_gps = t.read();
            
        }
        wait(1/LOOP_RATE);
       // status_LED=!status_LED;
       auto_ctrl_old = auto_ctrl;
    }

}
int readMessage(Serial *port, char * buffer){
int i = 0;

    if(port->readable()){
        char c = port->getc();
        if(c=='$'){
            buffer[i] = c;
            i++;
            while(1){
            buffer[i] = port->getc();
                if(buffer[i]=='\n' || buffer[i]=='\r'){
                    break;
                }
            i++;          
            }
        }
    }
    return i;
}

void parseMessage(char * msg){

    if(!strncmp(msg, "$CMD", 4)){
        int arg1;
        float arg2,arg3;

        sscanf(msg,"$CMD,%d,%f,%f\n",&arg1,&arg2,&arg3);
        cmd_mode = arg1;
        auto_clock = t.read();
         switch (cmd_mode){
            case DIRECT_MODE: {
                auto_ctrl = true;
                str_cmd = arg2;
                thr_cmd = arg3;
            }
            case COURSE_MODE: {
                auto_ctrl = true;
                des_psi = arg2;
                des_spd = arg3;
            }
            default:{
            }    
        }
    }   //emd of $CMD
    
    /*if(!strncmp(msg, "$PRM", 4)){
        float arg1,arg2,arg3;
        int type;
       // __disable_irq();        // disable interrupts
        sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3);
       // __enable_irq();         // enable interrupts
        
        switch(type){
            case 1:
            {
                Kp_psi = arg1;
                Kp_spd = arg2;
                Ki_spd = arg3; 
                printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
                break;
            }
            case 2:
            {
                printf("%s",msg);

                float ref_lat = arg1;
                float ref_lon = arg2;
                float ref_alt = arg3;
                printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);

              //  gps.setRefPoint(ref_lat,ref_lon,ref_alt);                  
                break;
            } 
            default:
            {
            }  
                
        }

    } */
    
    if(!strncmp(msg, "$LED", 4)){
        int arg1;
        float arg2;
        sscanf(msg,"$LED,%x,%f",&arg1,&arg2);
        printf("%s\r\n",msg);
        int colors[4]={arg1,arg1,arg1,arg1};
        float brightness = arg2;
        setLED(colors,brightness);        
    }


 

}
void setLED(int  *colors,float brightness){
        
        leds.setBrightness(brightness);

        int cidx = 0;
        int ctr = 0;
        for (int i=0;i<LED_PER_CLUSTER*LED_CLUSTERS;i++){
            
            if(ctr >11){
                ctr = 0;
                cidx++;                   
            }
            leds.setPixel(i,colors[cidx]);
            ctr++;
        }
        leds.write();
}
float saturateCmd(float cmd)
{
    if(cmd>1.0) {
        cmd = 1.0;
    }
    if(cmd < -1.0) {
        cmd = -1.0;
    }
    return cmd;
}
float saturateCmd(float cmd, float max,float min)
{
    if(cmd>max) {
        cmd = max;
    }
    if(cmd < min) {
        cmd = min;
    }
    return cmd;
}

float wrapToPi(float ang)
{

    if(ang > PI) {

        ang = ang - 2*PI;
    }
    if (ang < -PI) {
        ang = ang + 2*PI;
    }

    return ang;
}