File content as of revision 7:a8c2e9d049e8:

#include "mbed.h"
#include "utilityFunctions.h"
#include "GPSINT.h"
#include "BNO055.h"
#include "nav_ekf.h"
#include "ServoIn.h"
#include "ServoOut.h"
#include "NeoStrip.h"
#include "MODSERIAL.h"
#include "vehicleModel.h"


#define MAX_MESSAGE_SIZE 64
#define IMU_RATE 100.0
#define LOG_RATE 20.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

#define HARDWARE_MODE 0 
#define HIL_MODE 1 // commands map to hardware active simulation
#define SIL_MODE 2 // commands map to software only simulation

//I2C    i2c(p9, p10); // SDA, SCL
BNO055 imu(p9, p10);
GPSINT gps(p13,p14);

vector <int> str_buf;
int left;

// Function Prototypes
//float saturateCmd(float cmd);
//float wrapToPi(float ang);
int 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);

// Comms and control object definitions
//MODSERIAL pc(p13, p14); // tx, rx for serial USB interface to pc
//MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee

MODSERIAL pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
MODSERIAL xbee(p28, p27); // 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,t_sim,t_ctrl;
float str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
float psi_err,spd_err, psi_err_i,spd_err_i; // control variables
float t_hall, dt_hall,t_run,t_stop,t_log;
bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
float wheel_spd; // wheel speed variable
float arm_clock,auto_clock; // timing for arming procedures
bool str_cond,thr_cond,run_ctrl,log_data; // data saving variables?
bool log_imu,log_bno,log_odo,log_mag = false; // data saving variables?
int cmd_mode; // integer to set command mode of controller
float Kp_psi, Kp_spd, Ki_psi, Ki_spd; // controller gains
int led1,led2,led3,led4; // neo-strip variables?
volatile bool newpacket = false; // boolean identifier of new odroid packet
float x0; // initial x-position when in software or hardware in the loop simulation
float y0; // initial y-position when in software or hardware in the loop simulation
int sim_mode = 0; // sets simulation mode, zero by default, 1 for HIL, 2 for SIL
float x = 0.0; // simulation variables
float y = 0.0; // simulation variables
float psi = 0.0; // simulation variables
float spd = 0.0; // simulation speed

nav_EKF ekf; // initialize ekf states
vehicleModel vm; //create vehicle model object

void rxCallback(MODSERIAL_IRQ_INFO *q)
{
    MODSERIAL *serial = q->serial;
    if ( serial->rxGetLastChar() == '\n') {
        newpacket = true;
    }
}


