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@33:3e71c418e90d, 2015-02-02 (annotated)

Committer:
maximbolduc
Date:
Mon Feb 02 18:24:03 2015 +0000
Revision:
33:3e71c418e90d
Parent:
32:c57bc701d65c
Child:
34:c2bc9f9be7ff
litle update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 26:dc00998140af 1 #include "mbed.h"
maximbolduc 26:dc00998140af 2 #include "PinDetect.h"
maximbolduc 26:dc00998140af 3 #include "Point.h"
maximbolduc 26:dc00998140af 4 #include <vector>
maximbolduc 26:dc00998140af 5 #include "Line.h"
maximbolduc 26:dc00998140af 6 #include "stringUtils.h"
jhedmonton 28:5905886c76ee 7 #include "base.h"
jhedmonton 28:5905886c76ee 8 #include "Config.h"
maximbolduc 30:3afafa1ef16b 9 #include "imu_functions.h"
maximbolduc 32:c57bc701d65c 10 #include "atoh.h"
maximbolduc 26:dc00998140af 11
maximbolduc 33:3e71c418e90d 12 #define dot(u,v) ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
maximbolduc 33:3e71c418e90d 13 #define norm(v) sqrt(dot(v,v)) // norm = length of vector
maximbolduc 33:3e71c418e90d 14 #define d(u,v) norm(point_sub(u,v)) // distance = norm of difference
maximbolduc 33:3e71c418e90d 15
maximbolduc 30:3afafa1ef16b 16 char *version="FreePilot V2.11 Jtan 20, 2015\r\n";
jhedmonton 27:9ac59b261d87 17 long lastsend_version=0;
jhedmonton 27:9ac59b261d87 18 Timer vTimer; //this timer is int based! Max is 30 minutes
jhedmonton 27:9ac59b261d87 19
maximbolduc 26:dc00998140af 20 int checksumm;
maximbolduc 26:dc00998140af 21 double distance_from_line;
maximbolduc 26:dc00998140af 22 double cm_per_deg_lon;
maximbolduc 26:dc00998140af 23 double cm_per_deg_lat;
maximbolduc 26:dc00998140af 24 //all timing objects
maximbolduc 26:dc00998140af 25 Timer gps_connecting;
maximbolduc 26:dc00998140af 26 Timer autosteer_time;
maximbolduc 26:dc00998140af 27 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 28 Ticker accelerometerTicker;
maximbolduc 26:dc00998140af 29 Ticker gyroscopeTicker;
maximbolduc 26:dc00998140af 30 Ticker filterTicker;
maximbolduc 26:dc00998140af 31 Ticker angle_print;
maximbolduc 26:dc00998140af 32
jhedmonton 27:9ac59b261d87 33 //Motor
jhedmonton 27:9ac59b261d87 34 PinDetect motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 35 DigitalOut enable_motor(p7);
jhedmonton 28:5905886c76ee 36
jhedmonton 28:5905886c76ee 37 PwmOut pwm1(p22);
jhedmonton 28:5905886c76ee 38 PwmOut pwm2(p21);
jhedmonton 28:5905886c76ee 39
jhedmonton 27:9ac59b261d87 40 //equipment switches
jhedmonton 27:9ac59b261d87 41 PinDetect boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 42 PinDetect boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 43 PinDetect boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 44 PinDetect boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
maximbolduc 30:3afafa1ef16b 45
jhedmonton 27:9ac59b261d87 46 char boom18; //1 byte
jhedmonton 27:9ac59b261d87 47 char lastboom18; //1 byte
jhedmonton 27:9ac59b261d87 48 char boomstate[8]={'$','F','B','S',0,13,10,0 };
jhedmonton 27:9ac59b261d87 49
maximbolduc 26:dc00998140af 50 Point position;
maximbolduc 26:dc00998140af 51 Point looked_ahead;
maximbolduc 26:dc00998140af 52 Point line_start;
maximbolduc 26:dc00998140af 53 Point line_end;
maximbolduc 26:dc00998140af 54 Point tilt_compensated_position;
maximbolduc 26:dc00998140af 55 Point yaw_compensated_position;
maximbolduc 26:dc00998140af 56
maximbolduc 32:c57bc701d65c 57 extern int gyro_pos;
maximbolduc 32:c57bc701d65c 58
maximbolduc 26:dc00998140af 59 double distance_to_line;
maximbolduc 26:dc00998140af 60
maximbolduc 26:dc00998140af 61 //FreePilot variables
maximbolduc 26:dc00998140af 62 int timer_enabled;
jhedmonton 28:5905886c76ee 63 double motorspeed;
maximbolduc 26:dc00998140af 64 int enable_time;
maximbolduc 26:dc00998140af 65 char* motor_enable_state = 0;
jhedmonton 28:5905886c76ee 66 int motor_enable = 0;
jhedmonton 28:5905886c76ee 67 int lastmotor_enable = 1;
maximbolduc 26:dc00998140af 68 double pwm1_speed;
maximbolduc 26:dc00998140af 69 double pwm2_speed;
jhedmonton 28:5905886c76ee 70 long lastsend_motorstate=0;
jhedmonton 28:5905886c76ee 71 Timer motTimer; //this timer is int based! Max is 30 minutes
jhedmonton 28:5905886c76ee 72 Timer btTimer; //measure time for Bluetooth communication
jhedmonton 28:5905886c76ee 73 long lastgetBT=0;
maximbolduc 26:dc00998140af 74
maximbolduc 26:dc00998140af 75 int msg2_changed = 1;
maximbolduc 26:dc00998140af 76 char* buffer;
maximbolduc 26:dc00998140af 77 double meter_lat = 0;
maximbolduc 26:dc00998140af 78 double meter_lon = 0;
jhedmonton 28:5905886c76ee 79
maximbolduc 26:dc00998140af 80 char msg[256]; //GPS line buffer
maximbolduc 26:dc00998140af 81 char msg2[256];//PC line buffer
maximbolduc 26:dc00998140af 82 int printing;
maximbolduc 26:dc00998140af 83 int num_of_gps_sats;
maximbolduc 30:3afafa1ef16b 84
maximbolduc 26:dc00998140af 85 double decimal_lon;
maximbolduc 26:dc00998140af 86 float longitude;
maximbolduc 26:dc00998140af 87 float latitude;
maximbolduc 26:dc00998140af 88 char ns, ew;
maximbolduc 26:dc00998140af 89 int lock;
maximbolduc 26:dc00998140af 90 int flag_gga;
maximbolduc 26:dc00998140af 91 int reading;
maximbolduc 26:dc00998140af 92 double decimal_latitude;
maximbolduc 26:dc00998140af 93 int gps_satellite_quality;
maximbolduc 26:dc00998140af 94 int day;
maximbolduc 26:dc00998140af 95 int hour;
maximbolduc 26:dc00998140af 96 int minute;
maximbolduc 26:dc00998140af 97 int second;
maximbolduc 26:dc00998140af 98 int tenths;
maximbolduc 26:dc00998140af 99 int hundreths;
maximbolduc 26:dc00998140af 100 char status;
maximbolduc 26:dc00998140af 101 double track; // track made good . angle
maximbolduc 26:dc00998140af 102 char magvar_dir;
maximbolduc 26:dc00998140af 103 double magvar;
maximbolduc 26:dc00998140af 104 int year;
maximbolduc 26:dc00998140af 105 int month;
maximbolduc 26:dc00998140af 106 double speed_km;
maximbolduc 26:dc00998140af 107 double speed_m_s = 0;
maximbolduc 26:dc00998140af 108 double velocity; // speed in knot
maximbolduc 26:dc00998140af 109 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 110 int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
maximbolduc 30:3afafa1ef16b 111
maximbolduc 26:dc00998140af 112 int angle_send = 0;
maximbolduc 26:dc00998140af 113 int correct_rmc = 1;
maximbolduc 26:dc00998140af 114 double m_lat = 0;
maximbolduc 26:dc00998140af 115 double m_lon = 0;
maximbolduc 26:dc00998140af 116 char* degminsec;
maximbolduc 26:dc00998140af 117 double m_per_deg_lon;
maximbolduc 26:dc00998140af 118 double m_per_deg_lat;
maximbolduc 26:dc00998140af 119 double look_ahead_lon;
maximbolduc 26:dc00998140af 120 double look_ahead_lat;
maximbolduc 26:dc00998140af 121 int active_AB = 0;
maximbolduc 26:dc00998140af 122 double compensation_vector;
maximbolduc 26:dc00998140af 123 char output[256];
maximbolduc 26:dc00998140af 124
maximbolduc 26:dc00998140af 125 double yaw;
maximbolduc 26:dc00998140af 126 double pitch;
maximbolduc 26:dc00998140af 127 double roll;
maximbolduc 26:dc00998140af 128
maximbolduc 30:3afafa1ef16b 129 double a_x;
maximbolduc 30:3afafa1ef16b 130 double a_y;
maximbolduc 30:3afafa1ef16b 131 double a_z;
maximbolduc 30:3afafa1ef16b 132 double w_x;
maximbolduc 30:3afafa1ef16b 133 double w_y;
maximbolduc 30:3afafa1ef16b 134 double w_z;
maximbolduc 26:dc00998140af 135
maximbolduc 26:dc00998140af 136 int readings[3];
maximbolduc 26:dc00998140af 137
maximbolduc 26:dc00998140af 138 double Freepilot_lat;
maximbolduc 26:dc00998140af 139 double Freepilot_lon;
maximbolduc 26:dc00998140af 140 double Freepilot_lat1;
maximbolduc 26:dc00998140af 141 double Freepilot_lon1;
maximbolduc 26:dc00998140af 142 double Freepilot_bearing;
maximbolduc 26:dc00998140af 143
maximbolduc 26:dc00998140af 144 volatile bool newline_detected = false;
maximbolduc 26:dc00998140af 145
maximbolduc 26:dc00998140af 146 Point point_add(Point a, Point b)
maximbolduc 26:dc00998140af 147 {
maximbolduc 26:dc00998140af 148 return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY());
maximbolduc 26:dc00998140af 149 }
maximbolduc 26:dc00998140af 150
maximbolduc 26:dc00998140af 151 Point point_sub(Point a , Point b)
maximbolduc 26:dc00998140af 152 {
maximbolduc 26:dc00998140af 153 return Point(a.GetX() - b.GetX(), a.GetY() - b.GetY());
maximbolduc 26:dc00998140af 154 }
maximbolduc 26:dc00998140af 155
maximbolduc 33:3e71c418e90d 156 double get_yaw()
maximbolduc 33:3e71c418e90d 157 {
maximbolduc 33:3e71c418e90d 158 double yaw_angle = imuFilter.getYaw() * -1 ; // *-1 to reverse polarity as gyro is up-side-down
maximbolduc 33:3e71c418e90d 159 return yaw_angle;
maximbolduc 33:3e71c418e90d 160 }
maximbolduc 33:3e71c418e90d 161
maximbolduc 33:3e71c418e90d 162 void update_motor()
maximbolduc 33:3e71c418e90d 163 {
maximbolduc 33:3e71c418e90d 164
maximbolduc 33:3e71c418e90d 165 }
maximbolduc 33:3e71c418e90d 166
maximbolduc 33:3e71c418e90d 167 double get_roll()
maximbolduc 33:3e71c418e90d 168 {
maximbolduc 33:3e71c418e90d 169 double roll_angle = 0;
maximbolduc 33:3e71c418e90d 170 if ( gyro_pos == 0 )
maximbolduc 33:3e71c418e90d 171 {
maximbolduc 33:3e71c418e90d 172 roll_angle = imuFilter.getRoll();
maximbolduc 33:3e71c418e90d 173 }
maximbolduc 33:3e71c418e90d 174 else if ( gyro_pos == 1 )
maximbolduc 33:3e71c418e90d 175 {
maximbolduc 33:3e71c418e90d 176 roll_angle = imuFilter.getRoll() * -1;
maximbolduc 33:3e71c418e90d 177 }
maximbolduc 33:3e71c418e90d 178 else if( gyro_pos == 2 )
maximbolduc 33:3e71c418e90d 179 {
maximbolduc 33:3e71c418e90d 180 roll_angle = imuFilter.getPitch() * -1;
maximbolduc 33:3e71c418e90d 181 }
maximbolduc 33:3e71c418e90d 182 else if ( gyro_pos == 3 )
maximbolduc 33:3e71c418e90d 183 {
maximbolduc 33:3e71c418e90d 184 roll_angle = imuFilter.getPitch();
maximbolduc 33:3e71c418e90d 185 }
maximbolduc 33:3e71c418e90d 186 return roll_angle;
maximbolduc 33:3e71c418e90d 187 }
maximbolduc 33:3e71c418e90d 188
maximbolduc 33:3e71c418e90d 189 double get_pitch()
maximbolduc 33:3e71c418e90d 190 {
maximbolduc 33:3e71c418e90d 191 double pitch_angle = 0;
maximbolduc 33:3e71c418e90d 192 if ( gyro_pos == 0 )
maximbolduc 33:3e71c418e90d 193 {
maximbolduc 33:3e71c418e90d 194 pitch_angle = imuFilter.getPitch();
maximbolduc 33:3e71c418e90d 195 }
maximbolduc 33:3e71c418e90d 196 else if ( gyro_pos == 1 )
maximbolduc 33:3e71c418e90d 197 {
maximbolduc 33:3e71c418e90d 198 pitch_angle = imuFilter.getPitch() * -1;
maximbolduc 33:3e71c418e90d 199 }
maximbolduc 33:3e71c418e90d 200 else if( gyro_pos == 2 )
maximbolduc 33:3e71c418e90d 201 {
maximbolduc 33:3e71c418e90d 202 pitch_angle = imuFilter.getRoll();
maximbolduc 33:3e71c418e90d 203 }
maximbolduc 33:3e71c418e90d 204 else if ( gyro_pos == 3 )
maximbolduc 33:3e71c418e90d 205 {
maximbolduc 33:3e71c418e90d 206 pitch_angle = imuFilter.getRoll() * -1;
maximbolduc 33:3e71c418e90d 207 }
maximbolduc 33:3e71c418e90d 208 return pitch_angle;
maximbolduc 33:3e71c418e90d 209 }
maximbolduc 26:dc00998140af 210
maximbolduc 26:dc00998140af 211 double dist_Point_to_Line( Point P, Point line_start, Point line_end)
maximbolduc 26:dc00998140af 212 {
maximbolduc 26:dc00998140af 213 Point v = point_sub(line_end,line_start);
maximbolduc 26:dc00998140af 214 Point w = point_sub(P,line_start);
maximbolduc 26:dc00998140af 215
maximbolduc 26:dc00998140af 216 double c1 = dot(w,v);
maximbolduc 26:dc00998140af 217 double c2 = dot(v,v);
maximbolduc 26:dc00998140af 218 double b = c1 / c2;
maximbolduc 26:dc00998140af 219
maximbolduc 26:dc00998140af 220 Point resulting(b * v.GetX(),b*v.GetY());
maximbolduc 26:dc00998140af 221 Point Pb = point_add(line_start, resulting);
maximbolduc 26:dc00998140af 222 return d(P, Pb);
maximbolduc 26:dc00998140af 223 }
maximbolduc 26:dc00998140af 224
maximbolduc 26:dc00998140af 225 double lat_to_deg(char *s, char north_south)
maximbolduc 26:dc00998140af 226 {
maximbolduc 26:dc00998140af 227 int deg, min, sec;
maximbolduc 26:dc00998140af 228 double fsec, val;
maximbolduc 26:dc00998140af 229
maximbolduc 26:dc00998140af 230 deg = ( (s[0] - '0') * 10) + s[1] - '0';
maximbolduc 26:dc00998140af 231 min = ( (s[2] - '0') * 10) + s[3] - '0';
maximbolduc 26:dc00998140af 232 sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
maximbolduc 26:dc00998140af 233 fsec = (double)((double)sec /10000.0);
maximbolduc 26:dc00998140af 234 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 26:dc00998140af 235 if (north_south == 'S')
maximbolduc 26:dc00998140af 236 {
maximbolduc 26:dc00998140af 237 val *= -1.0;
maximbolduc 26:dc00998140af 238 }
maximbolduc 26:dc00998140af 239 decimal_latitude = val;
maximbolduc 26:dc00998140af 240 return val;
maximbolduc 26:dc00998140af 241 }
maximbolduc 26:dc00998140af 242
maximbolduc 30:3afafa1ef16b 243 // isLeft(): test if a point is Left|On|Right of an infinite 2D line.
maximbolduc 30:3afafa1ef16b 244 // Input: three points P0, P1, and P2
maximbolduc 30:3afafa1ef16b 245 // Return: >0 for P2 left of the line through P0 to P1
maximbolduc 30:3afafa1ef16b 246 // =0 for P2 on the line
maximbolduc 30:3afafa1ef16b 247 // <0 for P2 right of the line
maximbolduc 30:3afafa1ef16b 248 double isLeft( Point P0, Point P1, Point P2 )
maximbolduc 30:3afafa1ef16b 249 {
maximbolduc 30:3afafa1ef16b 250 return ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX())
maximbolduc 30:3afafa1ef16b 251 - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
maximbolduc 30:3afafa1ef16b 252 }
maximbolduc 30:3afafa1ef16b 253
maximbolduc 26:dc00998140af 254 double lon_to_deg(char *s, char east_west)
maximbolduc 26:dc00998140af 255 {
maximbolduc 26:dc00998140af 256 int deg, min, sec;
maximbolduc 26:dc00998140af 257 double fsec, val;
maximbolduc 26:dc00998140af 258 deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
maximbolduc 26:dc00998140af 259 min = ( (s[3] - '0') * 10) + s[4] - '0';
maximbolduc 26:dc00998140af 260 sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
maximbolduc 26:dc00998140af 261 fsec = (double)((double)sec /10000.0);
maximbolduc 26:dc00998140af 262 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 26:dc00998140af 263 if (east_west == 'W')
maximbolduc 26:dc00998140af 264 {
maximbolduc 26:dc00998140af 265 val *= -1.0;
maximbolduc 26:dc00998140af 266 }
maximbolduc 26:dc00998140af 267 decimal_lon = val;
maximbolduc 26:dc00998140af 268 return val;
maximbolduc 26:dc00998140af 269 }
maximbolduc 26:dc00998140af 270
maximbolduc 26:dc00998140af 271 void nmea_gga(char *s)
maximbolduc 26:dc00998140af 272 {
maximbolduc 26:dc00998140af 273 char *token;
maximbolduc 26:dc00998140af 274 int token_counter = 0;
maximbolduc 26:dc00998140af 275 char *latitude = (char *)NULL;
maximbolduc 26:dc00998140af 276 char *longitude = (char *)NULL;
maximbolduc 26:dc00998140af 277 char *lat_dir = (char *)NULL;
maximbolduc 26:dc00998140af 278 char *lon_dir = (char *)NULL;
maximbolduc 26:dc00998140af 279 char *qual = (char *)NULL;
maximbolduc 26:dc00998140af 280 char *altitude = (char *)NULL;
maximbolduc 26:dc00998140af 281 char *sats = (char *)NULL;
maximbolduc 26:dc00998140af 282
maximbolduc 26:dc00998140af 283 token = strtok(s, ",");
maximbolduc 26:dc00998140af 284 while (token)
maximbolduc 26:dc00998140af 285 {
maximbolduc 26:dc00998140af 286 switch (token_counter)
maximbolduc 26:dc00998140af 287 {
maximbolduc 26:dc00998140af 288 case 2:
maximbolduc 26:dc00998140af 289 latitude = token;
maximbolduc 26:dc00998140af 290 break;
maximbolduc 26:dc00998140af 291 case 4:
maximbolduc 26:dc00998140af 292 longitude = token;
maximbolduc 26:dc00998140af 293 break;
maximbolduc 26:dc00998140af 294 case 3:
maximbolduc 26:dc00998140af 295 lat_dir = token;
maximbolduc 26:dc00998140af 296 break;
maximbolduc 26:dc00998140af 297 case 5:
maximbolduc 26:dc00998140af 298 lon_dir = token;
maximbolduc 26:dc00998140af 299 break;
maximbolduc 26:dc00998140af 300 case 6:
maximbolduc 26:dc00998140af 301 qual = token;
maximbolduc 26:dc00998140af 302 break;
maximbolduc 26:dc00998140af 303 case 7:
maximbolduc 26:dc00998140af 304 sats = token;
maximbolduc 26:dc00998140af 305 break;
maximbolduc 26:dc00998140af 306 case 9:
maximbolduc 26:dc00998140af 307 altitude = token;
maximbolduc 26:dc00998140af 308 break;
maximbolduc 26:dc00998140af 309 }
maximbolduc 26:dc00998140af 310 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 311 token_counter++;
maximbolduc 26:dc00998140af 312 }
maximbolduc 26:dc00998140af 313
maximbolduc 26:dc00998140af 314 if (latitude && longitude && altitude && sats)
maximbolduc 26:dc00998140af 315 {
maximbolduc 26:dc00998140af 316 decimal_latitude = lat_to_deg(latitude, lat_dir[0]);
maximbolduc 26:dc00998140af 317 decimal_lon = lon_to_deg(longitude, lon_dir[0]);
maximbolduc 26:dc00998140af 318 num_of_gps_sats = atoi(sats);
maximbolduc 26:dc00998140af 319 gps_satellite_quality = atoi(qual);
maximbolduc 26:dc00998140af 320 }
maximbolduc 26:dc00998140af 321 else
maximbolduc 26:dc00998140af 322 {
maximbolduc 26:dc00998140af 323 gps_satellite_quality = 0;
maximbolduc 26:dc00998140af 324 }
maximbolduc 26:dc00998140af 325 }
maximbolduc 26:dc00998140af 326
maximbolduc 26:dc00998140af 327 //from farmerGPS code
maximbolduc 26:dc00998140af 328 void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
maximbolduc 26:dc00998140af 329 {
maximbolduc 26:dc00998140af 330 double ydist = 0;
maximbolduc 26:dc00998140af 331 double xdist = 0;
maximbolduc 26:dc00998140af 332 angle = angle + 180;
maximbolduc 26:dc00998140af 333 double radiant = angle * 3.14159265359 / 180;
maximbolduc 26:dc00998140af 334 double sinr = sin(radiant);
maximbolduc 26:dc00998140af 335 double cosr = cos(radiant);
maximbolduc 26:dc00998140af 336 xdist = cosr * distance;
maximbolduc 26:dc00998140af 337 ydist = sinr * distance;
maximbolduc 26:dc00998140af 338 lat2 = lat1 + (ydist / (69.09 * -1609.344));
maximbolduc 26:dc00998140af 339 lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
maximbolduc 26:dc00998140af 340 return;
maximbolduc 26:dc00998140af 341 }
maximbolduc 26:dc00998140af 342
maximbolduc 26:dc00998140af 343 Point compensation;
maximbolduc 26:dc00998140af 344 double compensation_angle;
maximbolduc 26:dc00998140af 345 //antenna compensation in cm
maximbolduc 26:dc00998140af 346 void tilt_compensate()
maximbolduc 26:dc00998140af 347 {
maximbolduc 33:3e71c418e90d 348 roll = get_roll();
jhedmonton 28:5905886c76ee 349 compensation_vector = antennaheight * sin(roll);
maximbolduc 33:3e71c418e90d 350 compensation.SetX(compensation_vector * cos(get_yaw() * -1 - (3.14159265359 / 2)));// 57.295779513));
maximbolduc 33:3e71c418e90d 351 compensation.SetY(compensation_vector * sin(get_yaw() * -1 - (3.14159265359 / 2)));// 57.295779513));
maximbolduc 33:3e71c418e90d 352 }
maximbolduc 33:3e71c418e90d 353 void pitch_compensate()
maximbolduc 33:3e71c418e90d 354 {
maximbolduc 33:3e71c418e90d 355 pitch = get_pitch();
maximbolduc 33:3e71c418e90d 356 compensation_vector = antennaheight * sin(pitch);
maximbolduc 33:3e71c418e90d 357 compensation.SetX(compensation.GetX() + compensation_vector * cos(get_yaw() * -1 - (3.14159265359 / 2)));// /57.295779513));
maximbolduc 33:3e71c418e90d 358 compensation.SetY(compensation.GetY() + compensation_vector * sin(get_yaw() * -1 - (3.14159265359 / 2)));// / 57.295779513));
maximbolduc 26:dc00998140af 359 }
maximbolduc 26:dc00998140af 360
maximbolduc 26:dc00998140af 361 void yaw_compensate()
maximbolduc 26:dc00998140af 362 {
maximbolduc 33:3e71c418e90d 363 yaw = get_yaw();
maximbolduc 26:dc00998140af 364 }
maximbolduc 26:dc00998140af 365
maximbolduc 33:3e71c418e90d 366
maximbolduc 26:dc00998140af 367 void process_GPSHEIGHT(char* height_string)
maximbolduc 26:dc00998140af 368 {
maximbolduc 26:dc00998140af 369 char *token;
maximbolduc 26:dc00998140af 370 int token_counter = 0;
maximbolduc 26:dc00998140af 371 char *height = (char *)NULL;
maximbolduc 26:dc00998140af 372 token = strtok(height_string, ",");
maximbolduc 26:dc00998140af 373 while (token)
maximbolduc 26:dc00998140af 374 {
maximbolduc 26:dc00998140af 375
maximbolduc 26:dc00998140af 376 switch (token_counter)
maximbolduc 26:dc00998140af 377 {
maximbolduc 26:dc00998140af 378 case 1:
maximbolduc 26:dc00998140af 379 height = token;
maximbolduc 26:dc00998140af 380 break;
maximbolduc 26:dc00998140af 381 }
maximbolduc 26:dc00998140af 382 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 383 token_counter++;
maximbolduc 26:dc00998140af 384 }
maximbolduc 26:dc00998140af 385 if ( height )
maximbolduc 26:dc00998140af 386 {
jhedmonton 28:5905886c76ee 387 antennaheight = atof(height);
maximbolduc 30:3afafa1ef16b 388 Config_Save();
maximbolduc 26:dc00998140af 389 }
maximbolduc 26:dc00998140af 390 }
maximbolduc 26:dc00998140af 391
maximbolduc 26:dc00998140af 392 void nmea_rmc(char *s)
maximbolduc 26:dc00998140af 393 {
maximbolduc 26:dc00998140af 394 char *token;
maximbolduc 26:dc00998140af 395 int token_counter = 0;
maximbolduc 26:dc00998140af 396 char *time = (char *)NULL;
maximbolduc 26:dc00998140af 397 char *date = (char *)NULL;
maximbolduc 26:dc00998140af 398 char *stat = (char *)NULL;
maximbolduc 26:dc00998140af 399 char *vel = (char *)NULL;
maximbolduc 26:dc00998140af 400 char *trk = (char *)NULL;
maximbolduc 26:dc00998140af 401 char *magv = (char *)NULL;
maximbolduc 26:dc00998140af 402 char *magd = (char *)NULL;
maximbolduc 26:dc00998140af 403 char *latitude = (char *)NULL;
maximbolduc 26:dc00998140af 404 char *longitude = (char *)NULL;
maximbolduc 26:dc00998140af 405 char *lat_dir = (char *)NULL;
maximbolduc 26:dc00998140af 406 char *lon_dir = (char *)NULL;
maximbolduc 26:dc00998140af 407
maximbolduc 26:dc00998140af 408 token = strtok(s, ",*");
maximbolduc 26:dc00998140af 409 while (token)
maximbolduc 26:dc00998140af 410 {
maximbolduc 26:dc00998140af 411 switch (token_counter)
maximbolduc 26:dc00998140af 412 {
maximbolduc 26:dc00998140af 413 case 9:
maximbolduc 26:dc00998140af 414 date = token;
maximbolduc 26:dc00998140af 415 break;
maximbolduc 26:dc00998140af 416 case 1:
maximbolduc 26:dc00998140af 417 time = token;
maximbolduc 26:dc00998140af 418 break;
maximbolduc 26:dc00998140af 419 case 2:
maximbolduc 26:dc00998140af 420 stat = token;
maximbolduc 26:dc00998140af 421 break;
maximbolduc 26:dc00998140af 422 case 7:
maximbolduc 26:dc00998140af 423 vel = token;
maximbolduc 26:dc00998140af 424 break;
maximbolduc 26:dc00998140af 425 case 8:
maximbolduc 26:dc00998140af 426 trk = token;
maximbolduc 26:dc00998140af 427 break;
maximbolduc 26:dc00998140af 428 case 10:
maximbolduc 26:dc00998140af 429 magv = token;
maximbolduc 26:dc00998140af 430 break;
maximbolduc 26:dc00998140af 431 case 11:
maximbolduc 26:dc00998140af 432 magd = token;
maximbolduc 26:dc00998140af 433 break;
maximbolduc 26:dc00998140af 434 case 3:
maximbolduc 26:dc00998140af 435 latitude = token;
maximbolduc 26:dc00998140af 436 break;
maximbolduc 26:dc00998140af 437 case 5:
maximbolduc 26:dc00998140af 438 longitude = token;
maximbolduc 26:dc00998140af 439 break;
maximbolduc 26:dc00998140af 440 case 4:
maximbolduc 26:dc00998140af 441 lat_dir = token;
maximbolduc 26:dc00998140af 442 break;
maximbolduc 26:dc00998140af 443 case 6:
maximbolduc 26:dc00998140af 444 lon_dir = token;
maximbolduc 26:dc00998140af 445 break;
maximbolduc 26:dc00998140af 446 }
maximbolduc 26:dc00998140af 447 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 448 token_counter++;
maximbolduc 26:dc00998140af 449 }
maximbolduc 26:dc00998140af 450 if (stat && date && time)
maximbolduc 26:dc00998140af 451 {
maximbolduc 26:dc00998140af 452 hour = (char)((time[0] - '0') * 10) + (time[1] - '0');
maximbolduc 26:dc00998140af 453 minute = (char)((time[2] - '0') * 10) + (time[3] - '0');
maximbolduc 26:dc00998140af 454 second = (char)((time[4] - '0') * 10) + (time[5] - '0');
maximbolduc 26:dc00998140af 455 day = (char)((date[0] - '0') * 10) + (date[1] - '0');
maximbolduc 26:dc00998140af 456 month = (char)((date[2] - '0') * 10) + (date[3] - '0');
maximbolduc 26:dc00998140af 457 year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
maximbolduc 26:dc00998140af 458 status = stat[0];
maximbolduc 26:dc00998140af 459 velocity = atof(vel);
maximbolduc 26:dc00998140af 460 speed_km = velocity * 1.852;
maximbolduc 26:dc00998140af 461 speed_m_s = speed_km * 3600.0 / 1000.0;
maximbolduc 26:dc00998140af 462 track = atof(trk);
maximbolduc 26:dc00998140af 463 magvar = atof(magv);
maximbolduc 26:dc00998140af 464 magvar_dir = magd[0];
maximbolduc 26:dc00998140af 465 }
maximbolduc 26:dc00998140af 466 decimal_latitude = lat_to_deg(latitude, lat_dir[0]);
maximbolduc 26:dc00998140af 467 decimal_lon = lon_to_deg(longitude, lon_dir[0]);
maximbolduc 26:dc00998140af 468 position.SetX(decimal_latitude);
maximbolduc 26:dc00998140af 469 position.SetY(decimal_lon);
maximbolduc 30:3afafa1ef16b 470 cm_per_deg_lat = 11054000;
maximbolduc 30:3afafa1ef16b 471 cm_per_deg_lon = 11132000 * cos(decimal_latitude);
maximbolduc 33:3e71c418e90d 472
maximbolduc 33:3e71c418e90d 473 // tilt_compensate(); //in centimeters
maximbolduc 33:3e71c418e90d 474 // pitch_compensate();
maximbolduc 33:3e71c418e90d 475 // yaw_compensate();
maximbolduc 33:3e71c418e90d 476
maximbolduc 26:dc00998140af 477 compensation.SetY(compensation.GetY() / cm_per_deg_lon);
maximbolduc 26:dc00998140af 478 compensation.SetX(compensation.GetX() / cm_per_deg_lat);
maximbolduc 26:dc00998140af 479 position = point_add(position,compensation);
maximbolduc 33:3e71c418e90d 480
jhedmonton 28:5905886c76ee 481 double lookaheaddistance = lookaheadtime * speed_m_s;
jhedmonton 28:5905886c76ee 482 get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,track,look_ahead_lon,look_ahead_lat);
maximbolduc 26:dc00998140af 483 looked_ahead.SetX(look_ahead_lat);
maximbolduc 26:dc00998140af 484 looked_ahead.SetY(look_ahead_lon);
maximbolduc 30:3afafa1ef16b 485 double filtering = 111111 / 2.0 + 111111 * cos(decimal_latitude)/2.0;
maximbolduc 26:dc00998140af 486 distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end);
maximbolduc 30:3afafa1ef16b 487 double sign = isLeft( line_start, line_end, looked_ahead);
maximbolduc 30:3afafa1ef16b 488 if ( sign < 0 )
maximbolduc 30:3afafa1ef16b 489 {
maximbolduc 30:3afafa1ef16b 490 distance_to_line = distance_to_line;
maximbolduc 30:3afafa1ef16b 491 }
maximbolduc 30:3afafa1ef16b 492 else if ( sign > 0 )
maximbolduc 30:3afafa1ef16b 493 {
maximbolduc 30:3afafa1ef16b 494 distance_to_line = -distance_to_line;
maximbolduc 30:3afafa1ef16b 495 }
maximbolduc 26:dc00998140af 496
maximbolduc 33:3e71c418e90d 497 //modify_rmc();
maximbolduc 33:3e71c418e90d 498
maximbolduc 30:3afafa1ef16b 499 sprintf(output,"$DIST_TO_LINE: % .12f %f\r\n\0",distance_to_line * filtering, sign);
maximbolduc 30:3afafa1ef16b 500 pc.puts(output);
maximbolduc 26:dc00998140af 501 }
maximbolduc 26:dc00998140af 502
maximbolduc 26:dc00998140af 503 void process_FGPSAB(char* ab)
maximbolduc 26:dc00998140af 504 {
maximbolduc 33:3e71c418e90d 505 char *token;
maximbolduc 33:3e71c418e90d 506 int token_counter = 0;
maximbolduc 33:3e71c418e90d 507 char *line_lat = (char *)NULL;
maximbolduc 33:3e71c418e90d 508 char *line_lon = (char *)NULL;
maximbolduc 33:3e71c418e90d 509 char *line_lat1 = (char *)NULL;
maximbolduc 33:3e71c418e90d 510 char *line_lon1 = (char *)NULL;
maximbolduc 33:3e71c418e90d 511 char *bearing = (char *)NULL;
maximbolduc 33:3e71c418e90d 512 token = strtok(ab, ",");
maximbolduc 33:3e71c418e90d 513 while (token)
maximbolduc 33:3e71c418e90d 514 {
maximbolduc 33:3e71c418e90d 515 switch (token_counter)
maximbolduc 26:dc00998140af 516 {
maximbolduc 33:3e71c418e90d 517 case 1:
maximbolduc 33:3e71c418e90d 518 line_lat = token;
maximbolduc 33:3e71c418e90d 519 break;
maximbolduc 33:3e71c418e90d 520 case 2:
maximbolduc 33:3e71c418e90d 521 line_lon = token;
maximbolduc 33:3e71c418e90d 522 break;
maximbolduc 33:3e71c418e90d 523 case 3:
maximbolduc 33:3e71c418e90d 524 line_lat1 = token;
maximbolduc 33:3e71c418e90d 525 break;
maximbolduc 33:3e71c418e90d 526 case 4:
maximbolduc 33:3e71c418e90d 527 line_lon1 = token;
maximbolduc 33:3e71c418e90d 528 break;
maximbolduc 33:3e71c418e90d 529 case 5:
maximbolduc 33:3e71c418e90d 530 bearing = token;
maximbolduc 33:3e71c418e90d 531 break;
maximbolduc 26:dc00998140af 532 }
maximbolduc 33:3e71c418e90d 533 token = strtok((char *)NULL, ",");
maximbolduc 33:3e71c418e90d 534 token_counter++;
maximbolduc 33:3e71c418e90d 535 }
maximbolduc 33:3e71c418e90d 536 Freepilot_lon = atof(line_lon);
maximbolduc 33:3e71c418e90d 537 Freepilot_lat = atof(line_lat);
maximbolduc 33:3e71c418e90d 538 Freepilot_lon1 = atof(line_lon1);
maximbolduc 33:3e71c418e90d 539 Freepilot_lat1 = atof(line_lat1);
maximbolduc 33:3e71c418e90d 540 Freepilot_bearing = atof(bearing);
maximbolduc 33:3e71c418e90d 541 line_start.SetX(Freepilot_lat);
maximbolduc 33:3e71c418e90d 542 line_start.SetY(Freepilot_lon);
maximbolduc 33:3e71c418e90d 543 line_end.SetX(Freepilot_lat1);
maximbolduc 33:3e71c418e90d 544 line_end.SetY(Freepilot_lon1);
maximbolduc 33:3e71c418e90d 545 active_AB = 1;
maximbolduc 33:3e71c418e90d 546
maximbolduc 33:3e71c418e90d 547 sprintf(output, "$ABLINE:%f , %f, %f, %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY());
maximbolduc 33:3e71c418e90d 548 pc.puts(output);
maximbolduc 26:dc00998140af 549 }
maximbolduc 26:dc00998140af 550
maximbolduc 26:dc00998140af 551 void autosteer_done()
maximbolduc 26:dc00998140af 552 {
maximbolduc 26:dc00998140af 553 //kill the autosteer once the timeout reech
maximbolduc 26:dc00998140af 554 enable_motor = 0;
maximbolduc 26:dc00998140af 555 }
maximbolduc 26:dc00998140af 556
maximbolduc 26:dc00998140af 557 void process_FGPSAUTO(char* FGPSAUTO)
maximbolduc 26:dc00998140af 558 {
maximbolduc 26:dc00998140af 559 char *token;
maximbolduc 26:dc00998140af 560 int token_counter = 0;
maximbolduc 26:dc00998140af 561 char *ahead = (char *)NULL;
maximbolduc 26:dc00998140af 562 char *center = (char *)NULL;
maximbolduc 26:dc00998140af 563 char *phase = (char *)NULL;
maximbolduc 26:dc00998140af 564 char *scl = (char *)NULL;
maximbolduc 26:dc00998140af 565 char *avg = (char *)NULL;
maximbolduc 26:dc00998140af 566 char *_kp = (char *)NULL;
maximbolduc 26:dc00998140af 567 char *_ki = (char *)NULL;
maximbolduc 26:dc00998140af 568 char *_kd = (char *)NULL;
maximbolduc 26:dc00998140af 569 token = strtok(FGPSAUTO, ",");
maximbolduc 26:dc00998140af 570 while (token)
maximbolduc 26:dc00998140af 571 {
maximbolduc 26:dc00998140af 572 switch (token_counter)
maximbolduc 26:dc00998140af 573 {
maximbolduc 26:dc00998140af 574 case 1:
maximbolduc 26:dc00998140af 575 phase = token;
maximbolduc 26:dc00998140af 576 break;
maximbolduc 26:dc00998140af 577 case 2:
maximbolduc 26:dc00998140af 578 center = token;
maximbolduc 26:dc00998140af 579 break;
maximbolduc 26:dc00998140af 580 case 4:
maximbolduc 26:dc00998140af 581 scl = token;
maximbolduc 26:dc00998140af 582 break;
maximbolduc 26:dc00998140af 583 case 5:
maximbolduc 26:dc00998140af 584 ahead = token;
maximbolduc 26:dc00998140af 585 break;
maximbolduc 26:dc00998140af 586 case 6:
maximbolduc 26:dc00998140af 587 avg = token;
maximbolduc 26:dc00998140af 588 break;
maximbolduc 26:dc00998140af 589 case 7:
maximbolduc 26:dc00998140af 590 _kp = token;
maximbolduc 26:dc00998140af 591 break;
maximbolduc 26:dc00998140af 592 case 8:
maximbolduc 26:dc00998140af 593 _ki = token;
maximbolduc 26:dc00998140af 594 break;
maximbolduc 26:dc00998140af 595 case 9:
maximbolduc 26:dc00998140af 596 _kd = token;
maximbolduc 26:dc00998140af 597 break;
maximbolduc 26:dc00998140af 598 }
maximbolduc 26:dc00998140af 599 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 600 token_counter++;
maximbolduc 26:dc00998140af 601 }
maximbolduc 26:dc00998140af 602 if ( _kp && _ki && _kd )
maximbolduc 26:dc00998140af 603 {
maximbolduc 26:dc00998140af 604 kp = atof(_kp);
maximbolduc 26:dc00998140af 605 ki = atof(_ki);
maximbolduc 26:dc00998140af 606 kd = atof(_kd);
maximbolduc 26:dc00998140af 607 }
maximbolduc 26:dc00998140af 608 if ( phase && center && scl && avg && ahead )
maximbolduc 26:dc00998140af 609 {
jhedmonton 28:5905886c76ee 610 lookaheadtime = atof(ahead);
maximbolduc 26:dc00998140af 611 scale = atof(scl);
maximbolduc 26:dc00998140af 612 phaseadv = atof(phase);
jhedmonton 28:5905886c76ee 613 avgpos = atof(avg);
jhedmonton 28:5905886c76ee 614 tcenter = atof(center);
maximbolduc 30:3afafa1ef16b 615 sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
maximbolduc 30:3afafa1ef16b 616 pc.puts(output);
maximbolduc 26:dc00998140af 617 }
maximbolduc 26:dc00998140af 618 }
maximbolduc 30:3afafa1ef16b 619
jhedmonton 27:9ac59b261d87 620 //sets pwm1 and pwm2 and enable_motor
maximbolduc 26:dc00998140af 621 void process_ASTEER(char* asteer)
maximbolduc 26:dc00998140af 622 {
maximbolduc 26:dc00998140af 623 char *token;
maximbolduc 26:dc00998140af 624 int token_counter = 0;
maximbolduc 26:dc00998140af 625 char *asteer_speed = (char *)NULL;
maximbolduc 26:dc00998140af 626 char *asteer_time = (char *)NULL;
maximbolduc 26:dc00998140af 627 token = strtok(asteer, ",");
maximbolduc 26:dc00998140af 628 while (token)
maximbolduc 26:dc00998140af 629 {
maximbolduc 26:dc00998140af 630 switch (token_counter)
maximbolduc 26:dc00998140af 631 {
maximbolduc 26:dc00998140af 632 case 1:
maximbolduc 26:dc00998140af 633 asteer_speed = token;
maximbolduc 26:dc00998140af 634 break;
maximbolduc 26:dc00998140af 635 case 2:
maximbolduc 26:dc00998140af 636 asteer_time = token;
maximbolduc 26:dc00998140af 637 break;
maximbolduc 26:dc00998140af 638 }
maximbolduc 26:dc00998140af 639 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 640 token_counter++;
maximbolduc 26:dc00998140af 641 }
maximbolduc 26:dc00998140af 642 if ( asteer_speed && asteer_time )
maximbolduc 26:dc00998140af 643 {
maximbolduc 26:dc00998140af 644 motorspeed = atof(asteer_speed);
maximbolduc 26:dc00998140af 645 enable_time = atof(asteer_time);
jhedmonton 28:5905886c76ee 646 autosteer_timeout.attach_us(autosteer_done,(double)enable_time * (double)1000.0);
jhedmonton 28:5905886c76ee 647 if ( motorspeed > 127.0 )
maximbolduc 26:dc00998140af 648 {
jhedmonton 28:5905886c76ee 649 pwm2_speed = 0.0;
jhedmonton 28:5905886c76ee 650 pwm1_speed = ((double)motorspeed - (double)127.0) / 127.0;
maximbolduc 26:dc00998140af 651 enable_motor = 1;
maximbolduc 26:dc00998140af 652 }
jhedmonton 28:5905886c76ee 653 else if ( motorspeed < 127.0 )
maximbolduc 26:dc00998140af 654 {
jhedmonton 28:5905886c76ee 655 pwm2_speed = ( ((double) 127-(double)motorspeed) / 127.0 );
jhedmonton 28:5905886c76ee 656 pwm1_speed = 0.0;
maximbolduc 26:dc00998140af 657 enable_motor = 1;
maximbolduc 26:dc00998140af 658 }
maximbolduc 26:dc00998140af 659 else
maximbolduc 26:dc00998140af 660 {
maximbolduc 26:dc00998140af 661 pwm1_speed = 0;
maximbolduc 26:dc00998140af 662 pwm2_speed = 0;
maximbolduc 26:dc00998140af 663 enable_motor = 0;
maximbolduc 26:dc00998140af 664 }
jhedmonton 29:23ccb2a50b6f 665 if(Authenticated)
jhedmonton 29:23ccb2a50b6f 666 {
jhedmonton 29:23ccb2a50b6f 667 pwm1 = pwm1_speed;
jhedmonton 29:23ccb2a50b6f 668 pwm2 = pwm2_speed;
jhedmonton 29:23ccb2a50b6f 669 }
jhedmonton 29:23ccb2a50b6f 670 else
jhedmonton 29:23ccb2a50b6f 671 {
jhedmonton 29:23ccb2a50b6f 672 sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);
jhedmonton 29:23ccb2a50b6f 673 pc.puts(output);
jhedmonton 29:23ccb2a50b6f 674 bluetooth.puts(output);
jhedmonton 29:23ccb2a50b6f 675 }
maximbolduc 26:dc00998140af 676 }
maximbolduc 26:dc00998140af 677 }
jhedmonton 28:5905886c76ee 678
maximbolduc 26:dc00998140af 679 void pc_analyse(char* pc_string)
maximbolduc 26:dc00998140af 680 {
maximbolduc 26:dc00998140af 681 if (!strncmp(pc_string, "$ASTEER", 7))
maximbolduc 26:dc00998140af 682 {
maximbolduc 26:dc00998140af 683 process_ASTEER(pc_string);
maximbolduc 26:dc00998140af 684 }
jhedmonton 29:23ccb2a50b6f 685 else if (!strncmp(pc_string, "$BANY",5))
jhedmonton 29:23ccb2a50b6f 686 {
jhedmonton 29:23ccb2a50b6f 687 _ID = Config_GetID();
jhedmonton 29:23ccb2a50b6f 688 Config_Save();
jhedmonton 29:23ccb2a50b6f 689 }
maximbolduc 30:3afafa1ef16b 690 else if (!strncmp(pc_string, "$GPSBAUD",8))
maximbolduc 26:dc00998140af 691 {
maximbolduc 26:dc00998140af 692 process_GPSBAUD(pc_string);
maximbolduc 30:3afafa1ef16b 693 Config_Save();
jhedmonton 27:9ac59b261d87 694 sprintf(output,"%s %d\r\n",pc_string,gps_baud);
jhedmonton 27:9ac59b261d87 695 pc.puts(output);
maximbolduc 26:dc00998140af 696 }
maximbolduc 26:dc00998140af 697 else if (!strncmp(pc_string, "$FGPS,",6))
maximbolduc 26:dc00998140af 698 {
jhedmonton 27:9ac59b261d87 699 int i=5;
jhedmonton 27:9ac59b261d87 700 char c=pc_string[i];
jhedmonton 27:9ac59b261d87 701 while (c!=0)
jhedmonton 27:9ac59b261d87 702 {
jhedmonton 27:9ac59b261d87 703 i++;
jhedmonton 27:9ac59b261d87 704 if (i>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 705 c=pc_string[i];
jhedmonton 28:5905886c76ee 706 gps.putc(c);
jhedmonton 27:9ac59b261d87 707 pc.putc(c);
jhedmonton 27:9ac59b261d87 708 }
maximbolduc 26:dc00998140af 709 }
maximbolduc 30:3afafa1ef16b 710
maximbolduc 30:3afafa1ef16b 711 else if (!strncmp(pc_string, "$GPSHEIGHT",10))
maximbolduc 26:dc00998140af 712 {
maximbolduc 26:dc00998140af 713 process_GPSHEIGHT(pc_string);
maximbolduc 26:dc00998140af 714 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 715 bluetooth.puts(output);
maximbolduc 30:3afafa1ef16b 716 Config_Save();
maximbolduc 26:dc00998140af 717 }
maximbolduc 26:dc00998140af 718 else if (!strncmp(pc_string, "$FGPSAUTO",9))
maximbolduc 26:dc00998140af 719 {
maximbolduc 26:dc00998140af 720 process_FGPSAUTO(pc_string);
maximbolduc 26:dc00998140af 721 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 722 bluetooth.puts(output);
maximbolduc 30:3afafa1ef16b 723 Config_Save();
maximbolduc 26:dc00998140af 724 }
maximbolduc 26:dc00998140af 725 else if (!strncmp(pc_string, "$FGPSAB",7))
maximbolduc 26:dc00998140af 726 {
maximbolduc 26:dc00998140af 727 process_FGPSAB(pc_string);
maximbolduc 30:3afafa1ef16b 728 }
maximbolduc 30:3afafa1ef16b 729 else if (!strncmp(pc_string, "$CALIBRATEACCEL",15))
maximbolduc 30:3afafa1ef16b 730 {
maximbolduc 32:c57bc701d65c 731 calibrateGyroscope();
maximbolduc 30:3afafa1ef16b 732 calibrateAccelerometer();
maximbolduc 30:3afafa1ef16b 733 Config_Save();
maximbolduc 32:c57bc701d65c 734 }
maximbolduc 32:c57bc701d65c 735 else if (!strncmp(pc_string, "$POSITION",9))
maximbolduc 32:c57bc701d65c 736 {
maximbolduc 32:c57bc701d65c 737
maximbolduc 32:c57bc701d65c 738 char* pointer;
maximbolduc 32:c57bc701d65c 739 char* Data[5];
maximbolduc 32:c57bc701d65c 740 int index = 0;
maximbolduc 32:c57bc701d65c 741 //Split data at commas
maximbolduc 32:c57bc701d65c 742 pointer = strtok(pc_string, ",");
maximbolduc 32:c57bc701d65c 743 if(pointer == NULL)
maximbolduc 32:c57bc701d65c 744 Data[0] = pc_string;
maximbolduc 32:c57bc701d65c 745 while(pointer != NULL)
maximbolduc 32:c57bc701d65c 746 {
maximbolduc 32:c57bc701d65c 747 Data[index] = pointer;
maximbolduc 32:c57bc701d65c 748 pointer = strtok(NULL, ",");
maximbolduc 32:c57bc701d65c 749 index++;
maximbolduc 32:c57bc701d65c 750 }
maximbolduc 33:3e71c418e90d 751 //int temp_pos =
maximbolduc 32:c57bc701d65c 752 gyro_pos = atoi(Data[1]);
maximbolduc 33:3e71c418e90d 753 pc.printf("POSITION=%i\r\n",gyro_pos);//("POSITION=");
maximbolduc 32:c57bc701d65c 754 Config_Save();
maximbolduc 32:c57bc701d65c 755 }
maximbolduc 26:dc00998140af 756 else
maximbolduc 26:dc00998140af 757 {
maximbolduc 26:dc00998140af 758 }
maximbolduc 26:dc00998140af 759 }
maximbolduc 26:dc00998140af 760
maximbolduc 26:dc00998140af 761 void gps_analyse(char* gps_string)
maximbolduc 26:dc00998140af 762 {
maximbolduc 30:3afafa1ef16b 763 pc.puts(gps_string);
maximbolduc 32:c57bc701d65c 764 //bluetooth.puts(gps_string);
maximbolduc 26:dc00998140af 765 if (!strncmp(gps_string, "$GPRMC", 6))
maximbolduc 26:dc00998140af 766 {
maximbolduc 26:dc00998140af 767 nmea_rmc(gps_string); //analyse and decompose the rmc string
maximbolduc 33:3e71c418e90d 768 }
maximbolduc 26:dc00998140af 769 }
maximbolduc 26:dc00998140af 770
jhedmonton 27:9ac59b261d87 771 int i2 = 0;
jhedmonton 27:9ac59b261d87 772 bool end2 = false;
jhedmonton 27:9ac59b261d87 773 bool start2 = false;
jhedmonton 27:9ac59b261d87 774
jhedmonton 27:9ac59b261d87 775 bool getline2()
maximbolduc 26:dc00998140af 776 {
jhedmonton 27:9ac59b261d87 777 int gotstring=false;
maximbolduc 33:3e71c418e90d 778 while (1)
maximbolduc 33:3e71c418e90d 779 {
maximbolduc 33:3e71c418e90d 780 if( !bluetooth.readable() )
jhedmonton 27:9ac59b261d87 781 {
jhedmonton 27:9ac59b261d87 782 break;
jhedmonton 27:9ac59b261d87 783 }
jhedmonton 27:9ac59b261d87 784 char c = bluetooth.getc();
maximbolduc 33:3e71c418e90d 785 if (c == 36 )
jhedmonton 29:23ccb2a50b6f 786 {
maximbolduc 33:3e71c418e90d 787 start2=true;
maximbolduc 33:3e71c418e90d 788 end2 = false;
maximbolduc 33:3e71c418e90d 789 i2 = 0;
maximbolduc 33:3e71c418e90d 790 }
maximbolduc 33:3e71c418e90d 791 if ((start2) && (c == 10))
maximbolduc 33:3e71c418e90d 792 {
maximbolduc 33:3e71c418e90d 793 end2=true;
jhedmonton 29:23ccb2a50b6f 794 start2 = false;
jhedmonton 29:23ccb2a50b6f 795 }
jhedmonton 29:23ccb2a50b6f 796 if (start2)
maximbolduc 26:dc00998140af 797 {
jhedmonton 27:9ac59b261d87 798 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 799 i2++;
jhedmonton 27:9ac59b261d87 800 if (i2>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 801 }
maximbolduc 33:3e71c418e90d 802 if (end2)
jhedmonton 27:9ac59b261d87 803 {
maximbolduc 33:3e71c418e90d 804 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 805 msg2[i2+1] = 0;
jhedmonton 27:9ac59b261d87 806 start2 = false;
jhedmonton 27:9ac59b261d87 807 gotstring = true;
jhedmonton 27:9ac59b261d87 808 end2=false;
jhedmonton 27:9ac59b261d87 809 i2=0;
jhedmonton 27:9ac59b261d87 810 break;
maximbolduc 26:dc00998140af 811 }
maximbolduc 26:dc00998140af 812 }
jhedmonton 27:9ac59b261d87 813 return gotstring;
maximbolduc 26:dc00998140af 814 }
maximbolduc 26:dc00998140af 815
jhedmonton 27:9ac59b261d87 816
jhedmonton 27:9ac59b261d87 817 int i=0;
jhedmonton 27:9ac59b261d87 818 bool start=false;
jhedmonton 27:9ac59b261d87 819 bool end=false;
jhedmonton 27:9ac59b261d87 820
jhedmonton 27:9ac59b261d87 821 bool getline(bool forward)
maximbolduc 26:dc00998140af 822 {
jhedmonton 27:9ac59b261d87 823 while (1)
jhedmonton 27:9ac59b261d87 824 {
jhedmonton 28:5905886c76ee 825 if( !gps.readable() )
jhedmonton 27:9ac59b261d87 826 {
jhedmonton 27:9ac59b261d87 827 break;
jhedmonton 27:9ac59b261d87 828 }
jhedmonton 28:5905886c76ee 829 char c = gps.getc();
jhedmonton 27:9ac59b261d87 830 if (forward) //simply forward all to Bluetooth
maximbolduc 26:dc00998140af 831 {
jhedmonton 27:9ac59b261d87 832 bluetooth.putc(c);
jhedmonton 27:9ac59b261d87 833 }
jhedmonton 27:9ac59b261d87 834 if (c == 36 ){start=true;end = false; i = 0;}
jhedmonton 29:23ccb2a50b6f 835 if ((start) && (c == 10))
jhedmonton 29:23ccb2a50b6f 836 {
jhedmonton 29:23ccb2a50b6f 837 end=true;
jhedmonton 29:23ccb2a50b6f 838 start = false;
jhedmonton 29:23ccb2a50b6f 839 }
jhedmonton 27:9ac59b261d87 840 if (start)
jhedmonton 27:9ac59b261d87 841 {
jhedmonton 27:9ac59b261d87 842 msg[i]=c;
jhedmonton 27:9ac59b261d87 843 i++;
jhedmonton 27:9ac59b261d87 844 if (i>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 845 }
jhedmonton 27:9ac59b261d87 846 if (end)
maximbolduc 30:3afafa1ef16b 847 {
jhedmonton 27:9ac59b261d87 848 msg[i]=c;
maximbolduc 26:dc00998140af 849 msg[i+1] = 0;
jhedmonton 29:23ccb2a50b6f 850 i=0;
jhedmonton 27:9ac59b261d87 851 start = false;
jhedmonton 27:9ac59b261d87 852 end = true;
jhedmonton 27:9ac59b261d87 853 break;
maximbolduc 26:dc00998140af 854 }
maximbolduc 26:dc00998140af 855 }
jhedmonton 27:9ac59b261d87 856 return end;
maximbolduc 26:dc00998140af 857 }
maximbolduc 26:dc00998140af 858
maximbolduc 26:dc00998140af 859 void keyPressedHeld( void )
maximbolduc 26:dc00998140af 860 {
jhedmonton 28:5905886c76ee 861 motor_enable_state = "$ENABLE,1\r\n";
jhedmonton 28:5905886c76ee 862 motor_enable = 1;
jhedmonton 28:5905886c76ee 863 ledGREEN=1; //show green for being ready to steer
maximbolduc 26:dc00998140af 864 }
maximbolduc 26:dc00998140af 865
maximbolduc 26:dc00998140af 866 void keyReleasedHeld( void )
maximbolduc 26:dc00998140af 867 {
jhedmonton 28:5905886c76ee 868 motor_enable_state = "$ENABLE,0\r\n";
jhedmonton 28:5905886c76ee 869 motor_enable = 0;
jhedmonton 28:5905886c76ee 870 ledGREEN=0;
jhedmonton 27:9ac59b261d87 871 }
jhedmonton 27:9ac59b261d87 872
jhedmonton 27:9ac59b261d87 873 void boom1PressedHeld( void )
jhedmonton 27:9ac59b261d87 874 {
jhedmonton 28:5905886c76ee 875 // ledGREEN=1;
jhedmonton 27:9ac59b261d87 876 boom18=boom18 & 0xFE;
jhedmonton 27:9ac59b261d87 877 }
jhedmonton 27:9ac59b261d87 878
jhedmonton 27:9ac59b261d87 879 void boom1ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 880 {
jhedmonton 28:5905886c76ee 881 //ledGREEN=0;
jhedmonton 27:9ac59b261d87 882 boom18=boom18 | 0x01;
jhedmonton 27:9ac59b261d87 883 }
jhedmonton 27:9ac59b261d87 884
jhedmonton 27:9ac59b261d87 885 void boom2PressedHeld( void )
jhedmonton 27:9ac59b261d87 886 {
jhedmonton 27:9ac59b261d87 887 boom18=boom18 & 0xFD;
maximbolduc 26:dc00998140af 888 }
maximbolduc 26:dc00998140af 889
jhedmonton 27:9ac59b261d87 890 void boom2ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 891 {
jhedmonton 27:9ac59b261d87 892 boom18=boom18 | 0x02;
jhedmonton 27:9ac59b261d87 893 }
jhedmonton 27:9ac59b261d87 894 void boom3PressedHeld( void )
jhedmonton 27:9ac59b261d87 895 {
jhedmonton 27:9ac59b261d87 896 boom18=boom18 & 0xFB;
jhedmonton 27:9ac59b261d87 897 }
jhedmonton 27:9ac59b261d87 898
jhedmonton 27:9ac59b261d87 899 void boom3ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 900 {
jhedmonton 27:9ac59b261d87 901 boom18=boom18 | 0x04;
jhedmonton 27:9ac59b261d87 902 }
jhedmonton 27:9ac59b261d87 903
jhedmonton 27:9ac59b261d87 904 void boom4PressedHeld( void )
jhedmonton 27:9ac59b261d87 905 {
maximbolduc 32:c57bc701d65c 906 boom18=boom18 & 0xF7;
jhedmonton 27:9ac59b261d87 907 }
jhedmonton 27:9ac59b261d87 908
jhedmonton 27:9ac59b261d87 909 void boom4ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 910 {
maximbolduc 32:c57bc701d65c 911 boom18=boom18 | 0x08;
jhedmonton 27:9ac59b261d87 912 }
jhedmonton 27:9ac59b261d87 913
maximbolduc 26:dc00998140af 914 void toprint()
maximbolduc 26:dc00998140af 915 {
maximbolduc 26:dc00998140af 916 angle_send = 1;
maximbolduc 26:dc00998140af 917 }
maximbolduc 26:dc00998140af 918
maximbolduc 32:c57bc701d65c 919 char* checksum2;
maximbolduc 30:3afafa1ef16b 920 int getCheckSum(char *string)
maximbolduc 26:dc00998140af 921 {
maximbolduc 30:3afafa1ef16b 922 int i;
maximbolduc 30:3afafa1ef16b 923 int XOR;
maximbolduc 30:3afafa1ef16b 924 int c;
maximbolduc 32:c57bc701d65c 925 bool started = false;
maximbolduc 30:3afafa1ef16b 926 for (XOR = 0, i = 0; i < strlen(string); i++)
maximbolduc 30:3afafa1ef16b 927 {
maximbolduc 30:3afafa1ef16b 928 c = (unsigned char)string[i];
maximbolduc 32:c57bc701d65c 929 if ( c == '$' )started = true;
maximbolduc 32:c57bc701d65c 930
maximbolduc 32:c57bc701d65c 931 if ( started == true )
maximbolduc 32:c57bc701d65c 932 {
maximbolduc 32:c57bc701d65c 933 if (c == '*')
maximbolduc 32:c57bc701d65c 934 {
maximbolduc 32:c57bc701d65c 935 break;
maximbolduc 32:c57bc701d65c 936 }
maximbolduc 30:3afafa1ef16b 937 if (c != '$') XOR ^= c;
maximbolduc 32:c57bc701d65c 938 }
maximbolduc 30:3afafa1ef16b 939 }
maximbolduc 30:3afafa1ef16b 940 return XOR;
maximbolduc 26:dc00998140af 941 }
maximbolduc 26:dc00998140af 942
maximbolduc 33:3e71c418e90d 943 bool validate_checksum(char* validating)
maximbolduc 33:3e71c418e90d 944 {
maximbolduc 33:3e71c418e90d 945 bool valid = false;
maximbolduc 33:3e71c418e90d 946 int tempo = getCheckSum(msg);
maximbolduc 33:3e71c418e90d 947 string token, mystring(msg);
maximbolduc 33:3e71c418e90d 948 while(token != mystring)
maximbolduc 33:3e71c418e90d 949 {
maximbolduc 33:3e71c418e90d 950 token = mystring.substr(0,mystring.find_first_of("*"));
maximbolduc 33:3e71c418e90d 951 mystring = mystring.substr(mystring.find_first_of("*") + 1,2);
maximbolduc 33:3e71c418e90d 952 }
maximbolduc 33:3e71c418e90d 953 checksumm = atoh <uint16_t>(token.c_str());
maximbolduc 33:3e71c418e90d 954 if (tempo == checksumm)
maximbolduc 33:3e71c418e90d 955 {
maximbolduc 33:3e71c418e90d 956 valid = true;
maximbolduc 33:3e71c418e90d 957 }
maximbolduc 33:3e71c418e90d 958 return valid;
maximbolduc 33:3e71c418e90d 959 }
maximbolduc 33:3e71c418e90d 960
maximbolduc 26:dc00998140af 961 int main()
maximbolduc 26:dc00998140af 962 {
jhedmonton 27:9ac59b261d87 963 int x=0;
jhedmonton 27:9ac59b261d87 964 bluetooth.baud(115200);
jhedmonton 28:5905886c76ee 965 gps.baud(38400);
maximbolduc 30:3afafa1ef16b 966 pc.baud(38400);
jhedmonton 27:9ac59b261d87 967
jhedmonton 27:9ac59b261d87 968 //JH prepare and send version info
jhedmonton 27:9ac59b261d87 969 vTimer.start();
jhedmonton 27:9ac59b261d87 970 vTimer.reset();
jhedmonton 28:5905886c76ee 971 motTimer.start();
jhedmonton 28:5905886c76ee 972 motTimer.reset();
jhedmonton 29:23ccb2a50b6f 973 lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
jhedmonton 29:23ccb2a50b6f 974 motor_enable_state = "$ENABLE,0\r\n";
maximbolduc 33:3e71c418e90d 975 initializeAccelerometer();
maximbolduc 33:3e71c418e90d 976 calibrateAccelerometer();
maximbolduc 33:3e71c418e90d 977 initializeGyroscope();
maximbolduc 33:3e71c418e90d 978 calibrateGyroscope();
jhedmonton 28:5905886c76ee 979 btTimer.start();
jhedmonton 28:5905886c76ee 980 btTimer.reset();
jhedmonton 28:5905886c76ee 981 lastgetBT= btTimer.read_ms();
jhedmonton 28:5905886c76ee 982
jhedmonton 27:9ac59b261d87 983 pc.puts(version);
jhedmonton 27:9ac59b261d87 984 bluetooth.puts(version);
jhedmonton 29:23ccb2a50b6f 985 lastsend_version=vTimer.read_ms()-18000;
jhedmonton 27:9ac59b261d87 986
maximbolduc 30:3afafa1ef16b 987 Config_Startup();
maximbolduc 30:3afafa1ef16b 988 _ID = Config_GetID();
maximbolduc 30:3afafa1ef16b 989 Config_Save();
jhedmonton 27:9ac59b261d87 990
jhedmonton 27:9ac59b261d87 991 boom1.attach_asserted_held( &boom1PressedHeld );
jhedmonton 27:9ac59b261d87 992 boom1.attach_deasserted_held( &boom1ReleasedHeld );
jhedmonton 27:9ac59b261d87 993 boom1.setSampleFrequency(); //default = 20 ms
jhedmonton 27:9ac59b261d87 994 boom1.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 995 boom1.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 996 boom2.attach_asserted_held( &boom2PressedHeld );
jhedmonton 27:9ac59b261d87 997 boom2.attach_deasserted_held( &boom2ReleasedHeld );
jhedmonton 27:9ac59b261d87 998 boom2.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 999 boom2.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1000 boom2.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1001 boom3.attach_asserted_held( &boom3PressedHeld );
jhedmonton 27:9ac59b261d87 1002 boom3.attach_deasserted_held( &boom3ReleasedHeld );
jhedmonton 27:9ac59b261d87 1003 boom3.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1004 boom3.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1005 boom3.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1006 boom4.attach_asserted_held( &boom4PressedHeld );
jhedmonton 27:9ac59b261d87 1007 boom4.attach_deasserted_held( &boom4ReleasedHeld );
jhedmonton 27:9ac59b261d87 1008 boom4.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1009 boom4.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1010 boom4.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1011
maximbolduc 26:dc00998140af 1012 motor_switch.setSampleFrequency(10000);
maximbolduc 26:dc00998140af 1013 motor_switch.attach_asserted_held( &keyPressedHeld );
maximbolduc 26:dc00998140af 1014 motor_switch.attach_deasserted_held( &keyReleasedHeld );
maximbolduc 33:3e71c418e90d 1015
maximbolduc 30:3afafa1ef16b 1016 accelerometerTicker.attach(&sampleAccelerometer, 0.005);
maximbolduc 30:3afafa1ef16b 1017 gyroscopeTicker.attach(&sampleGyroscope, 0.005);
maximbolduc 30:3afafa1ef16b 1018 filterTicker.attach(&filter, FILTER_RATE);
maximbolduc 30:3afafa1ef16b 1019 angle_print.attach(&toprint,0.2);
maximbolduc 26:dc00998140af 1020 activate_antenna();
jhedmonton 29:23ccb2a50b6f 1021
maximbolduc 26:dc00998140af 1022 while(1)
maximbolduc 26:dc00998140af 1023 {
jhedmonton 27:9ac59b261d87 1024 //JH send version information every 10 seconds to keep Bluetooth alive
jhedmonton 29:23ccb2a50b6f 1025 if ((vTimer.read_ms()-lastsend_version)>25000)
maximbolduc 26:dc00998140af 1026 {
jhedmonton 27:9ac59b261d87 1027 pc.puts(version);
jhedmonton 27:9ac59b261d87 1028 bluetooth.puts(version);
jhedmonton 27:9ac59b261d87 1029 vTimer.reset();
jhedmonton 27:9ac59b261d87 1030 lastsend_version=vTimer.read_ms();
jhedmonton 27:9ac59b261d87 1031 }
jhedmonton 27:9ac59b261d87 1032
jhedmonton 28:5905886c76ee 1033 if ( antenna_active == 1 && gps.readable())
jhedmonton 27:9ac59b261d87 1034 {
jhedmonton 27:9ac59b261d87 1035 if (getline(true))
maximbolduc 26:dc00998140af 1036 {
maximbolduc 33:3e71c418e90d 1037 if ( validate_checksum(msg))
maximbolduc 32:c57bc701d65c 1038 {
maximbolduc 33:3e71c418e90d 1039 gps_analyse(msg);
maximbolduc 33:3e71c418e90d 1040 }
maximbolduc 33:3e71c418e90d 1041 else
maximbolduc 33:3e71c418e90d 1042 {
maximbolduc 33:3e71c418e90d 1043 pc.puts("INVALID!!!!\r\n");
maximbolduc 32:c57bc701d65c 1044 }
maximbolduc 26:dc00998140af 1045 }
maximbolduc 26:dc00998140af 1046 }
maximbolduc 26:dc00998140af 1047 if ( bluetooth.readable())
maximbolduc 26:dc00998140af 1048 {
jhedmonton 27:9ac59b261d87 1049 if (getline2())
jhedmonton 27:9ac59b261d87 1050 {
jhedmonton 28:5905886c76ee 1051 btTimer.reset();
jhedmonton 28:5905886c76ee 1052 lastgetBT= btTimer.read_ms();
jhedmonton 27:9ac59b261d87 1053 x++;
maximbolduc 26:dc00998140af 1054 trim(msg2," ");
jhedmonton 27:9ac59b261d87 1055 sprintf(output,"%d %s",x,msg2);
maximbolduc 26:dc00998140af 1056 pc.puts(output);
maximbolduc 26:dc00998140af 1057 pc_analyse(msg2);
maximbolduc 26:dc00998140af 1058 }
maximbolduc 26:dc00998140af 1059 }
jhedmonton 28:5905886c76ee 1060 if ( btTimer.read_ms()-lastgetBT>1000)
jhedmonton 28:5905886c76ee 1061 {
jhedmonton 28:5905886c76ee 1062 //we did not get any commands over BT
jhedmonton 28:5905886c76ee 1063 ledRED=1; //turn red
jhedmonton 28:5905886c76ee 1064 }
jhedmonton 28:5905886c76ee 1065 else ledRED=0;
jhedmonton 28:5905886c76ee 1066
jhedmonton 29:23ccb2a50b6f 1067 if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable))
maximbolduc 26:dc00998140af 1068 {
maximbolduc 26:dc00998140af 1069 bluetooth.puts(motor_enable_state);
jhedmonton 28:5905886c76ee 1070 pc.puts(motor_enable_state);
jhedmonton 28:5905886c76ee 1071 motTimer.reset();
jhedmonton 28:5905886c76ee 1072 lastsend_motorstate=motTimer.read_ms();
jhedmonton 28:5905886c76ee 1073 lastmotor_enable=motor_enable;
maximbolduc 26:dc00998140af 1074 }
jhedmonton 27:9ac59b261d87 1075 if (boom18!=lastboom18)
jhedmonton 27:9ac59b261d87 1076 {
jhedmonton 27:9ac59b261d87 1077 boomstate[4]=boom18 | 0x80; //
jhedmonton 27:9ac59b261d87 1078 bluetooth.puts(boomstate);
jhedmonton 27:9ac59b261d87 1079 pc.puts(boomstate);
jhedmonton 27:9ac59b261d87 1080 lastboom18=boom18;
jhedmonton 27:9ac59b261d87 1081 }
maximbolduc 30:3afafa1ef16b 1082 if ( print_euler == 1 && angle_send == 1 ) //&& reading == 0)
maximbolduc 26:dc00998140af 1083 {
maximbolduc 33:3e71c418e90d 1084 sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(get_roll()),toDegrees(get_pitch()),toDegrees(get_yaw()));
maximbolduc 33:3e71c418e90d 1085 pc.puts(output);
maximbolduc 30:3afafa1ef16b 1086 bluetooth.puts(output);
maximbolduc 26:dc00998140af 1087 angle_send = 0;
jhedmonton 27:9ac59b261d87 1088 }
maximbolduc 26:dc00998140af 1089 }
maximbolduc 26:dc00998140af 1090 }