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@27:9ac59b261d87, 2015-01-19 (annotated)

Committer:
jhedmonton
Date:
Mon Jan 19 01:35:34 2015 +0000
Revision:
27:9ac59b261d87
Parent:
26:dc00998140af
Child:
28:5905886c76ee
Working version with motorcontroller, GPS pass through, GPS init and optocoupler read. Steering from FarmerGPS!

Who changed what in which revision?

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