Emaxx Navigation code ported for MBED
Dependencies: BNO055_fusion Emaxx_Navigation_Dynamic_HIL MODSERIAL ServoIn ServoOut Vehicle_Model mbed
Fork of Emaxx_Navigation_Dynamic_HIL by
Diff: main.cpp
- Revision:
- 2:0c9c3c1f3b18
- Parent:
- 0:f8fd381a5b8a
- Child:
- 3:9b6db94aa2b3
diff -r 89451993fd3b -r 0c9c3c1f3b18 main.cpp --- a/main.cpp Tue Aug 16 13:49:47 2016 +0000 +++ b/main.cpp Thu Aug 25 15:47:50 2016 +0000 @@ -10,6 +10,8 @@ #include "ServoIn.h" #include "ServoOut.h" #include "NeoStrip.h" +#include "MODSERIAL.h" + #define MAX_MESSAGE_SIZE 64 #define IMU_RATE 100.0 @@ -37,9 +39,7 @@ // 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); @@ -52,8 +52,9 @@ NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER); +// Comms and control object definitions Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc -Serial xbee(p13, p14); // tx, rx for Xbee +MODSERIAL xbee(p13, p14); // tx, rx for Xbee ServoIn CH1(p15); ServoIn CH2(p16); @@ -64,28 +65,43 @@ 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_imu,t_gps,t_cmd,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; -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; +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 + +void rxCallback(MODSERIAL_IRQ_INFO *q) +{ + MODSERIAL *serial = q->serial; + if ( serial->rxGetLastChar() == '\n') { + newpacket = true; + } +} + int main() { - nav_EKF ekf; + nav_EKF ekf; // initialize ekf states - pc.baud(115200); - xbee.baud(115200); + pc.baud(115200); // set baud rate of serial comm to pc + xbee.baud(115200); // 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 - - led1=led2=led3=led4 =WHITE; + + 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; @@ -110,16 +126,19 @@ run_ctrl = false; log_data = false; + // timer and timing initializations 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); + 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); @@ -130,119 +149,148 @@ 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; - } - imu_LED = 1; + } // end while(imu.calib) + imu_LED = 1; // turns on IMU light when calibration is successful } else { - pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); + pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); // catch statement if IMU is not connected correctly + // 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); - } - } - // int colors[4] = {ORANGE,YELLOW,GREEN,CYAN}; + } // end while(1) {blink if IMU is not connected} + } // end if(imu.check) - pc.printf("Emaxx Navigation Program\r\n"); - + // 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 5 seconds - auto_ctrl = false; - } - + 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(xbee.readable()) { + if(newpacket) { // if xbee port receives a complete message, parse it char buf[MAX_MESSAGE_SIZE]; - int n = readMessage(&xbee,buf); - parseMessage(buf); + + // 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++; + } + } + } + xbee.rxBufferFlush();// empty receive buffer + pc.printf("%s",buf); + parseMessage(buf); + newpacket = false; + } // end if(newpacket) - } if(!rc_conn) { // Is System Armed - // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock); + // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock); if(auto_ctrl) { - - switch (cmd_mode){ + + 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); - + // 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: { - + 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 = Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE; + + if (thr >= 1.0) { thr = 1.0; spd_err_i = 0; // Reset Integral If Saturated - } - if (thr < 0){ + } // end if thr>=1.0 + if (thr < 0) { thr = 0; - spd_err_i = 0; // Reset Integral If Saturated - } + spd_err_i = 0; // Reset Integral If Saturated + } // end iff thr<0 - //printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr); + //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; - } - default:{ + } // end COURSE_MODE case + default: { break; - } - - } + } // end default status in switch + + } // end switch(cmd_mode) Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse - - } else { - + + } else { // goes with if auto_ctrl + Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse Throttle.write(1500); //Write Throttle Pulse - - } - } else { + } // end else + + } else { // goes with if !rc_conn 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(); @@ -263,53 +311,35 @@ } 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); - + //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",); + // 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; + // 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) +{ -void parseMessage(char * msg){ - - if(!strncmp(msg, "$CMD", 4)){ + 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){ + switch (cmd_mode) { case DIRECT_MODE: { auto_ctrl = true; str_cmd = arg2; @@ -320,77 +350,81 @@ des_psi = arg2; des_spd = arg3; } - default:{ - } + default: { + } } } //emd of $CMD - - /*if(!strncmp(msg, "$PRM", 4)){ + + if(!strncmp(msg, "$PRM", 4)) { float arg1,arg2,arg3; int type; - // __disable_irq(); // disable interrupts + // __disable_irq(); // disable interrupts sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3); - // __enable_irq(); // enable interrupts - - switch(type){ - case 1: - { + // __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; - printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3); + 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: - { - printf("%s",msg); + 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; - printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3); - - // gps.setRefPoint(ref_lat,ref_lon,ref_alt); + 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: - { - } - + } + default: { + } + } - } */ - - if(!strncmp(msg, "$LED", 4)){ + } // end of $PRM + + 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}; + //pc.printf("%s\n",msg); + int colors[4]= {arg1,arg1,arg1,arg1}; float brightness = arg2; - setLED(colors,brightness); + setLED(colors,brightness); } - + } -void setLED(int *colors,float brightness){ - - leds.setBrightness(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++) { - 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++; + if(ctr >11) { + ctr = 0; + cidx++; } - leds.write(); + leds.setPixel(i,colors[cidx]); + ctr++; + } + leds.write(); } float saturateCmd(float cmd) { @@ -425,4 +459,4 @@ } return ang; -} +} \ No newline at end of file