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@36:8e84b5ade03e, 2015-02-21 (annotated)

Committer:
maximbolduc
Date:
Sat Feb 21 17:51:04 2015 +0000
Revision:
36:8e84b5ade03e
Parent:
35:f9caeb8ca31e
Child:
37:ac60a8a0ae8a
A lot of tiny improvments;

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