Fork of Emaxx Navigation code with dynamic vehicle model and HIL support. Strange bugs with mbed locking up upon receiving certain messages.
Dependencies: BNO055_fusion Vehicle_Model MODSERIAL ServoIn ServoOut mbed
Fork of Emaxx_Navigation by
Diff: main.cpp
- Revision:
- 7:a8c2e9d049e8
- Parent:
- 6:f64b1eba4d5e
- Child:
- 8:9817993e5df7
--- a/main.cpp Wed Dec 14 15:47:49 2016 +0000 +++ b/main.cpp Fri Dec 16 14:06:42 2016 +0000 @@ -1,9 +1,5 @@ - - - - #include "mbed.h" - +#include "utilityFunctions.h" #include "GPSINT.h" #include "BNO055.h" #include "nav_ekf.h" @@ -11,10 +7,12 @@ #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 @@ -29,20 +27,22 @@ #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized #define COURSE_MODE 1 //Commands map to heading and speed -#define HIL_MODE 0 // commands map to hardware active simulation -#define SIL_MODE 1 // commands map to software only simulation + +#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(p28,p27); +GPSINT gps(p13,p14); vector <int> str_buf; int left; // Function Prototypes -float saturateCmd(float cmd); -float wrapToPi(float ang); -void parseMessage(char * msg); +//float saturateCmd(float cmd); +//float wrapToPi(float ang); +int parseMessage(char * msg); void setLED(int *colors, float brightness); @@ -55,11 +55,11 @@ NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER); // Comms and control object definitions -Serial pc(p13, p14); // tx, rx for serial USB interface to pc -MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee +//MODSERIAL pc(p13, p14); // tx, rx for serial USB interface to pc +//MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee -// Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc -// MODSERIAL xbee(p13, p14); // 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); @@ -70,7 +70,8 @@ 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; // control parameters +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 @@ -90,6 +91,9 @@ 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; @@ -101,17 +105,17 @@ int main() { - nav_EKF ekf; // initialize ekf states + pc.baud(115200); // set baud rate of serial comm to pc - xbee.baud(115200); // set baud rate of serial comm of wireless xbee comms + 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? + led1=led2=led3=led4=WHITE; // set color of neo strip lights? // initialize necessary float and boolean variables left = 0; @@ -126,7 +130,7 @@ spd_err_i = 0; arm_clock = 0; auto_clock = 0; - Kp_psi = 1; + Kp_psi = 1/1.57; Kp_spd = 0.3; Ki_spd = 0.05; str_cond = false; @@ -137,11 +141,15 @@ 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 @@ -226,7 +234,6 @@ - if(newpacket) { // if xbee port receives a complete message, parse it char buf[MAX_MESSAGE_SIZE]; // create buffer for message @@ -248,7 +255,7 @@ } } // end if xbee.rxBufferGetCount xbee.rxBufferFlush();// empty receive buffer - pc.printf("%s",buf); // print message to PC + //pc.printf("%s",buf); // print message to PC parseMessage(buf); newpacket = false; // reset packet flag } // end if(newpacket) @@ -256,8 +263,10 @@ 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; @@ -269,34 +278,41 @@ case COURSE_MODE: { if(sim_mode==0) { // if hardware is enabled use gyro and ekf - psi_err = wrapToPi(des_psi-imu.euler.yaw); + 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-psi); - spd_err = des_spd - spd; + + 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/spd; + + + /* 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/LOOP_RATE; + thr = Kp_spd*spd_err + Ki_spd*spd_err_i*cdt; if (thr >= 1.0) { thr = 1.0; - spd_err_i = 0; // Reset Integral If Saturated + 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; // Reset Integral If Saturated + spd_err_i = 0.0; // Reset Integral If Saturated } // end iff thr<0 - //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); + + 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 @@ -305,7 +321,8 @@ } // 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 @@ -313,7 +330,7 @@ // 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 @@ -328,6 +345,7 @@ 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 @@ -336,42 +354,54 @@ }/// end else armed if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off - imu.get_angles(); - imu.get_accel(); - imu.get_gyro(); - imu.get_lia(); 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_imu = t.read(); + // 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)) { - //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; - } else { // else condition implies a simulation mode is enabled + /* } else { // else condition implies a simulation mode is enabled float dt = t.read()-t_imu; if(dt > (1/IMU_RATE)) { @@ -396,14 +426,14 @@ } // end if dt - } // end if sim_mode + } // end if sim_mode */ } // end while(1) } // end main -void parseMessage(char * msg) +int parseMessage(char * msg) { if(!strncmp(msg, "$CMD", 4)) { @@ -427,6 +457,7 @@ default: { } } + return 1; } //emd of $CMD if(!strncmp(msg, "$PRM", 4)) { @@ -463,9 +494,9 @@ } default: { } - + } - + return 1; } // end of $PRM if(!strncmp(msg, "$LED", 4)) { @@ -476,37 +507,37 @@ 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) - auto_clock = t.read(); - switch (cmd_mode) { + 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) @@ -527,37 +558,4 @@ } 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; -} \ No newline at end of file