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@0:f8fd381a5b8a, 2016-08-16 (annotated)

Committer:
jdawkins
Date:
Tue Aug 16 13:43:26 2016 +0000
Revision:
0:f8fd381a5b8a
Child:
2:0c9c3c1f3b18
Initial Commit of Emaxx code

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"
jdawkins 0:f8fd381a5b8a 13
jdawkins 0:f8fd381a5b8a 14 #define MAX_MESSAGE_SIZE 64
jdawkins 0:f8fd381a5b8a 15 #define IMU_RATE 100.0
jdawkins 0:f8fd381a5b8a 16 #define GPS_RATE 1.0
jdawkins 0:f8fd381a5b8a 17 #define LOOP_RATE 300.0
jdawkins 0:f8fd381a5b8a 18 #define CMD_TIMEOUT 1.0
jdawkins 0:f8fd381a5b8a 19 #define GEAR_RATIO (1/2.75)
jdawkins 0:f8fd381a5b8a 20 // Reference origin is at entrance to hospital point monument
jdawkins 0:f8fd381a5b8a 21 #define REF_LAT 38.986534
jdawkins 0:f8fd381a5b8a 22 #define REF_LON -76.489914
jdawkins 0:f8fd381a5b8a 23 #define REF_ALT 1.8
jdawkins 0:f8fd381a5b8a 24 #define NUM_LED 28
jdawkins 0:f8fd381a5b8a 25 #define LED_CLUSTERS 4
jdawkins 0:f8fd381a5b8a 26 #define LED_PER_CLUSTER 12
jdawkins 0:f8fd381a5b8a 27
jdawkins 0:f8fd381a5b8a 28 #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
jdawkins 0:f8fd381a5b8a 29 #define COURSE_MODE 1 //Commands map to heading and speed
jdawkins 0:f8fd381a5b8a 30
jdawkins 0:f8fd381a5b8a 31 //I2C i2c(p9, p10); // SDA, SCL
jdawkins 0:f8fd381a5b8a 32 BNO055 imu(p9, p10);
jdawkins 0:f8fd381a5b8a 33 GPSINT gps(p28,p27);
jdawkins 0:f8fd381a5b8a 34
jdawkins 0:f8fd381a5b8a 35 vector <int> str_buf;
jdawkins 0:f8fd381a5b8a 36 int left;
jdawkins 0:f8fd381a5b8a 37
jdawkins 0:f8fd381a5b8a 38 // Function Prototypes
jdawkins 0:f8fd381a5b8a 39 float saturateCmd(float cmd);
jdawkins 0:f8fd381a5b8a 40 void menuFunction(Serial *port);
jdawkins 0:f8fd381a5b8a 41 float wrapToPi(float ang);
jdawkins 0:f8fd381a5b8a 42 int readMessage(Serial *port, char * buffer);
jdawkins 0:f8fd381a5b8a 43 void parseMessage(char * msg);
jdawkins 0:f8fd381a5b8a 44 void setLED(int *colors, float brightness);
jdawkins 0:f8fd381a5b8a 45
jdawkins 0:f8fd381a5b8a 46
jdawkins 0:f8fd381a5b8a 47 // LED Definitions
jdawkins 0:f8fd381a5b8a 48 DigitalOut rc_LED(LED1);
jdawkins 0:f8fd381a5b8a 49 DigitalOut armed_LED(LED2);
jdawkins 0:f8fd381a5b8a 50 DigitalOut auto_LED(LED3);
jdawkins 0:f8fd381a5b8a 51 DigitalOut imu_LED(LED4);
jdawkins 0:f8fd381a5b8a 52
jdawkins 0:f8fd381a5b8a 53 NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);
jdawkins 0:f8fd381a5b8a 54
jdawkins 0:f8fd381a5b8a 55 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 0:f8fd381a5b8a 56 Serial xbee(p13, p14); // tx, rx for Xbee
jdawkins 0:f8fd381a5b8a 57 ServoIn CH1(p15);
jdawkins 0:f8fd381a5b8a 58 ServoIn CH2(p16);
jdawkins 0:f8fd381a5b8a 59
jdawkins 0:f8fd381a5b8a 60 //InterruptIn he_sensor(p11);
jdawkins 0:f8fd381a5b8a 61
jdawkins 0:f8fd381a5b8a 62 ServoOut Steer(p22);
jdawkins 0:f8fd381a5b8a 63 ServoOut Throttle(p21);
jdawkins 0:f8fd381a5b8a 64 Timer t; // create timer instance
jdawkins 0:f8fd381a5b8a 65 Ticker log_tick;
jdawkins 0:f8fd381a5b8a 66 Ticker heartbeat;
jdawkins 0:f8fd381a5b8a 67 float t_imu,t_gps,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd;
jdawkins 0:f8fd381a5b8a 68 float psi_err,spd_err, psi_err_i,spd_err_i;
jdawkins 0:f8fd381a5b8a 69 float t_hall, dt_hall,t_run,t_stop,t_log;
jdawkins 0:f8fd381a5b8a 70 bool armed, auto_ctrl,auto_ctrl_old,rc_conn;
jdawkins 0:f8fd381a5b8a 71 float wheel_spd;
jdawkins 0:f8fd381a5b8a 72 float arm_clock,auto_clock;
jdawkins 0:f8fd381a5b8a 73 bool str_cond,thr_cond,run_ctrl,log_data;
jdawkins 0:f8fd381a5b8a 74 bool log_imu,log_bno,log_odo,log_mag = false;
jdawkins 0:f8fd381a5b8a 75 int cmd_mode;
jdawkins 0:f8fd381a5b8a 76 float Kp_psi, Kp_spd, Ki_psi, Ki_spd;
jdawkins 0:f8fd381a5b8a 77 int led1,led2,led3,led4;
jdawkins 0:f8fd381a5b8a 78
jdawkins 0:f8fd381a5b8a 79 int main()
jdawkins 0:f8fd381a5b8a 80 {
jdawkins 0:f8fd381a5b8a 81 nav_EKF ekf;
jdawkins 0:f8fd381a5b8a 82
jdawkins 0:f8fd381a5b8a 83 pc.baud(115200);
jdawkins 0:f8fd381a5b8a 84 xbee.baud(115200);
jdawkins 0:f8fd381a5b8a 85 Steer.write(1500); //Set Steer PWM to center 1000-2000 range
jdawkins 0:f8fd381a5b8a 86 Throttle.write(1500); //Set Throttle to Low
jdawkins 0:f8fd381a5b8a 87
jdawkins 0:f8fd381a5b8a 88 led1=led2=led3=led4 =WHITE;
jdawkins 0:f8fd381a5b8a 89 left = 0;
jdawkins 0:f8fd381a5b8a 90 str_cmd = 0;
jdawkins 0:f8fd381a5b8a 91 str=0;
jdawkins 0:f8fd381a5b8a 92 thr=0;
jdawkins 0:f8fd381a5b8a 93 thr_cmd = 0;
jdawkins 0:f8fd381a5b8a 94 des_psi = 0;
jdawkins 0:f8fd381a5b8a 95 des_spd = 0;
jdawkins 0:f8fd381a5b8a 96 psi_err = 0;
jdawkins 0:f8fd381a5b8a 97 spd_err = 0;
jdawkins 0:f8fd381a5b8a 98 spd_err_i = 0;
jdawkins 0:f8fd381a5b8a 99 arm_clock = 0;
jdawkins 0:f8fd381a5b8a 100 auto_clock = 0;
jdawkins 0:f8fd381a5b8a 101 Kp_psi = 1;
jdawkins 0:f8fd381a5b8a 102 Kp_spd = 0.3;
jdawkins 0:f8fd381a5b8a 103 Ki_spd = 0.05;
jdawkins 0:f8fd381a5b8a 104 str_cond = false;
jdawkins 0:f8fd381a5b8a 105 thr_cond = false;
jdawkins 0:f8fd381a5b8a 106 armed = false;
jdawkins 0:f8fd381a5b8a 107 rc_conn = false;
jdawkins 0:f8fd381a5b8a 108 auto_ctrl = false;
jdawkins 0:f8fd381a5b8a 109 auto_ctrl_old = false;
jdawkins 0:f8fd381a5b8a 110 run_ctrl = false;
jdawkins 0:f8fd381a5b8a 111 log_data = false;
jdawkins 0:f8fd381a5b8a 112
jdawkins 0:f8fd381a5b8a 113 t.start();
jdawkins 0:f8fd381a5b8a 114 t_imu = t.read();
jdawkins 0:f8fd381a5b8a 115 t_gps = t.read();
jdawkins 0:f8fd381a5b8a 116 t_cmd = 0;
jdawkins 0:f8fd381a5b8a 117
jdawkins 0:f8fd381a5b8a 118 leds.setBrightness(0.5);
jdawkins 0:f8fd381a5b8a 119
jdawkins 0:f8fd381a5b8a 120 rc_LED = 0;
jdawkins 0:f8fd381a5b8a 121 imu_LED = 0;
jdawkins 0:f8fd381a5b8a 122 gps.setRefPoint(REF_LAT,REF_LON,REF_ALT);
jdawkins 0:f8fd381a5b8a 123 if(imu.check()) {
jdawkins 0:f8fd381a5b8a 124 pc.printf("BNO055 connected\r\n");
jdawkins 0:f8fd381a5b8a 125 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 0:f8fd381a5b8a 126 imu.SetExternalCrystal(1);
jdawkins 0:f8fd381a5b8a 127 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 0:f8fd381a5b8a 128 imu.set_angle_units(RADIANS);
jdawkins 0:f8fd381a5b8a 129 imu.set_accel_units(MPERSPERS);
jdawkins 0:f8fd381a5b8a 130 imu.setoutputformat(WINDOWS);
jdawkins 0:f8fd381a5b8a 131 imu.set_mapping(2);
jdawkins 0:f8fd381a5b8a 132
jdawkins 0:f8fd381a5b8a 133 while(int(imu.calib) < 0xCF) {
jdawkins 0:f8fd381a5b8a 134 pc.printf("Calibration = %x.\n\r",imu.calib);
jdawkins 0:f8fd381a5b8a 135 imu.get_calib();
jdawkins 0:f8fd381a5b8a 136 wait(0.5);
jdawkins 0:f8fd381a5b8a 137 imu_LED = !imu_LED;
jdawkins 0:f8fd381a5b8a 138 }
jdawkins 0:f8fd381a5b8a 139 imu_LED = 1;
jdawkins 0:f8fd381a5b8a 140
jdawkins 0:f8fd381a5b8a 141 } else {
jdawkins 0:f8fd381a5b8a 142 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 0:f8fd381a5b8a 143 rc_LED = 1;
jdawkins 0:f8fd381a5b8a 144 armed_LED = 1;
jdawkins 0:f8fd381a5b8a 145 imu_LED = 1;
jdawkins 0:f8fd381a5b8a 146 auto_LED = 1;
jdawkins 0:f8fd381a5b8a 147 while(1) {
jdawkins 0:f8fd381a5b8a 148 rc_LED = !rc_LED;
jdawkins 0:f8fd381a5b8a 149 armed_LED = !armed_LED;
jdawkins 0:f8fd381a5b8a 150 imu_LED = !imu_LED;
jdawkins 0:f8fd381a5b8a 151 auto_LED = !auto_LED;
jdawkins 0:f8fd381a5b8a 152
jdawkins 0:f8fd381a5b8a 153 wait(0.5);
jdawkins 0:f8fd381a5b8a 154 }
jdawkins 0:f8fd381a5b8a 155 }
jdawkins 0:f8fd381a5b8a 156 // int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};
jdawkins 0:f8fd381a5b8a 157
jdawkins 0:f8fd381a5b8a 158 pc.printf("Emaxx Navigation Program\r\n");
jdawkins 0:f8fd381a5b8a 159
jdawkins 0:f8fd381a5b8a 160 while(1) {
jdawkins 0:f8fd381a5b8a 161
jdawkins 0:f8fd381a5b8a 162 if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
jdawkins 0:f8fd381a5b8a 163 rc_conn = false;
jdawkins 0:f8fd381a5b8a 164 } else {
jdawkins 0:f8fd381a5b8a 165 rc_conn = true;
jdawkins 0:f8fd381a5b8a 166 }
jdawkins 0:f8fd381a5b8a 167
jdawkins 0:f8fd381a5b8a 168 rc_LED = rc_conn;
jdawkins 0:f8fd381a5b8a 169 auto_LED = auto_ctrl;
jdawkins 0:f8fd381a5b8a 170 armed_LED = armed;
jdawkins 0:f8fd381a5b8a 171
jdawkins 0:f8fd381a5b8a 172 str_cond = (CH1.servoPulse > 1800); // If steering is full right
jdawkins 0:f8fd381a5b8a 173 thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero
jdawkins 0:f8fd381a5b8a 174
jdawkins 0:f8fd381a5b8a 175 if(t.read()-auto_clock > 3){ //Auto control timeout if no commands recevied after 5 seconds
jdawkins 0:f8fd381a5b8a 176 auto_ctrl = false;
jdawkins 0:f8fd381a5b8a 177
jdawkins 0:f8fd381a5b8a 178 }
jdawkins 0:f8fd381a5b8a 179
jdawkins 0:f8fd381a5b8a 180
jdawkins 0:f8fd381a5b8a 181
jdawkins 0:f8fd381a5b8a 182 if(xbee.readable()) {
jdawkins 0:f8fd381a5b8a 183 char buf[MAX_MESSAGE_SIZE];
jdawkins 0:f8fd381a5b8a 184 int n = readMessage(&xbee,buf);
jdawkins 0:f8fd381a5b8a 185 parseMessage(buf);
jdawkins 0:f8fd381a5b8a 186
jdawkins 0:f8fd381a5b8a 187 }
jdawkins 0:f8fd381a5b8a 188
jdawkins 0:f8fd381a5b8a 189 if(!rc_conn) { // Is System Armed
jdawkins 0:f8fd381a5b8a 190 // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
jdawkins 0:f8fd381a5b8a 191 if(auto_ctrl) {
jdawkins 0:f8fd381a5b8a 192
jdawkins 0:f8fd381a5b8a 193 switch (cmd_mode){
jdawkins 0:f8fd381a5b8a 194 case DIRECT_MODE: {
jdawkins 0:f8fd381a5b8a 195 str = str_cmd;
jdawkins 0:f8fd381a5b8a 196 thr = thr_cmd;
jdawkins 0:f8fd381a5b8a 197 // printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
jdawkins 0:f8fd381a5b8a 198
jdawkins 0:f8fd381a5b8a 199 break;
jdawkins 0:f8fd381a5b8a 200 }
jdawkins 0:f8fd381a5b8a 201 case COURSE_MODE: {
jdawkins 0:f8fd381a5b8a 202
jdawkins 0:f8fd381a5b8a 203 psi_err = wrapToPi(des_psi-imu.euler.yaw);
jdawkins 0:f8fd381a5b8a 204 spd_err = des_spd - ekf.getSpd();
jdawkins 0:f8fd381a5b8a 205 spd_err_i += spd_err;
jdawkins 0:f8fd381a5b8a 206 str = -Kp_psi*psi_err;
jdawkins 0:f8fd381a5b8a 207
jdawkins 0:f8fd381a5b8a 208 thr = Kp_spd*spd_err + Ki_spd*spd_err_i;
jdawkins 0:f8fd381a5b8a 209
jdawkins 0:f8fd381a5b8a 210 if (thr >= 1.0){
jdawkins 0:f8fd381a5b8a 211 thr = 1.0;
jdawkins 0:f8fd381a5b8a 212 spd_err_i = 0; // Reset Integral If Saturated
jdawkins 0:f8fd381a5b8a 213 }
jdawkins 0:f8fd381a5b8a 214 if (thr < 0){
jdawkins 0:f8fd381a5b8a 215 thr = 0;
jdawkins 0:f8fd381a5b8a 216 spd_err_i = 0; // Reset Integral If Saturated
jdawkins 0:f8fd381a5b8a 217 }
jdawkins 0:f8fd381a5b8a 218
jdawkins 0:f8fd381a5b8a 219 //printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
jdawkins 0:f8fd381a5b8a 220 break;
jdawkins 0:f8fd381a5b8a 221
jdawkins 0:f8fd381a5b8a 222 }
jdawkins 0:f8fd381a5b8a 223 default:{
jdawkins 0:f8fd381a5b8a 224 break;
jdawkins 0:f8fd381a5b8a 225 }
jdawkins 0:f8fd381a5b8a 226
jdawkins 0:f8fd381a5b8a 227 }
jdawkins 0:f8fd381a5b8a 228 Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 0:f8fd381a5b8a 229 Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
jdawkins 0:f8fd381a5b8a 230
jdawkins 0:f8fd381a5b8a 231 } else {
jdawkins 0:f8fd381a5b8a 232
jdawkins 0:f8fd381a5b8a 233 Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 0:f8fd381a5b8a 234 Throttle.write(1500); //Write Throttle Pulse
jdawkins 0:f8fd381a5b8a 235
jdawkins 0:f8fd381a5b8a 236 }
jdawkins 0:f8fd381a5b8a 237
jdawkins 0:f8fd381a5b8a 238 } else {
jdawkins 0:f8fd381a5b8a 239 auto_ctrl = false;
jdawkins 0:f8fd381a5b8a 240 armed_LED = 0; //Turn off armed LED indicator
jdawkins 0:f8fd381a5b8a 241 str = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:f8fd381a5b8a 242 thr = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:f8fd381a5b8a 243 Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 0:f8fd381a5b8a 244 Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
jdawkins 0:f8fd381a5b8a 245
jdawkins 0:f8fd381a5b8a 246 }/// end else armed
jdawkins 0:f8fd381a5b8a 247
jdawkins 0:f8fd381a5b8a 248 imu.get_angles();
jdawkins 0:f8fd381a5b8a 249 imu.get_accel();
jdawkins 0:f8fd381a5b8a 250 imu.get_gyro();
jdawkins 0:f8fd381a5b8a 251 imu.get_lia();
jdawkins 0:f8fd381a5b8a 252 float dt = t.read()-t_imu;
jdawkins 0:f8fd381a5b8a 253 if(dt > (1/IMU_RATE)) {
jdawkins 0:f8fd381a5b8a 254
jdawkins 0:f8fd381a5b8a 255 float tic = t.read();
jdawkins 0:f8fd381a5b8a 256 ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
jdawkins 0:f8fd381a5b8a 257 ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);
jdawkins 0:f8fd381a5b8a 258
jdawkins 0:f8fd381a5b8a 259 xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
jdawkins 0:f8fd381a5b8a 260 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));
jdawkins 0:f8fd381a5b8a 261
jdawkins 0:f8fd381a5b8a 262 t_imu = t.read();
jdawkins 0:f8fd381a5b8a 263 }
jdawkins 0:f8fd381a5b8a 264
jdawkins 0:f8fd381a5b8a 265 if(t.read()-t_gps >(1/GPS_RATE)) {
jdawkins 0:f8fd381a5b8a 266 //printf("Kp_psi: %.2f Kp_spd: %.2f Ki_spd: %.2f\r\n", Kp_psi,Kp_spd,Ki_spd);
jdawkins 0:f8fd381a5b8a 267
jdawkins 0:f8fd381a5b8a 268
jdawkins 0:f8fd381a5b8a 269 float tic2 = t.read();
jdawkins 0:f8fd381a5b8a 270 ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
jdawkins 0:f8fd381a5b8a 271 // 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);
jdawkins 0:f8fd381a5b8a 272 // xbee.printf("$STA,%d,%d,%d,%d\r\n",);
jdawkins 0:f8fd381a5b8a 273
jdawkins 0:f8fd381a5b8a 274 t_gps = t.read();
jdawkins 0:f8fd381a5b8a 275
jdawkins 0:f8fd381a5b8a 276 }
jdawkins 0:f8fd381a5b8a 277 wait(1/LOOP_RATE);
jdawkins 0:f8fd381a5b8a 278 // status_LED=!status_LED;
jdawkins 0:f8fd381a5b8a 279 auto_ctrl_old = auto_ctrl;
jdawkins 0:f8fd381a5b8a 280 }
jdawkins 0:f8fd381a5b8a 281
jdawkins 0:f8fd381a5b8a 282 }
jdawkins 0:f8fd381a5b8a 283 int readMessage(Serial *port, char * buffer){
jdawkins 0:f8fd381a5b8a 284 int i = 0;
jdawkins 0:f8fd381a5b8a 285
jdawkins 0:f8fd381a5b8a 286 if(port->readable()){
jdawkins 0:f8fd381a5b8a 287 char c = port->getc();
jdawkins 0:f8fd381a5b8a 288 if(c=='$'){
jdawkins 0:f8fd381a5b8a 289 buffer[i] = c;
jdawkins 0:f8fd381a5b8a 290 i++;
jdawkins 0:f8fd381a5b8a 291 while(1){
jdawkins 0:f8fd381a5b8a 292 buffer[i] = port->getc();
jdawkins 0:f8fd381a5b8a 293 if(buffer[i]=='\n' || buffer[i]=='\r'){
jdawkins 0:f8fd381a5b8a 294 break;
jdawkins 0:f8fd381a5b8a 295 }
jdawkins 0:f8fd381a5b8a 296 i++;
jdawkins 0:f8fd381a5b8a 297 }
jdawkins 0:f8fd381a5b8a 298 }
jdawkins 0:f8fd381a5b8a 299 }
jdawkins 0:f8fd381a5b8a 300 return i;
jdawkins 0:f8fd381a5b8a 301 }
jdawkins 0:f8fd381a5b8a 302
jdawkins 0:f8fd381a5b8a 303 void parseMessage(char * msg){
jdawkins 0:f8fd381a5b8a 304
jdawkins 0:f8fd381a5b8a 305 if(!strncmp(msg, "$CMD", 4)){
jdawkins 0:f8fd381a5b8a 306 int arg1;
jdawkins 0:f8fd381a5b8a 307 float arg2,arg3;
jdawkins 0:f8fd381a5b8a 308
jdawkins 0:f8fd381a5b8a 309 sscanf(msg,"$CMD,%d,%f,%f\n",&arg1,&arg2,&arg3);
jdawkins 0:f8fd381a5b8a 310 cmd_mode = arg1;
jdawkins 0:f8fd381a5b8a 311 auto_clock = t.read();
jdawkins 0:f8fd381a5b8a 312 switch (cmd_mode){
jdawkins 0:f8fd381a5b8a 313 case DIRECT_MODE: {
jdawkins 0:f8fd381a5b8a 314 auto_ctrl = true;
jdawkins 0:f8fd381a5b8a 315 str_cmd = arg2;
jdawkins 0:f8fd381a5b8a 316 thr_cmd = arg3;
jdawkins 0:f8fd381a5b8a 317 }
jdawkins 0:f8fd381a5b8a 318 case COURSE_MODE: {
jdawkins 0:f8fd381a5b8a 319 auto_ctrl = true;
jdawkins 0:f8fd381a5b8a 320 des_psi = arg2;
jdawkins 0:f8fd381a5b8a 321 des_spd = arg3;
jdawkins 0:f8fd381a5b8a 322 }
jdawkins 0:f8fd381a5b8a 323 default:{
jdawkins 0:f8fd381a5b8a 324 }
jdawkins 0:f8fd381a5b8a 325 }
jdawkins 0:f8fd381a5b8a 326 } //emd of $CMD
jdawkins 0:f8fd381a5b8a 327
jdawkins 0:f8fd381a5b8a 328 /*if(!strncmp(msg, "$PRM", 4)){
jdawkins 0:f8fd381a5b8a 329 float arg1,arg2,arg3;
jdawkins 0:f8fd381a5b8a 330 int type;
jdawkins 0:f8fd381a5b8a 331 // __disable_irq(); // disable interrupts
jdawkins 0:f8fd381a5b8a 332 sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3);
jdawkins 0:f8fd381a5b8a 333 // __enable_irq(); // enable interrupts
jdawkins 0:f8fd381a5b8a 334
jdawkins 0:f8fd381a5b8a 335 switch(type){
jdawkins 0:f8fd381a5b8a 336 case 1:
jdawkins 0:f8fd381a5b8a 337 {
jdawkins 0:f8fd381a5b8a 338 Kp_psi = arg1;
jdawkins 0:f8fd381a5b8a 339 Kp_spd = arg2;
jdawkins 0:f8fd381a5b8a 340 Ki_spd = arg3;
jdawkins 0:f8fd381a5b8a 341 printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
jdawkins 0:f8fd381a5b8a 342 break;
jdawkins 0:f8fd381a5b8a 343 }
jdawkins 0:f8fd381a5b8a 344 case 2:
jdawkins 0:f8fd381a5b8a 345 {
jdawkins 0:f8fd381a5b8a 346 printf("%s",msg);
jdawkins 0:f8fd381a5b8a 347
jdawkins 0:f8fd381a5b8a 348 float ref_lat = arg1;
jdawkins 0:f8fd381a5b8a 349 float ref_lon = arg2;
jdawkins 0:f8fd381a5b8a 350 float ref_alt = arg3;
jdawkins 0:f8fd381a5b8a 351 printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
jdawkins 0:f8fd381a5b8a 352
jdawkins 0:f8fd381a5b8a 353 // gps.setRefPoint(ref_lat,ref_lon,ref_alt);
jdawkins 0:f8fd381a5b8a 354 break;
jdawkins 0:f8fd381a5b8a 355 }
jdawkins 0:f8fd381a5b8a 356 default:
jdawkins 0:f8fd381a5b8a 357 {
jdawkins 0:f8fd381a5b8a 358 }
jdawkins 0:f8fd381a5b8a 359
jdawkins 0:f8fd381a5b8a 360 }
jdawkins 0:f8fd381a5b8a 361
jdawkins 0:f8fd381a5b8a 362 } */
jdawkins 0:f8fd381a5b8a 363
jdawkins 0:f8fd381a5b8a 364 if(!strncmp(msg, "$LED", 4)){
jdawkins 0:f8fd381a5b8a 365 int arg1;
jdawkins 0:f8fd381a5b8a 366 float arg2;
jdawkins 0:f8fd381a5b8a 367 sscanf(msg,"$LED,%x,%f",&arg1,&arg2);
jdawkins 0:f8fd381a5b8a 368 printf("%s\r\n",msg);
jdawkins 0:f8fd381a5b8a 369 int colors[4]={arg1,arg1,arg1,arg1};
jdawkins 0:f8fd381a5b8a 370 float brightness = arg2;
jdawkins 0:f8fd381a5b8a 371 setLED(colors,brightness);
jdawkins 0:f8fd381a5b8a 372 }
jdawkins 0:f8fd381a5b8a 373
jdawkins 0:f8fd381a5b8a 374
jdawkins 0:f8fd381a5b8a 375
jdawkins 0:f8fd381a5b8a 376
jdawkins 0:f8fd381a5b8a 377 }
jdawkins 0:f8fd381a5b8a 378 void setLED(int *colors,float brightness){
jdawkins 0:f8fd381a5b8a 379
jdawkins 0:f8fd381a5b8a 380 leds.setBrightness(brightness);
jdawkins 0:f8fd381a5b8a 381
jdawkins 0:f8fd381a5b8a 382 int cidx = 0;
jdawkins 0:f8fd381a5b8a 383 int ctr = 0;
jdawkins 0:f8fd381a5b8a 384 for (int i=0;i<LED_PER_CLUSTER*LED_CLUSTERS;i++){
jdawkins 0:f8fd381a5b8a 385
jdawkins 0:f8fd381a5b8a 386 if(ctr >11){
jdawkins 0:f8fd381a5b8a 387 ctr = 0;
jdawkins 0:f8fd381a5b8a 388 cidx++;
jdawkins 0:f8fd381a5b8a 389 }
jdawkins 0:f8fd381a5b8a 390 leds.setPixel(i,colors[cidx]);
jdawkins 0:f8fd381a5b8a 391 ctr++;
jdawkins 0:f8fd381a5b8a 392 }
jdawkins 0:f8fd381a5b8a 393 leds.write();
jdawkins 0:f8fd381a5b8a 394 }
jdawkins 0:f8fd381a5b8a 395 float saturateCmd(float cmd)
jdawkins 0:f8fd381a5b8a 396 {
jdawkins 0:f8fd381a5b8a 397 if(cmd>1.0) {
jdawkins 0:f8fd381a5b8a 398 cmd = 1.0;
jdawkins 0:f8fd381a5b8a 399 }
jdawkins 0:f8fd381a5b8a 400 if(cmd < -1.0) {
jdawkins 0:f8fd381a5b8a 401 cmd = -1.0;
jdawkins 0:f8fd381a5b8a 402 }
jdawkins 0:f8fd381a5b8a 403 return cmd;
jdawkins 0:f8fd381a5b8a 404 }
jdawkins 0:f8fd381a5b8a 405 float saturateCmd(float cmd, float max,float min)
jdawkins 0:f8fd381a5b8a 406 {
jdawkins 0:f8fd381a5b8a 407 if(cmd>max) {
jdawkins 0:f8fd381a5b8a 408 cmd = max;
jdawkins 0:f8fd381a5b8a 409 }
jdawkins 0:f8fd381a5b8a 410 if(cmd < min) {
jdawkins 0:f8fd381a5b8a 411 cmd = min;
jdawkins 0:f8fd381a5b8a 412 }
jdawkins 0:f8fd381a5b8a 413 return cmd;
jdawkins 0:f8fd381a5b8a 414 }
jdawkins 0:f8fd381a5b8a 415
jdawkins 0:f8fd381a5b8a 416 float wrapToPi(float ang)
jdawkins 0:f8fd381a5b8a 417 {
jdawkins 0:f8fd381a5b8a 418
jdawkins 0:f8fd381a5b8a 419 if(ang > PI) {
jdawkins 0:f8fd381a5b8a 420
jdawkins 0:f8fd381a5b8a 421 ang = ang - 2*PI;
jdawkins 0:f8fd381a5b8a 422 }
jdawkins 0:f8fd381a5b8a 423 if (ang < -PI) {
jdawkins 0:f8fd381a5b8a 424 ang = ang + 2*PI;
jdawkins 0:f8fd381a5b8a 425 }
jdawkins 0:f8fd381a5b8a 426
jdawkins 0:f8fd381a5b8a 427 return ang;
jdawkins 0:f8fd381a5b8a 428 }