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@47:d3123bb4f673, 2015-03-14 (annotated)

Committer:
maximbolduc
Date:
Sat Mar 14 01:28:37 2015 +0000
Revision:
47:d3123bb4f673
Parent:
46:d7d6dc429153
Child:
48:5d9c63364c94
splitted into classes

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 47:d3123bb4f673 11 #include "string_utilities.h"
maximbolduc 47:d3123bb4f673 12
maximbolduc 34:c2bc9f9be7ff 13 #include "checksum.h"
maximbolduc 34:c2bc9f9be7ff 14 #include <string.h>
maximbolduc 47:d3123bb4f673 15 #include "gps.h"
maximbolduc 47:d3123bb4f673 16 #include "autosteer.h"
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 45:ecd8c2e27948 84 char* result;
maximbolduc 26:dc00998140af 85 int printing;
maximbolduc 26:dc00998140af 86 float longitude;
maximbolduc 26:dc00998140af 87 float latitude;
maximbolduc 26:dc00998140af 88 char ns, ew;
maximbolduc 26:dc00998140af 89 int lock;
maximbolduc 26:dc00998140af 90 int flag_gga;
maximbolduc 26:dc00998140af 91 int reading;
maximbolduc 26:dc00998140af 92 int day;
maximbolduc 26:dc00998140af 93 int hour;
maximbolduc 26:dc00998140af 94 int minute;
maximbolduc 26:dc00998140af 95 int second;
maximbolduc 26:dc00998140af 96 int tenths;
maximbolduc 26:dc00998140af 97 int hundreths;
maximbolduc 26:dc00998140af 98 char status;
maximbolduc 26:dc00998140af 99 double track; // track made good . angle
maximbolduc 26:dc00998140af 100 char magvar_dir;
maximbolduc 26:dc00998140af 101 double magvar;
maximbolduc 26:dc00998140af 102 int year;
maximbolduc 26:dc00998140af 103 int month;
maximbolduc 26:dc00998140af 104 double speed_km;
maximbolduc 26:dc00998140af 105 double speed_m_s = 0;
maximbolduc 26:dc00998140af 106 double velocity; // speed in knot
maximbolduc 26:dc00998140af 107 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 108 int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
maximbolduc 35:f9caeb8ca31e 109
maximbolduc 26:dc00998140af 110 int angle_send = 0;
maximbolduc 26:dc00998140af 111 int correct_rmc = 1;
maximbolduc 26:dc00998140af 112 double m_lat = 0;
maximbolduc 26:dc00998140af 113 double m_lon = 0;
maximbolduc 26:dc00998140af 114 char* degminsec;
maximbolduc 26:dc00998140af 115 double m_per_deg_lon;
maximbolduc 26:dc00998140af 116 double m_per_deg_lat;
maximbolduc 26:dc00998140af 117 double look_ahead_lon;
maximbolduc 26:dc00998140af 118 double look_ahead_lat;
maximbolduc 26:dc00998140af 119 int active_AB = 0;
maximbolduc 26:dc00998140af 120 double compensation_vector;
maximbolduc 26:dc00998140af 121 char output[256];
maximbolduc 35:f9caeb8ca31e 122
maximbolduc 26:dc00998140af 123 double yaw;
maximbolduc 26:dc00998140af 124 double pitch;
maximbolduc 26:dc00998140af 125 double roll;
maximbolduc 30:3afafa1ef16b 126 double a_x;
maximbolduc 30:3afafa1ef16b 127 double a_y;
maximbolduc 30:3afafa1ef16b 128 double a_z;
maximbolduc 30:3afafa1ef16b 129 double w_x;
maximbolduc 30:3afafa1ef16b 130 double w_y;
maximbolduc 30:3afafa1ef16b 131 double w_z;
maximbolduc 35:f9caeb8ca31e 132
maximbolduc 26:dc00998140af 133 int readings[3];
maximbolduc 26:dc00998140af 134 double Freepilot_bearing;
maximbolduc 36:8e84b5ade03e 135 int time_till_stop = 200;
maximbolduc 26:dc00998140af 136 volatile bool newline_detected = false;
maximbolduc 35:f9caeb8ca31e 137
maximbolduc 35:f9caeb8ca31e 138 void autosteer_done()
maximbolduc 35:f9caeb8ca31e 139 {
maximbolduc 35:f9caeb8ca31e 140 enable_motor = 0;
maximbolduc 35:f9caeb8ca31e 141 }
maximbolduc 35:f9caeb8ca31e 142
maximbolduc 26:dc00998140af 143 Point compensation;
maximbolduc 35:f9caeb8ca31e 144
maximbolduc 26:dc00998140af 145 void yaw_compensate()
maximbolduc 26:dc00998140af 146 {
maximbolduc 33:3e71c418e90d 147 yaw = get_yaw();
maximbolduc 26:dc00998140af 148 }
maximbolduc 34:c2bc9f9be7ff 149
maximbolduc 35:f9caeb8ca31e 150 void pitch_and_roll(double real_bearing)
maximbolduc 34:c2bc9f9be7ff 151 {
maximbolduc 34:c2bc9f9be7ff 152 pitch = get_pitch();
maximbolduc 34:c2bc9f9be7ff 153 roll = get_roll();
maximbolduc 34:c2bc9f9be7ff 154 compensation.SetX(antennaheight * tan(roll) * sin(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * cos(real_bearing/ 57.295779513));
maximbolduc 34:c2bc9f9be7ff 155 compensation.SetY(antennaheight * tan(roll) * cos(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * sin(real_bearing/ 57.295779513));
maximbolduc 34:c2bc9f9be7ff 156 }
maximbolduc 33:3e71c418e90d 157
maximbolduc 26:dc00998140af 158 void process_GPSHEIGHT(char* height_string)
maximbolduc 26:dc00998140af 159 {
maximbolduc 26:dc00998140af 160 char *token;
maximbolduc 26:dc00998140af 161 int token_counter = 0;
maximbolduc 26:dc00998140af 162 char *height = (char *)NULL;
maximbolduc 26:dc00998140af 163 token = strtok(height_string, ",");
maximbolduc 35:f9caeb8ca31e 164 while (token) {
maximbolduc 35:f9caeb8ca31e 165 switch (token_counter) {
maximbolduc 26:dc00998140af 166 case 1:
maximbolduc 26:dc00998140af 167 height = token;
maximbolduc 26:dc00998140af 168 break;
maximbolduc 26:dc00998140af 169 }
maximbolduc 26:dc00998140af 170 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 171 token_counter++;
maximbolduc 26:dc00998140af 172 }
maximbolduc 35:f9caeb8ca31e 173 if ( height ) {
jhedmonton 28:5905886c76ee 174 antennaheight = atof(height);
maximbolduc 42:854d8cc26bbb 175 // Config_Save();
maximbolduc 26:dc00998140af 176 }
maximbolduc 26:dc00998140af 177 }
maximbolduc 26:dc00998140af 178
maximbolduc 35:f9caeb8ca31e 179 //sets pwm1 and pwm2 and enable_motor
maximbolduc 46:d7d6dc429153 180 void process_ASTEER(char* asteer,bool from_pc)
maximbolduc 46:d7d6dc429153 181 {
maximbolduc 47:d3123bb4f673 182 if ( freepilot_v2 && !from_pc || from_pc && !freepilot_v2) {
maximbolduc 47:d3123bb4f673 183 char *token;
maximbolduc 47:d3123bb4f673 184 int token_counter = 0;
maximbolduc 47:d3123bb4f673 185 char *asteer_speed = (char *)NULL;
maximbolduc 47:d3123bb4f673 186 char *asteer_time = (char *)NULL;
maximbolduc 47:d3123bb4f673 187 token = strtok(asteer, ",");
maximbolduc 47:d3123bb4f673 188 while (token) {
maximbolduc 47:d3123bb4f673 189 switch (token_counter) {
maximbolduc 47:d3123bb4f673 190 case 1:
maximbolduc 47:d3123bb4f673 191 asteer_speed = token;
maximbolduc 47:d3123bb4f673 192 break;
maximbolduc 47:d3123bb4f673 193 case 2:
maximbolduc 47:d3123bb4f673 194 asteer_time = token;
maximbolduc 47:d3123bb4f673 195 break;
maximbolduc 47:d3123bb4f673 196 }
maximbolduc 47:d3123bb4f673 197 token = strtok((char *)NULL, ",");
maximbolduc 47:d3123bb4f673 198 token_counter++;
maximbolduc 35:f9caeb8ca31e 199 }
maximbolduc 47:d3123bb4f673 200 if ( asteer_speed && asteer_time ) {
maximbolduc 47:d3123bb4f673 201 motorspeed = atof(asteer_speed);
maximbolduc 47:d3123bb4f673 202 enable_time = atof(asteer_time);
maximbolduc 47:d3123bb4f673 203 autosteer_timeout.reset();
maximbolduc 47:d3123bb4f673 204 time_till_stop = atoi(asteer_time);
maximbolduc 47:d3123bb4f673 205 if ( motor_enable == 0 ) {
maximbolduc 47:d3123bb4f673 206 } else {
maximbolduc 47:d3123bb4f673 207 if ( motorspeed > 127.0 ) {
maximbolduc 47:d3123bb4f673 208 pwm2_speed = 0.0;
maximbolduc 47:d3123bb4f673 209 pwm1_speed = ((double)motorspeed - (double)127.0) / 128.0;
maximbolduc 37:ac60a8a0ae8a 210
maximbolduc 47:d3123bb4f673 211 } else if ( motorspeed < 127.0 ) {
maximbolduc 47:d3123bb4f673 212 pwm1_speed = 0.0;
maximbolduc 47:d3123bb4f673 213 pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
maximbolduc 47:d3123bb4f673 214 } else {
maximbolduc 47:d3123bb4f673 215 pwm1_speed = 0;
maximbolduc 47:d3123bb4f673 216 pwm2_speed = 0;
maximbolduc 47:d3123bb4f673 217 enable_motor = 0;
maximbolduc 47:d3123bb4f673 218 }
maximbolduc 47:d3123bb4f673 219 // if(Authenticated)
maximbolduc 47:d3123bb4f673 220 // {
maximbolduc 47:d3123bb4f673 221 pwm1 = pwm1_speed;
maximbolduc 47:d3123bb4f673 222 pwm2 = pwm2_speed;
maximbolduc 47:d3123bb4f673 223 enable_motor = 1;
maximbolduc 47:d3123bb4f673 224 //}
maximbolduc 37:ac60a8a0ae8a 225 }
maximbolduc 35:f9caeb8ca31e 226 }
maximbolduc 35:f9caeb8ca31e 227 }
maximbolduc 41:a26acd346c2f 228 }
maximbolduc 41:a26acd346c2f 229
maximbolduc 38:b5352d6f8166 230 Point old_position;
maximbolduc 38:b5352d6f8166 231
maximbolduc 38:b5352d6f8166 232 //char rmc_cpy[256];
maximbolduc 26:dc00998140af 233 void nmea_rmc(char *s)
maximbolduc 26:dc00998140af 234 {
maximbolduc 26:dc00998140af 235 char *token;
maximbolduc 26:dc00998140af 236 int token_counter = 0;
maximbolduc 26:dc00998140af 237 char *time = (char *)NULL;
maximbolduc 26:dc00998140af 238 char *date = (char *)NULL;
maximbolduc 26:dc00998140af 239 char *stat = (char *)NULL;
maximbolduc 26:dc00998140af 240 char *vel = (char *)NULL;
maximbolduc 26:dc00998140af 241 char *trk = (char *)NULL;
maximbolduc 26:dc00998140af 242 char *magv = (char *)NULL;
maximbolduc 38:b5352d6f8166 243 char *magd = (char *)NULL;
maximbolduc 38:b5352d6f8166 244 char *latit = "";
maximbolduc 38:b5352d6f8166 245 char *longit = "";
maximbolduc 26:dc00998140af 246 char *latitude = (char *)NULL;
maximbolduc 26:dc00998140af 247 char *longitude = (char *)NULL;
maximbolduc 26:dc00998140af 248 char *lat_dir = (char *)NULL;
maximbolduc 26:dc00998140af 249 char *lon_dir = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 250
maximbolduc 38:b5352d6f8166 251 while ((token = strsep(&s, ",")) != NULL) {
maximbolduc 35:f9caeb8ca31e 252 switch (token_counter) {
maximbolduc 26:dc00998140af 253 case 1:
maximbolduc 26:dc00998140af 254 time = token;
maximbolduc 26:dc00998140af 255 break;
maximbolduc 26:dc00998140af 256 case 2:
maximbolduc 26:dc00998140af 257 stat = token;
maximbolduc 26:dc00998140af 258 break;
maximbolduc 34:c2bc9f9be7ff 259 case 3:
maximbolduc 39:6767d4c840f9 260 if ( token ) {
maximbolduc 38:b5352d6f8166 261 latit = token;
maximbolduc 38:b5352d6f8166 262 latitude = token;
maximbolduc 38:b5352d6f8166 263 }
maximbolduc 34:c2bc9f9be7ff 264 break;
maximbolduc 34:c2bc9f9be7ff 265 case 4:
maximbolduc 34:c2bc9f9be7ff 266 lat_dir = token;
maximbolduc 34:c2bc9f9be7ff 267 break;
maximbolduc 34:c2bc9f9be7ff 268 case 5:
maximbolduc 38:b5352d6f8166 269 longit = token;
maximbolduc 34:c2bc9f9be7ff 270 longitude = token;
maximbolduc 34:c2bc9f9be7ff 271 break;
maximbolduc 34:c2bc9f9be7ff 272 case 6:
maximbolduc 34:c2bc9f9be7ff 273 lon_dir = token;
maximbolduc 34:c2bc9f9be7ff 274 break;
maximbolduc 26:dc00998140af 275 case 7:
maximbolduc 26:dc00998140af 276 vel = token;
maximbolduc 26:dc00998140af 277 break;
maximbolduc 26:dc00998140af 278 case 8:
maximbolduc 26:dc00998140af 279 trk = token;
maximbolduc 26:dc00998140af 280 break;
maximbolduc 34:c2bc9f9be7ff 281 case 9:
maximbolduc 34:c2bc9f9be7ff 282 date = token;
maximbolduc 34:c2bc9f9be7ff 283 break;
maximbolduc 26:dc00998140af 284 case 10:
maximbolduc 26:dc00998140af 285 magv = token;
maximbolduc 26:dc00998140af 286 break;
maximbolduc 38:b5352d6f8166 287 case 11:
maximbolduc 38:b5352d6f8166 288 magd = token;
maximbolduc 38:b5352d6f8166 289 break;
maximbolduc 26:dc00998140af 290 }
maximbolduc 26:dc00998140af 291 token_counter++;
maximbolduc 26:dc00998140af 292 }
maximbolduc 38:b5352d6f8166 293 if (stat!= '\0' && date!= '\0' && time!= '\0') {
maximbolduc 26:dc00998140af 294 hour = (char)((time[0] - '0') * 10) + (time[1] - '0');
maximbolduc 26:dc00998140af 295 minute = (char)((time[2] - '0') * 10) + (time[3] - '0');
maximbolduc 26:dc00998140af 296 second = (char)((time[4] - '0') * 10) + (time[5] - '0');
maximbolduc 26:dc00998140af 297 day = (char)((date[0] - '0') * 10) + (date[1] - '0');
maximbolduc 26:dc00998140af 298 month = (char)((date[2] - '0') * 10) + (date[3] - '0');
maximbolduc 26:dc00998140af 299 year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
maximbolduc 26:dc00998140af 300 status = stat[0];
maximbolduc 26:dc00998140af 301 velocity = atof(vel);
maximbolduc 26:dc00998140af 302 speed_km = velocity * 1.852;
maximbolduc 26:dc00998140af 303 speed_m_s = speed_km * 3600.0 / 1000.0;
maximbolduc 26:dc00998140af 304 track = atof(trk);
maximbolduc 26:dc00998140af 305 magvar = atof(magv);
maximbolduc 26:dc00998140af 306 }
maximbolduc 39:6767d4c840f9 307 double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
maximbolduc 39:6767d4c840f9 308 double diff_angle = Freepilot_bearing - angle;
maximbolduc 39:6767d4c840f9 309 diff_angle = ((int)diff_angle + 180) % 360 - 180;
maximbolduc 39:6767d4c840f9 310
maximbolduc 39:6767d4c840f9 311 // pc.printf("%f %f %f\r\n",diff_angle, Freepilot_bearing, (track - 90) * -1);
maximbolduc 39:6767d4c840f9 312 if ( abs(diff_angle) > 90 ) {
maximbolduc 39:6767d4c840f9 313 if ( (abs(360 - diff_angle)) > 90 ) {
maximbolduc 39:6767d4c840f9 314 Point temp = line_end;
maximbolduc 39:6767d4c840f9 315 line_end = line_start;
maximbolduc 39:6767d4c840f9 316 line_start = temp;
maximbolduc 39:6767d4c840f9 317 Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
maximbolduc 39:6767d4c840f9 318 }
maximbolduc 39:6767d4c840f9 319 }
maximbolduc 38:b5352d6f8166 320 if ( longit != '\0' && latit != '\0' ) {
maximbolduc 38:b5352d6f8166 321 old_position = position;
maximbolduc 38:b5352d6f8166 322 position.SetX(lat_to_deg(latitude, lat_dir[0]));
maximbolduc 38:b5352d6f8166 323 position.SetY(lon_to_deg(longitude, lon_dir[0]));
maximbolduc 38:b5352d6f8166 324 cm_per_deg_lat = 11054000;
maximbolduc 38:b5352d6f8166 325 cm_per_deg_lon = 11132000 * cos(decimal_latitude);
maximbolduc 35:f9caeb8ca31e 326
maximbolduc 42:854d8cc26bbb 327 pitch_and_roll((track-90)*-1);// as to be real bearing
maximbolduc 35:f9caeb8ca31e 328
maximbolduc 38:b5352d6f8166 329 compensation.SetY(compensation.GetY() / cm_per_deg_lon);
maximbolduc 38:b5352d6f8166 330 compensation.SetX(compensation.GetX() / cm_per_deg_lat);
maximbolduc 38:b5352d6f8166 331
maximbolduc 38:b5352d6f8166 332 position = point_add(position,compensation);
maximbolduc 35:f9caeb8ca31e 333
maximbolduc 38:b5352d6f8166 334 double lookaheaddistance = lookaheadtime * speed_m_s;
maximbolduc 38:b5352d6f8166 335 get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,(track-90)*-1,look_ahead_lon,look_ahead_lat);
maximbolduc 38:b5352d6f8166 336 looked_ahead.SetX(look_ahead_lat);
maximbolduc 38:b5352d6f8166 337 looked_ahead.SetY(look_ahead_lon);
maximbolduc 38:b5352d6f8166 338 double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
maximbolduc 41:a26acd346c2f 339 distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;/////////////////////////////////////////////////
maximbolduc 42:854d8cc26bbb 340 SetDigitalFilter(phaseadv,tcenter, 0 );
maximbolduc 35:f9caeb8ca31e 341
maximbolduc 41:a26acd346c2f 342 // int val = freepilot(position, looked_ahead, diff_angle, distance_to_line, scale, filterg , phaseadv,tcenter ,filtering);
maximbolduc 42:854d8cc26bbb 343 string steering = ControlSteerFilter(225, distance_to_line, speed_km , filterg, 20.0, 75.0, scale);
maximbolduc 42:854d8cc26bbb 344 //pc.printf("%f\r\n",distance_to_line);
maximbolduc 35:f9caeb8ca31e 345
maximbolduc 38:b5352d6f8166 346 char command[128];
maximbolduc 41:a26acd346c2f 347 sprintf(command,"%s\r\n\0",steering.c_str()); //(int)((((val/2)-127)/scale)+127),500);
maximbolduc 38:b5352d6f8166 348 pc.puts(command);
maximbolduc 41:a26acd346c2f 349
maximbolduc 46:d7d6dc429153 350 process_ASTEER(command,false);
maximbolduc 38:b5352d6f8166 351 }
maximbolduc 34:c2bc9f9be7ff 352 string rmc = "";
maximbolduc 38:b5352d6f8166 353 if(time!= '\0') {
maximbolduc 34:c2bc9f9be7ff 354 rmc = "$GPRMC,";
maximbolduc 34:c2bc9f9be7ff 355 rmc += string(time) + ",";
maximbolduc 34:c2bc9f9be7ff 356 } else {
maximbolduc 34:c2bc9f9be7ff 357 rmc = "$GPRMC,,";
maximbolduc 34:c2bc9f9be7ff 358 }
maximbolduc 38:b5352d6f8166 359 if(stat!= '\0') {
maximbolduc 34:c2bc9f9be7ff 360 rmc +=(string(stat) + ",");
maximbolduc 34:c2bc9f9be7ff 361 } else {
maximbolduc 34:c2bc9f9be7ff 362 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 363 }
maximbolduc 38:b5352d6f8166 364 if ( latit != '\0' && lat_dir!= '\0') {
maximbolduc 34:c2bc9f9be7ff 365 rmc +=( string(To_DMS(position.GetX())) + "," + string(lat_dir) +",");
maximbolduc 34:c2bc9f9be7ff 366 } else {
maximbolduc 34:c2bc9f9be7ff 367 rmc += ",,";
maximbolduc 34:c2bc9f9be7ff 368 }
maximbolduc 38:b5352d6f8166 369 if ( longit != '\0' && lon_dir!= '\0' ) {
maximbolduc 34:c2bc9f9be7ff 370 rmc += (string(To_DMS_lon(position.GetY())) + "," + string(lon_dir) + ",");
maximbolduc 34:c2bc9f9be7ff 371 } else {
maximbolduc 34:c2bc9f9be7ff 372 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 373 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 374 }
maximbolduc 38:b5352d6f8166 375 if (vel!= '\0') {
maximbolduc 38:b5352d6f8166 376 rmc += (string(vel) + ",");
maximbolduc 34:c2bc9f9be7ff 377 } else {
maximbolduc 34:c2bc9f9be7ff 378 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 379 }
maximbolduc 38:b5352d6f8166 380 if ((trk)!= '\0') {
maximbolduc 34:c2bc9f9be7ff 381 rmc += string(trk) + ",";
maximbolduc 34:c2bc9f9be7ff 382 } else {
maximbolduc 34:c2bc9f9be7ff 383 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 384 }
maximbolduc 38:b5352d6f8166 385 if (date!= '\0') {
maximbolduc 34:c2bc9f9be7ff 386 rmc += string(date) + ",";
maximbolduc 34:c2bc9f9be7ff 387 } else {
maximbolduc 34:c2bc9f9be7ff 388 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 389 }
maximbolduc 38:b5352d6f8166 390 if (magv!= '\0') {
maximbolduc 34:c2bc9f9be7ff 391 rmc += string(magv) + ",";
maximbolduc 34:c2bc9f9be7ff 392 } else {
maximbolduc 34:c2bc9f9be7ff 393 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 394 }
maximbolduc 38:b5352d6f8166 395 if (magd!= '\0') {
maximbolduc 38:b5352d6f8166 396 rmc += string(magd) + ",W";
maximbolduc 38:b5352d6f8166 397 } else {
maximbolduc 38:b5352d6f8166 398 rmc += ",W";
maximbolduc 38:b5352d6f8166 399 }
maximbolduc 34:c2bc9f9be7ff 400
maximbolduc 34:c2bc9f9be7ff 401 char test[256];
maximbolduc 38:b5352d6f8166 402 sprintf(test,"%s*\0",rmc.c_str());
maximbolduc 38:b5352d6f8166 403 sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));
maximbolduc 39:6767d4c840f9 404
maximbolduc 34:c2bc9f9be7ff 405 bluetooth.puts(output);
maximbolduc 26:dc00998140af 406 }
maximbolduc 35:f9caeb8ca31e 407
maximbolduc 26:dc00998140af 408 void process_FGPSAB(char* ab)
maximbolduc 26:dc00998140af 409 {
maximbolduc 33:3e71c418e90d 410 char *token;
maximbolduc 33:3e71c418e90d 411 int token_counter = 0;
maximbolduc 33:3e71c418e90d 412 char *line_lat = (char *)NULL;
maximbolduc 33:3e71c418e90d 413 char *line_lon = (char *)NULL;
maximbolduc 33:3e71c418e90d 414 char *line_lat1 = (char *)NULL;
maximbolduc 33:3e71c418e90d 415 char *line_lon1 = (char *)NULL;
maximbolduc 38:b5352d6f8166 416 //char *bearing = (char *)NULL;
maximbolduc 38:b5352d6f8166 417 string bearing = "";
maximbolduc 33:3e71c418e90d 418 token = strtok(ab, ",");
maximbolduc 35:f9caeb8ca31e 419 while (token) {
maximbolduc 35:f9caeb8ca31e 420 switch (token_counter) {
maximbolduc 33:3e71c418e90d 421 case 1:
maximbolduc 33:3e71c418e90d 422 line_lat = token;
maximbolduc 33:3e71c418e90d 423 break;
maximbolduc 33:3e71c418e90d 424 case 2:
maximbolduc 33:3e71c418e90d 425 line_lon = token;
maximbolduc 33:3e71c418e90d 426 break;
maximbolduc 33:3e71c418e90d 427 case 3:
maximbolduc 33:3e71c418e90d 428 line_lat1 = token;
maximbolduc 33:3e71c418e90d 429 break;
maximbolduc 33:3e71c418e90d 430 case 4:
maximbolduc 33:3e71c418e90d 431 line_lon1 = token;
maximbolduc 33:3e71c418e90d 432 break;
maximbolduc 33:3e71c418e90d 433 case 5:
maximbolduc 45:ecd8c2e27948 434 bearing = token;
maximbolduc 33:3e71c418e90d 435 break;
maximbolduc 26:dc00998140af 436 }
maximbolduc 33:3e71c418e90d 437 token = strtok((char *)NULL, ",");
maximbolduc 33:3e71c418e90d 438 token_counter++;
maximbolduc 33:3e71c418e90d 439 }
maximbolduc 39:6767d4c840f9 440 double Freepilot_lon = atof(line_lon);
maximbolduc 39:6767d4c840f9 441 double Freepilot_lat = atof(line_lat);
maximbolduc 39:6767d4c840f9 442 double Freepilot_lon1 = atof(line_lon1);
maximbolduc 39:6767d4c840f9 443 double Freepilot_lat1 = atof(line_lat1);
maximbolduc 39:6767d4c840f9 444 Freepilot_bearing = atof(bearing.c_str()) + 360;
maximbolduc 39:6767d4c840f9 445 Freepilot_bearing = ((int)Freepilot_bearing+ 180) % 360 - 180;
maximbolduc 33:3e71c418e90d 446 line_start.SetX(Freepilot_lat);
maximbolduc 33:3e71c418e90d 447 line_start.SetY(Freepilot_lon);
maximbolduc 33:3e71c418e90d 448 line_end.SetX(Freepilot_lat1);
maximbolduc 33:3e71c418e90d 449 line_end.SetY(Freepilot_lon1);
maximbolduc 39:6767d4c840f9 450
maximbolduc 33:3e71c418e90d 451 active_AB = 1;
maximbolduc 35:f9caeb8ca31e 452
maximbolduc 47:d3123bb4f673 453 // 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 46:d7d6dc429153 454 //pc.puts(output);
maximbolduc 26:dc00998140af 455 }
maximbolduc 35:f9caeb8ca31e 456
maximbolduc 26:dc00998140af 457 void process_FGPSAUTO(char* FGPSAUTO)
maximbolduc 26:dc00998140af 458 {
maximbolduc 26:dc00998140af 459 char *token;
maximbolduc 26:dc00998140af 460 int token_counter = 0;
maximbolduc 26:dc00998140af 461 char *ahead = (char *)NULL;
maximbolduc 26:dc00998140af 462 char *center = (char *)NULL;
maximbolduc 26:dc00998140af 463 char *phase = (char *)NULL;
maximbolduc 26:dc00998140af 464 char *scl = (char *)NULL;
maximbolduc 26:dc00998140af 465 char *avg = (char *)NULL;
maximbolduc 26:dc00998140af 466 char *_kp = (char *)NULL;
maximbolduc 26:dc00998140af 467 char *_ki = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 468 char *fg = (char *)NULL;
maximbolduc 26:dc00998140af 469 char *_kd = (char *)NULL;
maximbolduc 39:6767d4c840f9 470
maximbolduc 26:dc00998140af 471 token = strtok(FGPSAUTO, ",");
maximbolduc 35:f9caeb8ca31e 472 while (token) {
maximbolduc 35:f9caeb8ca31e 473 switch (token_counter) {
maximbolduc 26:dc00998140af 474 case 1:
maximbolduc 26:dc00998140af 475 phase = token;
maximbolduc 26:dc00998140af 476 break;
maximbolduc 26:dc00998140af 477 case 2:
maximbolduc 26:dc00998140af 478 center = token;
maximbolduc 26:dc00998140af 479 break;
maximbolduc 34:c2bc9f9be7ff 480 case 3:
maximbolduc 34:c2bc9f9be7ff 481 fg = token;
maximbolduc 34:c2bc9f9be7ff 482 break;
maximbolduc 26:dc00998140af 483 case 4:
maximbolduc 26:dc00998140af 484 scl = token;
maximbolduc 26:dc00998140af 485 break;
maximbolduc 26:dc00998140af 486 case 5:
maximbolduc 26:dc00998140af 487 ahead = token;
maximbolduc 26:dc00998140af 488 break;
maximbolduc 26:dc00998140af 489 case 6:
maximbolduc 26:dc00998140af 490 avg = token;
maximbolduc 26:dc00998140af 491 break;
maximbolduc 26:dc00998140af 492 case 7:
maximbolduc 26:dc00998140af 493 _kp = token;
maximbolduc 26:dc00998140af 494 break;
maximbolduc 26:dc00998140af 495 case 8:
maximbolduc 26:dc00998140af 496 _ki = token;
maximbolduc 26:dc00998140af 497 break;
maximbolduc 26:dc00998140af 498 case 9:
maximbolduc 26:dc00998140af 499 _kd = token;
maximbolduc 26:dc00998140af 500 break;
maximbolduc 26:dc00998140af 501 }
maximbolduc 26:dc00998140af 502 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 503 token_counter++;
maximbolduc 26:dc00998140af 504 }
maximbolduc 35:f9caeb8ca31e 505 if ( _kp && _ki && _kd ) {
maximbolduc 26:dc00998140af 506 kp = atof(_kp);
maximbolduc 26:dc00998140af 507 ki = atof(_ki);
maximbolduc 26:dc00998140af 508 kd = atof(_kd);
maximbolduc 26:dc00998140af 509 }
maximbolduc 35:f9caeb8ca31e 510 if ( phase && center && scl && avg && ahead ) {
jhedmonton 28:5905886c76ee 511 lookaheadtime = atof(ahead);
maximbolduc 41:a26acd346c2f 512 scale = atof(scl);
maximbolduc 26:dc00998140af 513 phaseadv = atof(phase);
jhedmonton 28:5905886c76ee 514 avgpos = atof(avg);
jhedmonton 28:5905886c76ee 515 tcenter = atof(center);
maximbolduc 34:c2bc9f9be7ff 516 filterg = atof(fg);
maximbolduc 38:b5352d6f8166 517
maximbolduc 41:a26acd346c2f 518 // scale = scale * -1;
maximbolduc 41:a26acd346c2f 519 SetDigitalFilter(phaseadv,tcenter, 0 );
maximbolduc 26:dc00998140af 520 }
maximbolduc 26:dc00998140af 521 }
maximbolduc 35:f9caeb8ca31e 522
maximbolduc 26:dc00998140af 523 void pc_analyse(char* pc_string)
maximbolduc 26:dc00998140af 524 {
maximbolduc 41:a26acd346c2f 525 pc.puts(pc_string);
maximbolduc 35:f9caeb8ca31e 526 if (!strncmp(pc_string, "$ASTEER", 7)) {
maximbolduc 39:6767d4c840f9 527 //stop sending when already exists
maximbolduc 46:d7d6dc429153 528 process_ASTEER(pc_string,true);
maximbolduc 35:f9caeb8ca31e 529 } else if (!strncmp(pc_string, "$BANY",5)) {
jhedmonton 29:23ccb2a50b6f 530 _ID = Config_GetID();
maximbolduc 42:854d8cc26bbb 531 // Config_Save();
maximbolduc 35:f9caeb8ca31e 532 } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
maximbolduc 45:ecd8c2e27948 533
maximbolduc 45:ecd8c2e27948 534 process_GPSBAUD(pc_string);
maximbolduc 42:854d8cc26bbb 535 // Config_Save();
maximbolduc 35:f9caeb8ca31e 536 } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
maximbolduc 34:c2bc9f9be7ff 537 process_FGPSAUTO(pc_string);
maximbolduc 34:c2bc9f9be7ff 538 sprintf(output,"%s\r\n",pc_string);
maximbolduc 34:c2bc9f9be7ff 539 bluetooth.puts(output);
maximbolduc 42:854d8cc26bbb 540 // Config_Save();
maximbolduc 35:f9caeb8ca31e 541 } else if (!strncmp(pc_string, "$FGPS,",6)) {
maximbolduc 35:f9caeb8ca31e 542 int i=5;
maximbolduc 35:f9caeb8ca31e 543 char c=pc_string[i];
maximbolduc 35:f9caeb8ca31e 544 while (c!=0) {
maximbolduc 35:f9caeb8ca31e 545 i++;
maximbolduc 35:f9caeb8ca31e 546 if (i>255) break; //protect msg buffer!
maximbolduc 35:f9caeb8ca31e 547 c=pc_string[i];
maximbolduc 35:f9caeb8ca31e 548 gps.putc(c);
maximbolduc 35:f9caeb8ca31e 549 pc.putc(c);
maximbolduc 35:f9caeb8ca31e 550 }
maximbolduc 36:8e84b5ade03e 551 } else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
maximbolduc 26:dc00998140af 552 process_GPSHEIGHT(pc_string);
maximbolduc 26:dc00998140af 553 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 554 bluetooth.puts(output);
maximbolduc 42:854d8cc26bbb 555 // Config_Save();
maximbolduc 37:ac60a8a0ae8a 556 } else if (!strncmp(pc_string, "$FGPSAB",7)) {
maximbolduc 26:dc00998140af 557 process_FGPSAB(pc_string);
maximbolduc 47:d3123bb4f673 558 } else if(strcmp(pc_string, "$V2,1") == 0) {
maximbolduc 46:d7d6dc429153 559 freepilot_v2 = true;
maximbolduc 47:d3123bb4f673 560 } else if(strcmp(pc_string, "$V2,0") == 0) {
maximbolduc 46:d7d6dc429153 561 freepilot_v2 = false;
maximbolduc 35:f9caeb8ca31e 562 } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
maximbolduc 32:c57bc701d65c 563 calibrateGyroscope();
maximbolduc 30:3afafa1ef16b 564 calibrateAccelerometer();
maximbolduc 42:854d8cc26bbb 565 // Config_Save();
maximbolduc 35:f9caeb8ca31e 566 } else if (!strncmp(pc_string, "$POSITION",9)) {
maximbolduc 32:c57bc701d65c 567 char* pointer;
maximbolduc 32:c57bc701d65c 568 char* Data[5];
maximbolduc 32:c57bc701d65c 569 int index = 0;
maximbolduc 32:c57bc701d65c 570 //Split data at commas
maximbolduc 32:c57bc701d65c 571 pointer = strtok(pc_string, ",");
maximbolduc 32:c57bc701d65c 572 if(pointer == NULL)
maximbolduc 32:c57bc701d65c 573 Data[0] = pc_string;
maximbolduc 35:f9caeb8ca31e 574 while(pointer != NULL) {
maximbolduc 32:c57bc701d65c 575 Data[index] = pointer;
maximbolduc 32:c57bc701d65c 576 pointer = strtok(NULL, ",");
maximbolduc 32:c57bc701d65c 577 index++;
maximbolduc 32:c57bc701d65c 578 }
maximbolduc 32:c57bc701d65c 579 gyro_pos = atoi(Data[1]);
maximbolduc 42:854d8cc26bbb 580 // Config_Save();
maximbolduc 35:f9caeb8ca31e 581 } else {
maximbolduc 26:dc00998140af 582 }
maximbolduc 26:dc00998140af 583 }
maximbolduc 35:f9caeb8ca31e 584
maximbolduc 26:dc00998140af 585 void gps_analyse(char* gps_string)
maximbolduc 26:dc00998140af 586 {
maximbolduc 35:f9caeb8ca31e 587 if (!strncmp(gps_string, "$GPRMC", 6)) {
maximbolduc 26:dc00998140af 588 nmea_rmc(gps_string); //analyse and decompose the rmc string
maximbolduc 36:8e84b5ade03e 589 } else {
maximbolduc 36:8e84b5ade03e 590 bluetooth.puts(gps_string);
maximbolduc 33:3e71c418e90d 591 }
maximbolduc 26:dc00998140af 592 }
maximbolduc 35:f9caeb8ca31e 593
jhedmonton 27:9ac59b261d87 594 int i2 = 0;
jhedmonton 27:9ac59b261d87 595 bool end2 = false;
jhedmonton 27:9ac59b261d87 596 bool start2 = false;
maximbolduc 35:f9caeb8ca31e 597
jhedmonton 27:9ac59b261d87 598 bool getline2()
maximbolduc 26:dc00998140af 599 {
jhedmonton 27:9ac59b261d87 600 int gotstring=false;
maximbolduc 35:f9caeb8ca31e 601 while (1) {
maximbolduc 35:f9caeb8ca31e 602 if( !bluetooth.readable() ) {
jhedmonton 27:9ac59b261d87 603 break;
jhedmonton 27:9ac59b261d87 604 }
jhedmonton 27:9ac59b261d87 605 char c = bluetooth.getc();
maximbolduc 35:f9caeb8ca31e 606 if (c == 36 ) {
maximbolduc 33:3e71c418e90d 607 start2=true;
maximbolduc 33:3e71c418e90d 608 end2 = false;
maximbolduc 33:3e71c418e90d 609 i2 = 0;
maximbolduc 33:3e71c418e90d 610 }
maximbolduc 35:f9caeb8ca31e 611 if ((start2) && (c == 10)) {
maximbolduc 33:3e71c418e90d 612 end2=true;
jhedmonton 29:23ccb2a50b6f 613 start2 = false;
jhedmonton 29:23ccb2a50b6f 614 }
maximbolduc 35:f9caeb8ca31e 615 if (start2) {
jhedmonton 27:9ac59b261d87 616 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 617 i2++;
jhedmonton 27:9ac59b261d87 618 if (i2>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 619 }
maximbolduc 35:f9caeb8ca31e 620 if (end2) {
maximbolduc 33:3e71c418e90d 621 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 622 msg2[i2+1] = 0;
jhedmonton 27:9ac59b261d87 623 start2 = false;
jhedmonton 27:9ac59b261d87 624 gotstring = true;
jhedmonton 27:9ac59b261d87 625 end2=false;
jhedmonton 27:9ac59b261d87 626 i2=0;
jhedmonton 27:9ac59b261d87 627 break;
maximbolduc 26:dc00998140af 628 }
maximbolduc 26:dc00998140af 629 }
jhedmonton 27:9ac59b261d87 630 return gotstring;
maximbolduc 26:dc00998140af 631 }
maximbolduc 35:f9caeb8ca31e 632
maximbolduc 35:f9caeb8ca31e 633
jhedmonton 27:9ac59b261d87 634 int i=0;
jhedmonton 27:9ac59b261d87 635 bool start=false;
jhedmonton 27:9ac59b261d87 636 bool end=false;
maximbolduc 35:f9caeb8ca31e 637
jhedmonton 27:9ac59b261d87 638 bool getline(bool forward)
maximbolduc 26:dc00998140af 639 {
maximbolduc 35:f9caeb8ca31e 640 while (1) {
maximbolduc 35:f9caeb8ca31e 641 if( !gps.readable() ) {
jhedmonton 27:9ac59b261d87 642 break;
jhedmonton 27:9ac59b261d87 643 }
jhedmonton 28:5905886c76ee 644 char c = gps.getc();
maximbolduc 35:f9caeb8ca31e 645 if (forward) { //simply forward all to Bluetooth
maximbolduc 35:f9caeb8ca31e 646 bluetooth.putc(c);
jhedmonton 27:9ac59b261d87 647 }
maximbolduc 35:f9caeb8ca31e 648 if (c == 36 ) {
maximbolduc 35:f9caeb8ca31e 649 start=true;
maximbolduc 35:f9caeb8ca31e 650 end = false;
maximbolduc 35:f9caeb8ca31e 651 i = 0;
maximbolduc 35:f9caeb8ca31e 652 }
maximbolduc 35:f9caeb8ca31e 653 if ((start) && (c == 10)) {
maximbolduc 35:f9caeb8ca31e 654 end=true;
jhedmonton 29:23ccb2a50b6f 655 start = false;
jhedmonton 29:23ccb2a50b6f 656 }
maximbolduc 35:f9caeb8ca31e 657 if (start) {
jhedmonton 27:9ac59b261d87 658 msg[i]=c;
jhedmonton 27:9ac59b261d87 659 i++;
jhedmonton 27:9ac59b261d87 660 if (i>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 661 }
maximbolduc 35:f9caeb8ca31e 662 if (end) {
maximbolduc 35:f9caeb8ca31e 663 msg[i]=c;
maximbolduc 26:dc00998140af 664 msg[i+1] = 0;
jhedmonton 29:23ccb2a50b6f 665 i=0;
jhedmonton 27:9ac59b261d87 666 start = false;
jhedmonton 27:9ac59b261d87 667 end = true;
jhedmonton 27:9ac59b261d87 668 break;
maximbolduc 26:dc00998140af 669 }
maximbolduc 26:dc00998140af 670 }
jhedmonton 27:9ac59b261d87 671 return end;
maximbolduc 26:dc00998140af 672 }
maximbolduc 35:f9caeb8ca31e 673
maximbolduc 26:dc00998140af 674 void keyPressedHeld( void )
maximbolduc 26:dc00998140af 675 {
maximbolduc 36:8e84b5ade03e 676 motor_enable_state = "$ENABLE,0\r\n";
maximbolduc 37:ac60a8a0ae8a 677 motor_enable = 0;
maximbolduc 37:ac60a8a0ae8a 678 pwm1 = 0.0;
maximbolduc 37:ac60a8a0ae8a 679 pwm2 = 0.0;
jhedmonton 28:5905886c76ee 680 ledGREEN=1; //show green for being ready to steer
maximbolduc 26:dc00998140af 681 }
maximbolduc 35:f9caeb8ca31e 682
maximbolduc 26:dc00998140af 683 void keyReleasedHeld( void )
maximbolduc 26:dc00998140af 684 {
maximbolduc 34:c2bc9f9be7ff 685 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 34:c2bc9f9be7ff 686 motor_enable_state = "$ENABLE,1\r\n";
maximbolduc 37:ac60a8a0ae8a 687 motor_enable = 1;
maximbolduc 37:ac60a8a0ae8a 688 pwm1 = 0.0;
maximbolduc 37:ac60a8a0ae8a 689 pwm2 = 0.0;
maximbolduc 35:f9caeb8ca31e 690 ledGREEN=0;
jhedmonton 27:9ac59b261d87 691 }
maximbolduc 35:f9caeb8ca31e 692
jhedmonton 27:9ac59b261d87 693 void boom1PressedHeld( void )
jhedmonton 27:9ac59b261d87 694 {
maximbolduc 35:f9caeb8ca31e 695 // ledGREEN=1;
maximbolduc 35:f9caeb8ca31e 696 boom18=boom18 & 0xFE;
jhedmonton 27:9ac59b261d87 697 }
maximbolduc 35:f9caeb8ca31e 698
jhedmonton 27:9ac59b261d87 699 void boom1ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 700 {
maximbolduc 35:f9caeb8ca31e 701 //ledGREEN=0;
maximbolduc 35:f9caeb8ca31e 702 boom18=boom18 | 0x01;
jhedmonton 27:9ac59b261d87 703 }
maximbolduc 35:f9caeb8ca31e 704
jhedmonton 27:9ac59b261d87 705 void boom2PressedHeld( void )
maximbolduc 35:f9caeb8ca31e 706 {
maximbolduc 35:f9caeb8ca31e 707 boom18=boom18 & 0xFD;
maximbolduc 26:dc00998140af 708 }
maximbolduc 35:f9caeb8ca31e 709
jhedmonton 27:9ac59b261d87 710 void boom2ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 711 {
maximbolduc 35:f9caeb8ca31e 712 boom18=boom18 | 0x02;
jhedmonton 27:9ac59b261d87 713 }
jhedmonton 27:9ac59b261d87 714 void boom3PressedHeld( void )
jhedmonton 27:9ac59b261d87 715 {
maximbolduc 35:f9caeb8ca31e 716 boom18=boom18 & 0xFB;
jhedmonton 27:9ac59b261d87 717 }
maximbolduc 35:f9caeb8ca31e 718
jhedmonton 27:9ac59b261d87 719 void boom3ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 720 {
maximbolduc 35:f9caeb8ca31e 721 boom18=boom18 | 0x04;
jhedmonton 27:9ac59b261d87 722 }
maximbolduc 35:f9caeb8ca31e 723
jhedmonton 27:9ac59b261d87 724 void boom4PressedHeld( void )
jhedmonton 27:9ac59b261d87 725 {
maximbolduc 32:c57bc701d65c 726 boom18=boom18 & 0xF7;
jhedmonton 27:9ac59b261d87 727 }
maximbolduc 35:f9caeb8ca31e 728
jhedmonton 27:9ac59b261d87 729 void boom4ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 730 {
maximbolduc 32:c57bc701d65c 731 boom18=boom18 | 0x08;
jhedmonton 27:9ac59b261d87 732 }
maximbolduc 35:f9caeb8ca31e 733
maximbolduc 26:dc00998140af 734 void toprint()
maximbolduc 26:dc00998140af 735 {
maximbolduc 26:dc00998140af 736 angle_send = 1;
maximbolduc 26:dc00998140af 737 }
maximbolduc 35:f9caeb8ca31e 738
maximbolduc 38:b5352d6f8166 739 double last_yaw = 0;
maximbolduc 38:b5352d6f8166 740 int counter = 0;
maximbolduc 38:b5352d6f8166 741 bool bear = false;
maximbolduc 38:b5352d6f8166 742 double lastroll = 0;
maximbolduc 38:b5352d6f8166 743 double lastpitch = 0;
maximbolduc 38:b5352d6f8166 744
maximbolduc 26:dc00998140af 745 int main()
maximbolduc 26:dc00998140af 746 {
jhedmonton 27:9ac59b261d87 747 bluetooth.baud(115200);
jhedmonton 28:5905886c76ee 748 gps.baud(38400);
maximbolduc 30:3afafa1ef16b 749 pc.baud(38400);
maximbolduc 35:f9caeb8ca31e 750
jhedmonton 27:9ac59b261d87 751 //JH prepare and send version info
jhedmonton 27:9ac59b261d87 752 vTimer.start();
jhedmonton 27:9ac59b261d87 753 vTimer.reset();
maximbolduc 38:b5352d6f8166 754
jhedmonton 28:5905886c76ee 755 motTimer.start();
jhedmonton 28:5905886c76ee 756 motTimer.reset();
jhedmonton 29:23ccb2a50b6f 757 lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
maximbolduc 34:c2bc9f9be7ff 758 motor_enable_state = "$ENABLE,1\r\n";
maximbolduc 36:8e84b5ade03e 759
jhedmonton 28:5905886c76ee 760 btTimer.start();
jhedmonton 28:5905886c76ee 761 btTimer.reset();
maximbolduc 38:b5352d6f8166 762 lastgetBT= btTimer.read_ms();
maximbolduc 35:f9caeb8ca31e 763
jhedmonton 27:9ac59b261d87 764 bluetooth.puts(version);
maximbolduc 38:b5352d6f8166 765
jhedmonton 29:23ccb2a50b6f 766 lastsend_version=vTimer.read_ms()-18000;
maximbolduc 38:b5352d6f8166 767 // pc.puts("test\r\n");
maximbolduc 38:b5352d6f8166 768 /* Config_Startup();
maximbolduc 38:b5352d6f8166 769 _ID = Config_GetID();
maximbolduc 38:b5352d6f8166 770 Config_Save();
maximbolduc 38:b5352d6f8166 771 */
jhedmonton 27:9ac59b261d87 772 boom1.attach_asserted_held( &boom1PressedHeld );
jhedmonton 27:9ac59b261d87 773 boom1.attach_deasserted_held( &boom1ReleasedHeld );
jhedmonton 27:9ac59b261d87 774 boom1.setSampleFrequency(); //default = 20 ms
jhedmonton 27:9ac59b261d87 775 boom1.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 776 boom1.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 777 boom2.attach_asserted_held( &boom2PressedHeld );
jhedmonton 27:9ac59b261d87 778 boom2.attach_deasserted_held( &boom2ReleasedHeld );
jhedmonton 27:9ac59b261d87 779 boom2.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 780 boom2.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 781 boom2.setSampleFrequency();
jhedmonton 27:9ac59b261d87 782 boom3.attach_asserted_held( &boom3PressedHeld );
jhedmonton 27:9ac59b261d87 783 boom3.attach_deasserted_held( &boom3ReleasedHeld );
jhedmonton 27:9ac59b261d87 784 boom3.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 785 boom3.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 786 boom3.setSampleFrequency();
jhedmonton 27:9ac59b261d87 787 boom4.attach_asserted_held( &boom4PressedHeld );
jhedmonton 27:9ac59b261d87 788 boom4.attach_deasserted_held( &boom4ReleasedHeld );
jhedmonton 27:9ac59b261d87 789 boom4.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 790 boom4.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 791 boom4.setSampleFrequency();
maximbolduc 47:d3123bb4f673 792
maximbolduc 44:e9d5cd98273d 793 motor_switch.setSamplesTillAssert(5);
maximbolduc 44:e9d5cd98273d 794 motor_switch.setSamplesTillHeld(5);
maximbolduc 44:e9d5cd98273d 795 motor_switch.setSampleFrequency();
maximbolduc 26:dc00998140af 796 motor_switch.attach_asserted_held( &keyPressedHeld );
maximbolduc 26:dc00998140af 797 motor_switch.attach_deasserted_held( &keyReleasedHeld );
maximbolduc 35:f9caeb8ca31e 798
maximbolduc 36:8e84b5ade03e 799 initializeAccelerometer();
maximbolduc 36:8e84b5ade03e 800 calibrateAccelerometer();
maximbolduc 36:8e84b5ade03e 801 initializeGyroscope();
maximbolduc 36:8e84b5ade03e 802 calibrateGyroscope();
maximbolduc 36:8e84b5ade03e 803
maximbolduc 30:3afafa1ef16b 804 accelerometerTicker.attach(&sampleAccelerometer, 0.005);
maximbolduc 30:3afafa1ef16b 805 gyroscopeTicker.attach(&sampleGyroscope, 0.005);
maximbolduc 30:3afafa1ef16b 806 filterTicker.attach(&filter, FILTER_RATE);
maximbolduc 30:3afafa1ef16b 807 angle_print.attach(&toprint,0.2);
maximbolduc 26:dc00998140af 808 activate_antenna();
maximbolduc 36:8e84b5ade03e 809 autosteer_timeout.start();
maximbolduc 38:b5352d6f8166 810
maximbolduc 44:e9d5cd98273d 811 //setTunings(0.25, 5, 1); //for PID
maximbolduc 44:e9d5cd98273d 812 SetDigitalFilter(phaseadv,tcenter, 0); //for FGPS guidance
maximbolduc 38:b5352d6f8166 813
maximbolduc 35:f9caeb8ca31e 814 while(1) {
jhedmonton 27:9ac59b261d87 815 //JH send version information every 10 seconds to keep Bluetooth alive
maximbolduc 35:f9caeb8ca31e 816 if ((vTimer.read_ms()-lastsend_version)>25000) {
jhedmonton 27:9ac59b261d87 817 pc.puts(version);
jhedmonton 27:9ac59b261d87 818 bluetooth.puts(version);
jhedmonton 27:9ac59b261d87 819 vTimer.reset();
jhedmonton 27:9ac59b261d87 820 lastsend_version=vTimer.read_ms();
maximbolduc 35:f9caeb8ca31e 821 }
maximbolduc 35:f9caeb8ca31e 822
maximbolduc 36:8e84b5ade03e 823 if ( autosteer_timeout.read_ms() > 5000 || autosteer_timeout.read_ms() > time_till_stop ) {
maximbolduc 36:8e84b5ade03e 824 autosteer_timeout.reset();
maximbolduc 36:8e84b5ade03e 825 enable_motor = 0;
maximbolduc 36:8e84b5ade03e 826 }
maximbolduc 35:f9caeb8ca31e 827 if ( antenna_active == 1 && gps.readable()) {
maximbolduc 35:f9caeb8ca31e 828 if (getline(false)) {
maximbolduc 45:ecd8c2e27948 829 if ( validate_checksum(msg,result)) {
maximbolduc 38:b5352d6f8166 830 //pc.puts(msg);
maximbolduc 45:ecd8c2e27948 831 gps_analyse(result);
maximbolduc 35:f9caeb8ca31e 832 } else {
maximbolduc 33:3e71c418e90d 833 pc.puts("INVALID!!!!\r\n");
maximbolduc 32:c57bc701d65c 834 }
maximbolduc 26:dc00998140af 835 }
maximbolduc 26:dc00998140af 836 }
maximbolduc 35:f9caeb8ca31e 837 if ( bluetooth.readable()) {
maximbolduc 35:f9caeb8ca31e 838 if (getline2()) {
maximbolduc 45:ecd8c2e27948 839 if ( validate_checksum(msg2,result)) {
maximbolduc 45:ecd8c2e27948 840 btTimer.reset();
maximbolduc 45:ecd8c2e27948 841 lastgetBT= btTimer.read_ms();
maximbolduc 45:ecd8c2e27948 842 // pc.puts(msg2);
maximbolduc 45:ecd8c2e27948 843 pc_analyse(result);
maximbolduc 45:ecd8c2e27948 844 }
maximbolduc 26:dc00998140af 845 }
maximbolduc 26:dc00998140af 846 }
maximbolduc 35:f9caeb8ca31e 847 if ( btTimer.read_ms()-lastgetBT>1000) {
maximbolduc 35:f9caeb8ca31e 848 //we did not get any commands over BT
maximbolduc 35:f9caeb8ca31e 849 ledRED=1; //turn red
maximbolduc 35:f9caeb8ca31e 850 } else ledRED=0;
maximbolduc 35:f9caeb8ca31e 851
maximbolduc 35:f9caeb8ca31e 852 if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) {
maximbolduc 26:dc00998140af 853 bluetooth.puts(motor_enable_state);
maximbolduc 38:b5352d6f8166 854 // pc.puts(motor_enable_state);
jhedmonton 28:5905886c76ee 855 motTimer.reset();
jhedmonton 28:5905886c76ee 856 lastsend_motorstate=motTimer.read_ms();
jhedmonton 28:5905886c76ee 857 lastmotor_enable=motor_enable;
maximbolduc 26:dc00998140af 858 }
maximbolduc 35:f9caeb8ca31e 859 if (boom18!=lastboom18) {
maximbolduc 35:f9caeb8ca31e 860 boomstate[4]=boom18 | 0x80; //
maximbolduc 35:f9caeb8ca31e 861 bluetooth.puts(boomstate);
maximbolduc 38:b5352d6f8166 862 // pc.puts(boomstate);
maximbolduc 35:f9caeb8ca31e 863 lastboom18=boom18;
jhedmonton 27:9ac59b261d87 864 }
maximbolduc 44:e9d5cd98273d 865 if ( print_euler == 1 && angle_send == 1 ) {
maximbolduc 44:e9d5cd98273d 866 //&& reading == 0){
maximbolduc 43:e3f064f5eecd 867 lastpitch = 0.9*lastpitch + 0.1 * (toDegrees(get_pitch()*3.5));
maximbolduc 43:e3f064f5eecd 868 lastroll = 0.9 * lastroll + 0.1 * toDegrees(get_roll()*3.5);
maximbolduc 38:b5352d6f8166 869
maximbolduc 38:b5352d6f8166 870 double dps = - last_yaw;
maximbolduc 38:b5352d6f8166 871 last_yaw =toDegrees( imuFilter.getYaw()) * -1;
maximbolduc 38:b5352d6f8166 872 dps = (dps + last_yaw) * 5; // update every 200 ms, so for dps need *5
maximbolduc 38:b5352d6f8166 873
maximbolduc 38:b5352d6f8166 874 sprintf(output,"$EULER,%f,%f,%f\r\n",lastroll,lastpitch,dps);
maximbolduc 30:3afafa1ef16b 875 bluetooth.puts(output);
maximbolduc 26:dc00998140af 876 angle_send = 0;
maximbolduc 38:b5352d6f8166 877 counter++;
maximbolduc 38:b5352d6f8166 878 if ( bear == false && counter > 3 ) { //reinitialise the gyro after 600ms for the reference to be changed.
maximbolduc 38:b5352d6f8166 879 imuFilter.reset();
maximbolduc 38:b5352d6f8166 880 bear = true;
maximbolduc 38:b5352d6f8166 881 }
jhedmonton 27:9ac59b261d87 882 }
maximbolduc 26:dc00998140af 883 }
maximbolduc 35:f9caeb8ca31e 884 }