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@42:854d8cc26bbb, 2015-03-09 (annotated)

Committer:
maximbolduc
Date:
Mon Mar 09 15:58:28 2015 +0000
Revision:
42:854d8cc26bbb
Parent:
41:a26acd346c2f
Child:
43:e3f064f5eecd
scale fix

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