USNA WSE ES456 / Mbed 2 deprecated Emaxx_Navigation_madpulse

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 Emaxx Navigation Group

main.cpp@4:e27ca0c82814, 2016-12-08 (annotated)

Committer:
lddevrie
Date:
Thu Dec 08 20:38:24 2016 +0000
Revision:
4:e27ca0c82814
Parent:
3:9b6db94aa2b3
Child:
5:d6d8ecd418cf
Update for hardware and software in the loop simulation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:f8fd381a5b8a 1
jdawkins 0:f8fd381a5b8a 2
jdawkins 0:f8fd381a5b8a 3
jdawkins 0:f8fd381a5b8a 4
jdawkins 0:f8fd381a5b8a 5 #include "mbed.h"
jdawkins 0:f8fd381a5b8a 6
jdawkins 0:f8fd381a5b8a 7 #include "GPSINT.h"
jdawkins 0:f8fd381a5b8a 8 #include "BNO055.h"
jdawkins 0:f8fd381a5b8a 9 #include "nav_ekf.h"
jdawkins 0:f8fd381a5b8a 10 #include "ServoIn.h"
jdawkins 0:f8fd381a5b8a 11 #include "ServoOut.h"
jdawkins 0:f8fd381a5b8a 12 #include "NeoStrip.h"
lddevrie 2:0c9c3c1f3b18 13 #include "MODSERIAL.h"
lddevrie 2:0c9c3c1f3b18 14
jdawkins 0:f8fd381a5b8a 15
jdawkins 0:f8fd381a5b8a 16 #define MAX_MESSAGE_SIZE 64
jdawkins 0:f8fd381a5b8a 17 #define IMU_RATE 100.0
jdawkins 0:f8fd381a5b8a 18 #define GPS_RATE 1.0
jdawkins 0:f8fd381a5b8a 19 #define LOOP_RATE 300.0
jdawkins 0:f8fd381a5b8a 20 #define CMD_TIMEOUT 1.0
jdawkins 0:f8fd381a5b8a 21 #define GEAR_RATIO (1/2.75)
jdawkins 0:f8fd381a5b8a 22 // Reference origin is at entrance to hospital point monument
jdawkins 0:f8fd381a5b8a 23 #define REF_LAT 38.986534
jdawkins 0:f8fd381a5b8a 24 #define REF_LON -76.489914
jdawkins 0:f8fd381a5b8a 25 #define REF_ALT 1.8
jdawkins 0:f8fd381a5b8a 26 #define NUM_LED 28
jdawkins 0:f8fd381a5b8a 27 #define LED_CLUSTERS 4
jdawkins 0:f8fd381a5b8a 28 #define LED_PER_CLUSTER 12
jdawkins 0:f8fd381a5b8a 29
jdawkins 0:f8fd381a5b8a 30 #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
jdawkins 0:f8fd381a5b8a 31 #define COURSE_MODE 1 //Commands map to heading and speed
lddevrie 4:e27ca0c82814 32 #define HIL_MODE 0 // commands map to hardware active simulation
lddevrie 4:e27ca0c82814 33 #define SIL_MODE 1 // commands map to software only simulation
jdawkins 0:f8fd381a5b8a 34
jdawkins 0:f8fd381a5b8a 35 //I2C i2c(p9, p10); // SDA, SCL
jdawkins 0:f8fd381a5b8a 36 BNO055 imu(p9, p10);
jdawkins 0:f8fd381a5b8a 37 GPSINT gps(p28,p27);
jdawkins 0:f8fd381a5b8a 38
jdawkins 0:f8fd381a5b8a 39 vector <int> str_buf;
jdawkins 0:f8fd381a5b8a 40 int left;
jdawkins 0:f8fd381a5b8a 41
jdawkins 0:f8fd381a5b8a 42 // Function Prototypes
jdawkins 0:f8fd381a5b8a 43 float saturateCmd(float cmd);
jdawkins 0:f8fd381a5b8a 44 float wrapToPi(float ang);
jdawkins 0:f8fd381a5b8a 45 void parseMessage(char * msg);
jdawkins 0:f8fd381a5b8a 46 void setLED(int *colors, float brightness);
jdawkins 0:f8fd381a5b8a 47
jdawkins 0:f8fd381a5b8a 48
jdawkins 0:f8fd381a5b8a 49 // LED Definitions
jdawkins 0:f8fd381a5b8a 50 DigitalOut rc_LED(LED1);
jdawkins 0:f8fd381a5b8a 51 DigitalOut armed_LED(LED2);
jdawkins 0:f8fd381a5b8a 52 DigitalOut auto_LED(LED3);
jdawkins 0:f8fd381a5b8a 53 DigitalOut imu_LED(LED4);
jdawkins 0:f8fd381a5b8a 54
jdawkins 0:f8fd381a5b8a 55 NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);
jdawkins 0:f8fd381a5b8a 56
lddevrie 2:0c9c3c1f3b18 57 // Comms and control object definitions
jdawkins 0:f8fd381a5b8a 58 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
lddevrie 2:0c9c3c1f3b18 59 MODSERIAL xbee(p13, p14); // tx, rx for Xbee
jdawkins 0:f8fd381a5b8a 60 ServoIn CH1(p15);
jdawkins 0:f8fd381a5b8a 61 ServoIn CH2(p16);
jdawkins 0:f8fd381a5b8a 62
jdawkins 0:f8fd381a5b8a 63 //InterruptIn he_sensor(p11);
jdawkins 0:f8fd381a5b8a 64
jdawkins 0:f8fd381a5b8a 65 ServoOut Steer(p22);
jdawkins 0:f8fd381a5b8a 66 ServoOut Throttle(p21);
jdawkins 0:f8fd381a5b8a 67 Timer t; // create timer instance
jdawkins 0:f8fd381a5b8a 68 Ticker log_tick;
jdawkins 0:f8fd381a5b8a 69 Ticker heartbeat;
lddevrie 2:0c9c3c1f3b18 70 float t_imu,t_gps,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
lddevrie 2:0c9c3c1f3b18 71 float psi_err,spd_err, psi_err_i,spd_err_i; // control variables
jdawkins 0:f8fd381a5b8a 72 float t_hall, dt_hall,t_run,t_stop,t_log;
lddevrie 2:0c9c3c1f3b18 73 bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
lddevrie 2:0c9c3c1f3b18 74 float wheel_spd; // wheel speed variable
lddevrie 2:0c9c3c1f3b18 75 float arm_clock,auto_clock; // timing for arming procedures
lddevrie 2:0c9c3c1f3b18 76 bool str_cond,thr_cond,run_ctrl,log_data; // data saving variables?
lddevrie 2:0c9c3c1f3b18 77 bool log_imu,log_bno,log_odo,log_mag = false; // data saving variables?
lddevrie 2:0c9c3c1f3b18 78 int cmd_mode; // integer to set command mode of controller
lddevrie 2:0c9c3c1f3b18 79 float Kp_psi, Kp_spd, Ki_psi, Ki_spd; // controller gains
lddevrie 2:0c9c3c1f3b18 80 int led1,led2,led3,led4; // neo-strip variables?
lddevrie 2:0c9c3c1f3b18 81 volatile bool newpacket = false; // boolean identifier of new odroid packet
lddevrie 4:e27ca0c82814 82 float x0; // initial x-position when in software or hardware in the loop simulation
lddevrie 4:e27ca0c82814 83 float y0; // initial y-position when in software or hardware in the loop simulation
lddevrie 4:e27ca0c82814 84 int sim_mode = 0; // sets simulation mode, zero by default, 1 for HIL, 2 for SIL
lddevrie 4:e27ca0c82814 85 float x = 0.0; // simulation variables
lddevrie 4:e27ca0c82814 86 float y = 0.0; // simulation variables
lddevrie 4:e27ca0c82814 87 float psi = 0.0; // simulation variables
lddevrie 4:e27ca0c82814 88 float spd = 0.005; // simulation speed
lddevrie 2:0c9c3c1f3b18 89
lddevrie 2:0c9c3c1f3b18 90 void rxCallback(MODSERIAL_IRQ_INFO *q)
lddevrie 2:0c9c3c1f3b18 91 {
lddevrie 2:0c9c3c1f3b18 92 MODSERIAL *serial = q->serial;
lddevrie 2:0c9c3c1f3b18 93 if ( serial->rxGetLastChar() == '\n') {
lddevrie 2:0c9c3c1f3b18 94 newpacket = true;
lddevrie 2:0c9c3c1f3b18 95 }
lddevrie 2:0c9c3c1f3b18 96 }
lddevrie 2:0c9c3c1f3b18 97
jdawkins 0:f8fd381a5b8a 98
jdawkins 0:f8fd381a5b8a 99 int main()
jdawkins 0:f8fd381a5b8a 100 {
lddevrie 2:0c9c3c1f3b18 101 nav_EKF ekf; // initialize ekf states
jdawkins 0:f8fd381a5b8a 102
lddevrie 2:0c9c3c1f3b18 103 pc.baud(115200); // set baud rate of serial comm to pc
lddevrie 2:0c9c3c1f3b18 104 xbee.baud(115200); // set baud rate of serial comm of wireless xbee comms
jdawkins 0:f8fd381a5b8a 105 Steer.write(1500); //Set Steer PWM to center 1000-2000 range
jdawkins 0:f8fd381a5b8a 106 Throttle.write(1500); //Set Throttle to Low
lddevrie 2:0c9c3c1f3b18 107
lddevrie 2:0c9c3c1f3b18 108 xbee.attach(&rxCallback, MODSERIAL::RxIrq);
lddevrie 2:0c9c3c1f3b18 109
lddevrie 2:0c9c3c1f3b18 110
lddevrie 2:0c9c3c1f3b18 111 led1=led2=led3=led4 =WHITE; // set color of neo strip lights?
lddevrie 2:0c9c3c1f3b18 112
lddevrie 2:0c9c3c1f3b18 113 // initialize necessary float and boolean variables
jdawkins 0:f8fd381a5b8a 114 left = 0;
jdawkins 0:f8fd381a5b8a 115 str_cmd = 0;
jdawkins 0:f8fd381a5b8a 116 str=0;
jdawkins 0:f8fd381a5b8a 117 thr=0;
jdawkins 0:f8fd381a5b8a 118 thr_cmd = 0;
jdawkins 0:f8fd381a5b8a 119 des_psi = 0;
jdawkins 0:f8fd381a5b8a 120 des_spd = 0;
jdawkins 0:f8fd381a5b8a 121 psi_err = 0;
jdawkins 0:f8fd381a5b8a 122 spd_err = 0;
jdawkins 0:f8fd381a5b8a 123 spd_err_i = 0;
jdawkins 0:f8fd381a5b8a 124 arm_clock = 0;
jdawkins 0:f8fd381a5b8a 125 auto_clock = 0;
jdawkins 0:f8fd381a5b8a 126 Kp_psi = 1;
jdawkins 0:f8fd381a5b8a 127 Kp_spd = 0.3;
jdawkins 0:f8fd381a5b8a 128 Ki_spd = 0.05;
jdawkins 0:f8fd381a5b8a 129 str_cond = false;
jdawkins 0:f8fd381a5b8a 130 thr_cond = false;
jdawkins 0:f8fd381a5b8a 131 armed = false;
jdawkins 0:f8fd381a5b8a 132 rc_conn = false;
jdawkins 0:f8fd381a5b8a 133 auto_ctrl = false;
jdawkins 0:f8fd381a5b8a 134 auto_ctrl_old = false;
jdawkins 0:f8fd381a5b8a 135 run_ctrl = false;
jdawkins 0:f8fd381a5b8a 136 log_data = false;
jdawkins 0:f8fd381a5b8a 137
lddevrie 2:0c9c3c1f3b18 138 // timer and timing initializations
jdawkins 0:f8fd381a5b8a 139 t.start();
jdawkins 0:f8fd381a5b8a 140 t_imu = t.read();
jdawkins 0:f8fd381a5b8a 141 t_gps = t.read();
jdawkins 0:f8fd381a5b8a 142 t_cmd = 0;
jdawkins 0:f8fd381a5b8a 143
lddevrie 2:0c9c3c1f3b18 144 leds.setBrightness(0.5); // set brightness of leds
lddevrie 2:0c9c3c1f3b18 145
lddevrie 2:0c9c3c1f3b18 146 rc_LED = 0; // turn off LED 1 to indicate no RC connection
lddevrie 2:0c9c3c1f3b18 147 imu_LED = 0; // turn off IMU indicator (LED 4)
lddevrie 2:0c9c3c1f3b18 148 gps.setRefPoint(REF_LAT,REF_LON,REF_ALT); // set local origin of reference frame for GPS conversion
lddevrie 2:0c9c3c1f3b18 149
lddevrie 4:e27ca0c82814 150
lddevrie 2:0c9c3c1f3b18 151 // procedure to ensure IMU is operating correctly
jdawkins 0:f8fd381a5b8a 152 if(imu.check()) {
jdawkins 0:f8fd381a5b8a 153 pc.printf("BNO055 connected\r\n");
jdawkins 0:f8fd381a5b8a 154 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 0:f8fd381a5b8a 155 imu.SetExternalCrystal(1);
jdawkins 0:f8fd381a5b8a 156 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 0:f8fd381a5b8a 157 imu.set_angle_units(RADIANS);
jdawkins 0:f8fd381a5b8a 158 imu.set_accel_units(MPERSPERS);
jdawkins 0:f8fd381a5b8a 159 imu.setoutputformat(WINDOWS);
jdawkins 0:f8fd381a5b8a 160 imu.set_mapping(2);
jdawkins 0:f8fd381a5b8a 161
lddevrie 2:0c9c3c1f3b18 162 // Blinks light if IMU is not calibrated, stops when calibration is complete
lddevrie 4:e27ca0c82814 163 /*while(int(imu.calib) < 0xCF) {
jdawkins 0:f8fd381a5b8a 164 pc.printf("Calibration = %x.\n\r",imu.calib);
jdawkins 0:f8fd381a5b8a 165 imu.get_calib();
jdawkins 0:f8fd381a5b8a 166 wait(0.5);
jdawkins 0:f8fd381a5b8a 167 imu_LED = !imu_LED;
lddevrie 2:0c9c3c1f3b18 168 } // end while(imu.calib)
lddevrie 4:e27ca0c82814 169 */
lddevrie 2:0c9c3c1f3b18 170 imu_LED = 1; // turns on IMU light when calibration is successful
jdawkins 0:f8fd381a5b8a 171
jdawkins 0:f8fd381a5b8a 172 } else {
lddevrie 4:e27ca0c82814 173 pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly
lddevrie 4:e27ca0c82814 174
lddevrie 4:e27ca0c82814 175 sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)
lddevrie 4:e27ca0c82814 176
lddevrie 4:e27ca0c82814 177
lddevrie 4:e27ca0c82814 178 /* // turn on all lights is IMU is not connected correctly
jdawkins 0:f8fd381a5b8a 179 rc_LED = 1;
jdawkins 0:f8fd381a5b8a 180 armed_LED = 1;
jdawkins 0:f8fd381a5b8a 181 imu_LED = 1;
jdawkins 0:f8fd381a5b8a 182 auto_LED = 1;
jdawkins 0:f8fd381a5b8a 183 while(1) {
lddevrie 2:0c9c3c1f3b18 184 // blink all lights if IMU is not connected correctly
jdawkins 0:f8fd381a5b8a 185 rc_LED = !rc_LED;
jdawkins 0:f8fd381a5b8a 186 armed_LED = !armed_LED;
jdawkins 0:f8fd381a5b8a 187 imu_LED = !imu_LED;
jdawkins 0:f8fd381a5b8a 188 auto_LED = !auto_LED;
lddevrie 2:0c9c3c1f3b18 189
jdawkins 0:f8fd381a5b8a 190 wait(0.5);
lddevrie 2:0c9c3c1f3b18 191 } // end while(1) {blink if IMU is not connected}
lddevrie 4:e27ca0c82814 192 */
lddevrie 4:e27ca0c82814 193
lddevrie 2:0c9c3c1f3b18 194 } // end if(imu.check)
jdawkins 0:f8fd381a5b8a 195
lddevrie 4:e27ca0c82814 196
lddevrie 2:0c9c3c1f3b18 197 // int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};
lddevrie 2:0c9c3c1f3b18 198
lddevrie 2:0c9c3c1f3b18 199 pc.printf("Emaxx Navigation Program\r\n"); // print indication that calibration is good and nav program is running
lddevrie 2:0c9c3c1f3b18 200
lddevrie 2:0c9c3c1f3b18 201
jdawkins 0:f8fd381a5b8a 202 while(1) {
jdawkins 0:f8fd381a5b8a 203
lddevrie 2:0c9c3c1f3b18 204 // check for servo pulse from either channel of receiver module
jdawkins 0:f8fd381a5b8a 205 if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
jdawkins 0:f8fd381a5b8a 206 rc_conn = false;
jdawkins 0:f8fd381a5b8a 207 } else {
jdawkins 0:f8fd381a5b8a 208 rc_conn = true;
lddevrie 2:0c9c3c1f3b18 209 } // end if(Channels connected)
lddevrie 2:0c9c3c1f3b18 210
lddevrie 2:0c9c3c1f3b18 211 // turn on RC led if transmitter is connected
jdawkins 0:f8fd381a5b8a 212 rc_LED = rc_conn;
jdawkins 0:f8fd381a5b8a 213 auto_LED = auto_ctrl;
jdawkins 0:f8fd381a5b8a 214 armed_LED = armed;
lddevrie 2:0c9c3c1f3b18 215
jdawkins 0:f8fd381a5b8a 216 str_cond = (CH1.servoPulse > 1800); // If steering is full right
jdawkins 0:f8fd381a5b8a 217 thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero
jdawkins 0:f8fd381a5b8a 218
lddevrie 2:0c9c3c1f3b18 219 if(t.read()-auto_clock > 3) { //Auto control timeout if no commands recevied after 3 seconds
lddevrie 2:0c9c3c1f3b18 220 auto_ctrl = false;
lddevrie 2:0c9c3c1f3b18 221 } // end if(t.read()-autoclock>3) timeout procedure
lddevrie 2:0c9c3c1f3b18 222
lddevrie 2:0c9c3c1f3b18 223
lddevrie 2:0c9c3c1f3b18 224
lddevrie 2:0c9c3c1f3b18 225
jdawkins 0:f8fd381a5b8a 226
lddevrie 2:0c9c3c1f3b18 227 if(newpacket) { // if xbee port receives a complete message, parse it
jdawkins 0:f8fd381a5b8a 228 char buf[MAX_MESSAGE_SIZE];
lddevrie 4:e27ca0c82814 229
lddevrie 2:0c9c3c1f3b18 230 // reads from modserial port buffer, stores characters into string "buf"
lddevrie 2:0c9c3c1f3b18 231 int i = 0;
lddevrie 2:0c9c3c1f3b18 232 if(xbee.rxBufferGetCount()>0) {
lddevrie 2:0c9c3c1f3b18 233 char c = xbee.getc();
lddevrie 2:0c9c3c1f3b18 234 //pc.printf("%s",c);
lddevrie 2:0c9c3c1f3b18 235 if(c=='$') {
lddevrie 2:0c9c3c1f3b18 236 buf[i] = c;
lddevrie 2:0c9c3c1f3b18 237 i++;
lddevrie 2:0c9c3c1f3b18 238 while(1) {
lddevrie 2:0c9c3c1f3b18 239 buf[i] = xbee.getc();
lddevrie 2:0c9c3c1f3b18 240 if(buf[i]=='\n') {
lddevrie 2:0c9c3c1f3b18 241 break;
lddevrie 2:0c9c3c1f3b18 242 }
lddevrie 2:0c9c3c1f3b18 243 i++;
lddevrie 2:0c9c3c1f3b18 244 }
lddevrie 2:0c9c3c1f3b18 245 }
lddevrie 2:0c9c3c1f3b18 246 }
lddevrie 2:0c9c3c1f3b18 247 xbee.rxBufferFlush();// empty receive buffer
lddevrie 2:0c9c3c1f3b18 248 pc.printf("%s",buf);
lddevrie 2:0c9c3c1f3b18 249 parseMessage(buf);
lddevrie 2:0c9c3c1f3b18 250 newpacket = false;
lddevrie 2:0c9c3c1f3b18 251 } // end if(newpacket)
jdawkins 0:f8fd381a5b8a 252
jdawkins 0:f8fd381a5b8a 253
jdawkins 0:f8fd381a5b8a 254 if(!rc_conn) { // Is System Armed
lddevrie 2:0c9c3c1f3b18 255 // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
jdawkins 0:f8fd381a5b8a 256 if(auto_ctrl) {
lddevrie 2:0c9c3c1f3b18 257
lddevrie 2:0c9c3c1f3b18 258 switch (cmd_mode) {
jdawkins 0:f8fd381a5b8a 259 case DIRECT_MODE: {
jdawkins 0:f8fd381a5b8a 260 str = str_cmd;
jdawkins 0:f8fd381a5b8a 261 thr = thr_cmd;
lddevrie 2:0c9c3c1f3b18 262 // pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
lddevrie 2:0c9c3c1f3b18 263 // xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
jdawkins 0:f8fd381a5b8a 264 break;
lddevrie 2:0c9c3c1f3b18 265 } // end direct mode case
jdawkins 0:f8fd381a5b8a 266 case COURSE_MODE: {
lddevrie 2:0c9c3c1f3b18 267
jdawkins 0:f8fd381a5b8a 268 psi_err = wrapToPi(des_psi-imu.euler.yaw);
jdawkins 0:f8fd381a5b8a 269 spd_err = des_spd - ekf.getSpd();
jdawkins 0:f8fd381a5b8a 270 spd_err_i += spd_err;
lddevrie 3:9b6db94aa2b3 271 str = -Kp_psi*psi_err/ekf.getSpd();
lddevrie 2:0c9c3c1f3b18 272
lddevrie 2:0c9c3c1f3b18 273 thr = Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;
lddevrie 2:0c9c3c1f3b18 274
lddevrie 2:0c9c3c1f3b18 275 if (thr >= 1.0) {
jdawkins 0:f8fd381a5b8a 276 thr = 1.0;
jdawkins 0:f8fd381a5b8a 277 spd_err_i = 0; // Reset Integral If Saturated
lddevrie 2:0c9c3c1f3b18 278 } // end if thr>=1.0
lddevrie 2:0c9c3c1f3b18 279 if (thr < 0) {
jdawkins 0:f8fd381a5b8a 280 thr = 0;
lddevrie 2:0c9c3c1f3b18 281 spd_err_i = 0; // Reset Integral If Saturated
lddevrie 2:0c9c3c1f3b18 282 } // end iff thr<0
jdawkins 0:f8fd381a5b8a 283
lddevrie 2:0c9c3c1f3b18 284 //pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
lddevrie 2:0c9c3c1f3b18 285 //xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
jdawkins 0:f8fd381a5b8a 286 break;
jdawkins 0:f8fd381a5b8a 287
lddevrie 2:0c9c3c1f3b18 288 } // end COURSE_MODE case
lddevrie 2:0c9c3c1f3b18 289 default: {
jdawkins 0:f8fd381a5b8a 290 break;
lddevrie 2:0c9c3c1f3b18 291 } // end default status in switch
lddevrie 2:0c9c3c1f3b18 292
lddevrie 2:0c9c3c1f3b18 293 } // end switch(cmd_mode)
lddevrie 2:0c9c3c1f3b18 294
lddevrie 4:e27ca0c82814 295 if(sim_mode<2) { // only actuates if in experiment or HIL modes
lddevrie 4:e27ca0c82814 296 Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
lddevrie 4:e27ca0c82814 297 Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
lddevrie 4:e27ca0c82814 298 } else {
lddevrie 4:e27ca0c82814 299 // won't send command to motor and servo if in SIL mode
lddevrie 4:e27ca0c82814 300 }
lddevrie 4:e27ca0c82814 301
lddevrie 4:e27ca0c82814 302
lddevrie 4:e27ca0c82814 303 } else { // goes with if auto_ctrl, manual control mode
lddevrie 2:0c9c3c1f3b18 304
jdawkins 0:f8fd381a5b8a 305 Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 0:f8fd381a5b8a 306 Throttle.write(1500); //Write Throttle Pulse
jdawkins 0:f8fd381a5b8a 307
lddevrie 2:0c9c3c1f3b18 308 } // end else
lddevrie 2:0c9c3c1f3b18 309
lddevrie 2:0c9c3c1f3b18 310 } else { // goes with if !rc_conn
jdawkins 0:f8fd381a5b8a 311 auto_ctrl = false;
jdawkins 0:f8fd381a5b8a 312 armed_LED = 0; //Turn off armed LED indicator
jdawkins 0:f8fd381a5b8a 313 str = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:f8fd381a5b8a 314 thr = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:f8fd381a5b8a 315 Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 0:f8fd381a5b8a 316 Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
lddevrie 2:0c9c3c1f3b18 317
jdawkins 0:f8fd381a5b8a 318 }/// end else armed
jdawkins 0:f8fd381a5b8a 319
lddevrie 4:e27ca0c82814 320 if(sim_mode==0) {
lddevrie 4:e27ca0c82814 321 imu.get_angles();
lddevrie 4:e27ca0c82814 322 imu.get_accel();
lddevrie 4:e27ca0c82814 323 imu.get_gyro();
lddevrie 4:e27ca0c82814 324 imu.get_lia();
lddevrie 4:e27ca0c82814 325 float dt = t.read()-t_imu;
lddevrie 4:e27ca0c82814 326 if(dt > (1/IMU_RATE)) {
lddevrie 4:e27ca0c82814 327
lddevrie 4:e27ca0c82814 328 float tic = t.read();
lddevrie 4:e27ca0c82814 329 ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
lddevrie 4:e27ca0c82814 330 ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);
lddevrie 4:e27ca0c82814 331
lddevrie 4:e27ca0c82814 332 xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
lddevrie 4:e27ca0c82814 333 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));
lddevrie 4:e27ca0c82814 334 pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
lddevrie 4:e27ca0c82814 335 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));
lddevrie 4:e27ca0c82814 336
lddevrie 4:e27ca0c82814 337 t_imu = t.read();
lddevrie 4:e27ca0c82814 338 }
lddevrie 4:e27ca0c82814 339
lddevrie 4:e27ca0c82814 340 if(t.read()-t_gps >(1/GPS_RATE)) {
lddevrie 4:e27ca0c82814 341 //printf("Kp_psi: %.2f Kp_spd: %.2f Ki_spd: %.2f\r\n", Kp_psi,Kp_spd,Ki_spd);
lddevrie 4:e27ca0c82814 342
lddevrie 4:e27ca0c82814 343
lddevrie 4:e27ca0c82814 344 float tic2 = t.read();
lddevrie 4:e27ca0c82814 345 ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
lddevrie 4:e27ca0c82814 346 // 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);
lddevrie 4:e27ca0c82814 347 // xbee.printf("$STA,%d,%d,%d,%d\r\n",);
jdawkins 0:f8fd381a5b8a 348
lddevrie 4:e27ca0c82814 349 t_gps = t.read();
lddevrie 4:e27ca0c82814 350
lddevrie 4:e27ca0c82814 351 }
lddevrie 4:e27ca0c82814 352 wait(1/LOOP_RATE);
lddevrie 4:e27ca0c82814 353 // status_LED=!status_LED;
lddevrie 4:e27ca0c82814 354 auto_ctrl_old = auto_ctrl;
lddevrie 4:e27ca0c82814 355
lddevrie 4:e27ca0c82814 356 } else { // else condition implies a simulation mode is enabled
lddevrie 4:e27ca0c82814 357
lddevrie 4:e27ca0c82814 358 float dt = t.read()-t_imu;
lddevrie 4:e27ca0c82814 359 if(dt > (1/IMU_RATE)) {
jdawkins 0:f8fd381a5b8a 360
lddevrie 4:e27ca0c82814 361 float tic = t.read();
lddevrie 4:e27ca0c82814 362
lddevrie 4:e27ca0c82814 363 psi_err = wrapToPi(des_psi-psi);
lddevrie 4:e27ca0c82814 364 spd_err = des_spd - spd;
lddevrie 4:e27ca0c82814 365 spd_err_i += spd_err;
lddevrie 4:e27ca0c82814 366 str = Kp_psi*psi_err/(spd+0.05);
lddevrie 4:e27ca0c82814 367 thr = Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;
lddevrie 4:e27ca0c82814 368
lddevrie 4:e27ca0c82814 369 x = x + spd*cos(psi)*dt;
lddevrie 4:e27ca0c82814 370 y = y + spd*sin(psi)*dt;
lddevrie 4:e27ca0c82814 371 psi = psi + str*dt;
lddevrie 4:e27ca0c82814 372 spd = spd + thr*dt;
jdawkins 0:f8fd381a5b8a 373
lddevrie 4:e27ca0c82814 374 xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
lddevrie 4:e27ca0c82814 375 pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
lddevrie 4:e27ca0c82814 376 t_imu = t.read();
lddevrie 4:e27ca0c82814 377 }
lddevrie 4:e27ca0c82814 378
lddevrie 4:e27ca0c82814 379
jdawkins 0:f8fd381a5b8a 380 }
jdawkins 0:f8fd381a5b8a 381
jdawkins 0:f8fd381a5b8a 382
lddevrie 4:e27ca0c82814 383 } // end while(1)
lddevrie 2:0c9c3c1f3b18 384
lddevrie 4:e27ca0c82814 385 } // end main
jdawkins 0:f8fd381a5b8a 386
lddevrie 2:0c9c3c1f3b18 387 void parseMessage(char * msg)
lddevrie 2:0c9c3c1f3b18 388 {
jdawkins 0:f8fd381a5b8a 389
lddevrie 2:0c9c3c1f3b18 390 if(!strncmp(msg, "$CMD", 4)) {
jdawkins 0:f8fd381a5b8a 391 int arg1;
jdawkins 0:f8fd381a5b8a 392 float arg2,arg3;
jdawkins 0:f8fd381a5b8a 393
jdawkins 0:f8fd381a5b8a 394 sscanf(msg,"$CMD,%d,%f,%f\n",&arg1,&arg2,&arg3);
jdawkins 0:f8fd381a5b8a 395 cmd_mode = arg1;
jdawkins 0:f8fd381a5b8a 396 auto_clock = t.read();
lddevrie 2:0c9c3c1f3b18 397 switch (cmd_mode) {
jdawkins 0:f8fd381a5b8a 398 case DIRECT_MODE: {
jdawkins 0:f8fd381a5b8a 399 auto_ctrl = true;
jdawkins 0:f8fd381a5b8a 400 str_cmd = arg2;
jdawkins 0:f8fd381a5b8a 401 thr_cmd = arg3;
jdawkins 0:f8fd381a5b8a 402 }
jdawkins 0:f8fd381a5b8a 403 case COURSE_MODE: {
jdawkins 0:f8fd381a5b8a 404 auto_ctrl = true;
jdawkins 0:f8fd381a5b8a 405 des_psi = arg2;
jdawkins 0:f8fd381a5b8a 406 des_spd = arg3;
jdawkins 0:f8fd381a5b8a 407 }
lddevrie 2:0c9c3c1f3b18 408 default: {
lddevrie 2:0c9c3c1f3b18 409 }
jdawkins 0:f8fd381a5b8a 410 }
jdawkins 0:f8fd381a5b8a 411 } //emd of $CMD
lddevrie 2:0c9c3c1f3b18 412
lddevrie 2:0c9c3c1f3b18 413 if(!strncmp(msg, "$PRM", 4)) {
jdawkins 0:f8fd381a5b8a 414 float arg1,arg2,arg3;
jdawkins 0:f8fd381a5b8a 415 int type;
lddevrie 2:0c9c3c1f3b18 416 // __disable_irq(); // disable interrupts
jdawkins 0:f8fd381a5b8a 417 sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3);
lddevrie 2:0c9c3c1f3b18 418 // __enable_irq(); // enable interrupts
lddevrie 2:0c9c3c1f3b18 419
lddevrie 2:0c9c3c1f3b18 420 switch(type) {
lddevrie 2:0c9c3c1f3b18 421 case 1: { // sets PID gains on heading and speed controller
lddevrie 2:0c9c3c1f3b18 422 //pc.printf("%s\n",msg);
jdawkins 0:f8fd381a5b8a 423 Kp_psi = arg1;
jdawkins 0:f8fd381a5b8a 424 Kp_spd = arg2;
lddevrie 2:0c9c3c1f3b18 425 Ki_spd = arg3;
lddevrie 4:e27ca0c82814 426
lddevrie 2:0c9c3c1f3b18 427 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
lddevrie 2:0c9c3c1f3b18 428 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
jdawkins 0:f8fd381a5b8a 429 break;
jdawkins 0:f8fd381a5b8a 430 }
lddevrie 2:0c9c3c1f3b18 431 case 2: { // sets origin of local reference frame
lddevrie 4:e27ca0c82814 432
lddevrie 2:0c9c3c1f3b18 433 //pc.printf("%s\n",msg);
jdawkins 0:f8fd381a5b8a 434
jdawkins 0:f8fd381a5b8a 435 float ref_lat = arg1;
jdawkins 0:f8fd381a5b8a 436 float ref_lon = arg2;
jdawkins 0:f8fd381a5b8a 437 float ref_alt = arg3;
lddevrie 2:0c9c3c1f3b18 438 gps.setRefPoint(ref_lat,ref_lon,ref_alt);
lddevrie 4:e27ca0c82814 439
lddevrie 2:0c9c3c1f3b18 440 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
lddevrie 2:0c9c3c1f3b18 441 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
lddevrie 4:e27ca0c82814 442
jdawkins 0:f8fd381a5b8a 443 break;
lddevrie 2:0c9c3c1f3b18 444 }
lddevrie 2:0c9c3c1f3b18 445 default: {
lddevrie 2:0c9c3c1f3b18 446 }
lddevrie 2:0c9c3c1f3b18 447
jdawkins 0:f8fd381a5b8a 448 }
jdawkins 0:f8fd381a5b8a 449
lddevrie 2:0c9c3c1f3b18 450 } // end of $PRM
lddevrie 2:0c9c3c1f3b18 451
lddevrie 2:0c9c3c1f3b18 452 if(!strncmp(msg, "$LED", 4)) {
jdawkins 0:f8fd381a5b8a 453 int arg1;
jdawkins 0:f8fd381a5b8a 454 float arg2;
jdawkins 0:f8fd381a5b8a 455 sscanf(msg,"$LED,%x,%f",&arg1,&arg2);
lddevrie 2:0c9c3c1f3b18 456 //pc.printf("%s\n",msg);
lddevrie 2:0c9c3c1f3b18 457 int colors[4]= {arg1,arg1,arg1,arg1};
jdawkins 0:f8fd381a5b8a 458 float brightness = arg2;
lddevrie 2:0c9c3c1f3b18 459 setLED(colors,brightness);
lddevrie 4:e27ca0c82814 460 } // end of $LED
lddevrie 4:e27ca0c82814 461
lddevrie 4:e27ca0c82814 462
lddevrie 4:e27ca0c82814 463 if(!strncmp(msg, "$SIM", 4)) {
lddevrie 4:e27ca0c82814 464 int arg1;
lddevrie 4:e27ca0c82814 465 float arg2,arg3,arg4;
lddevrie 4:e27ca0c82814 466
lddevrie 4:e27ca0c82814 467 sscanf(msg,"$SIM,%d,%f,%f,%f\n",&arg1,&arg2,&arg3,&arg4);
lddevrie 4:e27ca0c82814 468 sim_mode = arg1; // sets whether in hardware in the loop or software in the loop (actuators active or not during simulation)
lddevrie 4:e27ca0c82814 469 auto_clock = t.read();
lddevrie 4:e27ca0c82814 470 switch (cmd_mode) {
lddevrie 4:e27ca0c82814 471 case HIL_MODE: {
lddevrie 4:e27ca0c82814 472 auto_ctrl = true;
lddevrie 4:e27ca0c82814 473 x = arg2;
lddevrie 4:e27ca0c82814 474 y = arg3;
lddevrie 4:e27ca0c82814 475 psi = arg4;
lddevrie 4:e27ca0c82814 476 }
lddevrie 4:e27ca0c82814 477 case SIL_MODE: {
lddevrie 4:e27ca0c82814 478 auto_ctrl = true;
lddevrie 4:e27ca0c82814 479 x = arg2;
lddevrie 4:e27ca0c82814 480 y = arg3;
lddevrie 4:e27ca0c82814 481 psi = arg4;
lddevrie 4:e27ca0c82814 482 }
lddevrie 4:e27ca0c82814 483 default: {
lddevrie 4:e27ca0c82814 484 }
lddevrie 4:e27ca0c82814 485 }
lddevrie 4:e27ca0c82814 486 } //emd of $SIM
lddevrie 4:e27ca0c82814 487
jdawkins 0:f8fd381a5b8a 488
jdawkins 0:f8fd381a5b8a 489
lddevrie 2:0c9c3c1f3b18 490
jdawkins 0:f8fd381a5b8a 491
jdawkins 0:f8fd381a5b8a 492 }
lddevrie 2:0c9c3c1f3b18 493 void setLED(int *colors,float brightness)
lddevrie 2:0c9c3c1f3b18 494 {
lddevrie 2:0c9c3c1f3b18 495
lddevrie 2:0c9c3c1f3b18 496 leds.setBrightness(brightness);
lddevrie 2:0c9c3c1f3b18 497
lddevrie 2:0c9c3c1f3b18 498 int cidx = 0;
lddevrie 2:0c9c3c1f3b18 499 int ctr = 0;
lddevrie 2:0c9c3c1f3b18 500 for (int i=0; i<LED_PER_CLUSTER*LED_CLUSTERS; i++) {
jdawkins 0:f8fd381a5b8a 501
lddevrie 2:0c9c3c1f3b18 502 if(ctr >11) {
lddevrie 2:0c9c3c1f3b18 503 ctr = 0;
lddevrie 2:0c9c3c1f3b18 504 cidx++;
jdawkins 0:f8fd381a5b8a 505 }
lddevrie 2:0c9c3c1f3b18 506 leds.setPixel(i,colors[cidx]);
lddevrie 2:0c9c3c1f3b18 507 ctr++;
lddevrie 2:0c9c3c1f3b18 508 }
lddevrie 2:0c9c3c1f3b18 509 leds.write();
jdawkins 0:f8fd381a5b8a 510 }
jdawkins 0:f8fd381a5b8a 511 float saturateCmd(float cmd)
jdawkins 0:f8fd381a5b8a 512 {
jdawkins 0:f8fd381a5b8a 513 if(cmd>1.0) {
jdawkins 0:f8fd381a5b8a 514 cmd = 1.0;
jdawkins 0:f8fd381a5b8a 515 }
jdawkins 0:f8fd381a5b8a 516 if(cmd < -1.0) {
jdawkins 0:f8fd381a5b8a 517 cmd = -1.0;
jdawkins 0:f8fd381a5b8a 518 }
jdawkins 0:f8fd381a5b8a 519 return cmd;
jdawkins 0:f8fd381a5b8a 520 }
jdawkins 0:f8fd381a5b8a 521 float saturateCmd(float cmd, float max,float min)
jdawkins 0:f8fd381a5b8a 522 {
jdawkins 0:f8fd381a5b8a 523 if(cmd>max) {
jdawkins 0:f8fd381a5b8a 524 cmd = max;
jdawkins 0:f8fd381a5b8a 525 }
jdawkins 0:f8fd381a5b8a 526 if(cmd < min) {
jdawkins 0:f8fd381a5b8a 527 cmd = min;
jdawkins 0:f8fd381a5b8a 528 }
jdawkins 0:f8fd381a5b8a 529 return cmd;
jdawkins 0:f8fd381a5b8a 530 }
jdawkins 0:f8fd381a5b8a 531
jdawkins 0:f8fd381a5b8a 532 float wrapToPi(float ang)
jdawkins 0:f8fd381a5b8a 533 {
jdawkins 0:f8fd381a5b8a 534
jdawkins 0:f8fd381a5b8a 535 if(ang > PI) {
jdawkins 0:f8fd381a5b8a 536
jdawkins 0:f8fd381a5b8a 537 ang = ang - 2*PI;
jdawkins 0:f8fd381a5b8a 538 }
jdawkins 0:f8fd381a5b8a 539 if (ang < -PI) {
jdawkins 0:f8fd381a5b8a 540 ang = ang + 2*PI;
jdawkins 0:f8fd381a5b8a 541 }
jdawkins 0:f8fd381a5b8a 542
jdawkins 0:f8fd381a5b8a 543 return ang;
lddevrie 2:0c9c3c1f3b18 544 }