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@35:f9caeb8ca31e, 2015-02-21 (annotated)

Committer:
maximbolduc
Date:
Sat Feb 21 13:47:37 2015 +0000
Revision:
35:f9caeb8ca31e
Parent:
34:c2bc9f9be7ff
Child:
36:8e84b5ade03e
little basic autosteer routine

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