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@34:c2bc9f9be7ff, 2015-02-13 (annotated)

Committer:
maximbolduc
Date:
Fri Feb 13 17:22:53 2015 +0000
Revision:
34:c2bc9f9be7ff
Parent:
33:3e71c418e90d
Child:
35:f9caeb8ca31e
working version

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