int main()
{


    pc.baud(115200); // set baud rate of serial comm to pc
    xbee.baud(57600); // set baud rate of serial comm of wireless xbee comms
    Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
    Throttle.write(1500);   //Set Throttle to Low

    xbee.attach(&rxCallback, MODSERIAL::RxIrq);


    led1=led2=led3=led4=WHITE; // set color of neo strip lights?

    // initialize necessary float and boolean variables
    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/1.57;
    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;
    cmd_mode = 1;

    // timer and timing initializations
    t.start();
    t_imu = t.read();
    t_gps = t.read();
    t_sim = t.read();
    t_ctrl = t.read();
    t_log = t.read();
    t_cmd = 0;

    leds.setBrightness(0.5); // set brightness of leds

    rc_LED = 0; // turn off LED 1 to indicate no RC connection
    imu_LED = 0; // turn off IMU indicator (LED 4)
    gps.setRefPoint(REF_LAT,REF_LON,REF_ALT); // set local origin of reference frame for GPS conversion


    // procedure to ensure IMU is operating correctly
    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);

        // Blinks light if IMU is not calibrated, stops when calibration is complete
        /*while(int(imu.calib) < 0xCF) {
            pc.printf("Calibration = %x.\n\r",imu.calib);
            imu.get_calib();
            wait(0.5);
            imu_LED = !imu_LED;
        } // end while(imu.calib)
        */
        imu_LED = 1; // turns on IMU light when calibration is successful

    } else {
        pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly

        sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)


        /* // turn on all lights is IMU is not connected correctly
        rc_LED = 1;
        armed_LED = 1;
        imu_LED = 1;
        auto_LED = 1;
        while(1) {
            // blink all lights if IMU is not connected correctly
            rc_LED = !rc_LED;
            armed_LED = !armed_LED;
            imu_LED = !imu_LED;
            auto_LED = !auto_LED;

            wait(0.5);
        } // end while(1) {blink if IMU is not connected}
        */

    } // end if(imu.check)


    //  int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};

    pc.printf("Emaxx Navigation Program\r\n"); // print indication that calibration is good and nav program is running


    while(1) {

        // check for servo pulse from either channel of receiver module
        if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
            rc_conn = false;
        } else {
            rc_conn = true;
        } // end if(Channels connected)

        // turn on RC led if transmitter is connected
        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 3 seconds
            auto_ctrl = false;
        } // end if(t.read()-autoclock>3) timeout procedure




        if(newpacket) { // if xbee port receives a complete message, parse it
            char buf[MAX_MESSAGE_SIZE]; // create buffer for message

            // reads from modserial port buffer, stores characters into string "buf"
            int i = 0;
            if(xbee.rxBufferGetCount()>0) {
                char c = xbee.getc();
                //pc.printf("%s",c);
                if(c=='$') {
                    buf[i] = c;
                    i++;
                    while(1) {
                        buf[i] = xbee.getc();
                        if(buf[i]=='\n') {
                            break;
                        }
                        i++;
                    }
                }
            } // end if xbee.rxBufferGetCount
            xbee.rxBufferFlush();// empty receive buffer
            //pc.printf("%s",buf); // print message to PC
            parseMessage(buf);
            newpacket = false; // reset packet flag
        } // end if(newpacket)


        if(!rc_conn) { // Is System Armed, system armed if RC not connected
            // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);

            if(auto_ctrl) {
                float cdt = t.read()-t_ctrl;
                t_ctrl = t.read();
                switch (cmd_mode) {
                    case DIRECT_MODE: {
                        str = str_cmd;
                        thr = thr_cmd;
                        //  pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
                        //  xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
                        break;
                    } // end direct mode case
                    case COURSE_MODE: {

                        if(sim_mode==0) { // if hardware is enabled use gyro and ekf
                            psi_err = wrapToPi(des_psi-imu.euler.yaw); 
                            
                            spd_err = des_spd - ekf.getSpd();
                            spd_err_i += spd_err;
                            str = -Kp_psi*psi_err/ekf.getSpd();
                        } else { // otherwise design control using simulated variables, bypass ekf states
 
                            psi_err = wrapToPi(des_psi-vm.getYaw());
                            str = Kp_psi*psi_err;         
                                                    
                            spd_err = des_spd - vm.getSpd();
                            spd_err_i += spd_err;
                              
                             
                           /* if(spd>0.05) {
                                str = Kp_psi*psi_err/vm.getSpd();
                            } else {
                                str = Kp_psi*psi_err/0.05;
                            }*/
                        } // end if sim_mode

                        thr =  Kp_spd*spd_err + Ki_spd*spd_err_i*cdt;

                        if (thr >= 1.0) {
                            thr = 1.0;
                            spd_err_i = 0.0; // Reset Integral If Saturated
                        } // end if thr>=1.0
                        if (thr < 0.0) {
                            thr = 0.0;
                            spd_err_i = 0.0;  // Reset Integral If Saturated
                        } // end iff thr<0

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

                    } // end COURSE_MODE case
                    default: {
                        break;
                    } // end default status in switch

                } // end switch(cmd_mode)
                str = saturateCmd(str);
                //xbee.printf("Psi: %.3f Psi Err: %.3f Str Cmd %.3f\r\n",vm.getYaw(),psi_err,str);
                if(sim_mode<2) { // only actuates if in experiment or HIL modes
                    Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                    Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
                } else {
                    // won't send command to motor and servo if in SIL mode
                }

                
            } else { // goes with if auto_ctrl, manual control mode

                Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                Throttle.write(1500); //Write Throttle Pulse

            } // end else

        } else { // goes with if !rc_conn

            // for manual driving
            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
            
            if(sim_mode<2) { // if hardware is active send command to servo and throttle
                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

        if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off
            float dt = t.read()-t_imu;
            if(dt > (1/IMU_RATE)) {
                imu.get_angles();
                imu.get_accel();
                imu.get_gyro();
                imu.get_lia();
                
                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);
                t_imu = t.read();

            }
        }else{ // Else we are running a simulation
            float T = t.read()-t_sim;
            vm.stepModel(str,thr,T);            
            t_sim = t.read();
        }      
            
        if(t.read()-t_log > (1/LOG_RATE)) {
                             
            if(sim_mode > 0) {
               xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw())); 
               xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
               
              // pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));                
                              
              //  pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
              //  pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
            } else {
                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));
              //  pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
              //  pc.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_log = t.read();
        }

            if(t.read()-t_gps >(1/GPS_RATE)) {
                float tic2 = t.read();
                ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
                t_gps = t.read();
            }
            wait(1/LOOP_RATE);
            // status_LED=!status_LED;
            auto_ctrl_old = auto_ctrl;

       /* } else { // else condition implies a simulation mode is enabled

            float dt = t.read()-t_imu;
            if(dt > (1/IMU_RATE)) {

                float tic = t.read();


                x = x + spd*cos(psi)*dt; // self-propelled particle kinematics
                y = y + spd*sin(psi)*dt; // self-propelled particle kinematics
                psi = psi + str*dt; // turn rate kinematics
                float drag = 0.0;
                if(spd>1) {
                    drag = 0.0059*spd*spd; // based on drag on a cat sized object
                } else {
                    drag = 0.0059*spd;
                }
                spd = spd + (thr-drag)*dt; // give throttle offset

                xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
                pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi),thr,des_spd,spd,str);
                t_imu = t.read();
            } // end if dt


        } // end if sim_mode */


    } // end while(1)

} // end main

int 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: {
            }
        }
     return 1;
    }   //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: { // sets PID gains on heading and speed controller
                //pc.printf("%s\n",msg);
                Kp_psi = arg1;
                Kp_spd = arg2;
                Ki_spd = arg3;

                pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
                //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
                break;
            }
            case 2: { // sets origin of local reference frame

                //pc.printf("%s\n",msg);

                float ref_lat = arg1;
                float ref_lon = arg2;
                float ref_alt = arg3;
                gps.setRefPoint(ref_lat,ref_lon,ref_alt);

                pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
                //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);

                break;
            }
            default: {
            }
            
        }
        return 1;
    } // end of $PRM

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


    if(!strncmp(msg, "$SIM", 4)) {
        int arg1;
        float arg2,arg3,arg4;
        sscanf(msg,"$SIM,%d,%f,%f,%f\n",&arg1,&arg2,&arg3,&arg4);
        sim_mode = arg1; // sets whether in hardware in the loop or software in the loop (actuators active or not during simulation)
        switch (sim_mode) {
            case HIL_MODE: {
                auto_ctrl = true;
                x = arg2;
                y = arg3;
                psi = arg4;
                vm.initialize(x,y,psi);
            }
            case SIL_MODE: {
                auto_ctrl = true;
                x = arg2;
                y = arg3;
                psi = arg4;
                vm.initialize(x,y,psi);
                
            }
            default: {
            }
        }
        return 1;
    }   //emd of $SIM
    return 0;

}
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();
}