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