Agra-GPS / FreePilot_V2-3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FreePilot PinDetect mbed-src

Fork of FreePilot_V2-2 by Agra-GPS

main.cpp@28:5905886c76ee, 2015-01-21 (annotated)

Committer:
jhedmonton
Date:
Wed Jan 21 02:54:26 2015 +0000
Revision:
28:5905886c76ee
Parent:
27:9ac59b261d87
Child:
29:23ccb2a50b6f
Changes to config, adding a base.cpp to move globals

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 26:dc00998140af 1 #include "mbed.h"
jhedmonton 28:5905886c76ee 2 //#include "MODSERIAL.h"
maximbolduc 26:dc00998140af 3 #include "PinDetect.h"
maximbolduc 26:dc00998140af 4 #include "IMUfilter.h"
maximbolduc 26:dc00998140af 5 #include "ADXL345_I2C.h"
maximbolduc 26:dc00998140af 6 #include "ITG3200.h"
maximbolduc 26:dc00998140af 7 #include "Point.h"
maximbolduc 26:dc00998140af 8 #include <vector>
maximbolduc 26:dc00998140af 9 #include "Line.h"
maximbolduc 26:dc00998140af 10 #include "stringUtils.h"
jhedmonton 28:5905886c76ee 11 #include "base.h"
jhedmonton 28:5905886c76ee 12 #include "Config.h"
maximbolduc 26:dc00998140af 13
jhedmonton 28:5905886c76ee 14 char *version="FreePilot V2.11 Jan 20, 2015\r\n";
jhedmonton 27:9ac59b261d87 15 long lastsend_version=0;
jhedmonton 27:9ac59b261d87 16 Timer vTimer; //this timer is int based! Max is 30 minutes
jhedmonton 27:9ac59b261d87 17
maximbolduc 26:dc00998140af 18 int checksumm;
maximbolduc 26:dc00998140af 19
maximbolduc 26:dc00998140af 20 int dont = 0;
maximbolduc 26:dc00998140af 21 #define g0 9.812865328//Gravity at Earth's surface in m/s/s
maximbolduc 26:dc00998140af 22 #define SAMPLES 8//Number of samples to average.
maximbolduc 26:dc00998140af 23 #define CALIBRATION_SAMPLES 256//Number of samples to be averaged for a null bias calculation during calibration.
maximbolduc 26:dc00998140af 24 #define toDegrees(x) (x * 57.2957795)//Convert from radians to degrees.
maximbolduc 26:dc00998140af 25 #define toRadians(x) (x * 0.01745329252)//Convert from degrees to radians.
maximbolduc 26:dc00998140af 26 #define GYROSCOPE_GAIN (1 / 14.375)//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
maximbolduc 26:dc00998140af 27 #define ACCELEROMETER_GAIN (0.004 * g0)//Full scale resolution on the ADXL345 is 4mg/LSB.
maximbolduc 26:dc00998140af 28 #define GYRO_RATE 0.005//Sampling gyroscope at 200Hz.
maximbolduc 26:dc00998140af 29 #define ACC_RATE 0.005//Sampling accelerometer at 200Hz.
maximbolduc 26:dc00998140af 30 #define FILTER_RATE 0.1//Updating filter at 40Hz.
maximbolduc 26:dc00998140af 31 double distance_from_line;
maximbolduc 26:dc00998140af 32 double cm_per_deg_lon;
maximbolduc 26:dc00998140af 33 double cm_per_deg_lat;
maximbolduc 26:dc00998140af 34 //all timing objects
maximbolduc 26:dc00998140af 35 Timer gps_connecting;
maximbolduc 26:dc00998140af 36 Timer autosteer_time;
maximbolduc 26:dc00998140af 37 Timeout autosteer_timeout; //timeout work near as timer, but they don't give timing. they just trigger an interrupt once the time we assigned it is passed.
maximbolduc 26:dc00998140af 38 Ticker accelerometerTicker;
maximbolduc 26:dc00998140af 39 Ticker gyroscopeTicker;
maximbolduc 26:dc00998140af 40 Ticker filterTicker;
maximbolduc 26:dc00998140af 41 Ticker angle_print;
maximbolduc 26:dc00998140af 42 Ticker debug_leds;
maximbolduc 26:dc00998140af 43
jhedmonton 27:9ac59b261d87 44 //Motor
jhedmonton 27:9ac59b261d87 45 PinDetect motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 46 DigitalOut enable_motor(p7);
jhedmonton 28:5905886c76ee 47
jhedmonton 28:5905886c76ee 48 PwmOut pwm1(p22);
jhedmonton 28:5905886c76ee 49 PwmOut pwm2(p21);
jhedmonton 28:5905886c76ee 50
maximbolduc 26:dc00998140af 51
jhedmonton 27:9ac59b261d87 52 //equipment switches
jhedmonton 27:9ac59b261d87 53 PinDetect boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 54 PinDetect boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 55 PinDetect boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 56 PinDetect boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 57 //DigitalIn boom1(p20);
jhedmonton 27:9ac59b261d87 58 //DigitalIn boom2(p19);
jhedmonton 27:9ac59b261d87 59 //DigitalIn boom3(p18);
jhedmonton 27:9ac59b261d87 60 //DigitalIn boom4(p17);
jhedmonton 27:9ac59b261d87 61 char boom18; //1 byte
jhedmonton 27:9ac59b261d87 62 char lastboom18; //1 byte
jhedmonton 27:9ac59b261d87 63 char boomstate[8]={'$','F','B','S',0,13,10,0 };
jhedmonton 27:9ac59b261d87 64
jhedmonton 27:9ac59b261d87 65 //gyro
maximbolduc 26:dc00998140af 66 IMUfilter imuFilter(FILTER_RATE, 0.3);
maximbolduc 26:dc00998140af 67 ADXL345_I2C accelerometer(p28, p27);
maximbolduc 26:dc00998140af 68 ITG3200 gyroscope(p28,p27);
maximbolduc 26:dc00998140af 69
maximbolduc 26:dc00998140af 70 Point position;
maximbolduc 26:dc00998140af 71 Point looked_ahead;
maximbolduc 26:dc00998140af 72 Point line_start;
maximbolduc 26:dc00998140af 73 Point line_end;
maximbolduc 26:dc00998140af 74 Point tilt_compensated_position;
maximbolduc 26:dc00998140af 75 Point yaw_compensated_position;
maximbolduc 26:dc00998140af 76
maximbolduc 26:dc00998140af 77 double distance_to_line;
maximbolduc 26:dc00998140af 78
maximbolduc 26:dc00998140af 79 //FreePilot variables
maximbolduc 26:dc00998140af 80 int timer_enabled;
jhedmonton 28:5905886c76ee 81 double motorspeed;
maximbolduc 26:dc00998140af 82 int enable_time;
maximbolduc 26:dc00998140af 83 char* motor_enable_state = 0;
jhedmonton 28:5905886c76ee 84 int motor_enable = 0;
jhedmonton 28:5905886c76ee 85 int lastmotor_enable = 1;
maximbolduc 26:dc00998140af 86 double pwm1_speed;
maximbolduc 26:dc00998140af 87 double pwm2_speed;
jhedmonton 28:5905886c76ee 88 long lastsend_motorstate=0;
jhedmonton 28:5905886c76ee 89 Timer motTimer; //this timer is int based! Max is 30 minutes
jhedmonton 28:5905886c76ee 90 Timer btTimer; //measure time for Bluetooth communication
jhedmonton 28:5905886c76ee 91 long lastgetBT=0;
maximbolduc 26:dc00998140af 92
maximbolduc 26:dc00998140af 93
maximbolduc 26:dc00998140af 94 int msg2_changed = 1;
maximbolduc 26:dc00998140af 95 char* buffer;
maximbolduc 26:dc00998140af 96 double meter_lat = 0;
maximbolduc 26:dc00998140af 97 double meter_lon = 0;
jhedmonton 28:5905886c76ee 98
maximbolduc 26:dc00998140af 99 int antenna_active = 0;//do we have an antenna connected?
maximbolduc 26:dc00998140af 100 char msg[256]; //GPS line buffer
maximbolduc 26:dc00998140af 101 char msg2[256];//PC line buffer
maximbolduc 26:dc00998140af 102 int printing;
maximbolduc 26:dc00998140af 103 int num_of_gps_sats;
maximbolduc 26:dc00998140af 104 int print_vtg = 0; //FGPS asked for VTG?
maximbolduc 26:dc00998140af 105 double decimal_lon;
maximbolduc 26:dc00998140af 106 float longitude;
maximbolduc 26:dc00998140af 107 float latitude;
maximbolduc 26:dc00998140af 108 char ns, ew;
maximbolduc 26:dc00998140af 109 int lock;
maximbolduc 26:dc00998140af 110 int flag_gga;
maximbolduc 26:dc00998140af 111 int reading;
maximbolduc 26:dc00998140af 112 double decimal_latitude;
maximbolduc 26:dc00998140af 113 int gps_satellite_quality;
maximbolduc 26:dc00998140af 114 int day;
maximbolduc 26:dc00998140af 115 int hour;
maximbolduc 26:dc00998140af 116 int minute;
maximbolduc 26:dc00998140af 117 int second;
maximbolduc 26:dc00998140af 118 int tenths;
maximbolduc 26:dc00998140af 119 int hundreths;
maximbolduc 26:dc00998140af 120 char status;
maximbolduc 26:dc00998140af 121 double track; // track made good . angle
maximbolduc 26:dc00998140af 122 char magvar_dir;
maximbolduc 26:dc00998140af 123 double magvar;
maximbolduc 26:dc00998140af 124 int year;
maximbolduc 26:dc00998140af 125 int month;
maximbolduc 26:dc00998140af 126 double speed_km;
maximbolduc 26:dc00998140af 127 double speed_m_s = 0;
maximbolduc 26:dc00998140af 128 double velocity; // speed in knot
maximbolduc 26:dc00998140af 129 int connect_time = 10000; //variable to change the time that the serial output all the strings in order to verify if the command was right.
maximbolduc 26:dc00998140af 130 int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
maximbolduc 26:dc00998140af 131 int print_gsa = 0;//FGPS request GSA printing
maximbolduc 26:dc00998140af 132 int print_gsv = 1;//FGPS request GSV printing.
maximbolduc 26:dc00998140af 133 int angle_send = 0;
maximbolduc 26:dc00998140af 134 int correct_rmc = 1;
maximbolduc 26:dc00998140af 135 double m_lat = 0;
maximbolduc 26:dc00998140af 136 double m_lon = 0;
maximbolduc 26:dc00998140af 137 char* degminsec;
maximbolduc 26:dc00998140af 138 double m_per_deg_lon;
maximbolduc 26:dc00998140af 139 double m_per_deg_lat;
maximbolduc 26:dc00998140af 140 double look_ahead_lon;
maximbolduc 26:dc00998140af 141 double look_ahead_lat;
maximbolduc 26:dc00998140af 142 int active_AB = 0;
maximbolduc 26:dc00998140af 143 double compensation_vector;
maximbolduc 26:dc00998140af 144 char output[256];
maximbolduc 26:dc00998140af 145
maximbolduc 26:dc00998140af 146 double yaw;
maximbolduc 26:dc00998140af 147 double pitch;
maximbolduc 26:dc00998140af 148 double roll;
maximbolduc 26:dc00998140af 149
maximbolduc 26:dc00998140af 150 volatile double a_xAccumulator = 0;
maximbolduc 26:dc00998140af 151 volatile double a_yAccumulator = 0;
maximbolduc 26:dc00998140af 152 volatile double a_zAccumulator = 0;
maximbolduc 26:dc00998140af 153 volatile double w_xAccumulator = 0;
maximbolduc 26:dc00998140af 154 volatile double w_yAccumulator = 0;
maximbolduc 26:dc00998140af 155 volatile double w_zAccumulator = 0;
maximbolduc 26:dc00998140af 156
maximbolduc 26:dc00998140af 157 volatile double a_x;
maximbolduc 26:dc00998140af 158 volatile double a_y;
maximbolduc 26:dc00998140af 159 volatile double a_z;
maximbolduc 26:dc00998140af 160 volatile double w_x;
maximbolduc 26:dc00998140af 161 volatile double w_y;
maximbolduc 26:dc00998140af 162 volatile double w_z;
maximbolduc 26:dc00998140af 163
maximbolduc 26:dc00998140af 164 int readings[3];
maximbolduc 26:dc00998140af 165 int accelerometerSamples = 0;
maximbolduc 26:dc00998140af 166 int gyroscopeSamples = 0;
maximbolduc 26:dc00998140af 167 int print_euler = 1;
maximbolduc 26:dc00998140af 168
maximbolduc 26:dc00998140af 169 double Freepilot_lat;
maximbolduc 26:dc00998140af 170 double Freepilot_lon;
maximbolduc 26:dc00998140af 171 double Freepilot_lat1;
maximbolduc 26:dc00998140af 172 double Freepilot_lon1;
maximbolduc 26:dc00998140af 173 double Freepilot_bearing;
maximbolduc 26:dc00998140af 174 //double Freepilot_lon_meter;
maximbolduc 26:dc00998140af 175 //double Freepilot_lat_meter;
maximbolduc 26:dc00998140af 176
maximbolduc 26:dc00998140af 177 void initializeAcceleromter(void);
maximbolduc 26:dc00998140af 178 void calibrateAccelerometer(void);
maximbolduc 26:dc00998140af 179 void sampleAccelerometer(void);
maximbolduc 26:dc00998140af 180 void initializeGyroscope(void);
maximbolduc 26:dc00998140af 181 void calibrateGyroscope(void);
maximbolduc 26:dc00998140af 182 void sampleGyroscope(void);
maximbolduc 26:dc00998140af 183 void filter(void);
maximbolduc 26:dc00998140af 184
maximbolduc 26:dc00998140af 185 volatile bool newline_detected = false;
maximbolduc 26:dc00998140af 186 char tx_line[80];
maximbolduc 26:dc00998140af 187 char rx_line[80];
maximbolduc 26:dc00998140af 188
maximbolduc 26:dc00998140af 189 Point point_add(Point a, Point b)
maximbolduc 26:dc00998140af 190 {
maximbolduc 26:dc00998140af 191 return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY());
maximbolduc 26:dc00998140af 192 }
maximbolduc 26:dc00998140af 193
maximbolduc 26:dc00998140af 194 Point point_sub(Point a , Point b)
maximbolduc 26:dc00998140af 195 {
maximbolduc 26:dc00998140af 196 return Point(a.GetX() - b.GetX(), a.GetY() - b.GetY());
maximbolduc 26:dc00998140af 197 }
maximbolduc 26:dc00998140af 198
maximbolduc 26:dc00998140af 199 #define dot(u,v) ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
maximbolduc 26:dc00998140af 200 #define norm(v) sqrt(dot(v,v)) // norm = length of vector
maximbolduc 26:dc00998140af 201 #define d(u,v) norm(point_sub(u,v)) // distance = norm of difference
maximbolduc 26:dc00998140af 202
maximbolduc 26:dc00998140af 203 double dist_Point_to_Line( Point P, Point line_start, Point line_end)
maximbolduc 26:dc00998140af 204 {
maximbolduc 26:dc00998140af 205 //Point v = point_sub(L->point1,L.point0);
maximbolduc 26:dc00998140af 206 // Point w = point_sub(P,L.point0);
maximbolduc 26:dc00998140af 207 Point v = point_sub(line_end,line_start);
maximbolduc 26:dc00998140af 208 Point w = point_sub(P,line_start);
maximbolduc 26:dc00998140af 209
maximbolduc 26:dc00998140af 210 double c1 = dot(w,v);
maximbolduc 26:dc00998140af 211 double c2 = dot(v,v);
maximbolduc 26:dc00998140af 212 double b = c1 / c2;
maximbolduc 26:dc00998140af 213
maximbolduc 26:dc00998140af 214 Point resulting(b * v.GetX(),b*v.GetY());
maximbolduc 26:dc00998140af 215 Point Pb = point_add(line_start, resulting);
maximbolduc 26:dc00998140af 216 return d(P, Pb);
maximbolduc 26:dc00998140af 217 }
maximbolduc 26:dc00998140af 218
maximbolduc 26:dc00998140af 219 void initializeAccelerometer(void)
maximbolduc 26:dc00998140af 220 {
maximbolduc 26:dc00998140af 221 accelerometer.setPowerControl(0x00); //Go into standby mode to configure the device.
maximbolduc 26:dc00998140af 222 accelerometer.setDataFormatControl(0x0B); //Full resolution, +/-16g, 4mg/LSB.
maximbolduc 26:dc00998140af 223 accelerometer.setDataRate(ADXL345_200HZ); //200Hz data rate.
maximbolduc 26:dc00998140af 224 accelerometer.setPowerControl(0x08); //Measurement mode.
maximbolduc 26:dc00998140af 225 wait_ms(22); //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
maximbolduc 26:dc00998140af 226 }
maximbolduc 26:dc00998140af 227
maximbolduc 26:dc00998140af 228 void sampleAccelerometer(void)
maximbolduc 26:dc00998140af 229 {
maximbolduc 26:dc00998140af 230 if (accelerometerSamples == SAMPLES)
maximbolduc 26:dc00998140af 231 {
maximbolduc 26:dc00998140af 232 //Average the samples, remove the bias, and calculate the acceleration
maximbolduc 26:dc00998140af 233 //in m/s/s.
maximbolduc 26:dc00998140af 234 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
maximbolduc 26:dc00998140af 235 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
maximbolduc 26:dc00998140af 236 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
maximbolduc 26:dc00998140af 237 a_xAccumulator = 0;
maximbolduc 26:dc00998140af 238 a_yAccumulator = 0;
maximbolduc 26:dc00998140af 239 a_zAccumulator = 0;
maximbolduc 26:dc00998140af 240 accelerometerSamples = 0;
maximbolduc 26:dc00998140af 241 }
maximbolduc 26:dc00998140af 242 else
maximbolduc 26:dc00998140af 243 {
maximbolduc 26:dc00998140af 244 //Take another sample.
maximbolduc 26:dc00998140af 245 accelerometer.getOutput(readings);
maximbolduc 26:dc00998140af 246 a_xAccumulator += (int16_t) readings[0];
maximbolduc 26:dc00998140af 247 a_yAccumulator += (int16_t) readings[1];
maximbolduc 26:dc00998140af 248 a_zAccumulator += (int16_t) readings[2];
maximbolduc 26:dc00998140af 249 accelerometerSamples++;
maximbolduc 26:dc00998140af 250 }
maximbolduc 26:dc00998140af 251 }
maximbolduc 26:dc00998140af 252
maximbolduc 26:dc00998140af 253 void calibrateAccelerometer(void)
maximbolduc 26:dc00998140af 254 {
maximbolduc 26:dc00998140af 255 a_xAccumulator = 0;
maximbolduc 26:dc00998140af 256 a_yAccumulator = 0;
maximbolduc 26:dc00998140af 257 a_zAccumulator = 0;
maximbolduc 26:dc00998140af 258 //Take a number of readings and average them
maximbolduc 26:dc00998140af 259 //to calculate the zero g offset.
maximbolduc 26:dc00998140af 260 for (int i = 0; i < CALIBRATION_SAMPLES; i++)
maximbolduc 26:dc00998140af 261 {
maximbolduc 26:dc00998140af 262 accelerometer.getOutput(readings);
maximbolduc 26:dc00998140af 263 a_xAccumulator += (int16_t) readings[0];
maximbolduc 26:dc00998140af 264 a_yAccumulator += (int16_t) readings[1];
maximbolduc 26:dc00998140af 265 a_zAccumulator += (int16_t) readings[2];
maximbolduc 26:dc00998140af 266 wait(ACC_RATE);
maximbolduc 26:dc00998140af 267 }
maximbolduc 26:dc00998140af 268
maximbolduc 26:dc00998140af 269 a_xAccumulator /= CALIBRATION_SAMPLES;
maximbolduc 26:dc00998140af 270 a_yAccumulator /= CALIBRATION_SAMPLES;
maximbolduc 26:dc00998140af 271 a_zAccumulator /= CALIBRATION_SAMPLES;
maximbolduc 26:dc00998140af 272
maximbolduc 26:dc00998140af 273 //At 4mg/LSB, 250 LSBs is 1g.
maximbolduc 26:dc00998140af 274 a_xBias = a_xAccumulator;
maximbolduc 26:dc00998140af 275 a_yBias = a_yAccumulator;
maximbolduc 26:dc00998140af 276 a_zBias = (a_zAccumulator - 250);
maximbolduc 26:dc00998140af 277
maximbolduc 26:dc00998140af 278 a_xAccumulator = 0;
maximbolduc 26:dc00998140af 279 a_yAccumulator = 0;
maximbolduc 26:dc00998140af 280 a_zAccumulator = 0;
maximbolduc 26:dc00998140af 281 }
maximbolduc 26:dc00998140af 282
maximbolduc 26:dc00998140af 283 void initializeGyroscope(void)
maximbolduc 26:dc00998140af 284 {
maximbolduc 26:dc00998140af 285 gyroscope.setLpBandwidth(LPFBW_42HZ);
maximbolduc 26:dc00998140af 286 gyroscope.setSampleRateDivider(4);
maximbolduc 26:dc00998140af 287 }
maximbolduc 26:dc00998140af 288
maximbolduc 26:dc00998140af 289 void calibrateGyroscope(void)
maximbolduc 26:dc00998140af 290 {
maximbolduc 26:dc00998140af 291
maximbolduc 26:dc00998140af 292 w_xAccumulator = 0;
maximbolduc 26:dc00998140af 293 w_yAccumulator = 0;
maximbolduc 26:dc00998140af 294 w_zAccumulator = 0;
maximbolduc 26:dc00998140af 295
maximbolduc 26:dc00998140af 296 //Take a number of readings and average them
maximbolduc 26:dc00998140af 297 //to calculate the gyroscope bias offset.
maximbolduc 26:dc00998140af 298 for (int i = 0; i < CALIBRATION_SAMPLES; i++)
maximbolduc 26:dc00998140af 299 {
maximbolduc 26:dc00998140af 300 w_xAccumulator += gyroscope.getGyroX();
maximbolduc 26:dc00998140af 301 w_yAccumulator += gyroscope.getGyroY();
maximbolduc 26:dc00998140af 302 w_zAccumulator += gyroscope.getGyroZ();
maximbolduc 26:dc00998140af 303 wait(GYRO_RATE);
maximbolduc 26:dc00998140af 304 }
maximbolduc 26:dc00998140af 305 //Average the samples.
maximbolduc 26:dc00998140af 306 w_xAccumulator /= CALIBRATION_SAMPLES;
maximbolduc 26:dc00998140af 307 w_yAccumulator /= CALIBRATION_SAMPLES;
maximbolduc 26:dc00998140af 308 w_zAccumulator /= CALIBRATION_SAMPLES;
maximbolduc 26:dc00998140af 309
maximbolduc 26:dc00998140af 310 w_xBias = w_xAccumulator;
maximbolduc 26:dc00998140af 311 w_yBias = w_yAccumulator;
maximbolduc 26:dc00998140af 312 w_zBias = w_zAccumulator;
maximbolduc 26:dc00998140af 313
maximbolduc 26:dc00998140af 314 w_xAccumulator = 0;
maximbolduc 26:dc00998140af 315 w_yAccumulator = 0;
maximbolduc 26:dc00998140af 316 w_zAccumulator = 0;
maximbolduc 26:dc00998140af 317 }
maximbolduc 26:dc00998140af 318
maximbolduc 26:dc00998140af 319 void sampleGyroscope(void)
maximbolduc 26:dc00998140af 320 {
maximbolduc 26:dc00998140af 321 if (gyroscopeSamples == SAMPLES)
maximbolduc 26:dc00998140af 322 {
maximbolduc 26:dc00998140af 323 //Average the samples, remove the bias, and calculate the angular
maximbolduc 26:dc00998140af 324 //velocity in rad/s.
maximbolduc 26:dc00998140af 325 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
maximbolduc 26:dc00998140af 326 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
maximbolduc 26:dc00998140af 327 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
maximbolduc 26:dc00998140af 328
maximbolduc 26:dc00998140af 329 w_xAccumulator = 0;
maximbolduc 26:dc00998140af 330 w_yAccumulator = 0;
maximbolduc 26:dc00998140af 331 w_zAccumulator = 0;
maximbolduc 26:dc00998140af 332 gyroscopeSamples = 0;
maximbolduc 26:dc00998140af 333 }
maximbolduc 26:dc00998140af 334 else
maximbolduc 26:dc00998140af 335 {
maximbolduc 26:dc00998140af 336 w_xAccumulator += gyroscope.getGyroX();
maximbolduc 26:dc00998140af 337 w_yAccumulator += gyroscope.getGyroY();
maximbolduc 26:dc00998140af 338 w_zAccumulator += gyroscope.getGyroZ();
maximbolduc 26:dc00998140af 339 gyroscopeSamples++;
maximbolduc 26:dc00998140af 340 }
maximbolduc 26:dc00998140af 341 }
maximbolduc 26:dc00998140af 342
maximbolduc 26:dc00998140af 343 void filter(void)
maximbolduc 26:dc00998140af 344 {
maximbolduc 26:dc00998140af 345 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); //Update the filter variables.
maximbolduc 26:dc00998140af 346 imuFilter.computeEuler(); //Calculate the new Euler angles.
maximbolduc 26:dc00998140af 347 }
maximbolduc 26:dc00998140af 348
maximbolduc 26:dc00998140af 349 void activate_antenna()
maximbolduc 26:dc00998140af 350 {
jhedmonton 28:5905886c76ee 351 gps.baud(gps_baud);
maximbolduc 26:dc00998140af 352 antenna_active = 1;
maximbolduc 26:dc00998140af 353 }
maximbolduc 26:dc00998140af 354
maximbolduc 26:dc00998140af 355 float Round_digits(float x, int numdigits)
maximbolduc 26:dc00998140af 356 {
maximbolduc 26:dc00998140af 357 // return ceil(x * pow(10,numdigits))/pow(10,numdigits);
maximbolduc 26:dc00998140af 358 return ceil(x);
maximbolduc 26:dc00998140af 359 }
maximbolduc 26:dc00998140af 360
maximbolduc 26:dc00998140af 361 char* dec_deg_to_degminsec(double d_lat)
maximbolduc 26:dc00998140af 362 {
maximbolduc 26:dc00998140af 363 double coord = d_lat;
maximbolduc 26:dc00998140af 364 int sec = (int)Round_digits(coord * 3600,0);
maximbolduc 26:dc00998140af 365 int deg = sec / 3600;
maximbolduc 26:dc00998140af 366 sec = abs (sec % 3600);
maximbolduc 26:dc00998140af 367 int min = sec / 60;
maximbolduc 26:dc00998140af 368 sec %= 60;
maximbolduc 26:dc00998140af 369
maximbolduc 26:dc00998140af 370 sprintf(degminsec,"%i%i%i",deg,min,sec);
maximbolduc 26:dc00998140af 371 return degminsec;
maximbolduc 26:dc00998140af 372 }
maximbolduc 26:dc00998140af 373
maximbolduc 26:dc00998140af 374 double decimal_to_meters_lat(double lat)
maximbolduc 26:dc00998140af 375 {
maximbolduc 26:dc00998140af 376 m_per_deg_lat = 111.111;
maximbolduc 26:dc00998140af 377 m_lat = m_per_deg_lat * lat;
maximbolduc 26:dc00998140af 378 return m_lat;
maximbolduc 26:dc00998140af 379 }
maximbolduc 26:dc00998140af 380
maximbolduc 26:dc00998140af 381 double decimal_to_meters_lon(double lon, double lat)
maximbolduc 26:dc00998140af 382 {
maximbolduc 26:dc00998140af 383 m_per_deg_lon = 111.111 * cos(lat);
maximbolduc 26:dc00998140af 384 double m_lon = m_per_deg_lon * lon;
maximbolduc 26:dc00998140af 385 return m_lon;
maximbolduc 26:dc00998140af 386 }
maximbolduc 26:dc00998140af 387
maximbolduc 26:dc00998140af 388 double m_lat_to_dec_deg(double lat)
maximbolduc 26:dc00998140af 389 {
maximbolduc 26:dc00998140af 390 m_per_deg_lon = 111.111;
maximbolduc 26:dc00998140af 391 decimal_latitude = decimal_latitude / m_per_deg_lat;
maximbolduc 26:dc00998140af 392 return decimal_latitude;
maximbolduc 26:dc00998140af 393 }
maximbolduc 26:dc00998140af 394
maximbolduc 26:dc00998140af 395 double lat_to_deg(char *s, char north_south)
maximbolduc 26:dc00998140af 396 {
maximbolduc 26:dc00998140af 397 int deg, min, sec;
maximbolduc 26:dc00998140af 398 double fsec, val;
maximbolduc 26:dc00998140af 399
maximbolduc 26:dc00998140af 400 deg = ( (s[0] - '0') * 10) + s[1] - '0';
maximbolduc 26:dc00998140af 401 min = ( (s[2] - '0') * 10) + s[3] - '0';
maximbolduc 26:dc00998140af 402 sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
maximbolduc 26:dc00998140af 403 fsec = (double)((double)sec /10000.0);
maximbolduc 26:dc00998140af 404 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 26:dc00998140af 405 if (north_south == 'S')
maximbolduc 26:dc00998140af 406 {
maximbolduc 26:dc00998140af 407 val *= -1.0;
maximbolduc 26:dc00998140af 408 }
maximbolduc 26:dc00998140af 409 decimal_latitude = val;
maximbolduc 26:dc00998140af 410 return val;
maximbolduc 26:dc00998140af 411 }
maximbolduc 26:dc00998140af 412
maximbolduc 26:dc00998140af 413 double lon_to_deg(char *s, char east_west)
maximbolduc 26:dc00998140af 414 {
maximbolduc 26:dc00998140af 415 int deg, min, sec;
maximbolduc 26:dc00998140af 416 double fsec, val;
maximbolduc 26:dc00998140af 417 deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
maximbolduc 26:dc00998140af 418 min = ( (s[3] - '0') * 10) + s[4] - '0';
maximbolduc 26:dc00998140af 419 sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
maximbolduc 26:dc00998140af 420 fsec = (double)((double)sec /10000.0);
maximbolduc 26:dc00998140af 421 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 26:dc00998140af 422 if (east_west == 'W')
maximbolduc 26:dc00998140af 423 {
maximbolduc 26:dc00998140af 424 val *= -1.0;
maximbolduc 26:dc00998140af 425 }
maximbolduc 26:dc00998140af 426 decimal_lon = val;
maximbolduc 26:dc00998140af 427 return val;
maximbolduc 26:dc00998140af 428 }
maximbolduc 26:dc00998140af 429
maximbolduc 26:dc00998140af 430 void nmea_gga(char *s)
maximbolduc 26:dc00998140af 431 {
maximbolduc 26:dc00998140af 432 char *token;
maximbolduc 26:dc00998140af 433 int token_counter = 0;
maximbolduc 26:dc00998140af 434 char *latitude = (char *)NULL;
maximbolduc 26:dc00998140af 435 char *longitude = (char *)NULL;
maximbolduc 26:dc00998140af 436 char *lat_dir = (char *)NULL;
maximbolduc 26:dc00998140af 437 char *lon_dir = (char *)NULL;
maximbolduc 26:dc00998140af 438 char *qual = (char *)NULL;
maximbolduc 26:dc00998140af 439 char *altitude = (char *)NULL;
maximbolduc 26:dc00998140af 440 char *sats = (char *)NULL;
maximbolduc 26:dc00998140af 441
maximbolduc 26:dc00998140af 442 token = strtok(s, ",");
maximbolduc 26:dc00998140af 443 while (token)
maximbolduc 26:dc00998140af 444 {
maximbolduc 26:dc00998140af 445 switch (token_counter)
maximbolduc 26:dc00998140af 446 {
maximbolduc 26:dc00998140af 447 case 2:
maximbolduc 26:dc00998140af 448 latitude = token;
maximbolduc 26:dc00998140af 449 break;
maximbolduc 26:dc00998140af 450 case 4:
maximbolduc 26:dc00998140af 451 longitude = token;
maximbolduc 26:dc00998140af 452 break;
maximbolduc 26:dc00998140af 453 case 3:
maximbolduc 26:dc00998140af 454 lat_dir = token;
maximbolduc 26:dc00998140af 455 break;
maximbolduc 26:dc00998140af 456 case 5:
maximbolduc 26:dc00998140af 457 lon_dir = token;
maximbolduc 26:dc00998140af 458 break;
maximbolduc 26:dc00998140af 459 case 6:
maximbolduc 26:dc00998140af 460 qual = token;
maximbolduc 26:dc00998140af 461 break;
maximbolduc 26:dc00998140af 462 case 7:
maximbolduc 26:dc00998140af 463 sats = token;
maximbolduc 26:dc00998140af 464 break;
maximbolduc 26:dc00998140af 465 case 9:
maximbolduc 26:dc00998140af 466 altitude = token;
maximbolduc 26:dc00998140af 467 break;
maximbolduc 26:dc00998140af 468 }
maximbolduc 26:dc00998140af 469 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 470 token_counter++;
maximbolduc 26:dc00998140af 471 }
maximbolduc 26:dc00998140af 472
maximbolduc 26:dc00998140af 473 if (latitude && longitude && altitude && sats)
maximbolduc 26:dc00998140af 474 {
maximbolduc 26:dc00998140af 475 decimal_latitude = lat_to_deg(latitude, lat_dir[0]);
maximbolduc 26:dc00998140af 476 decimal_lon = lon_to_deg(longitude, lon_dir[0]);
maximbolduc 26:dc00998140af 477 num_of_gps_sats = atoi(sats);
maximbolduc 26:dc00998140af 478 gps_satellite_quality = atoi(qual);
maximbolduc 26:dc00998140af 479 }
maximbolduc 26:dc00998140af 480 else
maximbolduc 26:dc00998140af 481 {
maximbolduc 26:dc00998140af 482 gps_satellite_quality = 0;
maximbolduc 26:dc00998140af 483 }
maximbolduc 26:dc00998140af 484 }
maximbolduc 26:dc00998140af 485
maximbolduc 26:dc00998140af 486 //from farmerGPS code
maximbolduc 26:dc00998140af 487 void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
maximbolduc 26:dc00998140af 488 {
maximbolduc 26:dc00998140af 489 double ydist = 0;
maximbolduc 26:dc00998140af 490 double xdist = 0;
maximbolduc 26:dc00998140af 491 angle = angle + 180;
maximbolduc 26:dc00998140af 492 double radiant = angle * 3.14159265359 / 180;
maximbolduc 26:dc00998140af 493 double sinr = sin(radiant);
maximbolduc 26:dc00998140af 494 double cosr = cos(radiant);
maximbolduc 26:dc00998140af 495 xdist = cosr * distance;
maximbolduc 26:dc00998140af 496 ydist = sinr * distance;
maximbolduc 26:dc00998140af 497 lat2 = lat1 + (ydist / (69.09 * -1609.344));
maximbolduc 26:dc00998140af 498 lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
maximbolduc 26:dc00998140af 499 return;
maximbolduc 26:dc00998140af 500 }
maximbolduc 26:dc00998140af 501
maximbolduc 26:dc00998140af 502 Point compensation;
maximbolduc 26:dc00998140af 503 double compensation_angle;
maximbolduc 26:dc00998140af 504 //antenna compensation in cm
maximbolduc 26:dc00998140af 505 void tilt_compensate()
maximbolduc 26:dc00998140af 506 {
maximbolduc 26:dc00998140af 507 roll = imuFilter.getRoll();
jhedmonton 28:5905886c76ee 508 compensation_vector = antennaheight * sin(roll);
maximbolduc 26:dc00998140af 509 compensation.SetX(compensation_vector * cos((toDegrees(imuFilter.getYaw()) * -1 - 90)/57.295779513));
maximbolduc 26:dc00998140af 510 compensation.SetY(compensation_vector * sin((toDegrees(imuFilter.getYaw()) * -1 - 90)/57.295779513));
maximbolduc 26:dc00998140af 511 }
maximbolduc 26:dc00998140af 512
maximbolduc 26:dc00998140af 513 void yaw_compensate()
maximbolduc 26:dc00998140af 514 {
maximbolduc 26:dc00998140af 515 yaw = imuFilter.getYaw();
maximbolduc 26:dc00998140af 516
maximbolduc 26:dc00998140af 517 }
maximbolduc 26:dc00998140af 518
maximbolduc 26:dc00998140af 519 void modify_rmc()
maximbolduc 26:dc00998140af 520 {
maximbolduc 26:dc00998140af 521
maximbolduc 26:dc00998140af 522 }
maximbolduc 26:dc00998140af 523
maximbolduc 26:dc00998140af 524 void process_GPSHEIGHT(char* height_string)
maximbolduc 26:dc00998140af 525 {
maximbolduc 26:dc00998140af 526 char *token;
maximbolduc 26:dc00998140af 527 int token_counter = 0;
maximbolduc 26:dc00998140af 528 char *height = (char *)NULL;
maximbolduc 26:dc00998140af 529 token = strtok(height_string, ",");
maximbolduc 26:dc00998140af 530 while (token)
maximbolduc 26:dc00998140af 531 {
maximbolduc 26:dc00998140af 532
maximbolduc 26:dc00998140af 533 switch (token_counter)
maximbolduc 26:dc00998140af 534 {
maximbolduc 26:dc00998140af 535 case 1:
maximbolduc 26:dc00998140af 536 height = token;
maximbolduc 26:dc00998140af 537 break;
maximbolduc 26:dc00998140af 538 }
maximbolduc 26:dc00998140af 539 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 540 token_counter++;
maximbolduc 26:dc00998140af 541 }
maximbolduc 26:dc00998140af 542 if ( height )
maximbolduc 26:dc00998140af 543 {
jhedmonton 28:5905886c76ee 544 antennaheight = atof(height);
maximbolduc 26:dc00998140af 545 }
maximbolduc 26:dc00998140af 546 }
maximbolduc 26:dc00998140af 547
maximbolduc 26:dc00998140af 548 void process_cs(char* cs_string)
maximbolduc 26:dc00998140af 549 {
maximbolduc 26:dc00998140af 550 sprintf(output, "$cs: %s , %02X\r\n",cs_string, checksumm);
maximbolduc 26:dc00998140af 551 pc.puts(output);
maximbolduc 26:dc00998140af 552 char *token;
maximbolduc 26:dc00998140af 553 int token_counter = 0;
jhedmonton 27:9ac59b261d87 554 // char *cs = (char *)NULL;
maximbolduc 26:dc00998140af 555 token = strtok(cs_string, "*");
maximbolduc 26:dc00998140af 556 while (token)
maximbolduc 26:dc00998140af 557 {
maximbolduc 26:dc00998140af 558 switch (token_counter)
maximbolduc 26:dc00998140af 559 {
maximbolduc 26:dc00998140af 560 case 1:
maximbolduc 26:dc00998140af 561 sprintf(output, "$cs:%s , %02X\r\n",token, checksumm);
maximbolduc 26:dc00998140af 562 pc.puts(token);
maximbolduc 26:dc00998140af 563 break;
maximbolduc 26:dc00998140af 564 }
maximbolduc 26:dc00998140af 565 }
maximbolduc 26:dc00998140af 566 }
maximbolduc 26:dc00998140af 567
maximbolduc 26:dc00998140af 568 void nmea_rmc(char *s)
maximbolduc 26:dc00998140af 569 {
maximbolduc 26:dc00998140af 570 char *token;
maximbolduc 26:dc00998140af 571 int token_counter = 0;
maximbolduc 26:dc00998140af 572 char *time = (char *)NULL;
maximbolduc 26:dc00998140af 573 char *date = (char *)NULL;
maximbolduc 26:dc00998140af 574 char *stat = (char *)NULL;
maximbolduc 26:dc00998140af 575 char *vel = (char *)NULL;
maximbolduc 26:dc00998140af 576 char *trk = (char *)NULL;
maximbolduc 26:dc00998140af 577 char *magv = (char *)NULL;
maximbolduc 26:dc00998140af 578 char *magd = (char *)NULL;
maximbolduc 26:dc00998140af 579 char *latitude = (char *)NULL;
maximbolduc 26:dc00998140af 580 char *longitude = (char *)NULL;
maximbolduc 26:dc00998140af 581 char *lat_dir = (char *)NULL;
maximbolduc 26:dc00998140af 582 char *lon_dir = (char *)NULL;
maximbolduc 26:dc00998140af 583
maximbolduc 26:dc00998140af 584 token = strtok(s, ",*");
maximbolduc 26:dc00998140af 585 while (token)
maximbolduc 26:dc00998140af 586 {
maximbolduc 26:dc00998140af 587 switch (token_counter)
maximbolduc 26:dc00998140af 588 {
maximbolduc 26:dc00998140af 589 case 9:
maximbolduc 26:dc00998140af 590 date = token;
maximbolduc 26:dc00998140af 591 break;
maximbolduc 26:dc00998140af 592 case 1:
maximbolduc 26:dc00998140af 593 time = token;
maximbolduc 26:dc00998140af 594 break;
maximbolduc 26:dc00998140af 595 case 2:
maximbolduc 26:dc00998140af 596 stat = token;
maximbolduc 26:dc00998140af 597 break;
maximbolduc 26:dc00998140af 598 case 7:
maximbolduc 26:dc00998140af 599 vel = token;
maximbolduc 26:dc00998140af 600 break;
maximbolduc 26:dc00998140af 601 case 8:
maximbolduc 26:dc00998140af 602 trk = token;
maximbolduc 26:dc00998140af 603 break;
maximbolduc 26:dc00998140af 604 case 10:
maximbolduc 26:dc00998140af 605 magv = token;
maximbolduc 26:dc00998140af 606 break;
maximbolduc 26:dc00998140af 607 case 11:
maximbolduc 26:dc00998140af 608 magd = token;
maximbolduc 26:dc00998140af 609 break;
maximbolduc 26:dc00998140af 610 case 3:
maximbolduc 26:dc00998140af 611 latitude = token;
maximbolduc 26:dc00998140af 612 break;
maximbolduc 26:dc00998140af 613 case 5:
maximbolduc 26:dc00998140af 614 longitude = token;
maximbolduc 26:dc00998140af 615 break;
maximbolduc 26:dc00998140af 616 case 4:
maximbolduc 26:dc00998140af 617 lat_dir = token;
maximbolduc 26:dc00998140af 618 break;
maximbolduc 26:dc00998140af 619 case 6:
maximbolduc 26:dc00998140af 620 lon_dir = token;
maximbolduc 26:dc00998140af 621 break;
maximbolduc 26:dc00998140af 622 /* case 11:
maximbolduc 26:dc00998140af 623 process_cs(token);*/
maximbolduc 26:dc00998140af 624 }
maximbolduc 26:dc00998140af 625 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 626 token_counter++;
maximbolduc 26:dc00998140af 627 }
maximbolduc 26:dc00998140af 628 if (stat && date && time)
maximbolduc 26:dc00998140af 629 {
maximbolduc 26:dc00998140af 630 hour = (char)((time[0] - '0') * 10) + (time[1] - '0');
maximbolduc 26:dc00998140af 631 minute = (char)((time[2] - '0') * 10) + (time[3] - '0');
maximbolduc 26:dc00998140af 632 second = (char)((time[4] - '0') * 10) + (time[5] - '0');
maximbolduc 26:dc00998140af 633 day = (char)((date[0] - '0') * 10) + (date[1] - '0');
maximbolduc 26:dc00998140af 634 month = (char)((date[2] - '0') * 10) + (date[3] - '0');
maximbolduc 26:dc00998140af 635 year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
maximbolduc 26:dc00998140af 636 status = stat[0];
maximbolduc 26:dc00998140af 637 velocity = atof(vel);
maximbolduc 26:dc00998140af 638 speed_km = velocity * 1.852;
maximbolduc 26:dc00998140af 639 speed_m_s = speed_km * 3600.0 / 1000.0;
maximbolduc 26:dc00998140af 640 //speed_m_s = 5;
maximbolduc 26:dc00998140af 641 track = atof(trk);
maximbolduc 26:dc00998140af 642 magvar = atof(magv);
maximbolduc 26:dc00998140af 643 magvar_dir = magd[0];
maximbolduc 26:dc00998140af 644 }
maximbolduc 26:dc00998140af 645 decimal_latitude = lat_to_deg(latitude, lat_dir[0]);
maximbolduc 26:dc00998140af 646 decimal_lon = lon_to_deg(longitude, lon_dir[0]);
maximbolduc 26:dc00998140af 647 position.SetX(decimal_latitude);
maximbolduc 26:dc00998140af 648 position.SetY(decimal_lon);
maximbolduc 26:dc00998140af 649 cm_per_deg_lat = 111111;
maximbolduc 26:dc00998140af 650 cm_per_deg_lon = 111111 * cos(decimal_latitude);
maximbolduc 26:dc00998140af 651 tilt_compensate(); //in centimeters
maximbolduc 26:dc00998140af 652 compensation.SetY(compensation.GetY() / cm_per_deg_lon);
maximbolduc 26:dc00998140af 653 compensation.SetX(compensation.GetX() / cm_per_deg_lat);
maximbolduc 26:dc00998140af 654
maximbolduc 26:dc00998140af 655 // yaw_compensate();
maximbolduc 26:dc00998140af 656 position = point_add(position,compensation);
maximbolduc 26:dc00998140af 657 modify_rmc();
jhedmonton 28:5905886c76ee 658 double lookaheaddistance = lookaheadtime * speed_m_s;
maximbolduc 26:dc00998140af 659
jhedmonton 28:5905886c76ee 660 get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,track,look_ahead_lon,look_ahead_lat);
maximbolduc 26:dc00998140af 661 looked_ahead.SetX(look_ahead_lat);
maximbolduc 26:dc00998140af 662 looked_ahead.SetY(look_ahead_lon);
maximbolduc 26:dc00998140af 663 double filtering = 111.111 / 2.0 + 111.111 * cos(decimal_latitude)/2.0;
maximbolduc 26:dc00998140af 664 distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end);
maximbolduc 26:dc00998140af 665
jhedmonton 27:9ac59b261d87 666 // sprintf(output,"$DIST_TO_LINE: % .12f\r\n\0",distance_to_line * filtering);
jhedmonton 27:9ac59b261d87 667 // pc.puts(output);
maximbolduc 26:dc00998140af 668
maximbolduc 26:dc00998140af 669 }
maximbolduc 26:dc00998140af 670
maximbolduc 26:dc00998140af 671 void process_FGPSAB(char* ab)
maximbolduc 26:dc00998140af 672 {
maximbolduc 26:dc00998140af 673 char *token;
maximbolduc 26:dc00998140af 674 int token_counter = 0;
maximbolduc 26:dc00998140af 675 char *line_lat = (char *)NULL;
maximbolduc 26:dc00998140af 676 char *line_lon = (char *)NULL;
maximbolduc 26:dc00998140af 677 char *line_lat1 = (char *)NULL;
maximbolduc 26:dc00998140af 678 char *line_lon1 = (char *)NULL;
maximbolduc 26:dc00998140af 679 char *bearing = (char *)NULL;
maximbolduc 26:dc00998140af 680 token = strtok(ab, ",");
maximbolduc 26:dc00998140af 681 while (token)
maximbolduc 26:dc00998140af 682 {
maximbolduc 26:dc00998140af 683 switch (token_counter)
maximbolduc 26:dc00998140af 684 {
maximbolduc 26:dc00998140af 685 case 1:
maximbolduc 26:dc00998140af 686 line_lat = token;
maximbolduc 26:dc00998140af 687 break;
maximbolduc 26:dc00998140af 688 case 2:
maximbolduc 26:dc00998140af 689 line_lon = token;
maximbolduc 26:dc00998140af 690 break;
maximbolduc 26:dc00998140af 691 case 3:
maximbolduc 26:dc00998140af 692 line_lat1 = token;
maximbolduc 26:dc00998140af 693 break;
maximbolduc 26:dc00998140af 694 case 4:
maximbolduc 26:dc00998140af 695 line_lon1 = token;
maximbolduc 26:dc00998140af 696 break;
maximbolduc 26:dc00998140af 697 case 5:
maximbolduc 26:dc00998140af 698 bearing = token;
maximbolduc 26:dc00998140af 699 break;
maximbolduc 26:dc00998140af 700 }
maximbolduc 26:dc00998140af 701 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 702 token_counter++;
maximbolduc 26:dc00998140af 703 }
maximbolduc 26:dc00998140af 704 Freepilot_lon = atof(line_lon);
maximbolduc 26:dc00998140af 705 Freepilot_lat = atof(line_lat);
maximbolduc 26:dc00998140af 706 Freepilot_lon1 = atof(line_lon1);
maximbolduc 26:dc00998140af 707 Freepilot_lat1 = atof(line_lat1);
maximbolduc 26:dc00998140af 708 Freepilot_bearing = atof(bearing);
maximbolduc 26:dc00998140af 709 line_start.SetX(Freepilot_lat);
maximbolduc 26:dc00998140af 710 line_start.SetY(Freepilot_lon);
maximbolduc 26:dc00998140af 711 line_end.SetX(Freepilot_lat1);
maximbolduc 26:dc00998140af 712 line_end.SetY(Freepilot_lon1);
maximbolduc 26:dc00998140af 713 active_AB = 1;
maximbolduc 26:dc00998140af 714
maximbolduc 26:dc00998140af 715 sprintf(output, "$ABLINE:%f , %f, %f, %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY());
maximbolduc 26:dc00998140af 716 pc.puts(output);
maximbolduc 26:dc00998140af 717 }
maximbolduc 26:dc00998140af 718
maximbolduc 26:dc00998140af 719 void autosteer_done()
maximbolduc 26:dc00998140af 720 {
maximbolduc 26:dc00998140af 721 //kill the autosteer once the timeout reech
maximbolduc 26:dc00998140af 722 enable_motor = 0;
maximbolduc 26:dc00998140af 723 }
maximbolduc 26:dc00998140af 724
maximbolduc 26:dc00998140af 725 void process_FGPSAUTO(char* FGPSAUTO)
maximbolduc 26:dc00998140af 726 {
maximbolduc 26:dc00998140af 727 char *token;
maximbolduc 26:dc00998140af 728 int token_counter = 0;
maximbolduc 26:dc00998140af 729 char *ahead = (char *)NULL;
maximbolduc 26:dc00998140af 730 char *center = (char *)NULL;
maximbolduc 26:dc00998140af 731 char *phase = (char *)NULL;
maximbolduc 26:dc00998140af 732 char *scl = (char *)NULL;
maximbolduc 26:dc00998140af 733 char *avg = (char *)NULL;
maximbolduc 26:dc00998140af 734 char *_kp = (char *)NULL;
maximbolduc 26:dc00998140af 735 char *_ki = (char *)NULL;
maximbolduc 26:dc00998140af 736 char *_kd = (char *)NULL;
maximbolduc 26:dc00998140af 737
maximbolduc 26:dc00998140af 738 token = strtok(FGPSAUTO, ",");
maximbolduc 26:dc00998140af 739 while (token)
maximbolduc 26:dc00998140af 740 {
maximbolduc 26:dc00998140af 741 switch (token_counter)
maximbolduc 26:dc00998140af 742 {
maximbolduc 26:dc00998140af 743 case 1:
maximbolduc 26:dc00998140af 744 phase = token;
maximbolduc 26:dc00998140af 745 break;
maximbolduc 26:dc00998140af 746 case 2:
maximbolduc 26:dc00998140af 747 center = token;
maximbolduc 26:dc00998140af 748 break;
maximbolduc 26:dc00998140af 749 case 4:
maximbolduc 26:dc00998140af 750 scl = token;
maximbolduc 26:dc00998140af 751 break;
maximbolduc 26:dc00998140af 752 case 5:
maximbolduc 26:dc00998140af 753 ahead = token;
maximbolduc 26:dc00998140af 754 break;
maximbolduc 26:dc00998140af 755 case 6:
maximbolduc 26:dc00998140af 756 avg = token;
maximbolduc 26:dc00998140af 757 break;
maximbolduc 26:dc00998140af 758 case 7:
maximbolduc 26:dc00998140af 759 _kp = token;
maximbolduc 26:dc00998140af 760 break;
maximbolduc 26:dc00998140af 761 case 8:
maximbolduc 26:dc00998140af 762 _ki = token;
maximbolduc 26:dc00998140af 763 break;
maximbolduc 26:dc00998140af 764 case 9:
maximbolduc 26:dc00998140af 765 _kd = token;
maximbolduc 26:dc00998140af 766 break;
maximbolduc 26:dc00998140af 767 }
maximbolduc 26:dc00998140af 768 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 769 token_counter++;
maximbolduc 26:dc00998140af 770 }
maximbolduc 26:dc00998140af 771 if ( _kp && _ki && _kd )
maximbolduc 26:dc00998140af 772 {
maximbolduc 26:dc00998140af 773 kp = atof(_kp);
maximbolduc 26:dc00998140af 774 ki = atof(_ki);
maximbolduc 26:dc00998140af 775 kd = atof(_kd);
maximbolduc 26:dc00998140af 776 }
maximbolduc 26:dc00998140af 777 if ( phase && center && scl && avg && ahead )
maximbolduc 26:dc00998140af 778 {
jhedmonton 28:5905886c76ee 779 lookaheadtime = atof(ahead);
maximbolduc 26:dc00998140af 780 scale = atof(scl);
maximbolduc 26:dc00998140af 781 phaseadv = atof(phase);
jhedmonton 28:5905886c76ee 782 avgpos = atof(avg);
jhedmonton 28:5905886c76ee 783 tcenter = atof(center);
jhedmonton 28:5905886c76ee 784 sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
maximbolduc 26:dc00998140af 785 pc.puts(output);
maximbolduc 26:dc00998140af 786 }
maximbolduc 26:dc00998140af 787 }
jhedmonton 27:9ac59b261d87 788 //sets pwm1 and pwm2 and enable_motor
maximbolduc 26:dc00998140af 789 void process_ASTEER(char* asteer)
maximbolduc 26:dc00998140af 790 {
maximbolduc 26:dc00998140af 791 char *token;
maximbolduc 26:dc00998140af 792 int token_counter = 0;
maximbolduc 26:dc00998140af 793 char *asteer_speed = (char *)NULL;
maximbolduc 26:dc00998140af 794 char *asteer_time = (char *)NULL;
maximbolduc 26:dc00998140af 795
jhedmonton 28:5905886c76ee 796
maximbolduc 26:dc00998140af 797 token = strtok(asteer, ",");
maximbolduc 26:dc00998140af 798 while (token)
maximbolduc 26:dc00998140af 799 {
maximbolduc 26:dc00998140af 800 switch (token_counter)
maximbolduc 26:dc00998140af 801 {
maximbolduc 26:dc00998140af 802 case 1:
maximbolduc 26:dc00998140af 803 asteer_speed = token;
maximbolduc 26:dc00998140af 804 break;
maximbolduc 26:dc00998140af 805 case 2:
maximbolduc 26:dc00998140af 806 asteer_time = token;
maximbolduc 26:dc00998140af 807 break;
maximbolduc 26:dc00998140af 808 }
maximbolduc 26:dc00998140af 809 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 810 token_counter++;
maximbolduc 26:dc00998140af 811 }
maximbolduc 26:dc00998140af 812 if ( asteer_speed && asteer_time )
maximbolduc 26:dc00998140af 813 {
maximbolduc 26:dc00998140af 814 motorspeed = atof(asteer_speed);
maximbolduc 26:dc00998140af 815 enable_time = atof(asteer_time);
jhedmonton 28:5905886c76ee 816 autosteer_timeout.attach_us(autosteer_done,(double)enable_time * (double)1000.0);
jhedmonton 28:5905886c76ee 817 if ( motorspeed > 127.0 )
maximbolduc 26:dc00998140af 818 {
jhedmonton 28:5905886c76ee 819 pwm2_speed = 0.0;
jhedmonton 28:5905886c76ee 820 pwm1_speed = ((double)motorspeed - (double)127.0) / 127.0;
maximbolduc 26:dc00998140af 821 enable_motor = 1;
maximbolduc 26:dc00998140af 822 }
jhedmonton 28:5905886c76ee 823 else if ( motorspeed < 127.0 )
maximbolduc 26:dc00998140af 824 {
jhedmonton 28:5905886c76ee 825 pwm2_speed = ( ((double) 127-(double)motorspeed) / 127.0 );
jhedmonton 28:5905886c76ee 826 pwm1_speed = 0.0;
maximbolduc 26:dc00998140af 827 enable_motor = 1;
maximbolduc 26:dc00998140af 828 }
maximbolduc 26:dc00998140af 829 else
maximbolduc 26:dc00998140af 830 {
maximbolduc 26:dc00998140af 831 pwm1_speed = 0;
maximbolduc 26:dc00998140af 832 pwm2_speed = 0;
maximbolduc 26:dc00998140af 833 enable_motor = 0;
maximbolduc 26:dc00998140af 834 }
jhedmonton 28:5905886c76ee 835 sprintf(output,"STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);
jhedmonton 28:5905886c76ee 836 pc.puts(output);
maximbolduc 26:dc00998140af 837 pwm1 = pwm1_speed;
maximbolduc 26:dc00998140af 838 pwm2 = pwm2_speed;
maximbolduc 26:dc00998140af 839 }
maximbolduc 26:dc00998140af 840 }
jhedmonton 28:5905886c76ee 841
maximbolduc 26:dc00998140af 842
maximbolduc 26:dc00998140af 843 void process_initstring(char* init)
maximbolduc 26:dc00998140af 844 {
maximbolduc 26:dc00998140af 845 char *token;
maximbolduc 26:dc00998140af 846 int token_counter = 0;
maximbolduc 26:dc00998140af 847 char *init_string = (char *)NULL;
maximbolduc 26:dc00998140af 848 token = strtok(init, "$");
maximbolduc 26:dc00998140af 849 while (token)
maximbolduc 26:dc00998140af 850 {
maximbolduc 26:dc00998140af 851 switch (token_counter)
maximbolduc 26:dc00998140af 852 {
maximbolduc 26:dc00998140af 853 case 1:
maximbolduc 26:dc00998140af 854 init_string = token;
maximbolduc 26:dc00998140af 855 break;
maximbolduc 26:dc00998140af 856 }
maximbolduc 26:dc00998140af 857 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 858 token_counter++;
maximbolduc 26:dc00998140af 859 }
maximbolduc 26:dc00998140af 860 if ( init_string )
maximbolduc 26:dc00998140af 861 {
maximbolduc 26:dc00998140af 862 if ( antenna_active == 1 )
maximbolduc 26:dc00998140af 863 {
jhedmonton 28:5905886c76ee 864 while(!gps.writeable()) {} //disable uart3 interrupt (p9,p10)
maximbolduc 26:dc00998140af 865 sprintf(output,"$%s",init_string);
jhedmonton 28:5905886c76ee 866 gps.puts(output);
maximbolduc 26:dc00998140af 867 gps_connecting.start();
maximbolduc 26:dc00998140af 868 gps_connecting.reset();
maximbolduc 26:dc00998140af 869 connecting = 1;
maximbolduc 26:dc00998140af 870 }
maximbolduc 26:dc00998140af 871 }
maximbolduc 26:dc00998140af 872 }
maximbolduc 26:dc00998140af 873
maximbolduc 26:dc00998140af 874 void process_GPSBAUD(char* gpsbaud)
maximbolduc 26:dc00998140af 875 {
maximbolduc 26:dc00998140af 876 char *token;
maximbolduc 26:dc00998140af 877 int token_counter = 0;
maximbolduc 26:dc00998140af 878 char *baud = (char *)NULL;
maximbolduc 26:dc00998140af 879 token = strtok(gpsbaud, ",");
maximbolduc 26:dc00998140af 880 while (token)
maximbolduc 26:dc00998140af 881 {
maximbolduc 26:dc00998140af 882 switch (token_counter)
maximbolduc 26:dc00998140af 883 {
maximbolduc 26:dc00998140af 884 case 1:
maximbolduc 26:dc00998140af 885 baud = token;
maximbolduc 26:dc00998140af 886 break;
maximbolduc 26:dc00998140af 887 }
maximbolduc 26:dc00998140af 888 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 889 token_counter++;
maximbolduc 26:dc00998140af 890 }
maximbolduc 26:dc00998140af 891 if ( baud )
maximbolduc 26:dc00998140af 892 {
maximbolduc 26:dc00998140af 893 gps_baud = atoi(baud);
maximbolduc 26:dc00998140af 894 }
maximbolduc 26:dc00998140af 895 activate_antenna();
maximbolduc 26:dc00998140af 896 }
maximbolduc 26:dc00998140af 897
maximbolduc 26:dc00998140af 898 void pc_analyse(char* pc_string)
maximbolduc 26:dc00998140af 899 {
maximbolduc 26:dc00998140af 900 if (!strncmp(pc_string, "$ASTEER", 7))
maximbolduc 26:dc00998140af 901 {
jhedmonton 27:9ac59b261d87 902 //sets pwm1 and pwm2 and enable_motor
maximbolduc 26:dc00998140af 903 process_ASTEER(pc_string);
maximbolduc 26:dc00998140af 904 }
jhedmonton 27:9ac59b261d87 905 else if (!strncmp(pc_string, "$FGPS-BAUD",10))
maximbolduc 26:dc00998140af 906 {
maximbolduc 26:dc00998140af 907 process_GPSBAUD(pc_string);
jhedmonton 27:9ac59b261d87 908 sprintf(output,"%s %d\r\n",pc_string,gps_baud);
jhedmonton 27:9ac59b261d87 909 pc.puts(output);
jhedmonton 27:9ac59b261d87 910
maximbolduc 26:dc00998140af 911 }
maximbolduc 26:dc00998140af 912 else if (!strncmp(pc_string, "$FGPS,",6))
maximbolduc 26:dc00998140af 913 {
jhedmonton 27:9ac59b261d87 914
jhedmonton 27:9ac59b261d87 915 //process_initstring(pc_string);
jhedmonton 27:9ac59b261d87 916 int i=5;
jhedmonton 27:9ac59b261d87 917 char c=pc_string[i];
jhedmonton 27:9ac59b261d87 918 while (c!=0)
jhedmonton 27:9ac59b261d87 919 {
jhedmonton 27:9ac59b261d87 920 i++;
jhedmonton 27:9ac59b261d87 921 if (i>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 922 c=pc_string[i];
jhedmonton 28:5905886c76ee 923 gps.putc(c);
jhedmonton 27:9ac59b261d87 924 pc.putc(c);
jhedmonton 27:9ac59b261d87 925 }
jhedmonton 27:9ac59b261d87 926 // sprintf(output,"%s\r\n",pc_string);
jhedmonton 27:9ac59b261d87 927 // pc.puts(output);
jhedmonton 28:5905886c76ee 928 // gpsputs(output);
maximbolduc 26:dc00998140af 929 }
maximbolduc 26:dc00998140af 930 else if (!strncmp(pc_string, "$PRINT_VTG=0",12))
maximbolduc 26:dc00998140af 931 {
maximbolduc 26:dc00998140af 932 print_vtg = 0;
maximbolduc 26:dc00998140af 933 }
maximbolduc 26:dc00998140af 934 else if (!strncmp(pc_string, "$PRINT_VTG=1",12))
maximbolduc 26:dc00998140af 935 {
maximbolduc 26:dc00998140af 936 print_vtg = 1;
maximbolduc 26:dc00998140af 937 }
maximbolduc 26:dc00998140af 938 else if (!strncmp(pc_string, "$PRINT_GSV=0",12))
maximbolduc 26:dc00998140af 939 {
maximbolduc 26:dc00998140af 940 print_gsv = 0;
maximbolduc 26:dc00998140af 941 }
maximbolduc 26:dc00998140af 942 else if (!strncmp(pc_string, "$PRINT_GSV=1",12))
maximbolduc 26:dc00998140af 943 {
maximbolduc 26:dc00998140af 944 print_gsv = 1;
maximbolduc 26:dc00998140af 945 }
maximbolduc 26:dc00998140af 946 else if (!strncmp(pc_string, "$PRINT_GSA=0",12))
maximbolduc 26:dc00998140af 947 {
maximbolduc 26:dc00998140af 948 print_gsa = 0;
maximbolduc 26:dc00998140af 949 }
maximbolduc 26:dc00998140af 950 else if (!strncmp(pc_string, "$PRINT_GSA=1",12))
maximbolduc 26:dc00998140af 951 {
maximbolduc 26:dc00998140af 952 print_gsa = 1;
maximbolduc 26:dc00998140af 953 }
maximbolduc 26:dc00998140af 954 else if (!strncmp(pc_string, "$PRINT_EULER=1",14))
maximbolduc 26:dc00998140af 955 {
maximbolduc 26:dc00998140af 956 print_euler = 1;
maximbolduc 26:dc00998140af 957 }
maximbolduc 26:dc00998140af 958 else if (!strncmp(pc_string, "$PRINT_EULER=0",14))
maximbolduc 26:dc00998140af 959 {
maximbolduc 26:dc00998140af 960 print_euler = 0;
maximbolduc 26:dc00998140af 961 }
maximbolduc 26:dc00998140af 962 else if (!strncmp(pc_string, "$GPS-HEIGHT",11))
maximbolduc 26:dc00998140af 963 {
maximbolduc 26:dc00998140af 964 process_GPSHEIGHT(pc_string);
maximbolduc 26:dc00998140af 965 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 966 bluetooth.puts(output);
maximbolduc 26:dc00998140af 967 }
maximbolduc 26:dc00998140af 968 else if (!strncmp(pc_string, "$CORRECT_RMC=1",14))
maximbolduc 26:dc00998140af 969 {
maximbolduc 26:dc00998140af 970 correct_rmc = 1;
maximbolduc 26:dc00998140af 971 }
maximbolduc 26:dc00998140af 972 else if (!strncmp(pc_string, "$CORRECT_RMC=0",14))
maximbolduc 26:dc00998140af 973 {
maximbolduc 26:dc00998140af 974 correct_rmc = 0;
maximbolduc 26:dc00998140af 975 }
maximbolduc 26:dc00998140af 976 else if (!strncmp(pc_string, "$FGPSAUTO",9))
maximbolduc 26:dc00998140af 977 {
maximbolduc 26:dc00998140af 978 process_FGPSAUTO(pc_string);
maximbolduc 26:dc00998140af 979 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 980 bluetooth.puts(output);
maximbolduc 26:dc00998140af 981 }
maximbolduc 26:dc00998140af 982 else if (!strncmp(pc_string, "$FGPSAB",7))
maximbolduc 26:dc00998140af 983 {
maximbolduc 26:dc00998140af 984 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 985 // bluetooth.puts(output);
maximbolduc 26:dc00998140af 986 pc.puts(output);
maximbolduc 26:dc00998140af 987 process_FGPSAB(pc_string);
maximbolduc 26:dc00998140af 988
maximbolduc 26:dc00998140af 989 }
maximbolduc 26:dc00998140af 990 else
maximbolduc 26:dc00998140af 991 {
maximbolduc 26:dc00998140af 992 }
maximbolduc 26:dc00998140af 993 }
maximbolduc 26:dc00998140af 994
maximbolduc 26:dc00998140af 995 void gps_analyse(char* gps_string)
maximbolduc 26:dc00998140af 996 {
maximbolduc 26:dc00998140af 997 if ( connecting == 1 )
maximbolduc 26:dc00998140af 998 {
maximbolduc 26:dc00998140af 999 if ( gps_connecting.read_ms() < connect_time && reading == 0 )
maximbolduc 26:dc00998140af 1000 {
maximbolduc 26:dc00998140af 1001 if ( bluetooth.writeable() > 0 )
maximbolduc 26:dc00998140af 1002 {
maximbolduc 26:dc00998140af 1003 bluetooth.puts(gps_string);
maximbolduc 26:dc00998140af 1004 }
maximbolduc 26:dc00998140af 1005 }
maximbolduc 26:dc00998140af 1006 else
maximbolduc 26:dc00998140af 1007 {
maximbolduc 26:dc00998140af 1008 connecting = 0;
maximbolduc 26:dc00998140af 1009 gps_connecting.stop();
maximbolduc 26:dc00998140af 1010 }
maximbolduc 26:dc00998140af 1011 }
maximbolduc 26:dc00998140af 1012 if (!strncmp(gps_string, "$GPRMC", 6))
maximbolduc 26:dc00998140af 1013 {
maximbolduc 26:dc00998140af 1014 // if ( connecting == 0 )// && correct_rmc == 1 )
maximbolduc 26:dc00998140af 1015 // {
maximbolduc 26:dc00998140af 1016 bluetooth.puts(gps_string);
maximbolduc 26:dc00998140af 1017 // }
maximbolduc 26:dc00998140af 1018 nmea_rmc(gps_string); //analyse and decompose the rmc string
maximbolduc 26:dc00998140af 1019 }
maximbolduc 26:dc00998140af 1020 else if (!strncmp(gps_string, "$GPGGA", 6))
maximbolduc 26:dc00998140af 1021 {
maximbolduc 26:dc00998140af 1022 // if ( connecting == 0 )
maximbolduc 26:dc00998140af 1023 // {
maximbolduc 26:dc00998140af 1024 bluetooth.puts(gps_string);
maximbolduc 26:dc00998140af 1025 // }
maximbolduc 26:dc00998140af 1026 //nmea_gga(gps_string);//analyse and decompose the gga string
maximbolduc 26:dc00998140af 1027 }
maximbolduc 26:dc00998140af 1028 else if (!strncmp(gps_string, "$GPVTG", 6))
maximbolduc 26:dc00998140af 1029 {
maximbolduc 26:dc00998140af 1030 // if ( print_vtg == 1 && connecting == 0 )
maximbolduc 26:dc00998140af 1031 // {
maximbolduc 26:dc00998140af 1032 bluetooth.puts(gps_string);
maximbolduc 26:dc00998140af 1033 // }
maximbolduc 26:dc00998140af 1034 }
maximbolduc 26:dc00998140af 1035 else if (!strncmp(gps_string, "$GPGSV", 6))
maximbolduc 26:dc00998140af 1036 {
maximbolduc 26:dc00998140af 1037 if ( print_gsv == 1 && connecting == 0 )
maximbolduc 26:dc00998140af 1038 {
maximbolduc 26:dc00998140af 1039 bluetooth.puts(gps_string);
maximbolduc 26:dc00998140af 1040 }
maximbolduc 26:dc00998140af 1041 }
maximbolduc 26:dc00998140af 1042 else if (!strncmp(gps_string, "$GPGSA", 6))
maximbolduc 26:dc00998140af 1043 {
maximbolduc 26:dc00998140af 1044 if ( print_gsa == 1 && connecting == 0 )
maximbolduc 26:dc00998140af 1045 {
maximbolduc 26:dc00998140af 1046 bluetooth.puts(gps_string);
maximbolduc 26:dc00998140af 1047 }
maximbolduc 26:dc00998140af 1048 }
maximbolduc 26:dc00998140af 1049 else
maximbolduc 26:dc00998140af 1050 {
maximbolduc 26:dc00998140af 1051 }
maximbolduc 26:dc00998140af 1052 }
maximbolduc 26:dc00998140af 1053
maximbolduc 26:dc00998140af 1054 /*void rxCallback(MODSERIAL_IRQ_INFO *q)
maximbolduc 26:dc00998140af 1055 {
maximbolduc 26:dc00998140af 1056 if ( bluetooth.rxGetLastChar() == '\n')
maximbolduc 26:dc00998140af 1057 {
maximbolduc 26:dc00998140af 1058 newline_detected = true;
maximbolduc 26:dc00998140af 1059 }
maximbolduc 26:dc00998140af 1060 }*/
maximbolduc 26:dc00998140af 1061
jhedmonton 27:9ac59b261d87 1062
jhedmonton 27:9ac59b261d87 1063
jhedmonton 27:9ac59b261d87 1064
jhedmonton 27:9ac59b261d87 1065 int i2 = 0;
jhedmonton 27:9ac59b261d87 1066 bool end2 = false;
jhedmonton 27:9ac59b261d87 1067 bool start2 = false;
jhedmonton 27:9ac59b261d87 1068
jhedmonton 27:9ac59b261d87 1069 bool getline2()
maximbolduc 26:dc00998140af 1070 {
jhedmonton 27:9ac59b261d87 1071 int gotstring=false;
jhedmonton 27:9ac59b261d87 1072 while (1)
jhedmonton 27:9ac59b261d87 1073 {
jhedmonton 27:9ac59b261d87 1074 if( !bluetooth.readable() )
jhedmonton 27:9ac59b261d87 1075 {
jhedmonton 27:9ac59b261d87 1076 break;
jhedmonton 27:9ac59b261d87 1077 }
jhedmonton 27:9ac59b261d87 1078 char c = bluetooth.getc();
jhedmonton 27:9ac59b261d87 1079 if (c == 36 ){start2=true;end2 = false; i2 = 0;}
jhedmonton 27:9ac59b261d87 1080 if (c == 10) {end2=true; start2 = false;}
jhedmonton 27:9ac59b261d87 1081 if (start2)
maximbolduc 26:dc00998140af 1082 {
jhedmonton 27:9ac59b261d87 1083 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 1084 i2++;
jhedmonton 27:9ac59b261d87 1085 if (i2>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 1086 }
jhedmonton 27:9ac59b261d87 1087 if (end2)
jhedmonton 27:9ac59b261d87 1088 {
jhedmonton 27:9ac59b261d87 1089 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 1090 msg2[i2+1] = 0;
jhedmonton 27:9ac59b261d87 1091 start2 = false;
jhedmonton 27:9ac59b261d87 1092 gotstring = true;
jhedmonton 27:9ac59b261d87 1093 end2=false;
jhedmonton 27:9ac59b261d87 1094 i2=0;
jhedmonton 27:9ac59b261d87 1095 break;
maximbolduc 26:dc00998140af 1096 }
maximbolduc 26:dc00998140af 1097 }
jhedmonton 27:9ac59b261d87 1098 return gotstring;
maximbolduc 26:dc00998140af 1099 }
maximbolduc 26:dc00998140af 1100
jhedmonton 27:9ac59b261d87 1101
jhedmonton 27:9ac59b261d87 1102 int i=0;
jhedmonton 27:9ac59b261d87 1103 bool start=false;
jhedmonton 27:9ac59b261d87 1104 bool end=false;
jhedmonton 27:9ac59b261d87 1105
jhedmonton 27:9ac59b261d87 1106 bool getline(bool forward)
maximbolduc 26:dc00998140af 1107 {
jhedmonton 27:9ac59b261d87 1108 while (1)
jhedmonton 27:9ac59b261d87 1109 {
jhedmonton 28:5905886c76ee 1110 if( !gps.readable() )
jhedmonton 27:9ac59b261d87 1111 {
jhedmonton 27:9ac59b261d87 1112 break;
jhedmonton 27:9ac59b261d87 1113 }
jhedmonton 28:5905886c76ee 1114 char c = gps.getc();
jhedmonton 27:9ac59b261d87 1115 if (forward) //simply forward all to Bluetooth
maximbolduc 26:dc00998140af 1116 {
jhedmonton 27:9ac59b261d87 1117 bluetooth.putc(c);
jhedmonton 27:9ac59b261d87 1118 }
jhedmonton 27:9ac59b261d87 1119 if (c == 36 ){start=true;end = false; i = 0;}
jhedmonton 27:9ac59b261d87 1120 if (c == 10) {end=true; start = false;}
jhedmonton 27:9ac59b261d87 1121 if (start)
jhedmonton 27:9ac59b261d87 1122 {
jhedmonton 27:9ac59b261d87 1123 msg[i]=c;
jhedmonton 27:9ac59b261d87 1124 i++;
jhedmonton 27:9ac59b261d87 1125 if (i>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 1126 }
jhedmonton 27:9ac59b261d87 1127 if (end)
jhedmonton 27:9ac59b261d87 1128 {
jhedmonton 27:9ac59b261d87 1129 msg[i]=c;
maximbolduc 26:dc00998140af 1130 msg[i+1] = 0;
jhedmonton 27:9ac59b261d87 1131 start = false;
jhedmonton 27:9ac59b261d87 1132 end = true;
jhedmonton 27:9ac59b261d87 1133 break;
maximbolduc 26:dc00998140af 1134 }
maximbolduc 26:dc00998140af 1135 }
jhedmonton 27:9ac59b261d87 1136 return end;
maximbolduc 26:dc00998140af 1137 }
maximbolduc 26:dc00998140af 1138
jhedmonton 27:9ac59b261d87 1139
maximbolduc 26:dc00998140af 1140 int sample()
maximbolduc 26:dc00998140af 1141 {
maximbolduc 26:dc00998140af 1142 while (1)
maximbolduc 26:dc00998140af 1143 {
jhedmonton 27:9ac59b261d87 1144 getline(false);
maximbolduc 26:dc00998140af 1145 return 1;
maximbolduc 26:dc00998140af 1146 }
maximbolduc 26:dc00998140af 1147 }
maximbolduc 26:dc00998140af 1148
maximbolduc 26:dc00998140af 1149 int samplepc()
maximbolduc 26:dc00998140af 1150 {
maximbolduc 26:dc00998140af 1151 while (1)
maximbolduc 26:dc00998140af 1152 {
maximbolduc 26:dc00998140af 1153 getline2();
maximbolduc 26:dc00998140af 1154 return 1;
maximbolduc 26:dc00998140af 1155 }
maximbolduc 26:dc00998140af 1156 }
maximbolduc 26:dc00998140af 1157
maximbolduc 26:dc00998140af 1158 void keyPressedHeld( void )
maximbolduc 26:dc00998140af 1159 {
jhedmonton 28:5905886c76ee 1160 motor_enable_state = "$ENABLE,1\r\n";
jhedmonton 28:5905886c76ee 1161 motor_enable = 1;
jhedmonton 28:5905886c76ee 1162 ledGREEN=1; //show green for being ready to steer
maximbolduc 26:dc00998140af 1163 }
maximbolduc 26:dc00998140af 1164
maximbolduc 26:dc00998140af 1165 void keyReleasedHeld( void )
maximbolduc 26:dc00998140af 1166 {
jhedmonton 28:5905886c76ee 1167 motor_enable_state = "$ENABLE,0\r\n";
jhedmonton 28:5905886c76ee 1168 motor_enable = 0;
jhedmonton 28:5905886c76ee 1169 ledGREEN=0;
jhedmonton 27:9ac59b261d87 1170 }
jhedmonton 27:9ac59b261d87 1171
jhedmonton 27:9ac59b261d87 1172 void boom1PressedHeld( void )
jhedmonton 27:9ac59b261d87 1173 {
jhedmonton 27:9ac59b261d87 1174
jhedmonton 28:5905886c76ee 1175 // ledGREEN=1;
jhedmonton 27:9ac59b261d87 1176 boom18=boom18 & 0xFE;
jhedmonton 27:9ac59b261d87 1177 // sprintf(output,"Boom1 Pressed %d",boom18);
jhedmonton 27:9ac59b261d87 1178 // pc.puts(output);
jhedmonton 27:9ac59b261d87 1179 }
jhedmonton 27:9ac59b261d87 1180
jhedmonton 27:9ac59b261d87 1181 void boom1ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1182 {
jhedmonton 28:5905886c76ee 1183 //ledGREEN=0;
jhedmonton 27:9ac59b261d87 1184 boom18=boom18 | 0x01;
jhedmonton 27:9ac59b261d87 1185
jhedmonton 27:9ac59b261d87 1186 }
jhedmonton 27:9ac59b261d87 1187
jhedmonton 27:9ac59b261d87 1188 void boom2PressedHeld( void )
jhedmonton 27:9ac59b261d87 1189 {
jhedmonton 27:9ac59b261d87 1190 boom18=boom18 & 0xFD;
jhedmonton 27:9ac59b261d87 1191
maximbolduc 26:dc00998140af 1192 }
maximbolduc 26:dc00998140af 1193
jhedmonton 27:9ac59b261d87 1194 void boom2ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1195 {
jhedmonton 27:9ac59b261d87 1196
jhedmonton 27:9ac59b261d87 1197 boom18=boom18 | 0x02;
jhedmonton 27:9ac59b261d87 1198 }
jhedmonton 27:9ac59b261d87 1199 void boom3PressedHeld( void )
jhedmonton 27:9ac59b261d87 1200 {
jhedmonton 27:9ac59b261d87 1201
jhedmonton 27:9ac59b261d87 1202 boom18=boom18 & 0xFB;
jhedmonton 27:9ac59b261d87 1203 }
jhedmonton 27:9ac59b261d87 1204
jhedmonton 27:9ac59b261d87 1205 void boom3ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1206 {
jhedmonton 27:9ac59b261d87 1207
jhedmonton 27:9ac59b261d87 1208 boom18=boom18 | 0x04;
jhedmonton 27:9ac59b261d87 1209 }
jhedmonton 27:9ac59b261d87 1210
jhedmonton 27:9ac59b261d87 1211 void boom4PressedHeld( void )
jhedmonton 27:9ac59b261d87 1212 {
jhedmonton 27:9ac59b261d87 1213
jhedmonton 27:9ac59b261d87 1214 boom18=boom18 & 0xF7;
jhedmonton 27:9ac59b261d87 1215 }
jhedmonton 27:9ac59b261d87 1216
jhedmonton 27:9ac59b261d87 1217 void boom4ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1218 {
jhedmonton 27:9ac59b261d87 1219
jhedmonton 27:9ac59b261d87 1220 boom18=boom18 | 0x08;
jhedmonton 27:9ac59b261d87 1221 }
jhedmonton 27:9ac59b261d87 1222
jhedmonton 27:9ac59b261d87 1223
jhedmonton 27:9ac59b261d87 1224
maximbolduc 26:dc00998140af 1225 void toprint()
maximbolduc 26:dc00998140af 1226 {
maximbolduc 26:dc00998140af 1227 angle_send = 1;
maximbolduc 26:dc00998140af 1228 }
maximbolduc 26:dc00998140af 1229
maximbolduc 26:dc00998140af 1230 void debugging_leds( void )
maximbolduc 26:dc00998140af 1231 {
maximbolduc 26:dc00998140af 1232 }
maximbolduc 26:dc00998140af 1233
maximbolduc 26:dc00998140af 1234 int getCheckSum(char *string) {
maximbolduc 26:dc00998140af 1235 int i;
maximbolduc 26:dc00998140af 1236 int XOR;
maximbolduc 26:dc00998140af 1237 int c;
maximbolduc 26:dc00998140af 1238 // Calculate checksum ignoring any $'s in the string
maximbolduc 26:dc00998140af 1239 for (XOR = 0, i = 0; i < strlen(string); i++) {
maximbolduc 26:dc00998140af 1240 c = (unsigned char)string[i];
maximbolduc 26:dc00998140af 1241 if (c == '*') break;
maximbolduc 26:dc00998140af 1242 if (c != '$') XOR ^= c;
maximbolduc 26:dc00998140af 1243 }
maximbolduc 26:dc00998140af 1244 return XOR;
maximbolduc 26:dc00998140af 1245 }
maximbolduc 26:dc00998140af 1246
maximbolduc 26:dc00998140af 1247 int main()
maximbolduc 26:dc00998140af 1248 {
jhedmonton 27:9ac59b261d87 1249 int x=0;
jhedmonton 27:9ac59b261d87 1250 bluetooth.baud(115200);
jhedmonton 28:5905886c76ee 1251 gps.baud(38400);
maximbolduc 26:dc00998140af 1252 pc.baud(38400);
jhedmonton 27:9ac59b261d87 1253
jhedmonton 27:9ac59b261d87 1254 //JH prepare and send version info
jhedmonton 27:9ac59b261d87 1255 vTimer.start();
jhedmonton 27:9ac59b261d87 1256 vTimer.reset();
jhedmonton 28:5905886c76ee 1257 motTimer.start();
jhedmonton 28:5905886c76ee 1258 motTimer.reset();
jhedmonton 28:5905886c76ee 1259 btTimer.start();
jhedmonton 28:5905886c76ee 1260 btTimer.reset();
jhedmonton 28:5905886c76ee 1261 lastgetBT= btTimer.read_ms();
jhedmonton 28:5905886c76ee 1262
jhedmonton 27:9ac59b261d87 1263 pc.puts(version);
jhedmonton 27:9ac59b261d87 1264 bluetooth.puts(version);
jhedmonton 27:9ac59b261d87 1265 lastsend_version=vTimer.read_ms();
jhedmonton 27:9ac59b261d87 1266
jhedmonton 27:9ac59b261d87 1267
jhedmonton 27:9ac59b261d87 1268 boom1.attach_asserted_held( &boom1PressedHeld );
jhedmonton 27:9ac59b261d87 1269 boom1.attach_deasserted_held( &boom1ReleasedHeld );
jhedmonton 27:9ac59b261d87 1270 boom1.setSampleFrequency(); //default = 20 ms
jhedmonton 27:9ac59b261d87 1271 boom1.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1272 boom1.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1273 boom2.attach_asserted_held( &boom2PressedHeld );
jhedmonton 27:9ac59b261d87 1274 boom2.attach_deasserted_held( &boom2ReleasedHeld );
jhedmonton 27:9ac59b261d87 1275 boom2.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1276 boom2.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1277 boom2.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1278 boom3.attach_asserted_held( &boom3PressedHeld );
jhedmonton 27:9ac59b261d87 1279 boom3.attach_deasserted_held( &boom3ReleasedHeld );
jhedmonton 27:9ac59b261d87 1280 boom3.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1281 boom3.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1282 boom3.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1283 boom4.attach_asserted_held( &boom4PressedHeld );
jhedmonton 27:9ac59b261d87 1284 boom4.attach_deasserted_held( &boom4ReleasedHeld );
jhedmonton 27:9ac59b261d87 1285 boom4.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1286 boom4.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1287 boom4.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1288
jhedmonton 28:5905886c76ee 1289 Config_Startup();
jhedmonton 28:5905886c76ee 1290 // overide_config(gyro_position,gps_baud,antenna_height, a_zBias, a_yBias, a_zBias,w_xBias, w_yBias,w_zBias, copy_FREEPILOT);
jhedmonton 28:5905886c76ee 1291 // int _ID=Config_SetID();
jhedmonton 28:5905886c76ee 1292 // Config_Save(_ID,_btMode,phaseadv,tcenter,filter_gain,scale,avg_pos,gyro_position,gps_baud,antenna_height, a_zBias, a_yBias, a_zBias,w_xBias, w_yBias,w_zBias);
maximbolduc 26:dc00998140af 1293 motor_switch.setSampleFrequency(10000);
maximbolduc 26:dc00998140af 1294 motor_switch.attach_asserted_held( &keyPressedHeld );
maximbolduc 26:dc00998140af 1295 motor_switch.attach_deasserted_held( &keyReleasedHeld );
maximbolduc 26:dc00998140af 1296 initializeAccelerometer();
maximbolduc 26:dc00998140af 1297 calibrateAccelerometer();
maximbolduc 26:dc00998140af 1298 initializeGyroscope();
maximbolduc 26:dc00998140af 1299 calibrateGyroscope();
maximbolduc 26:dc00998140af 1300 accelerometerTicker.attach(&sampleAccelerometer, 0.005);
maximbolduc 26:dc00998140af 1301 //debug_leds.atatch(&debugging_leds,2);
maximbolduc 26:dc00998140af 1302 gyroscopeTicker.attach(&sampleGyroscope, 0.005);
maximbolduc 26:dc00998140af 1303 filterTicker.attach(&filter, FILTER_RATE);
maximbolduc 26:dc00998140af 1304 angle_print.attach(&toprint,0.2);
maximbolduc 26:dc00998140af 1305 activate_antenna();
maximbolduc 26:dc00998140af 1306 while(1)
maximbolduc 26:dc00998140af 1307 {
jhedmonton 27:9ac59b261d87 1308
jhedmonton 27:9ac59b261d87 1309 //JH send version information every 10 seconds to keep Bluetooth alive
jhedmonton 28:5905886c76ee 1310 if ((vTimer.read_ms()-lastsend_version)>15000)
maximbolduc 26:dc00998140af 1311 {
jhedmonton 27:9ac59b261d87 1312
jhedmonton 27:9ac59b261d87 1313 pc.puts(version);
jhedmonton 27:9ac59b261d87 1314 bluetooth.puts(version);
jhedmonton 27:9ac59b261d87 1315 vTimer.reset();
jhedmonton 27:9ac59b261d87 1316 lastsend_version=vTimer.read_ms();
jhedmonton 27:9ac59b261d87 1317 // sprintf(output,"GPS Baudrate %d \r\n",gps_baud);
jhedmonton 27:9ac59b261d87 1318 // pc.puts(output);
jhedmonton 27:9ac59b261d87 1319 // sprintf(output,"Boom18 %d \r\n",boom18);
jhedmonton 27:9ac59b261d87 1320 // pc.puts(output);
jhedmonton 27:9ac59b261d87 1321 }
jhedmonton 27:9ac59b261d87 1322
jhedmonton 28:5905886c76ee 1323 if ( antenna_active == 1 && gps.readable())
jhedmonton 27:9ac59b261d87 1324 {
jhedmonton 27:9ac59b261d87 1325 if (getline(true))
maximbolduc 26:dc00998140af 1326 {
jhedmonton 27:9ac59b261d87 1327 // checksumm = getCheckSum(msg);
jhedmonton 27:9ac59b261d87 1328 // gps_analyse(msg);
maximbolduc 26:dc00998140af 1329 }
maximbolduc 26:dc00998140af 1330 }
maximbolduc 26:dc00998140af 1331
jhedmonton 27:9ac59b261d87 1332
maximbolduc 26:dc00998140af 1333 if ( bluetooth.readable())
maximbolduc 26:dc00998140af 1334 {
jhedmonton 27:9ac59b261d87 1335 if (getline2())
jhedmonton 27:9ac59b261d87 1336 {
jhedmonton 28:5905886c76ee 1337 btTimer.reset();
jhedmonton 28:5905886c76ee 1338 lastgetBT= btTimer.read_ms();
jhedmonton 27:9ac59b261d87 1339 x++;
jhedmonton 27:9ac59b261d87 1340 // trim(msg2,"\r");
jhedmonton 27:9ac59b261d87 1341 // trim(msg2,"\n");
jhedmonton 27:9ac59b261d87 1342 // trim(msg2,"\0");
maximbolduc 26:dc00998140af 1343 trim(msg2," ");
jhedmonton 27:9ac59b261d87 1344 sprintf(output,"%d %s",x,msg2);
maximbolduc 26:dc00998140af 1345 pc.puts(output);
jhedmonton 27:9ac59b261d87 1346 //pc.puts(output);
maximbolduc 26:dc00998140af 1347 pc_analyse(msg2);
jhedmonton 27:9ac59b261d87 1348
maximbolduc 26:dc00998140af 1349 }
maximbolduc 26:dc00998140af 1350 }
jhedmonton 27:9ac59b261d87 1351
jhedmonton 28:5905886c76ee 1352 if ( btTimer.read_ms()-lastgetBT>1000)
jhedmonton 28:5905886c76ee 1353 {
jhedmonton 28:5905886c76ee 1354 //we did not get any commands over BT
jhedmonton 28:5905886c76ee 1355 ledRED=1; //turn red
jhedmonton 28:5905886c76ee 1356 }
jhedmonton 28:5905886c76ee 1357 else ledRED=0;
jhedmonton 28:5905886c76ee 1358
jhedmonton 28:5905886c76ee 1359 if ( ((motTimer.read_ms()-lastsend_motorstate)>5000) || (motor_enable!=lastmotor_enable))
maximbolduc 26:dc00998140af 1360 {
jhedmonton 28:5905886c76ee 1361
maximbolduc 26:dc00998140af 1362 bluetooth.puts(motor_enable_state);
jhedmonton 28:5905886c76ee 1363 pc.puts(motor_enable_state);
jhedmonton 28:5905886c76ee 1364 motTimer.reset();
jhedmonton 28:5905886c76ee 1365 lastsend_motorstate=motTimer.read_ms();
jhedmonton 28:5905886c76ee 1366 lastmotor_enable=motor_enable;
maximbolduc 26:dc00998140af 1367 }
jhedmonton 27:9ac59b261d87 1368
jhedmonton 27:9ac59b261d87 1369 //bounces too much
jhedmonton 27:9ac59b261d87 1370 //if (boom1) boom18=boom18 & 0xFE;
jhedmonton 27:9ac59b261d87 1371 //else boom18=boom18 | 0x01;
jhedmonton 27:9ac59b261d87 1372
jhedmonton 27:9ac59b261d87 1373 if (boom18!=lastboom18)
jhedmonton 27:9ac59b261d87 1374 {
jhedmonton 27:9ac59b261d87 1375 // sprintf(output,"Change boom18= %d\r\n",boom18);
jhedmonton 27:9ac59b261d87 1376 // pc.puts(output);
jhedmonton 27:9ac59b261d87 1377
jhedmonton 27:9ac59b261d87 1378 boomstate[4]=boom18 | 0x80; //
jhedmonton 27:9ac59b261d87 1379 bluetooth.puts(boomstate);
jhedmonton 27:9ac59b261d87 1380 pc.puts(boomstate);
jhedmonton 27:9ac59b261d87 1381 lastboom18=boom18;
jhedmonton 27:9ac59b261d87 1382 }
jhedmonton 27:9ac59b261d87 1383
jhedmonton 27:9ac59b261d87 1384 /* if ( print_euler == 1 && angle_send == 1 ) //&& reading == 0)
maximbolduc 26:dc00998140af 1385 {
maximbolduc 26:dc00998140af 1386 sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),toDegrees(imuFilter.getYaw()));
maximbolduc 26:dc00998140af 1387 pc.puts(output);
maximbolduc 26:dc00998140af 1388 bluetooth.puts(output);
maximbolduc 26:dc00998140af 1389 angle_send = 0;
jhedmonton 27:9ac59b261d87 1390 }
jhedmonton 27:9ac59b261d87 1391 */
maximbolduc 26:dc00998140af 1392 }
maximbolduc 26:dc00998140af 1393 }