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@2:0c9c3c1f3b18, 2016-08-25 (annotated)

Committer:
lddevrie
Date:
Thu Aug 25 15:47:50 2016 +0000
Revision:
2:0c9c3c1f3b18
Parent:
0:f8fd381a5b8a
Child:
3:9b6db94aa2b3
Updated xbee serial comms using MODSERIAL. Accommodates serial messages greater than 16 characters.

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