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@31:c40f16ff3a2f, 2015-01-16 (annotated)
- Committer:
- maximbolduc
- Date:
- Fri Jan 16 20:56:11 2015 +0000
- Revision:
- 31:c40f16ff3a2f
- Parent:
- 26:dc00998140af
v2.2;
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| maximbolduc | 26:dc00998140af | 1 | #include "mbed.h" |
| maximbolduc | 26:dc00998140af | 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" |
| maximbolduc | 26:dc00998140af | 11 | |
| maximbolduc | 31:c40f16ff3a2f | 12 | FILE * fp; |
| maximbolduc | 31:c40f16ff3a2f | 13 | LocalFileSystem local("local"); // Create the local filesystem under the name "local" |
| maximbolduc | 26:dc00998140af | 14 | // Connect the TX of the GPS module to p10 RX input |
| maximbolduc | 26:dc00998140af | 15 | MODSERIAL b(p9, p10); |
| maximbolduc | 26:dc00998140af | 16 | MODSERIAL pc(USBTX, USBRX); |
| maximbolduc | 26:dc00998140af | 17 | MODSERIAL bluetooth(p13, p14); |
| maximbolduc | 26:dc00998140af | 18 | int checksumm; |
| maximbolduc | 26:dc00998140af | 19 | int dont = 0; |
| maximbolduc | 26:dc00998140af | 20 | #define g0 9.812865328//Gravity at Earth's surface in m/s/s |
| maximbolduc | 26:dc00998140af | 21 | #define SAMPLES 8//Number of samples to average. |
| maximbolduc | 26:dc00998140af | 22 | #define CALIBRATION_SAMPLES 256//Number of samples to be averaged for a null bias calculation during calibration. |
| maximbolduc | 26:dc00998140af | 23 | #define toDegrees(x) (x * 57.2957795)//Convert from radians to degrees. |
| maximbolduc | 26:dc00998140af | 24 | #define toRadians(x) (x * 0.01745329252)//Convert from degrees to radians. |
| maximbolduc | 26:dc00998140af | 25 | #define GYROSCOPE_GAIN (1 / 14.375)//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). |
| maximbolduc | 26:dc00998140af | 26 | #define ACCELEROMETER_GAIN (0.004 * g0)//Full scale resolution on the ADXL345 is 4mg/LSB. |
| maximbolduc | 26:dc00998140af | 27 | #define GYRO_RATE 0.005//Sampling gyroscope at 200Hz. |
| maximbolduc | 26:dc00998140af | 28 | #define ACC_RATE 0.005//Sampling accelerometer at 200Hz. |
| maximbolduc | 26:dc00998140af | 29 | #define FILTER_RATE 0.1//Updating filter at 40Hz. |
| maximbolduc | 26:dc00998140af | 30 | double distance_from_line; |
| maximbolduc | 26:dc00998140af | 31 | double cm_per_deg_lon; |
| maximbolduc | 26:dc00998140af | 32 | double cm_per_deg_lat; |
| maximbolduc | 26:dc00998140af | 33 | //all timing objects |
| maximbolduc | 26:dc00998140af | 34 | Timer gps_connecting; |
| maximbolduc | 26:dc00998140af | 35 | Timer autosteer_time; |
| maximbolduc | 26:dc00998140af | 36 | 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 | 37 | Ticker accelerometerTicker; |
| maximbolduc | 26:dc00998140af | 38 | Ticker gyroscopeTicker; |
| maximbolduc | 26:dc00998140af | 39 | Ticker filterTicker; |
| maximbolduc | 26:dc00998140af | 40 | Ticker angle_print; |
| maximbolduc | 26:dc00998140af | 41 | Ticker debug_leds; |
| maximbolduc | 26:dc00998140af | 42 | |
| maximbolduc | 26:dc00998140af | 43 | PinDetect motor_switch(p24); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce. |
| maximbolduc | 26:dc00998140af | 44 | DigitalOut enable_motor(p21); |
| maximbolduc | 26:dc00998140af | 45 | DigitalOut led1(p11); |
| maximbolduc | 26:dc00998140af | 46 | DigitalOut led2(p12); |
| maximbolduc | 26:dc00998140af | 47 | PwmOut pwm1(p22); |
| maximbolduc | 26:dc00998140af | 48 | PwmOut pwm2(p23); |
| maximbolduc | 26:dc00998140af | 49 | |
| maximbolduc | 26:dc00998140af | 50 | IMUfilter imuFilter(FILTER_RATE, 0.3); |
| maximbolduc | 26:dc00998140af | 51 | ADXL345_I2C accelerometer(p28, p27); |
| maximbolduc | 26:dc00998140af | 52 | ITG3200 gyroscope(p28,p27); |
| maximbolduc | 26:dc00998140af | 53 | |
| maximbolduc | 26:dc00998140af | 54 | Point position; |
| maximbolduc | 26:dc00998140af | 55 | Point looked_ahead; |
| maximbolduc | 26:dc00998140af | 56 | Point line_start; |
| maximbolduc | 26:dc00998140af | 57 | Point line_end; |
| maximbolduc | 26:dc00998140af | 58 | Point tilt_compensated_position; |
| maximbolduc | 26:dc00998140af | 59 | Point yaw_compensated_position; |
| maximbolduc | 26:dc00998140af | 60 | |
| maximbolduc | 26:dc00998140af | 61 | double distance_to_line; |
| maximbolduc | 26:dc00998140af | 62 | //FreePilot parameters |
| maximbolduc | 26:dc00998140af | 63 | double look_ahead_time = 2; |
| maximbolduc | 26:dc00998140af | 64 | double look_ahead_distance = 5; |
| maximbolduc | 26:dc00998140af | 65 | double scale = 1; |
| maximbolduc | 26:dc00998140af | 66 | double phaseadv = 50; |
| maximbolduc | 26:dc00998140af | 67 | double _Tcenter = 5; |
| maximbolduc | 26:dc00998140af | 68 | double filter_gain = 125; |
| maximbolduc | 26:dc00998140af | 69 | double avg_pos = -4; |
| maximbolduc | 26:dc00998140af | 70 | |
| maximbolduc | 26:dc00998140af | 71 | //FreePilot variables |
| maximbolduc | 26:dc00998140af | 72 | int timer_enabled; |
| maximbolduc | 26:dc00998140af | 73 | int motorspeed; |
| maximbolduc | 26:dc00998140af | 74 | int enable_time; |
| maximbolduc | 26:dc00998140af | 75 | char* motor_enable_state = 0; |
| maximbolduc | 26:dc00998140af | 76 | int motor_enable_tosend = 0; |
| maximbolduc | 26:dc00998140af | 77 | double pwm1_speed; |
| maximbolduc | 26:dc00998140af | 78 | double pwm2_speed; |
| maximbolduc | 26:dc00998140af | 79 | |
| maximbolduc | 26:dc00998140af | 80 | // in prevision of PID addition to FreePilot |
| maximbolduc | 26:dc00998140af | 81 | double kp = 0; |
| maximbolduc | 26:dc00998140af | 82 | double ki = 0; |
| maximbolduc | 26:dc00998140af | 83 | double kd = 0; |
| maximbolduc | 26:dc00998140af | 84 | |
| maximbolduc | 26:dc00998140af | 85 | int msg2_changed = 1; |
| maximbolduc | 26:dc00998140af | 86 | char* buffer; |
| maximbolduc | 26:dc00998140af | 87 | double meter_lat = 0; |
| maximbolduc | 26:dc00998140af | 88 | double meter_lon = 0; |
| maximbolduc | 26:dc00998140af | 89 | double antenna_height = 200; |
| maximbolduc | 26:dc00998140af | 90 | int antenna_active = 0;//do we have an antenna connected? |
| maximbolduc | 26:dc00998140af | 91 | char msg[256]; //GPS line buffer |
| maximbolduc | 26:dc00998140af | 92 | char msg2[256];//PC line buffer |
| maximbolduc | 26:dc00998140af | 93 | int printing; |
| maximbolduc | 26:dc00998140af | 94 | int num_of_gps_sats; |
| maximbolduc | 26:dc00998140af | 95 | int print_vtg = 0; //FGPS asked for VTG? |
| maximbolduc | 26:dc00998140af | 96 | double decimal_lon; |
| maximbolduc | 26:dc00998140af | 97 | float longitude; |
| maximbolduc | 26:dc00998140af | 98 | float latitude; |
| maximbolduc | 26:dc00998140af | 99 | char ns, ew; |
| maximbolduc | 26:dc00998140af | 100 | int lock; |
| maximbolduc | 26:dc00998140af | 101 | int flag_gga; |
| maximbolduc | 26:dc00998140af | 102 | int reading; |
| maximbolduc | 26:dc00998140af | 103 | double decimal_latitude; |
| maximbolduc | 26:dc00998140af | 104 | int gps_satellite_quality; |
| maximbolduc | 26:dc00998140af | 105 | int day; |
| maximbolduc | 26:dc00998140af | 106 | int hour; |
| maximbolduc | 26:dc00998140af | 107 | int minute; |
| maximbolduc | 26:dc00998140af | 108 | int second; |
| maximbolduc | 26:dc00998140af | 109 | int tenths; |
| maximbolduc | 26:dc00998140af | 110 | int hundreths; |
| maximbolduc | 26:dc00998140af | 111 | char status; |
| maximbolduc | 26:dc00998140af | 112 | double track; // track made good . angle |
| maximbolduc | 26:dc00998140af | 113 | char magvar_dir; |
| maximbolduc | 26:dc00998140af | 114 | double magvar; |
| maximbolduc | 26:dc00998140af | 115 | int year; |
| maximbolduc | 26:dc00998140af | 116 | int month; |
| maximbolduc | 26:dc00998140af | 117 | double speed_km; |
| maximbolduc | 26:dc00998140af | 118 | double speed_m_s = 0; |
| maximbolduc | 26:dc00998140af | 119 | double velocity; // speed in knot |
| maximbolduc | 26:dc00998140af | 120 | int gps_baud = 38400; //default at 115200, but FGPS will pass the real baud-rate. |
| maximbolduc | 26:dc00998140af | 121 | 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 | 122 | int connecting = 0; //are we still in phase of connecting? based on the connect_time value. |
| maximbolduc | 26:dc00998140af | 123 | int print_gsa = 0;//FGPS request GSA printing |
| maximbolduc | 26:dc00998140af | 124 | int print_gsv = 1;//FGPS request GSV printing. |
| maximbolduc | 26:dc00998140af | 125 | int angle_send = 0; |
| maximbolduc | 26:dc00998140af | 126 | int correct_rmc = 1; |
| maximbolduc | 26:dc00998140af | 127 | double m_lat = 0; |
| maximbolduc | 26:dc00998140af | 128 | double m_lon = 0; |
| maximbolduc | 26:dc00998140af | 129 | char* degminsec; |
| maximbolduc | 26:dc00998140af | 130 | double m_per_deg_lon; |
| maximbolduc | 26:dc00998140af | 131 | double m_per_deg_lat; |
| maximbolduc | 26:dc00998140af | 132 | double look_ahead_lon; |
| maximbolduc | 26:dc00998140af | 133 | double look_ahead_lat; |
| maximbolduc | 26:dc00998140af | 134 | int active_AB = 0; |
| maximbolduc | 26:dc00998140af | 135 | double compensation_vector; |
| maximbolduc | 26:dc00998140af | 136 | char output[256]; |
| maximbolduc | 26:dc00998140af | 137 | //offsets |
| maximbolduc | 26:dc00998140af | 138 | double w_xBias; |
| maximbolduc | 26:dc00998140af | 139 | double w_yBias; |
| maximbolduc | 26:dc00998140af | 140 | double w_zBias; |
| maximbolduc | 26:dc00998140af | 141 | double a_xBias; |
| maximbolduc | 26:dc00998140af | 142 | double a_yBias; |
| maximbolduc | 26:dc00998140af | 143 | double a_zBias; |
| maximbolduc | 26:dc00998140af | 144 | |
| maximbolduc | 26:dc00998140af | 145 | double yaw; |
| maximbolduc | 26:dc00998140af | 146 | double pitch; |
| maximbolduc | 26:dc00998140af | 147 | double roll; |
| maximbolduc | 26:dc00998140af | 148 | |
| maximbolduc | 26:dc00998140af | 149 | volatile double a_xAccumulator = 0; |
| maximbolduc | 26:dc00998140af | 150 | volatile double a_yAccumulator = 0; |
| maximbolduc | 26:dc00998140af | 151 | volatile double a_zAccumulator = 0; |
| maximbolduc | 26:dc00998140af | 152 | volatile double w_xAccumulator = 0; |
| maximbolduc | 26:dc00998140af | 153 | volatile double w_yAccumulator = 0; |
| maximbolduc | 26:dc00998140af | 154 | volatile double w_zAccumulator = 0; |
| maximbolduc | 26:dc00998140af | 155 | |
| maximbolduc | 26:dc00998140af | 156 | volatile double a_x; |
| maximbolduc | 26:dc00998140af | 157 | volatile double a_y; |
| maximbolduc | 26:dc00998140af | 158 | volatile double a_z; |
| maximbolduc | 26:dc00998140af | 159 | volatile double w_x; |
| maximbolduc | 26:dc00998140af | 160 | volatile double w_y; |
| maximbolduc | 26:dc00998140af | 161 | volatile double w_z; |
| maximbolduc | 26:dc00998140af | 162 | |
| maximbolduc | 26:dc00998140af | 163 | int readings[3]; |
| maximbolduc | 26:dc00998140af | 164 | int accelerometerSamples = 0; |
| maximbolduc | 26:dc00998140af | 165 | int gyroscopeSamples = 0; |
| maximbolduc | 26:dc00998140af | 166 | int print_euler = 1; |
| maximbolduc | 26:dc00998140af | 167 | |
| maximbolduc | 26:dc00998140af | 168 | double Freepilot_lat; |
| maximbolduc | 26:dc00998140af | 169 | double Freepilot_lon; |
| maximbolduc | 26:dc00998140af | 170 | double Freepilot_lat1; |
| maximbolduc | 26:dc00998140af | 171 | double Freepilot_lon1; |
| maximbolduc | 26:dc00998140af | 172 | double Freepilot_bearing; |
| maximbolduc | 26:dc00998140af | 173 | //double Freepilot_lon_meter; |
| maximbolduc | 26:dc00998140af | 174 | //double Freepilot_lat_meter; |
| maximbolduc | 26:dc00998140af | 175 | |
| maximbolduc | 26:dc00998140af | 176 | void initializeAcceleromter(void); |
| maximbolduc | 26:dc00998140af | 177 | void calibrateAccelerometer(void); |
| maximbolduc | 26:dc00998140af | 178 | void sampleAccelerometer(void); |
| maximbolduc | 26:dc00998140af | 179 | void initializeGyroscope(void); |
| maximbolduc | 26:dc00998140af | 180 | void calibrateGyroscope(void); |
| maximbolduc | 26:dc00998140af | 181 | void sampleGyroscope(void); |
| maximbolduc | 26:dc00998140af | 182 | void filter(void); |
| maximbolduc | 26:dc00998140af | 183 | |
| maximbolduc | 26:dc00998140af | 184 | volatile bool newline_detected = false; |
| maximbolduc | 26:dc00998140af | 185 | char tx_line[80]; |
| maximbolduc | 26:dc00998140af | 186 | char rx_line[80]; |
| maximbolduc | 26:dc00998140af | 187 | |
| maximbolduc | 26:dc00998140af | 188 | Point point_add(Point a, Point b) |
| maximbolduc | 26:dc00998140af | 189 | { |
| maximbolduc | 26:dc00998140af | 190 | return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY()); |
| maximbolduc | 26:dc00998140af | 191 | } |
| maximbolduc | 26:dc00998140af | 192 | |
| maximbolduc | 26:dc00998140af | 193 | Point point_sub(Point a , Point b) |
| maximbolduc | 26:dc00998140af | 194 | { |
| maximbolduc | 26:dc00998140af | 195 | return Point(a.GetX() - b.GetX(), a.GetY() - b.GetY()); |
| maximbolduc | 26:dc00998140af | 196 | } |
| maximbolduc | 26:dc00998140af | 197 | |
| maximbolduc | 31:c40f16ff3a2f | 198 | //defining vectore opperation for distance to line |
| 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 | { |
| maximbolduc | 31:c40f16ff3a2f | 351 | b.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(); |
| maximbolduc | 26:dc00998140af | 508 | compensation_vector = antenna_height * 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 | { |
| maximbolduc | 26:dc00998140af | 544 | antenna_height = 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; |
| maximbolduc | 26:dc00998140af | 554 | token = strtok(cs_string, "*"); |
| maximbolduc | 26:dc00998140af | 555 | while (token) |
| maximbolduc | 26:dc00998140af | 556 | { |
| maximbolduc | 26:dc00998140af | 557 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 558 | { |
| maximbolduc | 26:dc00998140af | 559 | case 1: |
| maximbolduc | 26:dc00998140af | 560 | sprintf(output, "$cs:%s , %02X\r\n",token, checksumm); |
| maximbolduc | 26:dc00998140af | 561 | pc.puts(token); |
| maximbolduc | 26:dc00998140af | 562 | break; |
| maximbolduc | 26:dc00998140af | 563 | } |
| maximbolduc | 26:dc00998140af | 564 | } |
| maximbolduc | 26:dc00998140af | 565 | } |
| maximbolduc | 26:dc00998140af | 566 | |
| maximbolduc | 26:dc00998140af | 567 | void nmea_rmc(char *s) |
| maximbolduc | 26:dc00998140af | 568 | { |
| maximbolduc | 26:dc00998140af | 569 | char *token; |
| maximbolduc | 26:dc00998140af | 570 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 571 | char *time = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 572 | char *date = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 573 | char *stat = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 574 | char *vel = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 575 | char *trk = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 576 | char *magv = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 577 | char *magd = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 578 | char *latitude = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 579 | char *longitude = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 580 | char *lat_dir = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 581 | char *lon_dir = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 582 | |
| maximbolduc | 26:dc00998140af | 583 | token = strtok(s, ",*"); |
| maximbolduc | 26:dc00998140af | 584 | while (token) |
| maximbolduc | 26:dc00998140af | 585 | { |
| maximbolduc | 26:dc00998140af | 586 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 587 | { |
| maximbolduc | 26:dc00998140af | 588 | case 9: |
| maximbolduc | 26:dc00998140af | 589 | date = token; |
| maximbolduc | 26:dc00998140af | 590 | break; |
| maximbolduc | 26:dc00998140af | 591 | case 1: |
| maximbolduc | 26:dc00998140af | 592 | time = token; |
| maximbolduc | 26:dc00998140af | 593 | break; |
| maximbolduc | 26:dc00998140af | 594 | case 2: |
| maximbolduc | 26:dc00998140af | 595 | stat = token; |
| maximbolduc | 26:dc00998140af | 596 | break; |
| maximbolduc | 26:dc00998140af | 597 | case 7: |
| maximbolduc | 26:dc00998140af | 598 | vel = token; |
| maximbolduc | 26:dc00998140af | 599 | break; |
| maximbolduc | 26:dc00998140af | 600 | case 8: |
| maximbolduc | 26:dc00998140af | 601 | trk = token; |
| maximbolduc | 26:dc00998140af | 602 | break; |
| maximbolduc | 26:dc00998140af | 603 | case 10: |
| maximbolduc | 26:dc00998140af | 604 | magv = token; |
| maximbolduc | 26:dc00998140af | 605 | break; |
| maximbolduc | 26:dc00998140af | 606 | case 11: |
| maximbolduc | 26:dc00998140af | 607 | magd = token; |
| maximbolduc | 26:dc00998140af | 608 | break; |
| maximbolduc | 26:dc00998140af | 609 | case 3: |
| maximbolduc | 26:dc00998140af | 610 | latitude = token; |
| maximbolduc | 26:dc00998140af | 611 | break; |
| maximbolduc | 26:dc00998140af | 612 | case 5: |
| maximbolduc | 26:dc00998140af | 613 | longitude = token; |
| maximbolduc | 26:dc00998140af | 614 | break; |
| maximbolduc | 26:dc00998140af | 615 | case 4: |
| maximbolduc | 26:dc00998140af | 616 | lat_dir = token; |
| maximbolduc | 26:dc00998140af | 617 | break; |
| maximbolduc | 26:dc00998140af | 618 | case 6: |
| maximbolduc | 26:dc00998140af | 619 | lon_dir = token; |
| maximbolduc | 26:dc00998140af | 620 | break; |
| maximbolduc | 26:dc00998140af | 621 | /* case 11: |
| maximbolduc | 26:dc00998140af | 622 | process_cs(token);*/ |
| maximbolduc | 26:dc00998140af | 623 | } |
| maximbolduc | 26:dc00998140af | 624 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 625 | token_counter++; |
| maximbolduc | 26:dc00998140af | 626 | } |
| maximbolduc | 26:dc00998140af | 627 | if (stat && date && time) |
| maximbolduc | 26:dc00998140af | 628 | { |
| maximbolduc | 26:dc00998140af | 629 | hour = (char)((time[0] - '0') * 10) + (time[1] - '0'); |
| maximbolduc | 26:dc00998140af | 630 | minute = (char)((time[2] - '0') * 10) + (time[3] - '0'); |
| maximbolduc | 26:dc00998140af | 631 | second = (char)((time[4] - '0') * 10) + (time[5] - '0'); |
| maximbolduc | 26:dc00998140af | 632 | day = (char)((date[0] - '0') * 10) + (date[1] - '0'); |
| maximbolduc | 26:dc00998140af | 633 | month = (char)((date[2] - '0') * 10) + (date[3] - '0'); |
| maximbolduc | 26:dc00998140af | 634 | year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000; |
| maximbolduc | 26:dc00998140af | 635 | status = stat[0]; |
| maximbolduc | 26:dc00998140af | 636 | velocity = atof(vel); |
| maximbolduc | 26:dc00998140af | 637 | speed_km = velocity * 1.852; |
| maximbolduc | 26:dc00998140af | 638 | speed_m_s = speed_km * 3600.0 / 1000.0; |
| maximbolduc | 26:dc00998140af | 639 | //speed_m_s = 5; |
| maximbolduc | 26:dc00998140af | 640 | track = atof(trk); |
| maximbolduc | 26:dc00998140af | 641 | magvar = atof(magv); |
| maximbolduc | 26:dc00998140af | 642 | magvar_dir = magd[0]; |
| maximbolduc | 26:dc00998140af | 643 | } |
| maximbolduc | 26:dc00998140af | 644 | decimal_latitude = lat_to_deg(latitude, lat_dir[0]); |
| maximbolduc | 26:dc00998140af | 645 | decimal_lon = lon_to_deg(longitude, lon_dir[0]); |
| maximbolduc | 26:dc00998140af | 646 | position.SetX(decimal_latitude); |
| maximbolduc | 26:dc00998140af | 647 | position.SetY(decimal_lon); |
| maximbolduc | 26:dc00998140af | 648 | cm_per_deg_lat = 111111; |
| maximbolduc | 26:dc00998140af | 649 | cm_per_deg_lon = 111111 * cos(decimal_latitude); |
| maximbolduc | 26:dc00998140af | 650 | tilt_compensate(); //in centimeters |
| maximbolduc | 26:dc00998140af | 651 | compensation.SetY(compensation.GetY() / cm_per_deg_lon); |
| maximbolduc | 26:dc00998140af | 652 | compensation.SetX(compensation.GetX() / cm_per_deg_lat); |
| maximbolduc | 26:dc00998140af | 653 | |
| maximbolduc | 26:dc00998140af | 654 | // yaw_compensate(); |
| maximbolduc | 26:dc00998140af | 655 | position = point_add(position,compensation); |
| maximbolduc | 26:dc00998140af | 656 | modify_rmc(); |
| maximbolduc | 26:dc00998140af | 657 | look_ahead_distance = look_ahead_time * speed_m_s; |
| maximbolduc | 26:dc00998140af | 658 | |
| maximbolduc | 26:dc00998140af | 659 | get_latlon_byangle(position.GetX(),position.GetY(),look_ahead_distance,track,look_ahead_lon,look_ahead_lat); |
| maximbolduc | 26:dc00998140af | 660 | looked_ahead.SetX(look_ahead_lat); |
| maximbolduc | 26:dc00998140af | 661 | looked_ahead.SetY(look_ahead_lon); |
| maximbolduc | 26:dc00998140af | 662 | double filtering = 111.111 / 2.0 + 111.111 * cos(decimal_latitude)/2.0; |
| maximbolduc | 26:dc00998140af | 663 | distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end); |
| maximbolduc | 26:dc00998140af | 664 | |
| maximbolduc | 26:dc00998140af | 665 | sprintf(output,"$DIST_TO_LINE: % .12f\r\n\0",distance_to_line * filtering); |
| maximbolduc | 26:dc00998140af | 666 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 667 | |
| maximbolduc | 26:dc00998140af | 668 | } |
| maximbolduc | 26:dc00998140af | 669 | |
| maximbolduc | 26:dc00998140af | 670 | void process_FGPSAB(char* ab) |
| maximbolduc | 26:dc00998140af | 671 | { |
| maximbolduc | 26:dc00998140af | 672 | char *token; |
| maximbolduc | 26:dc00998140af | 673 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 674 | char *line_lat = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 675 | char *line_lon = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 676 | char *line_lat1 = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 677 | char *line_lon1 = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 678 | char *bearing = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 679 | token = strtok(ab, ","); |
| maximbolduc | 26:dc00998140af | 680 | while (token) |
| maximbolduc | 26:dc00998140af | 681 | { |
| maximbolduc | 26:dc00998140af | 682 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 683 | { |
| maximbolduc | 26:dc00998140af | 684 | case 1: |
| maximbolduc | 26:dc00998140af | 685 | line_lat = token; |
| maximbolduc | 26:dc00998140af | 686 | break; |
| maximbolduc | 26:dc00998140af | 687 | case 2: |
| maximbolduc | 26:dc00998140af | 688 | line_lon = token; |
| maximbolduc | 26:dc00998140af | 689 | break; |
| maximbolduc | 26:dc00998140af | 690 | case 3: |
| maximbolduc | 26:dc00998140af | 691 | line_lat1 = token; |
| maximbolduc | 26:dc00998140af | 692 | break; |
| maximbolduc | 26:dc00998140af | 693 | case 4: |
| maximbolduc | 26:dc00998140af | 694 | line_lon1 = token; |
| maximbolduc | 26:dc00998140af | 695 | break; |
| maximbolduc | 26:dc00998140af | 696 | case 5: |
| maximbolduc | 26:dc00998140af | 697 | bearing = token; |
| maximbolduc | 26:dc00998140af | 698 | break; |
| maximbolduc | 26:dc00998140af | 699 | } |
| maximbolduc | 26:dc00998140af | 700 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 701 | token_counter++; |
| maximbolduc | 26:dc00998140af | 702 | } |
| maximbolduc | 26:dc00998140af | 703 | Freepilot_lon = atof(line_lon); |
| maximbolduc | 26:dc00998140af | 704 | Freepilot_lat = atof(line_lat); |
| maximbolduc | 26:dc00998140af | 705 | Freepilot_lon1 = atof(line_lon1); |
| maximbolduc | 26:dc00998140af | 706 | Freepilot_lat1 = atof(line_lat1); |
| maximbolduc | 26:dc00998140af | 707 | Freepilot_bearing = atof(bearing); |
| maximbolduc | 26:dc00998140af | 708 | line_start.SetX(Freepilot_lat); |
| maximbolduc | 26:dc00998140af | 709 | line_start.SetY(Freepilot_lon); |
| maximbolduc | 26:dc00998140af | 710 | line_end.SetX(Freepilot_lat1); |
| maximbolduc | 26:dc00998140af | 711 | line_end.SetY(Freepilot_lon1); |
| maximbolduc | 26:dc00998140af | 712 | active_AB = 1; |
| maximbolduc | 26:dc00998140af | 713 | |
| maximbolduc | 26:dc00998140af | 714 | sprintf(output, "$ABLINE:%f , %f, %f, %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY()); |
| maximbolduc | 26:dc00998140af | 715 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 716 | } |
| maximbolduc | 26:dc00998140af | 717 | |
| maximbolduc | 26:dc00998140af | 718 | void autosteer_done() |
| maximbolduc | 26:dc00998140af | 719 | { |
| maximbolduc | 26:dc00998140af | 720 | //kill the autosteer once the timeout reech |
| maximbolduc | 26:dc00998140af | 721 | enable_motor = 0; |
| maximbolduc | 26:dc00998140af | 722 | } |
| maximbolduc | 26:dc00998140af | 723 | |
| maximbolduc | 26:dc00998140af | 724 | void process_FGPSAUTO(char* FGPSAUTO) |
| maximbolduc | 26:dc00998140af | 725 | { |
| maximbolduc | 26:dc00998140af | 726 | char *token; |
| maximbolduc | 26:dc00998140af | 727 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 728 | char *ahead = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 729 | char *center = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 730 | char *phase = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 731 | char *scl = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 732 | char *avg = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 733 | char *_kp = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 734 | char *_ki = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 735 | char *_kd = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 736 | |
| maximbolduc | 26:dc00998140af | 737 | token = strtok(FGPSAUTO, ","); |
| maximbolduc | 26:dc00998140af | 738 | while (token) |
| maximbolduc | 26:dc00998140af | 739 | { |
| maximbolduc | 26:dc00998140af | 740 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 741 | { |
| maximbolduc | 26:dc00998140af | 742 | case 1: |
| maximbolduc | 26:dc00998140af | 743 | phase = token; |
| maximbolduc | 26:dc00998140af | 744 | break; |
| maximbolduc | 26:dc00998140af | 745 | case 2: |
| maximbolduc | 26:dc00998140af | 746 | center = token; |
| maximbolduc | 26:dc00998140af | 747 | break; |
| maximbolduc | 26:dc00998140af | 748 | case 4: |
| maximbolduc | 26:dc00998140af | 749 | scl = token; |
| maximbolduc | 26:dc00998140af | 750 | break; |
| maximbolduc | 26:dc00998140af | 751 | case 5: |
| maximbolduc | 26:dc00998140af | 752 | ahead = token; |
| maximbolduc | 26:dc00998140af | 753 | break; |
| maximbolduc | 26:dc00998140af | 754 | case 6: |
| maximbolduc | 26:dc00998140af | 755 | avg = token; |
| maximbolduc | 26:dc00998140af | 756 | break; |
| maximbolduc | 26:dc00998140af | 757 | case 7: |
| maximbolduc | 26:dc00998140af | 758 | _kp = token; |
| maximbolduc | 26:dc00998140af | 759 | break; |
| maximbolduc | 26:dc00998140af | 760 | case 8: |
| maximbolduc | 26:dc00998140af | 761 | _ki = token; |
| maximbolduc | 26:dc00998140af | 762 | break; |
| maximbolduc | 26:dc00998140af | 763 | case 9: |
| maximbolduc | 26:dc00998140af | 764 | _kd = token; |
| maximbolduc | 26:dc00998140af | 765 | break; |
| maximbolduc | 26:dc00998140af | 766 | } |
| maximbolduc | 26:dc00998140af | 767 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 768 | token_counter++; |
| maximbolduc | 26:dc00998140af | 769 | } |
| maximbolduc | 26:dc00998140af | 770 | if ( _kp && _ki && _kd ) |
| maximbolduc | 26:dc00998140af | 771 | { |
| maximbolduc | 26:dc00998140af | 772 | kp = atof(_kp); |
| maximbolduc | 26:dc00998140af | 773 | ki = atof(_ki); |
| maximbolduc | 26:dc00998140af | 774 | kd = atof(_kd); |
| maximbolduc | 26:dc00998140af | 775 | } |
| maximbolduc | 26:dc00998140af | 776 | if ( phase && center && scl && avg && ahead ) |
| maximbolduc | 26:dc00998140af | 777 | { |
| maximbolduc | 26:dc00998140af | 778 | look_ahead_time = atof(ahead); |
| maximbolduc | 26:dc00998140af | 779 | scale = atof(scl); |
| maximbolduc | 26:dc00998140af | 780 | phaseadv = atof(phase); |
| maximbolduc | 26:dc00998140af | 781 | avg_pos = atof(avg); |
| maximbolduc | 26:dc00998140af | 782 | _Tcenter = atof(center); |
| maximbolduc | 26:dc00998140af | 783 | sprintf(output, "$SETTINGS:%f\r\n", look_ahead_time); |
| maximbolduc | 26:dc00998140af | 784 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 785 | } |
| maximbolduc | 26:dc00998140af | 786 | } |
| maximbolduc | 26:dc00998140af | 787 | |
| maximbolduc | 26:dc00998140af | 788 | void process_ASTEER(char* asteer) |
| maximbolduc | 26:dc00998140af | 789 | { |
| maximbolduc | 26:dc00998140af | 790 | char *token; |
| maximbolduc | 26:dc00998140af | 791 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 792 | char *asteer_speed = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 793 | char *asteer_time = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 794 | |
| maximbolduc | 26:dc00998140af | 795 | token = strtok(asteer, ","); |
| maximbolduc | 26:dc00998140af | 796 | while (token) |
| maximbolduc | 26:dc00998140af | 797 | { |
| maximbolduc | 26:dc00998140af | 798 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 799 | { |
| maximbolduc | 26:dc00998140af | 800 | case 1: |
| maximbolduc | 26:dc00998140af | 801 | asteer_speed = token; |
| maximbolduc | 26:dc00998140af | 802 | break; |
| maximbolduc | 26:dc00998140af | 803 | case 2: |
| maximbolduc | 26:dc00998140af | 804 | asteer_time = token; |
| maximbolduc | 26:dc00998140af | 805 | break; |
| maximbolduc | 26:dc00998140af | 806 | } |
| maximbolduc | 26:dc00998140af | 807 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 808 | token_counter++; |
| maximbolduc | 26:dc00998140af | 809 | } |
| maximbolduc | 26:dc00998140af | 810 | if ( asteer_speed && asteer_time ) |
| maximbolduc | 26:dc00998140af | 811 | { |
| maximbolduc | 26:dc00998140af | 812 | motorspeed = atof(asteer_speed); |
| maximbolduc | 26:dc00998140af | 813 | enable_time = atof(asteer_time); |
| maximbolduc | 26:dc00998140af | 814 | autosteer_timeout.attach(autosteer_done,(double)enable_time / (double)1000); |
| maximbolduc | 26:dc00998140af | 815 | if ( motorspeed > 127 ) |
| maximbolduc | 26:dc00998140af | 816 | { |
| maximbolduc | 26:dc00998140af | 817 | pwm2_speed = 0; |
| maximbolduc | 26:dc00998140af | 818 | pwm1_speed = ((double)motorspeed - (double)127) / 127; |
| maximbolduc | 26:dc00998140af | 819 | enable_motor = 1; |
| maximbolduc | 26:dc00998140af | 820 | } |
| maximbolduc | 26:dc00998140af | 821 | else if ( motorspeed < 127 ) |
| maximbolduc | 26:dc00998140af | 822 | { |
| maximbolduc | 26:dc00998140af | 823 | pwm2_speed = ((double)motorspeed / 127 ); |
| maximbolduc | 26:dc00998140af | 824 | pwm1_speed = 0; |
| maximbolduc | 26:dc00998140af | 825 | enable_motor = 1; |
| maximbolduc | 26:dc00998140af | 826 | } |
| maximbolduc | 26:dc00998140af | 827 | else |
| maximbolduc | 26:dc00998140af | 828 | { |
| maximbolduc | 26:dc00998140af | 829 | pwm1_speed = 0; |
| maximbolduc | 26:dc00998140af | 830 | pwm2_speed = 0; |
| maximbolduc | 26:dc00998140af | 831 | enable_motor = 0; |
| maximbolduc | 26:dc00998140af | 832 | } |
| maximbolduc | 26:dc00998140af | 833 | pwm1 = pwm1_speed; |
| maximbolduc | 26:dc00998140af | 834 | pwm2 = pwm2_speed; |
| maximbolduc | 26:dc00998140af | 835 | } |
| maximbolduc | 26:dc00998140af | 836 | } |
| maximbolduc | 26:dc00998140af | 837 | |
| maximbolduc | 26:dc00998140af | 838 | void process_initstring(char* init) |
| maximbolduc | 26:dc00998140af | 839 | { |
| maximbolduc | 26:dc00998140af | 840 | char *token; |
| maximbolduc | 26:dc00998140af | 841 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 842 | char *init_string = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 843 | token = strtok(init, "$"); |
| maximbolduc | 26:dc00998140af | 844 | while (token) |
| maximbolduc | 26:dc00998140af | 845 | { |
| maximbolduc | 26:dc00998140af | 846 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 847 | { |
| maximbolduc | 26:dc00998140af | 848 | case 1: |
| maximbolduc | 26:dc00998140af | 849 | init_string = token; |
| maximbolduc | 26:dc00998140af | 850 | break; |
| maximbolduc | 26:dc00998140af | 851 | } |
| maximbolduc | 26:dc00998140af | 852 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 853 | token_counter++; |
| maximbolduc | 26:dc00998140af | 854 | } |
| maximbolduc | 26:dc00998140af | 855 | if ( init_string ) |
| maximbolduc | 26:dc00998140af | 856 | { |
| maximbolduc | 26:dc00998140af | 857 | if ( antenna_active == 1 ) |
| maximbolduc | 26:dc00998140af | 858 | { |
| maximbolduc | 26:dc00998140af | 859 | while(!b.writeable()) {} //disable uart3 interrupt (p9,p10) |
| maximbolduc | 26:dc00998140af | 860 | sprintf(output,"$%s",init_string); |
| maximbolduc | 26:dc00998140af | 861 | b.puts(output); |
| maximbolduc | 26:dc00998140af | 862 | gps_connecting.start(); |
| maximbolduc | 26:dc00998140af | 863 | gps_connecting.reset(); |
| maximbolduc | 26:dc00998140af | 864 | connecting = 1; |
| maximbolduc | 26:dc00998140af | 865 | } |
| maximbolduc | 26:dc00998140af | 866 | } |
| maximbolduc | 26:dc00998140af | 867 | } |
| maximbolduc | 26:dc00998140af | 868 | |
| maximbolduc | 26:dc00998140af | 869 | void process_GPSBAUD(char* gpsbaud) |
| maximbolduc | 26:dc00998140af | 870 | { |
| maximbolduc | 26:dc00998140af | 871 | char *token; |
| maximbolduc | 26:dc00998140af | 872 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 873 | char *baud = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 874 | token = strtok(gpsbaud, ","); |
| maximbolduc | 26:dc00998140af | 875 | while (token) |
| maximbolduc | 26:dc00998140af | 876 | { |
| maximbolduc | 26:dc00998140af | 877 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 878 | { |
| maximbolduc | 26:dc00998140af | 879 | case 1: |
| maximbolduc | 26:dc00998140af | 880 | baud = token; |
| maximbolduc | 26:dc00998140af | 881 | break; |
| maximbolduc | 26:dc00998140af | 882 | } |
| maximbolduc | 26:dc00998140af | 883 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 884 | token_counter++; |
| maximbolduc | 26:dc00998140af | 885 | } |
| maximbolduc | 26:dc00998140af | 886 | if ( baud ) |
| maximbolduc | 26:dc00998140af | 887 | { |
| maximbolduc | 26:dc00998140af | 888 | gps_baud = atoi(baud); |
| maximbolduc | 26:dc00998140af | 889 | } |
| maximbolduc | 26:dc00998140af | 890 | activate_antenna(); |
| maximbolduc | 26:dc00998140af | 891 | } |
| maximbolduc | 26:dc00998140af | 892 | |
| maximbolduc | 26:dc00998140af | 893 | void pc_analyse(char* pc_string) |
| maximbolduc | 26:dc00998140af | 894 | { |
| maximbolduc | 26:dc00998140af | 895 | if (!strncmp(pc_string, "$ASTEER", 7)) |
| maximbolduc | 26:dc00998140af | 896 | { |
| maximbolduc | 26:dc00998140af | 897 | process_ASTEER(pc_string); |
| maximbolduc | 26:dc00998140af | 898 | } |
| maximbolduc | 26:dc00998140af | 899 | else if (!strncmp(pc_string, "$GPS-BAUD",9)) |
| maximbolduc | 26:dc00998140af | 900 | { |
| maximbolduc | 26:dc00998140af | 901 | process_GPSBAUD(pc_string); |
| maximbolduc | 26:dc00998140af | 902 | sprintf(output,"%s\r\n",pc_string); |
| maximbolduc | 26:dc00998140af | 903 | bluetooth.puts(output); |
| maximbolduc | 26:dc00998140af | 904 | } |
| maximbolduc | 26:dc00998140af | 905 | else if (!strncmp(pc_string, "$FGPS,",6)) |
| maximbolduc | 26:dc00998140af | 906 | { |
| maximbolduc | 26:dc00998140af | 907 | process_initstring(pc_string); |
| maximbolduc | 26:dc00998140af | 908 | sprintf(output,"%s\r\n",pc_string); |
| maximbolduc | 26:dc00998140af | 909 | bluetooth.puts(output); |
| maximbolduc | 26:dc00998140af | 910 | } |
| maximbolduc | 26:dc00998140af | 911 | else if (!strncmp(pc_string, "$PRINT_EULER=1",14)) |
| maximbolduc | 26:dc00998140af | 912 | { |
| maximbolduc | 26:dc00998140af | 913 | print_euler = 1; |
| maximbolduc | 26:dc00998140af | 914 | } |
| maximbolduc | 26:dc00998140af | 915 | else if (!strncmp(pc_string, "$PRINT_EULER=0",14)) |
| maximbolduc | 26:dc00998140af | 916 | { |
| maximbolduc | 26:dc00998140af | 917 | print_euler = 0; |
| maximbolduc | 26:dc00998140af | 918 | } |
| maximbolduc | 26:dc00998140af | 919 | else if (!strncmp(pc_string, "$GPS-HEIGHT",11)) |
| maximbolduc | 26:dc00998140af | 920 | { |
| maximbolduc | 26:dc00998140af | 921 | process_GPSHEIGHT(pc_string); |
| maximbolduc | 26:dc00998140af | 922 | sprintf(output,"%s\r\n",pc_string); |
| maximbolduc | 26:dc00998140af | 923 | bluetooth.puts(output); |
| maximbolduc | 26:dc00998140af | 924 | } |
| maximbolduc | 26:dc00998140af | 925 | else if (!strncmp(pc_string, "$CORRECT_RMC=1",14)) |
| maximbolduc | 26:dc00998140af | 926 | { |
| maximbolduc | 26:dc00998140af | 927 | correct_rmc = 1; |
| maximbolduc | 26:dc00998140af | 928 | } |
| maximbolduc | 26:dc00998140af | 929 | else if (!strncmp(pc_string, "$CORRECT_RMC=0",14)) |
| maximbolduc | 26:dc00998140af | 930 | { |
| maximbolduc | 26:dc00998140af | 931 | correct_rmc = 0; |
| maximbolduc | 26:dc00998140af | 932 | } |
| maximbolduc | 26:dc00998140af | 933 | else if (!strncmp(pc_string, "$FGPSAUTO",9)) |
| maximbolduc | 26:dc00998140af | 934 | { |
| maximbolduc | 26:dc00998140af | 935 | process_FGPSAUTO(pc_string); |
| maximbolduc | 26:dc00998140af | 936 | sprintf(output,"%s\r\n",pc_string); |
| maximbolduc | 26:dc00998140af | 937 | bluetooth.puts(output); |
| maximbolduc | 26:dc00998140af | 938 | } |
| maximbolduc | 26:dc00998140af | 939 | else if (!strncmp(pc_string, "$FGPSAB",7)) |
| maximbolduc | 26:dc00998140af | 940 | { |
| maximbolduc | 26:dc00998140af | 941 | sprintf(output,"%s\r\n",pc_string); |
| maximbolduc | 26:dc00998140af | 942 | // bluetooth.puts(output); |
| maximbolduc | 26:dc00998140af | 943 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 944 | process_FGPSAB(pc_string); |
| maximbolduc | 26:dc00998140af | 945 | |
| maximbolduc | 26:dc00998140af | 946 | } |
| maximbolduc | 26:dc00998140af | 947 | else |
| maximbolduc | 26:dc00998140af | 948 | { |
| maximbolduc | 26:dc00998140af | 949 | } |
| maximbolduc | 26:dc00998140af | 950 | } |
| maximbolduc | 26:dc00998140af | 951 | |
| maximbolduc | 26:dc00998140af | 952 | void gps_analyse(char* gps_string) |
| maximbolduc | 26:dc00998140af | 953 | { |
| maximbolduc | 26:dc00998140af | 954 | if (!strncmp(gps_string, "$GPRMC", 6)) |
| maximbolduc | 26:dc00998140af | 955 | { |
| maximbolduc | 26:dc00998140af | 956 | // if ( connecting == 0 )// && correct_rmc == 1 ) |
| maximbolduc | 26:dc00998140af | 957 | // { |
| maximbolduc | 26:dc00998140af | 958 | bluetooth.puts(gps_string); |
| maximbolduc | 26:dc00998140af | 959 | // } |
| maximbolduc | 26:dc00998140af | 960 | nmea_rmc(gps_string); //analyse and decompose the rmc string |
| maximbolduc | 26:dc00998140af | 961 | } |
| maximbolduc | 26:dc00998140af | 962 | else if (!strncmp(gps_string, "$GPGGA", 6)) |
| maximbolduc | 26:dc00998140af | 963 | { |
| maximbolduc | 26:dc00998140af | 964 | bluetooth.puts(gps_string); |
| maximbolduc | 31:c40f16ff3a2f | 965 | //nmea_gga(gps_string);//analyse and decompose the gga string---no need to use it, just passing to farmerGPS |
| maximbolduc | 26:dc00998140af | 966 | } |
| maximbolduc | 26:dc00998140af | 967 | else |
| maximbolduc | 26:dc00998140af | 968 | { |
| maximbolduc | 31:c40f16ff3a2f | 969 | bluetooth.puts(gps_string); |
| maximbolduc | 26:dc00998140af | 970 | } |
| maximbolduc | 26:dc00998140af | 971 | } |
| maximbolduc | 26:dc00998140af | 972 | |
| maximbolduc | 31:c40f16ff3a2f | 973 | int i2 = 0; |
| maximbolduc | 31:c40f16ff3a2f | 974 | bool end2 = false; |
| maximbolduc | 31:c40f16ff3a2f | 975 | bool start2 = false; |
| maximbolduc | 26:dc00998140af | 976 | |
| maximbolduc | 31:c40f16ff3a2f | 977 | bool getline2() |
| maximbolduc | 26:dc00998140af | 978 | { |
| maximbolduc | 31:c40f16ff3a2f | 979 | while (1) |
| maximbolduc | 31:c40f16ff3a2f | 980 | { |
| maximbolduc | 31:c40f16ff3a2f | 981 | if( !bluetooth.readable() ) |
| maximbolduc | 31:c40f16ff3a2f | 982 | { |
| maximbolduc | 31:c40f16ff3a2f | 983 | break; |
| maximbolduc | 31:c40f16ff3a2f | 984 | } |
| maximbolduc | 31:c40f16ff3a2f | 985 | char c = bluetooth.getc(); |
| maximbolduc | 31:c40f16ff3a2f | 986 | if (c == 36 ){start2=true;end2 = false; i2 = 0;} |
| maximbolduc | 31:c40f16ff3a2f | 987 | if (c == 10) {end2=true; start2 = false;} |
| maximbolduc | 31:c40f16ff3a2f | 988 | if (start2) |
| maximbolduc | 26:dc00998140af | 989 | { |
| maximbolduc | 31:c40f16ff3a2f | 990 | msg2[i2]=c; |
| maximbolduc | 31:c40f16ff3a2f | 991 | i2++; |
| maximbolduc | 31:c40f16ff3a2f | 992 | if (i2>255) break; //protect msg buffer! |
| maximbolduc | 31:c40f16ff3a2f | 993 | } |
| maximbolduc | 31:c40f16ff3a2f | 994 | if (end2) |
| maximbolduc | 31:c40f16ff3a2f | 995 | { |
| maximbolduc | 31:c40f16ff3a2f | 996 | msg2[i2]=c; |
| maximbolduc | 31:c40f16ff3a2f | 997 | msg2[i2+1] = 0; |
| maximbolduc | 31:c40f16ff3a2f | 998 | start2 = false; |
| maximbolduc | 31:c40f16ff3a2f | 999 | end2 = true; |
| maximbolduc | 31:c40f16ff3a2f | 1000 | break; |
| maximbolduc | 26:dc00998140af | 1001 | } |
| maximbolduc | 26:dc00998140af | 1002 | } |
| maximbolduc | 31:c40f16ff3a2f | 1003 | return end2; |
| maximbolduc | 26:dc00998140af | 1004 | } |
| maximbolduc | 26:dc00998140af | 1005 | |
| maximbolduc | 31:c40f16ff3a2f | 1006 | int i=0; |
| maximbolduc | 31:c40f16ff3a2f | 1007 | bool start=false; |
| maximbolduc | 31:c40f16ff3a2f | 1008 | bool end=false; |
| maximbolduc | 31:c40f16ff3a2f | 1009 | |
| maximbolduc | 31:c40f16ff3a2f | 1010 | bool getline() |
| maximbolduc | 26:dc00998140af | 1011 | { |
| maximbolduc | 31:c40f16ff3a2f | 1012 | while (1) |
| maximbolduc | 31:c40f16ff3a2f | 1013 | { |
| maximbolduc | 31:c40f16ff3a2f | 1014 | if( !b.readable() ) |
| maximbolduc | 31:c40f16ff3a2f | 1015 | { |
| maximbolduc | 31:c40f16ff3a2f | 1016 | break; |
| maximbolduc | 31:c40f16ff3a2f | 1017 | } |
| maximbolduc | 31:c40f16ff3a2f | 1018 | char c = b.getc(); |
| maximbolduc | 31:c40f16ff3a2f | 1019 | if (c == 36 ){start=true;end = false; i = 0;} |
| maximbolduc | 31:c40f16ff3a2f | 1020 | if (c == 10) {end=true; start = false;} |
| maximbolduc | 31:c40f16ff3a2f | 1021 | if (start) |
| maximbolduc | 26:dc00998140af | 1022 | { |
| maximbolduc | 31:c40f16ff3a2f | 1023 | msg[i]=c; |
| maximbolduc | 31:c40f16ff3a2f | 1024 | i++; |
| maximbolduc | 31:c40f16ff3a2f | 1025 | if (i>255) break; //protect msg buffer! |
| maximbolduc | 31:c40f16ff3a2f | 1026 | } |
| maximbolduc | 31:c40f16ff3a2f | 1027 | if (end) |
| maximbolduc | 31:c40f16ff3a2f | 1028 | { |
| maximbolduc | 31:c40f16ff3a2f | 1029 | msg[i]=c; |
| maximbolduc | 26:dc00998140af | 1030 | msg[i+1] = 0; |
| maximbolduc | 31:c40f16ff3a2f | 1031 | start = false; |
| maximbolduc | 31:c40f16ff3a2f | 1032 | end = true; |
| maximbolduc | 31:c40f16ff3a2f | 1033 | break; |
| maximbolduc | 26:dc00998140af | 1034 | } |
| maximbolduc | 26:dc00998140af | 1035 | } |
| maximbolduc | 31:c40f16ff3a2f | 1036 | return end; |
| maximbolduc | 26:dc00998140af | 1037 | } |
| maximbolduc | 26:dc00998140af | 1038 | |
| maximbolduc | 26:dc00998140af | 1039 | void keyPressedHeld( void ) |
| maximbolduc | 26:dc00998140af | 1040 | { |
| maximbolduc | 26:dc00998140af | 1041 | motor_enable_state = "$ENABLE=1\r\n"; |
| maximbolduc | 26:dc00998140af | 1042 | motor_enable_tosend = 1; |
| maximbolduc | 26:dc00998140af | 1043 | } |
| maximbolduc | 26:dc00998140af | 1044 | |
| maximbolduc | 26:dc00998140af | 1045 | void keyReleasedHeld( void ) |
| maximbolduc | 26:dc00998140af | 1046 | { |
| maximbolduc | 26:dc00998140af | 1047 | motor_enable_state = "$ENABLE=0\r\n"; |
| maximbolduc | 26:dc00998140af | 1048 | motor_enable_tosend = 1; |
| maximbolduc | 26:dc00998140af | 1049 | } |
| maximbolduc | 26:dc00998140af | 1050 | |
| maximbolduc | 26:dc00998140af | 1051 | void toprint() |
| maximbolduc | 26:dc00998140af | 1052 | { |
| maximbolduc | 26:dc00998140af | 1053 | angle_send = 1; |
| maximbolduc | 26:dc00998140af | 1054 | } |
| maximbolduc | 26:dc00998140af | 1055 | |
| maximbolduc | 26:dc00998140af | 1056 | void debugging_leds( void ) |
| maximbolduc | 26:dc00998140af | 1057 | { |
| maximbolduc | 26:dc00998140af | 1058 | } |
| maximbolduc | 26:dc00998140af | 1059 | |
| maximbolduc | 31:c40f16ff3a2f | 1060 | int getCheckSum(char *string) |
| maximbolduc | 31:c40f16ff3a2f | 1061 | { |
| maximbolduc | 31:c40f16ff3a2f | 1062 | int i; |
| maximbolduc | 31:c40f16ff3a2f | 1063 | int XOR; |
| maximbolduc | 31:c40f16ff3a2f | 1064 | int c; |
| maximbolduc | 26:dc00998140af | 1065 | // Calculate checksum ignoring any $'s in the string |
| maximbolduc | 31:c40f16ff3a2f | 1066 | for (XOR = 0, i = 0; i < strlen(string); i++) |
| maximbolduc | 31:c40f16ff3a2f | 1067 | { |
| maximbolduc | 31:c40f16ff3a2f | 1068 | c = (unsigned char)string[i]; |
| maximbolduc | 31:c40f16ff3a2f | 1069 | if (c == '*') break; |
| maximbolduc | 31:c40f16ff3a2f | 1070 | if (c != '$') XOR ^= c; |
| maximbolduc | 31:c40f16ff3a2f | 1071 | } |
| maximbolduc | 31:c40f16ff3a2f | 1072 | return XOR; |
| maximbolduc | 26:dc00998140af | 1073 | } |
| maximbolduc | 31:c40f16ff3a2f | 1074 | |
| maximbolduc | 31:c40f16ff3a2f | 1075 | void Dispatch(char* line, bool config /* = false */) |
| maximbolduc | 31:c40f16ff3a2f | 1076 | { |
| maximbolduc | 31:c40f16ff3a2f | 1077 | //pc.puts(line); |
| maximbolduc | 31:c40f16ff3a2f | 1078 | char* pointer; |
| maximbolduc | 31:c40f16ff3a2f | 1079 | char* Data[10]; //Can have max of 5 peices of data split by commas |
| maximbolduc | 31:c40f16ff3a2f | 1080 | int index = 0; |
| maximbolduc | 31:c40f16ff3a2f | 1081 | //Split data at commas |
| maximbolduc | 31:c40f16ff3a2f | 1082 | pointer = strtok(line, ","); |
| maximbolduc | 31:c40f16ff3a2f | 1083 | while(pointer != NULL) |
| maximbolduc | 31:c40f16ff3a2f | 1084 | { |
| maximbolduc | 31:c40f16ff3a2f | 1085 | Data[index] = pointer; |
| maximbolduc | 31:c40f16ff3a2f | 1086 | pointer = strtok(NULL, ","); |
| maximbolduc | 31:c40f16ff3a2f | 1087 | index++; |
| maximbolduc | 31:c40f16ff3a2f | 1088 | } |
| maximbolduc | 31:c40f16ff3a2f | 1089 | |
| maximbolduc | 31:c40f16ff3a2f | 1090 | if(strcmp(Data[0], "$BAUD") == 0) |
| maximbolduc | 31:c40f16ff3a2f | 1091 | { |
| maximbolduc | 31:c40f16ff3a2f | 1092 | // pc.puts("\r\nWE GOT BAUD!!!\r\n"); |
| maximbolduc | 31:c40f16ff3a2f | 1093 | gps_baud = atoi(Data[1]); |
| maximbolduc | 31:c40f16ff3a2f | 1094 | activate_antenna(); |
| maximbolduc | 31:c40f16ff3a2f | 1095 | } |
| maximbolduc | 31:c40f16ff3a2f | 1096 | else if(strcmp(Data[0], "$FGPSAUTO") == 0) |
| maximbolduc | 31:c40f16ff3a2f | 1097 | { |
| maximbolduc | 31:c40f16ff3a2f | 1098 | |
| maximbolduc | 31:c40f16ff3a2f | 1099 | } |
| maximbolduc | 31:c40f16ff3a2f | 1100 | else if(strcmp(Data[0], "$GYRO") == 0) |
| maximbolduc | 31:c40f16ff3a2f | 1101 | { |
| maximbolduc | 31:c40f16ff3a2f | 1102 | |
| maximbolduc | 31:c40f16ff3a2f | 1103 | } |
| maximbolduc | 31:c40f16ff3a2f | 1104 | else if(strcmp(Data[0], "$HEIGHT") == 0) |
| maximbolduc | 31:c40f16ff3a2f | 1105 | { |
| maximbolduc | 31:c40f16ff3a2f | 1106 | |
| maximbolduc | 31:c40f16ff3a2f | 1107 | } |
| maximbolduc | 31:c40f16ff3a2f | 1108 | } |
| maximbolduc | 31:c40f16ff3a2f | 1109 | |
| maximbolduc | 31:c40f16ff3a2f | 1110 | void Config_Startup() |
| maximbolduc | 31:c40f16ff3a2f | 1111 | { |
| maximbolduc | 31:c40f16ff3a2f | 1112 | char line[256]; |
| maximbolduc | 31:c40f16ff3a2f | 1113 | fp = fopen("/local/config.txt", "r"); |
| maximbolduc | 31:c40f16ff3a2f | 1114 | while (fgets(line, sizeof(line), fp)) //Read through config file line by line |
| maximbolduc | 31:c40f16ff3a2f | 1115 | Dispatch(line, true); //Send line to dispatcher, true indicates its coming from the config file |
| maximbolduc | 31:c40f16ff3a2f | 1116 | fclose(fp); |
| maximbolduc | 31:c40f16ff3a2f | 1117 | } |
| maximbolduc | 31:c40f16ff3a2f | 1118 | |
| maximbolduc | 31:c40f16ff3a2f | 1119 | void overide_config(int baud, int height, int x, int y, int z, char* freepilot_sets) |
| maximbolduc | 31:c40f16ff3a2f | 1120 | { |
| maximbolduc | 31:c40f16ff3a2f | 1121 | fp = fopen("/local/config.txt", "w"); |
| maximbolduc | 31:c40f16ff3a2f | 1122 | fprintf(fp, "\r\n$BAUD,%i\r\n$HEIGHT,%d\r\n$GYRO,%d,%d,%d\r\n%s",baud,height,x,y,z,freepilot_sets); //Rewrite text file for TESTING |
| maximbolduc | 31:c40f16ff3a2f | 1123 | fclose(fp); |
| maximbolduc | 26:dc00998140af | 1124 | } |
| maximbolduc | 26:dc00998140af | 1125 | |
| maximbolduc | 26:dc00998140af | 1126 | int main() |
| maximbolduc | 26:dc00998140af | 1127 | { |
| maximbolduc | 26:dc00998140af | 1128 | bluetooth.baud(38400); |
| maximbolduc | 31:c40f16ff3a2f | 1129 | pc.baud(38400); |
| maximbolduc | 31:c40f16ff3a2f | 1130 | Config_Startup(); |
| maximbolduc | 26:dc00998140af | 1131 | motor_switch.setSampleFrequency(10000); |
| maximbolduc | 26:dc00998140af | 1132 | motor_switch.attach_asserted_held( &keyPressedHeld ); |
| maximbolduc | 26:dc00998140af | 1133 | motor_switch.attach_deasserted_held( &keyReleasedHeld ); |
| maximbolduc | 31:c40f16ff3a2f | 1134 | initializeAccelerometer(); |
| maximbolduc | 31:c40f16ff3a2f | 1135 | calibrateAccelerometer(); |
| maximbolduc | 31:c40f16ff3a2f | 1136 | initializeGyroscope(); |
| maximbolduc | 26:dc00998140af | 1137 | calibrateGyroscope(); |
| maximbolduc | 31:c40f16ff3a2f | 1138 | accelerometerTicker.attach(&sampleAccelerometer, 0.005); |
| maximbolduc | 31:c40f16ff3a2f | 1139 | //debug_leds.atatch(&debugging_leds,2); |
| maximbolduc | 31:c40f16ff3a2f | 1140 | gyroscopeTicker.attach(&sampleGyroscope, 0.005); |
| maximbolduc | 31:c40f16ff3a2f | 1141 | filterTicker.attach(&filter, FILTER_RATE); //needing a timer here as gyro loop as filter loop need to be exactly at right timing |
| maximbolduc | 31:c40f16ff3a2f | 1142 | angle_print.attach(&toprint,0.2); |
| maximbolduc | 31:c40f16ff3a2f | 1143 | |
| maximbolduc | 26:dc00998140af | 1144 | while(1) |
| maximbolduc | 26:dc00998140af | 1145 | { |
| maximbolduc | 31:c40f16ff3a2f | 1146 | if ( antenna_active == 1 && b.readable()) |
| maximbolduc | 26:dc00998140af | 1147 | { |
| maximbolduc | 31:c40f16ff3a2f | 1148 | if (getline()) |
| maximbolduc | 26:dc00998140af | 1149 | { |
| maximbolduc | 31:c40f16ff3a2f | 1150 | checksumm = getCheckSum(msg); |
| maximbolduc | 31:c40f16ff3a2f | 1151 | if ( pc.writeable()) |
| maximbolduc | 26:dc00998140af | 1152 | { |
| maximbolduc | 31:c40f16ff3a2f | 1153 | sprintf(output,"%s\r\n\0",msg); |
| maximbolduc | 31:c40f16ff3a2f | 1154 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 1155 | } |
| maximbolduc | 31:c40f16ff3a2f | 1156 | gps_analyse(msg); |
| maximbolduc | 26:dc00998140af | 1157 | } |
| maximbolduc | 26:dc00998140af | 1158 | } |
| maximbolduc | 26:dc00998140af | 1159 | |
| maximbolduc | 26:dc00998140af | 1160 | if ( bluetooth.readable()) |
| maximbolduc | 26:dc00998140af | 1161 | { |
| maximbolduc | 31:c40f16ff3a2f | 1162 | if (getline2()) |
| maximbolduc | 26:dc00998140af | 1163 | { |
| maximbolduc | 26:dc00998140af | 1164 | trim(msg2,"\r"); |
| maximbolduc | 26:dc00998140af | 1165 | trim(msg2,"\n"); |
| maximbolduc | 26:dc00998140af | 1166 | trim(msg2,"\0"); |
| maximbolduc | 26:dc00998140af | 1167 | trim(msg2," "); |
| maximbolduc | 26:dc00998140af | 1168 | sprintf(output,"%s\0",msg2); |
| maximbolduc | 26:dc00998140af | 1169 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 1170 | pc_analyse(msg2); |
| maximbolduc | 26:dc00998140af | 1171 | } |
| maximbolduc | 26:dc00998140af | 1172 | } |
| maximbolduc | 26:dc00998140af | 1173 | if ( motor_enable_tosend == 1 && reading == 0 ) |
| maximbolduc | 26:dc00998140af | 1174 | { |
| maximbolduc | 26:dc00998140af | 1175 | bluetooth.puts(motor_enable_state); |
| maximbolduc | 26:dc00998140af | 1176 | motor_enable_tosend = 0; |
| maximbolduc | 26:dc00998140af | 1177 | } |
| maximbolduc | 26:dc00998140af | 1178 | /* if ( print_euler == 1 && angle_send == 1 && reading == 0) |
| maximbolduc | 26:dc00998140af | 1179 | { |
| maximbolduc | 26:dc00998140af | 1180 | sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),toDegrees(imuFilter.getYaw())); |
| maximbolduc | 26:dc00998140af | 1181 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 1182 | bluetooth.puts(output); |
| maximbolduc | 26:dc00998140af | 1183 | angle_send = 0; |
| maximbolduc | 26:dc00998140af | 1184 | }*/ |
| maximbolduc | 26:dc00998140af | 1185 | } |
| maximbolduc | 26:dc00998140af | 1186 | } |