![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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:
- 4:e27ca0c82814
- Parent:
- 3:9b6db94aa2b3
- Child:
- 5:d6d8ecd418cf
--- a/main.cpp Wed Sep 14 18:19:50 2016 +0000 +++ b/main.cpp Thu Dec 08 20:38:24 2016 +0000 @@ -29,6 +29,8 @@ #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 //I2C i2c(p9, p10); // SDA, SCL BNO055 imu(p9, p10); @@ -77,6 +79,13 @@ 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.005; // simulation speed void rxCallback(MODSERIAL_IRQ_INFO *q) { @@ -138,6 +147,7 @@ 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"); @@ -150,17 +160,22 @@ imu.set_mapping(2); // Blinks light if IMU is not calibrated, stops when calibration is complete - while(int(imu.calib) < 0xCF) { + /*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 Program Trap."); // catch statement if IMU is not connected correctly - // turn on all lights is IMU is not connected correctly + 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; @@ -174,8 +189,11 @@ 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 @@ -208,7 +226,7 @@ if(newpacket) { // if xbee port receives a complete message, parse it char buf[MAX_MESSAGE_SIZE]; - + // reads from modserial port buffer, stores characters into string "buf" int i = 0; if(xbee.rxBufferGetCount()>0) { @@ -273,10 +291,16 @@ } // 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 { // goes with if auto_ctrl + 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 @@ -293,41 +317,72 @@ }/// 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)) { + if(sim_mode==0) { + 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)); + 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(); + } + + 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",); - 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_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)) { - 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)); + float tic = t.read(); + + psi_err = wrapToPi(des_psi-psi); + spd_err = des_spd - spd; + spd_err_i += spd_err; + str = Kp_psi*psi_err/(spd+0.05); + thr = Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE; + + x = x + spd*cos(psi)*dt; + y = y + spd*sin(psi)*dt; + psi = psi + str*dt; + spd = spd + thr*dt; - t_imu = t.read(); + 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\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi)); + 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(); + } // end while(1) - } - wait(1/LOOP_RATE); - // status_LED=!status_LED; - auto_ctrl_old = auto_ctrl; - } - -} +} // end main void parseMessage(char * msg) { @@ -368,23 +423,23 @@ 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: { @@ -402,7 +457,34 @@ int colors[4]= {arg1,arg1,arg1,arg1}; float brightness = arg2; setLED(colors,brightness); - } + } // 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) { + case HIL_MODE: { + auto_ctrl = true; + x = arg2; + y = arg3; + psi = arg4; + } + case SIL_MODE: { + auto_ctrl = true; + x = arg2; + y = arg3; + psi = arg4; + } + default: { + } + } + } //emd of $SIM +