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@29:23ccb2a50b6f, 2015-01-22 (annotated)

Committer:
jhedmonton
Date:
Thu Jan 22 20:35:42 2015 +0000
Revision:
29:23ccb2a50b6f
Parent:
28:5905886c76ee
Child:
30:3afafa1ef16b
Jan22 stuff, ENABLED etc.

Who changed what in which revision?

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