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@40:a68cc1a1a1e7, 2015-03-06 (annotated)

Committer:
maximbolduc
Date:
Fri Mar 06 16:31:19 2015 +0000
Revision:
40:a68cc1a1a1e7
Parent:
39:6767d4c840f9
Child:
41:a26acd346c2f
farmerGPS autosteer routine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 26:dc00998140af 1 #include "mbed.h"
maximbolduc 26:dc00998140af 2 #include "PinDetect.h"
maximbolduc 26:dc00998140af 3 #include "Point.h"
maximbolduc 26:dc00998140af 4 #include <vector>
maximbolduc 26:dc00998140af 5 #include "Line.h"
maximbolduc 26:dc00998140af 6 #include "stringUtils.h"
jhedmonton 28:5905886c76ee 7 #include "base.h"
jhedmonton 28:5905886c76ee 8 #include "Config.h"
maximbolduc 30:3afafa1ef16b 9 #include "imu_functions.h"
maximbolduc 32:c57bc701d65c 10 #include "atoh.h"
maximbolduc 34:c2bc9f9be7ff 11 #include "checksum.h"
maximbolduc 34:c2bc9f9be7ff 12 #include <string.h>
maximbolduc 35:f9caeb8ca31e 13
maximbolduc 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 38:b5352d6f8166 160 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 38:b5352d6f8166 161 //PID
maximbolduc 38:b5352d6f8166 162 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 39:6767d4c840f9 163 /*double outMin_ = 0.0;
maximbolduc 38:b5352d6f8166 164 double outMax_ = 510.0;
maximbolduc 38:b5352d6f8166 165 double inMin_ = 0.0;
maximbolduc 38:b5352d6f8166 166 double inMax_ = 10.0;
maximbolduc 38:b5352d6f8166 167 double prevControllerOutput_ = 5.0;
maximbolduc 38:b5352d6f8166 168 double accError_ = 0.0;
maximbolduc 38:b5352d6f8166 169 double prevProcessVariable_ = 5.0;
maximbolduc 38:b5352d6f8166 170 double pParam_;
maximbolduc 38:b5352d6f8166 171 double iParam_;
maximbolduc 38:b5352d6f8166 172 double dParam_;
maximbolduc 38:b5352d6f8166 173 double tSample_ = 0.2;
maximbolduc 38:b5352d6f8166 174 double Kc_ ;
maximbolduc 38:b5352d6f8166 175 double tauR_ ;
maximbolduc 38:b5352d6f8166 176 double tauD_ ;
maximbolduc 38:b5352d6f8166 177 bool inAuto = true;
maximbolduc 38:b5352d6f8166 178 double bias_ = 0.0;
maximbolduc 38:b5352d6f8166 179 bool usingFeedForward = true;
maximbolduc 38:b5352d6f8166 180 double processVariable_ = 5.0;
maximbolduc 38:b5352d6f8166 181
maximbolduc 38:b5352d6f8166 182 void setTunings(float Kc, float tauI, float tauD)
maximbolduc 38:b5352d6f8166 183 {
maximbolduc 38:b5352d6f8166 184 //Verify that the tunings make sense.
maximbolduc 38:b5352d6f8166 185 if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
maximbolduc 38:b5352d6f8166 186 return;
maximbolduc 38:b5352d6f8166 187 }
maximbolduc 38:b5352d6f8166 188 //Store raw values to hand back to user on request.
maximbolduc 38:b5352d6f8166 189 pParam_ = Kc;
maximbolduc 38:b5352d6f8166 190 iParam_ = tauI;
maximbolduc 38:b5352d6f8166 191 dParam_ = tauD;
maximbolduc 38:b5352d6f8166 192 float tempTauR;
maximbolduc 38:b5352d6f8166 193 if (tauI == 0.0) {
maximbolduc 38:b5352d6f8166 194 tempTauR = 0.0;
maximbolduc 38:b5352d6f8166 195 } else {
maximbolduc 38:b5352d6f8166 196 tempTauR = (1.0 / tauI) * tSample_;
maximbolduc 38:b5352d6f8166 197 }
maximbolduc 38:b5352d6f8166 198 //For "bumpless transfer" we need to rescale the accumulated error.
maximbolduc 38:b5352d6f8166 199 if (inAuto) {
maximbolduc 38:b5352d6f8166 200 if (tempTauR == 0.0) {
maximbolduc 38:b5352d6f8166 201 accError_ = 0.0;
maximbolduc 38:b5352d6f8166 202 } else {
maximbolduc 38:b5352d6f8166 203 accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
maximbolduc 38:b5352d6f8166 204 }
maximbolduc 38:b5352d6f8166 205 }
maximbolduc 38:b5352d6f8166 206 Kc_ = Kc;
maximbolduc 38:b5352d6f8166 207 tauR_ = tempTauR;
maximbolduc 38:b5352d6f8166 208 tauD_ = tauD / tSample_;
maximbolduc 38:b5352d6f8166 209
maximbolduc 38:b5352d6f8166 210 }
maximbolduc 38:b5352d6f8166 211
maximbolduc 38:b5352d6f8166 212 void reset(void)
maximbolduc 38:b5352d6f8166 213 {
maximbolduc 38:b5352d6f8166 214 double outSpan_ = outMax_ - outMin_;
maximbolduc 38:b5352d6f8166 215 double inSpan_ = inMax_ - inMin_;
maximbolduc 38:b5352d6f8166 216 float scaledBias = 0.0;
maximbolduc 38:b5352d6f8166 217
maximbolduc 38:b5352d6f8166 218 // if (usingFeedForward) {
maximbolduc 38:b5352d6f8166 219 scaledBias = (bias_ - outMin_) / outSpan_;
maximbolduc 38:b5352d6f8166 220
maximbolduc 38:b5352d6f8166 221 prevControllerOutput_ = scaledBias;
maximbolduc 38:b5352d6f8166 222 prevProcessVariable_ = (processVariable_ - inMin_) / inSpan_;
maximbolduc 38:b5352d6f8166 223
maximbolduc 38:b5352d6f8166 224 //Clear any error in the integral.
maximbolduc 38:b5352d6f8166 225 accError_ = 0;
maximbolduc 38:b5352d6f8166 226 }
maximbolduc 38:b5352d6f8166 227
maximbolduc 38:b5352d6f8166 228 float compute(double setPoint_)
maximbolduc 38:b5352d6f8166 229 {
maximbolduc 38:b5352d6f8166 230 double outSpan_ = outMax_ - outMin_;
maximbolduc 38:b5352d6f8166 231 double inSpan_ = inMax_ - inMin_;
maximbolduc 38:b5352d6f8166 232 //Pull in the input and setpoint, and scale them into percent span.
maximbolduc 38:b5352d6f8166 233 float scaledPV = (processVariable_ - inMin_) / inSpan_;
maximbolduc 38:b5352d6f8166 234
maximbolduc 38:b5352d6f8166 235 if (scaledPV > 1.0) {
maximbolduc 38:b5352d6f8166 236 scaledPV = 1.0;
maximbolduc 38:b5352d6f8166 237 } else if (scaledPV < 0.0) {
maximbolduc 38:b5352d6f8166 238 scaledPV = 0.0;
maximbolduc 38:b5352d6f8166 239 }
maximbolduc 38:b5352d6f8166 240
maximbolduc 38:b5352d6f8166 241 float scaledSP = (setPoint_ - inMin_) / inSpan_;
maximbolduc 38:b5352d6f8166 242 if (scaledSP > 1.0) {
maximbolduc 38:b5352d6f8166 243 scaledSP = 1;
maximbolduc 38:b5352d6f8166 244 } else if (scaledSP < 0.0) {
maximbolduc 38:b5352d6f8166 245 scaledSP = 0;
maximbolduc 38:b5352d6f8166 246 }
maximbolduc 38:b5352d6f8166 247 float error = scaledSP - scaledPV;
maximbolduc 38:b5352d6f8166 248
maximbolduc 38:b5352d6f8166 249 //Check and see if the output is pegged at a limit and only
maximbolduc 38:b5352d6f8166 250 //integrate if it is not. This is to prevent reset-windup.
maximbolduc 38:b5352d6f8166 251 if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
maximbolduc 38:b5352d6f8166 252 accError_ += error;
maximbolduc 38:b5352d6f8166 253 }
maximbolduc 38:b5352d6f8166 254
maximbolduc 38:b5352d6f8166 255 //Compute the current slope of the input signal.
maximbolduc 38:b5352d6f8166 256 float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
maximbolduc 38:b5352d6f8166 257 float scaledBias = 0.0;
maximbolduc 38:b5352d6f8166 258
maximbolduc 38:b5352d6f8166 259 if (usingFeedForward) {
maximbolduc 38:b5352d6f8166 260 scaledBias = (bias_ - outMin_) / outSpan_;
maximbolduc 38:b5352d6f8166 261 }
maximbolduc 38:b5352d6f8166 262
maximbolduc 38:b5352d6f8166 263 //Perform the PID calculation.
maximbolduc 38:b5352d6f8166 264 double controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
maximbolduc 38:b5352d6f8166 265 //Make sure the computed output is within output constraints.
maximbolduc 38:b5352d6f8166 266 if (controllerOutput_ < 0.0) {
maximbolduc 38:b5352d6f8166 267 controllerOutput_ = 0.0;
maximbolduc 38:b5352d6f8166 268 } else if (controllerOutput_ > 1.0) {
maximbolduc 38:b5352d6f8166 269 controllerOutput_ = 1.0;
maximbolduc 38:b5352d6f8166 270 }
maximbolduc 38:b5352d6f8166 271
maximbolduc 38:b5352d6f8166 272 //Remember this output for the windup check next time.
maximbolduc 38:b5352d6f8166 273 prevControllerOutput_ = controllerOutput_;
maximbolduc 38:b5352d6f8166 274 //Remember the input for the derivative calculation next time.
maximbolduc 38:b5352d6f8166 275 prevProcessVariable_ = scaledPV;
maximbolduc 38:b5352d6f8166 276
maximbolduc 38:b5352d6f8166 277 //Scale the output from percent span back out to a real world number.
maximbolduc 38:b5352d6f8166 278 return ((controllerOutput_ * outSpan_) + outMin_);
maximbolduc 39:6767d4c840f9 279 }*/
maximbolduc 35:f9caeb8ca31e 280
maximbolduc 33:3e71c418e90d 281 double get_roll()
maximbolduc 33:3e71c418e90d 282 {
maximbolduc 33:3e71c418e90d 283 double roll_angle = 0;
maximbolduc 35:f9caeb8ca31e 284 if ( gyro_pos == 0 ) {
maximbolduc 33:3e71c418e90d 285 roll_angle = imuFilter.getRoll();
maximbolduc 35:f9caeb8ca31e 286 } else if ( gyro_pos == 1 ) {
maximbolduc 33:3e71c418e90d 287 roll_angle = imuFilter.getRoll() * -1;
maximbolduc 35:f9caeb8ca31e 288 } else if( gyro_pos == 2 ) {
maximbolduc 33:3e71c418e90d 289 roll_angle = imuFilter.getPitch() * -1;
maximbolduc 35:f9caeb8ca31e 290 } else if ( gyro_pos == 3 ) {
maximbolduc 33:3e71c418e90d 291 roll_angle = imuFilter.getPitch();
maximbolduc 33:3e71c418e90d 292 }
maximbolduc 33:3e71c418e90d 293 return roll_angle;
maximbolduc 33:3e71c418e90d 294 }
maximbolduc 35:f9caeb8ca31e 295
maximbolduc 33:3e71c418e90d 296 double get_pitch()
maximbolduc 33:3e71c418e90d 297 {
maximbolduc 33:3e71c418e90d 298 double pitch_angle = 0;
maximbolduc 35:f9caeb8ca31e 299 if ( gyro_pos == 0 ) {
maximbolduc 35:f9caeb8ca31e 300 pitch_angle = imuFilter.getPitch();
maximbolduc 35:f9caeb8ca31e 301 } else if ( gyro_pos == 1 ) {
maximbolduc 33:3e71c418e90d 302 pitch_angle = imuFilter.getPitch() * -1;
maximbolduc 35:f9caeb8ca31e 303 } else if( gyro_pos == 2 ) {
maximbolduc 33:3e71c418e90d 304 pitch_angle = imuFilter.getRoll();
maximbolduc 35:f9caeb8ca31e 305 } else if ( gyro_pos == 3 ) {
maximbolduc 35:f9caeb8ca31e 306 pitch_angle = imuFilter.getRoll() * -1;
maximbolduc 35:f9caeb8ca31e 307 }
maximbolduc 33:3e71c418e90d 308 return pitch_angle;
maximbolduc 33:3e71c418e90d 309 }
maximbolduc 35:f9caeb8ca31e 310
maximbolduc 26:dc00998140af 311 double dist_Point_to_Line( Point P, Point line_start, Point line_end)
maximbolduc 26:dc00998140af 312 {
maximbolduc 35:f9caeb8ca31e 313 Point v = point_sub(line_end,line_start);
maximbolduc 35:f9caeb8ca31e 314 Point w = point_sub(P,line_start);
maximbolduc 35:f9caeb8ca31e 315
maximbolduc 35:f9caeb8ca31e 316 double c1 = dot(w,v);
maximbolduc 35:f9caeb8ca31e 317 double c2 = dot(v,v);
maximbolduc 35:f9caeb8ca31e 318 double b = c1 / c2;
maximbolduc 35:f9caeb8ca31e 319
maximbolduc 35:f9caeb8ca31e 320 Point resulting(b * v.GetX(),b*v.GetY());
maximbolduc 35:f9caeb8ca31e 321 Point Pb = point_add(line_start, resulting);
maximbolduc 38:b5352d6f8166 322
maximbolduc 35:f9caeb8ca31e 323 return d(P, Pb);
maximbolduc 26:dc00998140af 324 }
maximbolduc 35:f9caeb8ca31e 325
maximbolduc 26:dc00998140af 326 double lat_to_deg(char *s, char north_south)
maximbolduc 26:dc00998140af 327 {
maximbolduc 26:dc00998140af 328 int deg, min, sec;
maximbolduc 26:dc00998140af 329 double fsec, val;
maximbolduc 35:f9caeb8ca31e 330
maximbolduc 26:dc00998140af 331 deg = ( (s[0] - '0') * 10) + s[1] - '0';
maximbolduc 26:dc00998140af 332 min = ( (s[2] - '0') * 10) + s[3] - '0';
maximbolduc 26:dc00998140af 333 sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
maximbolduc 26:dc00998140af 334 fsec = (double)((double)sec /10000.0);
maximbolduc 26:dc00998140af 335 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 35:f9caeb8ca31e 336 if (north_south == 'S') {
maximbolduc 26:dc00998140af 337 val *= -1.0;
maximbolduc 26:dc00998140af 338 }
maximbolduc 26:dc00998140af 339 return val;
maximbolduc 26:dc00998140af 340 }
maximbolduc 35:f9caeb8ca31e 341
maximbolduc 30:3afafa1ef16b 342 // isLeft(): test if a point is Left|On|Right of an infinite 2D line.
maximbolduc 30:3afafa1ef16b 343 // Input: three points P0, P1, and P2
maximbolduc 30:3afafa1ef16b 344 // Return: >0 for P2 left of the line through P0 to P1
maximbolduc 30:3afafa1ef16b 345 // =0 for P2 on the line
maximbolduc 30:3afafa1ef16b 346 // <0 for P2 right of the line
maximbolduc 35:f9caeb8ca31e 347 int isLeft( Point P0, Point P1, Point P2 )
maximbolduc 30:3afafa1ef16b 348 {
maximbolduc 35:f9caeb8ca31e 349 double isleft = ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX()) - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
maximbolduc 35:f9caeb8ca31e 350 if ( isleft > 0 ) {
maximbolduc 35:f9caeb8ca31e 351 isleft = 1;
maximbolduc 35:f9caeb8ca31e 352 } else {
maximbolduc 35:f9caeb8ca31e 353 isleft = -1;
maximbolduc 35:f9caeb8ca31e 354 }
maximbolduc 35:f9caeb8ca31e 355 return (int)isleft;
maximbolduc 30:3afafa1ef16b 356 }
maximbolduc 35:f9caeb8ca31e 357
maximbolduc 26:dc00998140af 358 double lon_to_deg(char *s, char east_west)
maximbolduc 26:dc00998140af 359 {
maximbolduc 26:dc00998140af 360 int deg, min, sec;
maximbolduc 26:dc00998140af 361 double fsec, val;
maximbolduc 26:dc00998140af 362 deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
maximbolduc 26:dc00998140af 363 min = ( (s[3] - '0') * 10) + s[4] - '0';
maximbolduc 26:dc00998140af 364 sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
maximbolduc 26:dc00998140af 365 fsec = (double)((double)sec /10000.0);
maximbolduc 26:dc00998140af 366 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 38:b5352d6f8166 367
maximbolduc 35:f9caeb8ca31e 368 if (east_west == 'W') {
maximbolduc 26:dc00998140af 369 val *= -1.0;
maximbolduc 26:dc00998140af 370 }
maximbolduc 26:dc00998140af 371 return val;
maximbolduc 26:dc00998140af 372 }
maximbolduc 35:f9caeb8ca31e 373
maximbolduc 26:dc00998140af 374 void nmea_gga(char *s)
maximbolduc 26:dc00998140af 375 {
maximbolduc 26:dc00998140af 376 char *token;
maximbolduc 26:dc00998140af 377 int token_counter = 0;
maximbolduc 26:dc00998140af 378 char *latitude = (char *)NULL;
maximbolduc 26:dc00998140af 379 char *longitude = (char *)NULL;
maximbolduc 26:dc00998140af 380 char *lat_dir = (char *)NULL;
maximbolduc 26:dc00998140af 381 char *lon_dir = (char *)NULL;
maximbolduc 26:dc00998140af 382 char *qual = (char *)NULL;
maximbolduc 26:dc00998140af 383 char *altitude = (char *)NULL;
maximbolduc 26:dc00998140af 384 char *sats = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 385
maximbolduc 26:dc00998140af 386 token = strtok(s, ",");
maximbolduc 35:f9caeb8ca31e 387 while (token) {
maximbolduc 35:f9caeb8ca31e 388 switch (token_counter) {
maximbolduc 26:dc00998140af 389 case 2:
maximbolduc 26:dc00998140af 390 latitude = token;
maximbolduc 26:dc00998140af 391 break;
maximbolduc 26:dc00998140af 392 case 4:
maximbolduc 26:dc00998140af 393 longitude = token;
maximbolduc 26:dc00998140af 394 break;
maximbolduc 26:dc00998140af 395 case 3:
maximbolduc 26:dc00998140af 396 lat_dir = token;
maximbolduc 26:dc00998140af 397 break;
maximbolduc 26:dc00998140af 398 case 5:
maximbolduc 26:dc00998140af 399 lon_dir = token;
maximbolduc 26:dc00998140af 400 break;
maximbolduc 26:dc00998140af 401 case 6:
maximbolduc 26:dc00998140af 402 qual = token;
maximbolduc 26:dc00998140af 403 break;
maximbolduc 26:dc00998140af 404 case 7:
maximbolduc 26:dc00998140af 405 sats = token;
maximbolduc 26:dc00998140af 406 break;
maximbolduc 26:dc00998140af 407 case 9:
maximbolduc 26:dc00998140af 408 altitude = token;
maximbolduc 26:dc00998140af 409 break;
maximbolduc 26:dc00998140af 410 }
maximbolduc 26:dc00998140af 411 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 412 token_counter++;
maximbolduc 26:dc00998140af 413 }
maximbolduc 35:f9caeb8ca31e 414 if (latitude && longitude && altitude && sats) {
maximbolduc 26:dc00998140af 415 decimal_latitude = lat_to_deg(latitude, lat_dir[0]);
maximbolduc 26:dc00998140af 416 decimal_lon = lon_to_deg(longitude, lon_dir[0]);
maximbolduc 26:dc00998140af 417 num_of_gps_sats = atoi(sats);
maximbolduc 26:dc00998140af 418 gps_satellite_quality = atoi(qual);
maximbolduc 35:f9caeb8ca31e 419 } else {
maximbolduc 26:dc00998140af 420 gps_satellite_quality = 0;
maximbolduc 26:dc00998140af 421 }
maximbolduc 26:dc00998140af 422 }
maximbolduc 35:f9caeb8ca31e 423
maximbolduc 35:f9caeb8ca31e 424 void autosteer_done()
maximbolduc 35:f9caeb8ca31e 425 {
maximbolduc 35:f9caeb8ca31e 426 enable_motor = 0;
maximbolduc 35:f9caeb8ca31e 427 }
maximbolduc 35:f9caeb8ca31e 428
maximbolduc 26:dc00998140af 429 //from farmerGPS code
maximbolduc 26:dc00998140af 430 void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
maximbolduc 26:dc00998140af 431 {
maximbolduc 26:dc00998140af 432 double ydist = 0;
maximbolduc 26:dc00998140af 433 double xdist = 0;
maximbolduc 26:dc00998140af 434 angle = angle + 180;
maximbolduc 26:dc00998140af 435 double radiant = angle * 3.14159265359 / 180;
maximbolduc 26:dc00998140af 436 double sinr = sin(radiant);
maximbolduc 26:dc00998140af 437 double cosr = cos(radiant);
maximbolduc 26:dc00998140af 438 xdist = cosr * distance;
maximbolduc 26:dc00998140af 439 ydist = sinr * distance;
maximbolduc 26:dc00998140af 440 lat2 = lat1 + (ydist / (69.09 * -1609.344));
maximbolduc 26:dc00998140af 441 lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
maximbolduc 26:dc00998140af 442 }
maximbolduc 35:f9caeb8ca31e 443
maximbolduc 26:dc00998140af 444 Point compensation;
maximbolduc 35:f9caeb8ca31e 445
maximbolduc 26:dc00998140af 446 void yaw_compensate()
maximbolduc 26:dc00998140af 447 {
maximbolduc 33:3e71c418e90d 448 yaw = get_yaw();
maximbolduc 26:dc00998140af 449 }
maximbolduc 34:c2bc9f9be7ff 450
maximbolduc 35:f9caeb8ca31e 451 void pitch_and_roll(double real_bearing)
maximbolduc 34:c2bc9f9be7ff 452 {
maximbolduc 34:c2bc9f9be7ff 453 pitch = get_pitch();
maximbolduc 34:c2bc9f9be7ff 454 roll = get_roll();
maximbolduc 34:c2bc9f9be7ff 455 compensation.SetX(antennaheight * tan(roll) * sin(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * cos(real_bearing/ 57.295779513));
maximbolduc 34:c2bc9f9be7ff 456 compensation.SetY(antennaheight * tan(roll) * cos(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * sin(real_bearing/ 57.295779513));
maximbolduc 34:c2bc9f9be7ff 457 }
maximbolduc 33:3e71c418e90d 458
maximbolduc 26:dc00998140af 459 void process_GPSHEIGHT(char* height_string)
maximbolduc 26:dc00998140af 460 {
maximbolduc 26:dc00998140af 461 char *token;
maximbolduc 26:dc00998140af 462 int token_counter = 0;
maximbolduc 26:dc00998140af 463 char *height = (char *)NULL;
maximbolduc 26:dc00998140af 464 token = strtok(height_string, ",");
maximbolduc 35:f9caeb8ca31e 465 while (token) {
maximbolduc 35:f9caeb8ca31e 466 switch (token_counter) {
maximbolduc 26:dc00998140af 467 case 1:
maximbolduc 26:dc00998140af 468 height = token;
maximbolduc 26:dc00998140af 469 break;
maximbolduc 26:dc00998140af 470 }
maximbolduc 26:dc00998140af 471 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 472 token_counter++;
maximbolduc 26:dc00998140af 473 }
maximbolduc 35:f9caeb8ca31e 474 if ( height ) {
jhedmonton 28:5905886c76ee 475 antennaheight = atof(height);
maximbolduc 30:3afafa1ef16b 476 Config_Save();
maximbolduc 26:dc00998140af 477 }
maximbolduc 26:dc00998140af 478 }
maximbolduc 26:dc00998140af 479
maximbolduc 35:f9caeb8ca31e 480 char dms[128];
maximbolduc 34:c2bc9f9be7ff 481 char* To_DMS(double dec_deg)
maximbolduc 34:c2bc9f9be7ff 482 {
maximbolduc 34:c2bc9f9be7ff 483 dec_deg = abs(dec_deg);
maximbolduc 34:c2bc9f9be7ff 484 int d = (int)(dec_deg);
maximbolduc 34:c2bc9f9be7ff 485 sprintf(dms,"%0.2i\0",d);
maximbolduc 34:c2bc9f9be7ff 486 double m = (double)(((double)dec_deg - (double)d) * 60.0);
maximbolduc 34:c2bc9f9be7ff 487 if (m < 10 ) {
maximbolduc 34:c2bc9f9be7ff 488 sprintf(dms,"%s0%0.9f\0",dms,m);
maximbolduc 34:c2bc9f9be7ff 489 } else {
maximbolduc 34:c2bc9f9be7ff 490 sprintf(dms,"%s%0.9f\0",dms,m);
maximbolduc 34:c2bc9f9be7ff 491 }
maximbolduc 34:c2bc9f9be7ff 492 return dms;
maximbolduc 34:c2bc9f9be7ff 493 }
maximbolduc 34:c2bc9f9be7ff 494
maximbolduc 34:c2bc9f9be7ff 495 char* To_DMS_lon(double dec_deg)
maximbolduc 34:c2bc9f9be7ff 496 {
maximbolduc 34:c2bc9f9be7ff 497 dec_deg = abs(dec_deg);
maximbolduc 34:c2bc9f9be7ff 498 int d = (int)(dec_deg);
maximbolduc 34:c2bc9f9be7ff 499 sprintf(dms,"%0.3i\0",d);
maximbolduc 34:c2bc9f9be7ff 500 double m = (double)(((double)dec_deg - (double)d) * 60.0);
maximbolduc 34:c2bc9f9be7ff 501 if (m < 10 ) {
maximbolduc 34:c2bc9f9be7ff 502 sprintf(dms,"%s0%0.9f\0",dms,m);
maximbolduc 34:c2bc9f9be7ff 503 } else {
maximbolduc 34:c2bc9f9be7ff 504 sprintf(dms,"%s%0.9f\0",dms,m);
maximbolduc 34:c2bc9f9be7ff 505 }
maximbolduc 34:c2bc9f9be7ff 506 return dms;
maximbolduc 34:c2bc9f9be7ff 507 }
maximbolduc 34:c2bc9f9be7ff 508
maximbolduc 35:f9caeb8ca31e 509 //sets pwm1 and pwm2 and enable_motor
maximbolduc 35:f9caeb8ca31e 510 void process_ASTEER(char* asteer)
maximbolduc 35:f9caeb8ca31e 511 {
maximbolduc 35:f9caeb8ca31e 512 char *token;
maximbolduc 35:f9caeb8ca31e 513 int token_counter = 0;
maximbolduc 35:f9caeb8ca31e 514 char *asteer_speed = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 515 char *asteer_time = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 516 token = strtok(asteer, ",");
maximbolduc 35:f9caeb8ca31e 517 while (token) {
maximbolduc 35:f9caeb8ca31e 518 switch (token_counter) {
maximbolduc 35:f9caeb8ca31e 519 case 1:
maximbolduc 35:f9caeb8ca31e 520 asteer_speed = token;
maximbolduc 35:f9caeb8ca31e 521 break;
maximbolduc 35:f9caeb8ca31e 522 case 2:
maximbolduc 35:f9caeb8ca31e 523 asteer_time = token;
maximbolduc 35:f9caeb8ca31e 524 break;
maximbolduc 35:f9caeb8ca31e 525 }
maximbolduc 35:f9caeb8ca31e 526 token = strtok((char *)NULL, ",");
maximbolduc 35:f9caeb8ca31e 527 token_counter++;
maximbolduc 35:f9caeb8ca31e 528 }
maximbolduc 35:f9caeb8ca31e 529 if ( asteer_speed && asteer_time ) {
maximbolduc 35:f9caeb8ca31e 530 motorspeed = atof(asteer_speed);
maximbolduc 35:f9caeb8ca31e 531 enable_time = atof(asteer_time);
maximbolduc 36:8e84b5ade03e 532 autosteer_timeout.reset();
maximbolduc 36:8e84b5ade03e 533 time_till_stop = atoi(asteer_time);
maximbolduc 37:ac60a8a0ae8a 534 if ( motor_enable == 0 ) {
maximbolduc 35:f9caeb8ca31e 535 } else {
maximbolduc 37:ac60a8a0ae8a 536 if ( motorspeed > 127.0 ) {
maximbolduc 37:ac60a8a0ae8a 537 pwm2_speed = 0.0;
maximbolduc 37:ac60a8a0ae8a 538 pwm1_speed = ((double)motorspeed - (double)127.0) / 128.0;
maximbolduc 37:ac60a8a0ae8a 539
maximbolduc 37:ac60a8a0ae8a 540 } else if ( motorspeed < 127.0 ) {
maximbolduc 37:ac60a8a0ae8a 541 pwm1_speed = 0.0;
maximbolduc 37:ac60a8a0ae8a 542 pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
maximbolduc 37:ac60a8a0ae8a 543 } else {
maximbolduc 37:ac60a8a0ae8a 544 pwm1_speed = 0;
maximbolduc 37:ac60a8a0ae8a 545 pwm2_speed = 0;
maximbolduc 37:ac60a8a0ae8a 546 enable_motor = 0;
maximbolduc 37:ac60a8a0ae8a 547 }
maximbolduc 37:ac60a8a0ae8a 548 // if(Authenticated)
maximbolduc 37:ac60a8a0ae8a 549 // {
maximbolduc 37:ac60a8a0ae8a 550 pwm1 = pwm1_speed;
maximbolduc 37:ac60a8a0ae8a 551 pwm2 = pwm2_speed;
maximbolduc 37:ac60a8a0ae8a 552 enable_motor = 1;
maximbolduc 38:b5352d6f8166 553 //}
maximbolduc 35:f9caeb8ca31e 554 }
maximbolduc 35:f9caeb8ca31e 555 }
maximbolduc 35:f9caeb8ca31e 556 }
maximbolduc 35:f9caeb8ca31e 557
maximbolduc 38:b5352d6f8166 558 double lastval = 0;
maximbolduc 35:f9caeb8ca31e 559 //gets the motor value between -255 and 255 (- means left, positive means right)
maximbolduc 35:f9caeb8ca31e 560 //distance in meters, always positive
maximbolduc 35:f9caeb8ca31e 561 //angle in degrees
maximbolduc 35:f9caeb8ca31e 562 //Points in lat/lon format
maximbolduc 37:ac60a8a0ae8a 563 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 564 {
maximbolduc 38:b5352d6f8166 565 dist_line = abs(dist_line);
maximbolduc 38:b5352d6f8166 566 double error = 1;
maximbolduc 35:f9caeb8ca31e 567 int position_sign = isLeft( line_start, line_end, current);
maximbolduc 35:f9caeb8ca31e 568 int forward_sign = isLeft(line_start,line_end, target);
maximbolduc 37:ac60a8a0ae8a 569 double position_dist = abs(dist_Point_to_Line( current,line_start,line_end) * filtering);
maximbolduc 35:f9caeb8ca31e 570
maximbolduc 37:ac60a8a0ae8a 571 if ( forward_sign == -1 ) {
maximbolduc 37:ac60a8a0ae8a 572 error = error * -1;
maximbolduc 37:ac60a8a0ae8a 573 } else if ( forward_sign == 1 ) {
maximbolduc 37:ac60a8a0ae8a 574 error = error;
maximbolduc 35:f9caeb8ca31e 575 }
maximbolduc 40:a68cc1a1a1e7 576 //if ( abs(position_dist) < 0.5 ) {
maximbolduc 39:6767d4c840f9 577 if ( forward_sign == position_sign ) {
maximbolduc 40:a68cc1a1a1e7 578 if ( position_dist > dist_line ) { // && abs(position_dist) <
maximbolduc 40:a68cc1a1a1e7 579 // error = error * (dist_line * filter_g - (position_dist * phase_adv));
maximbolduc 40:a68cc1a1a1e7 580 // error = 0;
maximbolduc 39:6767d4c840f9 581 } else {
maximbolduc 40:a68cc1a1a1e7 582 // error = error * (dist_line * filter_g - (position_dist * phase_adv)*0.8);
maximbolduc 39:6767d4c840f9 583 error = error * dist_line * filter_g;
maximbolduc 39:6767d4c840f9 584 }
maximbolduc 39:6767d4c840f9 585 } else { //
maximbolduc 40:a68cc1a1a1e7 586 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 37:ac60a8a0ae8a 587 }
maximbolduc 40:a68cc1a1a1e7 588 //} else { //target coming back at 15-20 degrees on the line
maximbolduc 40:a68cc1a1a1e7 589 error = error;// + heading_err * 2 ;
maximbolduc 40:a68cc1a1a1e7 590 //}
maximbolduc 38:b5352d6f8166 591
maximbolduc 35:f9caeb8ca31e 592 error = error * scale;
maximbolduc 38:b5352d6f8166 593 if ( error > 0 ) {
maximbolduc 38:b5352d6f8166 594 error = error + center;
maximbolduc 38:b5352d6f8166 595 } else if (error < 0 ) {
maximbolduc 38:b5352d6f8166 596 error = error - center;
maximbolduc 38:b5352d6f8166 597 }
maximbolduc 35:f9caeb8ca31e 598 if ( error > 255 ) {
maximbolduc 35:f9caeb8ca31e 599 error = 255;
maximbolduc 35:f9caeb8ca31e 600 } else if ( error < -255 ) {
maximbolduc 35:f9caeb8ca31e 601 error = -255;
maximbolduc 35:f9caeb8ca31e 602 }
maximbolduc 38:b5352d6f8166 603
maximbolduc 35:f9caeb8ca31e 604 error = error + 255;
maximbolduc 35:f9caeb8ca31e 605 error = (int)(error / 2);
maximbolduc 35:f9caeb8ca31e 606
maximbolduc 35:f9caeb8ca31e 607 return (int)error;
maximbolduc 35:f9caeb8ca31e 608 }
maximbolduc 35:f9caeb8ca31e 609
maximbolduc 38:b5352d6f8166 610 char *strsep(char **stringp, char *delim)
maximbolduc 38:b5352d6f8166 611 {
maximbolduc 38:b5352d6f8166 612 char *s;
maximbolduc 38:b5352d6f8166 613 const char *spanp;
maximbolduc 38:b5352d6f8166 614 int c, sc;
maximbolduc 38:b5352d6f8166 615 char *tok;
maximbolduc 38:b5352d6f8166 616 if ((s = *stringp) == NULL)
maximbolduc 38:b5352d6f8166 617 return (NULL);
maximbolduc 38:b5352d6f8166 618 for (tok = s;;) {
maximbolduc 38:b5352d6f8166 619 c = *s++;
maximbolduc 38:b5352d6f8166 620 spanp = delim;
maximbolduc 38:b5352d6f8166 621 do {
maximbolduc 38:b5352d6f8166 622 if ((sc = *spanp++) == c) {
maximbolduc 38:b5352d6f8166 623 if (c == 0)
maximbolduc 38:b5352d6f8166 624 s = NULL;
maximbolduc 38:b5352d6f8166 625 else
maximbolduc 38:b5352d6f8166 626 s[-1] = 0;
maximbolduc 38:b5352d6f8166 627 *stringp = s;
maximbolduc 38:b5352d6f8166 628 return (tok);
maximbolduc 38:b5352d6f8166 629 }
maximbolduc 38:b5352d6f8166 630 } while (sc != 0);
maximbolduc 38:b5352d6f8166 631 }
maximbolduc 38:b5352d6f8166 632 /* NOTREACHED */
maximbolduc 38:b5352d6f8166 633 }
maximbolduc 38:b5352d6f8166 634
maximbolduc 38:b5352d6f8166 635 Point old_position;
maximbolduc 38:b5352d6f8166 636
maximbolduc 38:b5352d6f8166 637 //char rmc_cpy[256];
maximbolduc 26:dc00998140af 638 void nmea_rmc(char *s)
maximbolduc 26:dc00998140af 639 {
maximbolduc 26:dc00998140af 640 char *token;
maximbolduc 26:dc00998140af 641 int token_counter = 0;
maximbolduc 26:dc00998140af 642 char *time = (char *)NULL;
maximbolduc 26:dc00998140af 643 char *date = (char *)NULL;
maximbolduc 26:dc00998140af 644 char *stat = (char *)NULL;
maximbolduc 26:dc00998140af 645 char *vel = (char *)NULL;
maximbolduc 26:dc00998140af 646 char *trk = (char *)NULL;
maximbolduc 26:dc00998140af 647 char *magv = (char *)NULL;
maximbolduc 38:b5352d6f8166 648 char *magd = (char *)NULL;
maximbolduc 38:b5352d6f8166 649 char *latit = "";
maximbolduc 38:b5352d6f8166 650 char *longit = "";
maximbolduc 26:dc00998140af 651 char *latitude = (char *)NULL;
maximbolduc 26:dc00998140af 652 char *longitude = (char *)NULL;
maximbolduc 26:dc00998140af 653 char *lat_dir = (char *)NULL;
maximbolduc 26:dc00998140af 654 char *lon_dir = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 655
maximbolduc 38:b5352d6f8166 656 while ((token = strsep(&s, ",")) != NULL) {
maximbolduc 35:f9caeb8ca31e 657 switch (token_counter) {
maximbolduc 26:dc00998140af 658 case 1:
maximbolduc 26:dc00998140af 659 time = token;
maximbolduc 26:dc00998140af 660 break;
maximbolduc 26:dc00998140af 661 case 2:
maximbolduc 26:dc00998140af 662 stat = token;
maximbolduc 26:dc00998140af 663 break;
maximbolduc 34:c2bc9f9be7ff 664 case 3:
maximbolduc 39:6767d4c840f9 665 if ( token ) {
maximbolduc 38:b5352d6f8166 666 latit = token;
maximbolduc 38:b5352d6f8166 667 latitude = token;
maximbolduc 38:b5352d6f8166 668 }
maximbolduc 34:c2bc9f9be7ff 669 break;
maximbolduc 34:c2bc9f9be7ff 670 case 4:
maximbolduc 34:c2bc9f9be7ff 671 lat_dir = token;
maximbolduc 34:c2bc9f9be7ff 672 break;
maximbolduc 34:c2bc9f9be7ff 673 case 5:
maximbolduc 38:b5352d6f8166 674 longit = token;
maximbolduc 34:c2bc9f9be7ff 675 longitude = token;
maximbolduc 34:c2bc9f9be7ff 676 break;
maximbolduc 34:c2bc9f9be7ff 677 case 6:
maximbolduc 34:c2bc9f9be7ff 678 lon_dir = token;
maximbolduc 34:c2bc9f9be7ff 679 break;
maximbolduc 26:dc00998140af 680 case 7:
maximbolduc 26:dc00998140af 681 vel = token;
maximbolduc 26:dc00998140af 682 break;
maximbolduc 26:dc00998140af 683 case 8:
maximbolduc 26:dc00998140af 684 trk = token;
maximbolduc 26:dc00998140af 685 break;
maximbolduc 34:c2bc9f9be7ff 686 case 9:
maximbolduc 34:c2bc9f9be7ff 687 date = token;
maximbolduc 34:c2bc9f9be7ff 688 break;
maximbolduc 26:dc00998140af 689 case 10:
maximbolduc 26:dc00998140af 690 magv = token;
maximbolduc 26:dc00998140af 691 break;
maximbolduc 38:b5352d6f8166 692 case 11:
maximbolduc 38:b5352d6f8166 693 magd = token;
maximbolduc 38:b5352d6f8166 694 break;
maximbolduc 26:dc00998140af 695 }
maximbolduc 26:dc00998140af 696 token_counter++;
maximbolduc 26:dc00998140af 697 }
maximbolduc 38:b5352d6f8166 698 if (stat!= '\0' && date!= '\0' && time!= '\0') {
maximbolduc 26:dc00998140af 699 hour = (char)((time[0] - '0') * 10) + (time[1] - '0');
maximbolduc 26:dc00998140af 700 minute = (char)((time[2] - '0') * 10) + (time[3] - '0');
maximbolduc 26:dc00998140af 701 second = (char)((time[4] - '0') * 10) + (time[5] - '0');
maximbolduc 26:dc00998140af 702 day = (char)((date[0] - '0') * 10) + (date[1] - '0');
maximbolduc 26:dc00998140af 703 month = (char)((date[2] - '0') * 10) + (date[3] - '0');
maximbolduc 26:dc00998140af 704 year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
maximbolduc 26:dc00998140af 705 status = stat[0];
maximbolduc 26:dc00998140af 706 velocity = atof(vel);
maximbolduc 26:dc00998140af 707 speed_km = velocity * 1.852;
maximbolduc 26:dc00998140af 708 speed_m_s = speed_km * 3600.0 / 1000.0;
maximbolduc 26:dc00998140af 709 track = atof(trk);
maximbolduc 26:dc00998140af 710 magvar = atof(magv);
maximbolduc 26:dc00998140af 711 }
maximbolduc 39:6767d4c840f9 712 double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
maximbolduc 39:6767d4c840f9 713 double diff_angle = Freepilot_bearing - angle;
maximbolduc 39:6767d4c840f9 714 diff_angle = ((int)diff_angle + 180) % 360 - 180;
maximbolduc 39:6767d4c840f9 715
maximbolduc 39:6767d4c840f9 716 // pc.printf("%f %f %f\r\n",diff_angle, Freepilot_bearing, (track - 90) * -1);
maximbolduc 39:6767d4c840f9 717 if ( abs(diff_angle) > 90 ) {
maximbolduc 39:6767d4c840f9 718 if ( (abs(360 - diff_angle)) > 90 ) {
maximbolduc 39:6767d4c840f9 719 Point temp = line_end;
maximbolduc 39:6767d4c840f9 720 line_end = line_start;
maximbolduc 39:6767d4c840f9 721 line_start = temp;
maximbolduc 39:6767d4c840f9 722 Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
maximbolduc 39:6767d4c840f9 723 }
maximbolduc 39:6767d4c840f9 724 }
maximbolduc 38:b5352d6f8166 725 if ( longit != '\0' && latit != '\0' ) {
maximbolduc 38:b5352d6f8166 726 old_position = position;
maximbolduc 38:b5352d6f8166 727 position.SetX(lat_to_deg(latitude, lat_dir[0]));
maximbolduc 38:b5352d6f8166 728 position.SetY(lon_to_deg(longitude, lon_dir[0]));
maximbolduc 38:b5352d6f8166 729 cm_per_deg_lat = 11054000;
maximbolduc 38:b5352d6f8166 730 cm_per_deg_lon = 11132000 * cos(decimal_latitude);
maximbolduc 35:f9caeb8ca31e 731
maximbolduc 38:b5352d6f8166 732 // pitch_and_roll((track-90)*-1);// as to be real bearing
maximbolduc 35:f9caeb8ca31e 733
maximbolduc 38:b5352d6f8166 734 compensation.SetY(compensation.GetY() / cm_per_deg_lon);
maximbolduc 38:b5352d6f8166 735 compensation.SetX(compensation.GetX() / cm_per_deg_lat);
maximbolduc 38:b5352d6f8166 736
maximbolduc 38:b5352d6f8166 737 position = point_add(position,compensation);
maximbolduc 35:f9caeb8ca31e 738
maximbolduc 38:b5352d6f8166 739 double lookaheaddistance = lookaheadtime * speed_m_s;
maximbolduc 38:b5352d6f8166 740 get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,(track-90)*-1,look_ahead_lon,look_ahead_lat);
maximbolduc 38:b5352d6f8166 741 looked_ahead.SetX(look_ahead_lat);
maximbolduc 38:b5352d6f8166 742 looked_ahead.SetY(look_ahead_lon);
maximbolduc 38:b5352d6f8166 743 double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
maximbolduc 38:b5352d6f8166 744 distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;
maximbolduc 35:f9caeb8ca31e 745
maximbolduc 39:6767d4c840f9 746 int val = freepilot(position, looked_ahead, diff_angle, distance_to_line, scale, filterg , phaseadv,tcenter ,filtering);
maximbolduc 35:f9caeb8ca31e 747
maximbolduc 38:b5352d6f8166 748 char command[128];
maximbolduc 38:b5352d6f8166 749 sprintf(command,"$ASTEER,%i,%i\r\n",val,250); //(int)((((val/2)-127)/scale)+127),500);
maximbolduc 38:b5352d6f8166 750 pc.puts(command);
maximbolduc 38:b5352d6f8166 751 process_ASTEER(command);
maximbolduc 38:b5352d6f8166 752 }
maximbolduc 34:c2bc9f9be7ff 753 string rmc = "";
maximbolduc 38:b5352d6f8166 754 if(time!= '\0') {
maximbolduc 34:c2bc9f9be7ff 755 rmc = "$GPRMC,";
maximbolduc 34:c2bc9f9be7ff 756 rmc += string(time) + ",";
maximbolduc 34:c2bc9f9be7ff 757 } else {
maximbolduc 34:c2bc9f9be7ff 758 rmc = "$GPRMC,,";
maximbolduc 34:c2bc9f9be7ff 759 }
maximbolduc 38:b5352d6f8166 760 if(stat!= '\0') {
maximbolduc 34:c2bc9f9be7ff 761 rmc +=(string(stat) + ",");
maximbolduc 34:c2bc9f9be7ff 762 } else {
maximbolduc 34:c2bc9f9be7ff 763 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 764 }
maximbolduc 38:b5352d6f8166 765 if ( latit != '\0' && lat_dir!= '\0') {
maximbolduc 34:c2bc9f9be7ff 766 rmc +=( string(To_DMS(position.GetX())) + "," + string(lat_dir) +",");
maximbolduc 34:c2bc9f9be7ff 767 } else {
maximbolduc 34:c2bc9f9be7ff 768 rmc += ",,";
maximbolduc 34:c2bc9f9be7ff 769 }
maximbolduc 38:b5352d6f8166 770 if ( longit != '\0' && lon_dir!= '\0' ) {
maximbolduc 34:c2bc9f9be7ff 771 rmc += (string(To_DMS_lon(position.GetY())) + "," + string(lon_dir) + ",");
maximbolduc 34:c2bc9f9be7ff 772 } else {
maximbolduc 34:c2bc9f9be7ff 773 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 774 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 775 }
maximbolduc 38:b5352d6f8166 776 if (vel!= '\0') {
maximbolduc 38:b5352d6f8166 777 rmc += (string(vel) + ",");
maximbolduc 34:c2bc9f9be7ff 778 } else {
maximbolduc 34:c2bc9f9be7ff 779 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 780 }
maximbolduc 38:b5352d6f8166 781 if ((trk)!= '\0') {
maximbolduc 34:c2bc9f9be7ff 782 rmc += string(trk) + ",";
maximbolduc 34:c2bc9f9be7ff 783 } else {
maximbolduc 34:c2bc9f9be7ff 784 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 785 }
maximbolduc 38:b5352d6f8166 786 if (date!= '\0') {
maximbolduc 34:c2bc9f9be7ff 787 rmc += string(date) + ",";
maximbolduc 34:c2bc9f9be7ff 788 } else {
maximbolduc 34:c2bc9f9be7ff 789 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 790 }
maximbolduc 38:b5352d6f8166 791 if (magv!= '\0') {
maximbolduc 34:c2bc9f9be7ff 792 rmc += string(magv) + ",";
maximbolduc 34:c2bc9f9be7ff 793 } else {
maximbolduc 34:c2bc9f9be7ff 794 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 795 }
maximbolduc 38:b5352d6f8166 796 if (magd!= '\0') {
maximbolduc 38:b5352d6f8166 797 rmc += string(magd) + ",W";
maximbolduc 38:b5352d6f8166 798 } else {
maximbolduc 38:b5352d6f8166 799 rmc += ",W";
maximbolduc 38:b5352d6f8166 800 }
maximbolduc 34:c2bc9f9be7ff 801
maximbolduc 34:c2bc9f9be7ff 802 char test[256];
maximbolduc 38:b5352d6f8166 803 sprintf(test,"%s*\0",rmc.c_str());
maximbolduc 38:b5352d6f8166 804 sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));
maximbolduc 39:6767d4c840f9 805
maximbolduc 34:c2bc9f9be7ff 806 bluetooth.puts(output);
maximbolduc 26:dc00998140af 807 }
maximbolduc 35:f9caeb8ca31e 808
maximbolduc 26:dc00998140af 809 void process_FGPSAB(char* ab)
maximbolduc 26:dc00998140af 810 {
maximbolduc 33:3e71c418e90d 811 char *token;
maximbolduc 33:3e71c418e90d 812 int token_counter = 0;
maximbolduc 33:3e71c418e90d 813 char *line_lat = (char *)NULL;
maximbolduc 33:3e71c418e90d 814 char *line_lon = (char *)NULL;
maximbolduc 33:3e71c418e90d 815 char *line_lat1 = (char *)NULL;
maximbolduc 33:3e71c418e90d 816 char *line_lon1 = (char *)NULL;
maximbolduc 38:b5352d6f8166 817 //char *bearing = (char *)NULL;
maximbolduc 38:b5352d6f8166 818 string bearing = "";
maximbolduc 33:3e71c418e90d 819 token = strtok(ab, ",");
maximbolduc 35:f9caeb8ca31e 820 while (token) {
maximbolduc 35:f9caeb8ca31e 821 switch (token_counter) {
maximbolduc 33:3e71c418e90d 822 case 1:
maximbolduc 33:3e71c418e90d 823 line_lat = token;
maximbolduc 33:3e71c418e90d 824 break;
maximbolduc 33:3e71c418e90d 825 case 2:
maximbolduc 33:3e71c418e90d 826 line_lon = token;
maximbolduc 33:3e71c418e90d 827 break;
maximbolduc 33:3e71c418e90d 828 case 3:
maximbolduc 33:3e71c418e90d 829 line_lat1 = token;
maximbolduc 33:3e71c418e90d 830 break;
maximbolduc 33:3e71c418e90d 831 case 4:
maximbolduc 33:3e71c418e90d 832 line_lon1 = token;
maximbolduc 33:3e71c418e90d 833 break;
maximbolduc 33:3e71c418e90d 834 case 5:
maximbolduc 38:b5352d6f8166 835 for (int n=0; n < sizeof(token); n++) {
maximbolduc 38:b5352d6f8166 836 if ( token[n] == '*' ) {
maximbolduc 38:b5352d6f8166 837 break;
maximbolduc 38:b5352d6f8166 838 } else {
maximbolduc 38:b5352d6f8166 839 bearing += token[n];
maximbolduc 38:b5352d6f8166 840 }
maximbolduc 38:b5352d6f8166 841 }
maximbolduc 33:3e71c418e90d 842 break;
maximbolduc 26:dc00998140af 843 }
maximbolduc 33:3e71c418e90d 844 token = strtok((char *)NULL, ",");
maximbolduc 33:3e71c418e90d 845 token_counter++;
maximbolduc 33:3e71c418e90d 846 }
maximbolduc 39:6767d4c840f9 847 double Freepilot_lon = atof(line_lon);
maximbolduc 39:6767d4c840f9 848 double Freepilot_lat = atof(line_lat);
maximbolduc 39:6767d4c840f9 849 double Freepilot_lon1 = atof(line_lon1);
maximbolduc 39:6767d4c840f9 850 double Freepilot_lat1 = atof(line_lat1);
maximbolduc 39:6767d4c840f9 851 Freepilot_bearing = atof(bearing.c_str()) + 360;
maximbolduc 39:6767d4c840f9 852 Freepilot_bearing = ((int)Freepilot_bearing+ 180) % 360 - 180;
maximbolduc 33:3e71c418e90d 853 line_start.SetX(Freepilot_lat);
maximbolduc 33:3e71c418e90d 854 line_start.SetY(Freepilot_lon);
maximbolduc 33:3e71c418e90d 855 line_end.SetX(Freepilot_lat1);
maximbolduc 33:3e71c418e90d 856 line_end.SetY(Freepilot_lon1);
maximbolduc 39:6767d4c840f9 857
maximbolduc 33:3e71c418e90d 858 active_AB = 1;
maximbolduc 35:f9caeb8ca31e 859
maximbolduc 38:b5352d6f8166 860 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 861 pc.puts(output);
maximbolduc 26:dc00998140af 862 }
maximbolduc 35:f9caeb8ca31e 863
maximbolduc 26:dc00998140af 864 void process_FGPSAUTO(char* FGPSAUTO)
maximbolduc 26:dc00998140af 865 {
maximbolduc 26:dc00998140af 866 char *token;
maximbolduc 26:dc00998140af 867 int token_counter = 0;
maximbolduc 26:dc00998140af 868 char *ahead = (char *)NULL;
maximbolduc 26:dc00998140af 869 char *center = (char *)NULL;
maximbolduc 26:dc00998140af 870 char *phase = (char *)NULL;
maximbolduc 26:dc00998140af 871 char *scl = (char *)NULL;
maximbolduc 26:dc00998140af 872 char *avg = (char *)NULL;
maximbolduc 26:dc00998140af 873 char *_kp = (char *)NULL;
maximbolduc 26:dc00998140af 874 char *_ki = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 875 char *fg = (char *)NULL;
maximbolduc 26:dc00998140af 876 char *_kd = (char *)NULL;
maximbolduc 39:6767d4c840f9 877
maximbolduc 26:dc00998140af 878 token = strtok(FGPSAUTO, ",");
maximbolduc 35:f9caeb8ca31e 879 while (token) {
maximbolduc 35:f9caeb8ca31e 880 switch (token_counter) {
maximbolduc 26:dc00998140af 881 case 1:
maximbolduc 26:dc00998140af 882 phase = token;
maximbolduc 26:dc00998140af 883 break;
maximbolduc 26:dc00998140af 884 case 2:
maximbolduc 26:dc00998140af 885 center = token;
maximbolduc 26:dc00998140af 886 break;
maximbolduc 34:c2bc9f9be7ff 887 case 3:
maximbolduc 34:c2bc9f9be7ff 888 fg = token;
maximbolduc 34:c2bc9f9be7ff 889 break;
maximbolduc 26:dc00998140af 890 case 4:
maximbolduc 26:dc00998140af 891 scl = token;
maximbolduc 26:dc00998140af 892 break;
maximbolduc 26:dc00998140af 893 case 5:
maximbolduc 26:dc00998140af 894 ahead = token;
maximbolduc 26:dc00998140af 895 break;
maximbolduc 26:dc00998140af 896 case 6:
maximbolduc 26:dc00998140af 897 avg = token;
maximbolduc 26:dc00998140af 898 break;
maximbolduc 26:dc00998140af 899 case 7:
maximbolduc 26:dc00998140af 900 _kp = token;
maximbolduc 26:dc00998140af 901 break;
maximbolduc 26:dc00998140af 902 case 8:
maximbolduc 26:dc00998140af 903 _ki = token;
maximbolduc 26:dc00998140af 904 break;
maximbolduc 26:dc00998140af 905 case 9:
maximbolduc 26:dc00998140af 906 _kd = token;
maximbolduc 26:dc00998140af 907 break;
maximbolduc 26:dc00998140af 908 }
maximbolduc 26:dc00998140af 909 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 910 token_counter++;
maximbolduc 26:dc00998140af 911 }
maximbolduc 35:f9caeb8ca31e 912 if ( _kp && _ki && _kd ) {
maximbolduc 26:dc00998140af 913 kp = atof(_kp);
maximbolduc 26:dc00998140af 914 ki = atof(_ki);
maximbolduc 26:dc00998140af 915 kd = atof(_kd);
maximbolduc 26:dc00998140af 916 }
maximbolduc 35:f9caeb8ca31e 917 if ( phase && center && scl && avg && ahead ) {
jhedmonton 28:5905886c76ee 918 lookaheadtime = atof(ahead);
maximbolduc 38:b5352d6f8166 919 scale = atof(scl); ///////////////////////////////////////////////////////////////////////////////
maximbolduc 26:dc00998140af 920 phaseadv = atof(phase);
jhedmonton 28:5905886c76ee 921 avgpos = atof(avg);
jhedmonton 28:5905886c76ee 922 tcenter = atof(center);
maximbolduc 34:c2bc9f9be7ff 923 filterg = atof(fg);
maximbolduc 38:b5352d6f8166 924
maximbolduc 39:6767d4c840f9 925 scale = scale * -1;
maximbolduc 26:dc00998140af 926 }
maximbolduc 26:dc00998140af 927 }
maximbolduc 35:f9caeb8ca31e 928
maximbolduc 26:dc00998140af 929 void pc_analyse(char* pc_string)
maximbolduc 26:dc00998140af 930 {
maximbolduc 35:f9caeb8ca31e 931 if (!strncmp(pc_string, "$ASTEER", 7)) {
maximbolduc 39:6767d4c840f9 932 //stop sending when already exists
maximbolduc 39:6767d4c840f9 933
maximbolduc 35:f9caeb8ca31e 934 } else if (!strncmp(pc_string, "$BANY",5)) {
jhedmonton 29:23ccb2a50b6f 935 _ID = Config_GetID();
jhedmonton 29:23ccb2a50b6f 936 Config_Save();
maximbolduc 35:f9caeb8ca31e 937 } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
maximbolduc 40:a68cc1a1a1e7 938 string mystring = pc_string;
maximbolduc 40:a68cc1a1a1e7 939 string baud = pc_string;
maximbolduc 40:a68cc1a1a1e7 940 if ( mystring.find('*') > 0 ) {
maximbolduc 40:a68cc1a1a1e7 941 string baud = mystring.substr(0,mystring.find('*'));
maximbolduc 40:a68cc1a1a1e7 942 }
maximbolduc 40:a68cc1a1a1e7 943 process_GPSBAUD((char*)baud.c_str());
maximbolduc 30:3afafa1ef16b 944 Config_Save();
jhedmonton 27:9ac59b261d87 945 sprintf(output,"%s %d\r\n",pc_string,gps_baud);
maximbolduc 38:b5352d6f8166 946 // pc.puts(output);
maximbolduc 35:f9caeb8ca31e 947 } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
maximbolduc 34:c2bc9f9be7ff 948 process_FGPSAUTO(pc_string);
maximbolduc 34:c2bc9f9be7ff 949 sprintf(output,"%s\r\n",pc_string);
maximbolduc 34:c2bc9f9be7ff 950 bluetooth.puts(output);
maximbolduc 34:c2bc9f9be7ff 951 Config_Save();
maximbolduc 35:f9caeb8ca31e 952 } else if (!strncmp(pc_string, "$FGPS,",6)) {
maximbolduc 35:f9caeb8ca31e 953 int i=5;
maximbolduc 35:f9caeb8ca31e 954 char c=pc_string[i];
maximbolduc 35:f9caeb8ca31e 955 while (c!=0) {
maximbolduc 35:f9caeb8ca31e 956 i++;
maximbolduc 35:f9caeb8ca31e 957 if (i>255) break; //protect msg buffer!
maximbolduc 35:f9caeb8ca31e 958 c=pc_string[i];
maximbolduc 35:f9caeb8ca31e 959 gps.putc(c);
maximbolduc 35:f9caeb8ca31e 960 pc.putc(c);
maximbolduc 35:f9caeb8ca31e 961 }
maximbolduc 36:8e84b5ade03e 962 } else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
maximbolduc 26:dc00998140af 963 process_GPSHEIGHT(pc_string);
maximbolduc 26:dc00998140af 964 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 965 bluetooth.puts(output);
maximbolduc 30:3afafa1ef16b 966 Config_Save();
maximbolduc 37:ac60a8a0ae8a 967 } else if (!strncmp(pc_string, "$FGPSAB",7)) {
maximbolduc 26:dc00998140af 968 process_FGPSAB(pc_string);
maximbolduc 35:f9caeb8ca31e 969 } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
maximbolduc 32:c57bc701d65c 970 calibrateGyroscope();
maximbolduc 30:3afafa1ef16b 971 calibrateAccelerometer();
maximbolduc 30:3afafa1ef16b 972 Config_Save();
maximbolduc 35:f9caeb8ca31e 973 } else if (!strncmp(pc_string, "$POSITION",9)) {
maximbolduc 32:c57bc701d65c 974 char* pointer;
maximbolduc 32:c57bc701d65c 975 char* Data[5];
maximbolduc 32:c57bc701d65c 976 int index = 0;
maximbolduc 32:c57bc701d65c 977 //Split data at commas
maximbolduc 32:c57bc701d65c 978 pointer = strtok(pc_string, ",");
maximbolduc 32:c57bc701d65c 979 if(pointer == NULL)
maximbolduc 32:c57bc701d65c 980 Data[0] = pc_string;
maximbolduc 35:f9caeb8ca31e 981 while(pointer != NULL) {
maximbolduc 32:c57bc701d65c 982 Data[index] = pointer;
maximbolduc 32:c57bc701d65c 983 pointer = strtok(NULL, ",");
maximbolduc 32:c57bc701d65c 984 index++;
maximbolduc 32:c57bc701d65c 985 }
maximbolduc 32:c57bc701d65c 986 gyro_pos = atoi(Data[1]);
maximbolduc 32:c57bc701d65c 987 Config_Save();
maximbolduc 35:f9caeb8ca31e 988 } else {
maximbolduc 26:dc00998140af 989 }
maximbolduc 26:dc00998140af 990 }
maximbolduc 35:f9caeb8ca31e 991
maximbolduc 26:dc00998140af 992 void gps_analyse(char* gps_string)
maximbolduc 26:dc00998140af 993 {
maximbolduc 35:f9caeb8ca31e 994 if (!strncmp(gps_string, "$GPRMC", 6)) {
maximbolduc 26:dc00998140af 995 nmea_rmc(gps_string); //analyse and decompose the rmc string
maximbolduc 36:8e84b5ade03e 996 } else {
maximbolduc 36:8e84b5ade03e 997 bluetooth.puts(gps_string);
maximbolduc 33:3e71c418e90d 998 }
maximbolduc 26:dc00998140af 999 }
maximbolduc 35:f9caeb8ca31e 1000
jhedmonton 27:9ac59b261d87 1001 int i2 = 0;
jhedmonton 27:9ac59b261d87 1002 bool end2 = false;
jhedmonton 27:9ac59b261d87 1003 bool start2 = false;
maximbolduc 35:f9caeb8ca31e 1004
jhedmonton 27:9ac59b261d87 1005 bool getline2()
maximbolduc 26:dc00998140af 1006 {
jhedmonton 27:9ac59b261d87 1007 int gotstring=false;
maximbolduc 35:f9caeb8ca31e 1008 while (1) {
maximbolduc 35:f9caeb8ca31e 1009 if( !bluetooth.readable() ) {
jhedmonton 27:9ac59b261d87 1010 break;
jhedmonton 27:9ac59b261d87 1011 }
jhedmonton 27:9ac59b261d87 1012 char c = bluetooth.getc();
maximbolduc 35:f9caeb8ca31e 1013 if (c == 36 ) {
maximbolduc 33:3e71c418e90d 1014 start2=true;
maximbolduc 33:3e71c418e90d 1015 end2 = false;
maximbolduc 33:3e71c418e90d 1016 i2 = 0;
maximbolduc 33:3e71c418e90d 1017 }
maximbolduc 35:f9caeb8ca31e 1018 if ((start2) && (c == 10)) {
maximbolduc 33:3e71c418e90d 1019 end2=true;
jhedmonton 29:23ccb2a50b6f 1020 start2 = false;
jhedmonton 29:23ccb2a50b6f 1021 }
maximbolduc 35:f9caeb8ca31e 1022 if (start2) {
jhedmonton 27:9ac59b261d87 1023 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 1024 i2++;
jhedmonton 27:9ac59b261d87 1025 if (i2>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 1026 }
maximbolduc 35:f9caeb8ca31e 1027 if (end2) {
maximbolduc 33:3e71c418e90d 1028 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 1029 msg2[i2+1] = 0;
jhedmonton 27:9ac59b261d87 1030 start2 = false;
jhedmonton 27:9ac59b261d87 1031 gotstring = true;
jhedmonton 27:9ac59b261d87 1032 end2=false;
jhedmonton 27:9ac59b261d87 1033 i2=0;
jhedmonton 27:9ac59b261d87 1034 break;
maximbolduc 26:dc00998140af 1035 }
maximbolduc 26:dc00998140af 1036 }
jhedmonton 27:9ac59b261d87 1037 return gotstring;
maximbolduc 26:dc00998140af 1038 }
maximbolduc 35:f9caeb8ca31e 1039
maximbolduc 35:f9caeb8ca31e 1040
jhedmonton 27:9ac59b261d87 1041 int i=0;
jhedmonton 27:9ac59b261d87 1042 bool start=false;
jhedmonton 27:9ac59b261d87 1043 bool end=false;
maximbolduc 35:f9caeb8ca31e 1044
jhedmonton 27:9ac59b261d87 1045 bool getline(bool forward)
maximbolduc 26:dc00998140af 1046 {
maximbolduc 35:f9caeb8ca31e 1047 while (1) {
maximbolduc 35:f9caeb8ca31e 1048 if( !gps.readable() ) {
jhedmonton 27:9ac59b261d87 1049 break;
jhedmonton 27:9ac59b261d87 1050 }
jhedmonton 28:5905886c76ee 1051 char c = gps.getc();
maximbolduc 35:f9caeb8ca31e 1052 if (forward) { //simply forward all to Bluetooth
maximbolduc 35:f9caeb8ca31e 1053 bluetooth.putc(c);
jhedmonton 27:9ac59b261d87 1054 }
maximbolduc 35:f9caeb8ca31e 1055 if (c == 36 ) {
maximbolduc 35:f9caeb8ca31e 1056 start=true;
maximbolduc 35:f9caeb8ca31e 1057 end = false;
maximbolduc 35:f9caeb8ca31e 1058 i = 0;
maximbolduc 35:f9caeb8ca31e 1059 }
maximbolduc 35:f9caeb8ca31e 1060 if ((start) && (c == 10)) {
maximbolduc 35:f9caeb8ca31e 1061 end=true;
jhedmonton 29:23ccb2a50b6f 1062 start = false;
jhedmonton 29:23ccb2a50b6f 1063 }
maximbolduc 35:f9caeb8ca31e 1064 if (start) {
jhedmonton 27:9ac59b261d87 1065 msg[i]=c;
jhedmonton 27:9ac59b261d87 1066 i++;
jhedmonton 27:9ac59b261d87 1067 if (i>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 1068 }
maximbolduc 35:f9caeb8ca31e 1069 if (end) {
maximbolduc 35:f9caeb8ca31e 1070 msg[i]=c;
maximbolduc 26:dc00998140af 1071 msg[i+1] = 0;
jhedmonton 29:23ccb2a50b6f 1072 i=0;
jhedmonton 27:9ac59b261d87 1073 start = false;
jhedmonton 27:9ac59b261d87 1074 end = true;
jhedmonton 27:9ac59b261d87 1075 break;
maximbolduc 26:dc00998140af 1076 }
maximbolduc 26:dc00998140af 1077 }
jhedmonton 27:9ac59b261d87 1078 return end;
maximbolduc 26:dc00998140af 1079 }
maximbolduc 35:f9caeb8ca31e 1080
maximbolduc 26:dc00998140af 1081 void keyPressedHeld( void )
maximbolduc 26:dc00998140af 1082 {
maximbolduc 36:8e84b5ade03e 1083 motor_enable_state = "$ENABLE,0\r\n";
maximbolduc 37:ac60a8a0ae8a 1084 motor_enable = 0;
maximbolduc 37:ac60a8a0ae8a 1085 pwm1 = 0.0;
maximbolduc 37:ac60a8a0ae8a 1086 pwm2 = 0.0;
jhedmonton 28:5905886c76ee 1087 ledGREEN=1; //show green for being ready to steer
maximbolduc 26:dc00998140af 1088 }
maximbolduc 35:f9caeb8ca31e 1089
maximbolduc 26:dc00998140af 1090 void keyReleasedHeld( void )
maximbolduc 26:dc00998140af 1091 {
maximbolduc 34:c2bc9f9be7ff 1092 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 34:c2bc9f9be7ff 1093 motor_enable_state = "$ENABLE,1\r\n";
maximbolduc 37:ac60a8a0ae8a 1094 motor_enable = 1;
maximbolduc 37:ac60a8a0ae8a 1095 pwm1 = 0.0;
maximbolduc 37:ac60a8a0ae8a 1096 pwm2 = 0.0;
maximbolduc 35:f9caeb8ca31e 1097 ledGREEN=0;
jhedmonton 27:9ac59b261d87 1098 }
maximbolduc 35:f9caeb8ca31e 1099
jhedmonton 27:9ac59b261d87 1100 void boom1PressedHeld( void )
jhedmonton 27:9ac59b261d87 1101 {
maximbolduc 35:f9caeb8ca31e 1102 // ledGREEN=1;
maximbolduc 35:f9caeb8ca31e 1103 boom18=boom18 & 0xFE;
jhedmonton 27:9ac59b261d87 1104 }
maximbolduc 35:f9caeb8ca31e 1105
jhedmonton 27:9ac59b261d87 1106 void boom1ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1107 {
maximbolduc 35:f9caeb8ca31e 1108 //ledGREEN=0;
maximbolduc 35:f9caeb8ca31e 1109 boom18=boom18 | 0x01;
jhedmonton 27:9ac59b261d87 1110 }
maximbolduc 35:f9caeb8ca31e 1111
jhedmonton 27:9ac59b261d87 1112 void boom2PressedHeld( void )
maximbolduc 35:f9caeb8ca31e 1113 {
maximbolduc 35:f9caeb8ca31e 1114 boom18=boom18 & 0xFD;
maximbolduc 26:dc00998140af 1115 }
maximbolduc 35:f9caeb8ca31e 1116
jhedmonton 27:9ac59b261d87 1117 void boom2ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1118 {
maximbolduc 35:f9caeb8ca31e 1119 boom18=boom18 | 0x02;
jhedmonton 27:9ac59b261d87 1120 }
jhedmonton 27:9ac59b261d87 1121 void boom3PressedHeld( void )
jhedmonton 27:9ac59b261d87 1122 {
maximbolduc 35:f9caeb8ca31e 1123 boom18=boom18 & 0xFB;
jhedmonton 27:9ac59b261d87 1124 }
maximbolduc 35:f9caeb8ca31e 1125
jhedmonton 27:9ac59b261d87 1126 void boom3ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1127 {
maximbolduc 35:f9caeb8ca31e 1128 boom18=boom18 | 0x04;
jhedmonton 27:9ac59b261d87 1129 }
maximbolduc 35:f9caeb8ca31e 1130
jhedmonton 27:9ac59b261d87 1131 void boom4PressedHeld( void )
jhedmonton 27:9ac59b261d87 1132 {
maximbolduc 32:c57bc701d65c 1133 boom18=boom18 & 0xF7;
jhedmonton 27:9ac59b261d87 1134 }
maximbolduc 35:f9caeb8ca31e 1135
jhedmonton 27:9ac59b261d87 1136 void boom4ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1137 {
maximbolduc 32:c57bc701d65c 1138 boom18=boom18 | 0x08;
jhedmonton 27:9ac59b261d87 1139 }
maximbolduc 35:f9caeb8ca31e 1140
maximbolduc 26:dc00998140af 1141 void toprint()
maximbolduc 26:dc00998140af 1142 {
maximbolduc 26:dc00998140af 1143 angle_send = 1;
maximbolduc 26:dc00998140af 1144 }
maximbolduc 35:f9caeb8ca31e 1145
maximbolduc 38:b5352d6f8166 1146 double last_yaw = 0;
maximbolduc 38:b5352d6f8166 1147 int counter = 0;
maximbolduc 38:b5352d6f8166 1148 bool bear = false;
maximbolduc 38:b5352d6f8166 1149 double lastroll = 0;
maximbolduc 38:b5352d6f8166 1150 double lastpitch = 0;
maximbolduc 38:b5352d6f8166 1151
maximbolduc 26:dc00998140af 1152 int main()
maximbolduc 26:dc00998140af 1153 {
jhedmonton 27:9ac59b261d87 1154 bluetooth.baud(115200);
jhedmonton 28:5905886c76ee 1155 gps.baud(38400);
maximbolduc 30:3afafa1ef16b 1156 pc.baud(38400);
maximbolduc 35:f9caeb8ca31e 1157
jhedmonton 27:9ac59b261d87 1158 //JH prepare and send version info
jhedmonton 27:9ac59b261d87 1159 vTimer.start();
jhedmonton 27:9ac59b261d87 1160 vTimer.reset();
maximbolduc 38:b5352d6f8166 1161
jhedmonton 28:5905886c76ee 1162 motTimer.start();
jhedmonton 28:5905886c76ee 1163 motTimer.reset();
jhedmonton 29:23ccb2a50b6f 1164 lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
maximbolduc 34:c2bc9f9be7ff 1165 motor_enable_state = "$ENABLE,1\r\n";
maximbolduc 36:8e84b5ade03e 1166
jhedmonton 28:5905886c76ee 1167 btTimer.start();
jhedmonton 28:5905886c76ee 1168 btTimer.reset();
maximbolduc 38:b5352d6f8166 1169 lastgetBT= btTimer.read_ms();
maximbolduc 35:f9caeb8ca31e 1170
jhedmonton 27:9ac59b261d87 1171 bluetooth.puts(version);
maximbolduc 38:b5352d6f8166 1172
jhedmonton 29:23ccb2a50b6f 1173 lastsend_version=vTimer.read_ms()-18000;
maximbolduc 38:b5352d6f8166 1174 // pc.puts("test\r\n");
maximbolduc 38:b5352d6f8166 1175 /* Config_Startup();
maximbolduc 38:b5352d6f8166 1176 _ID = Config_GetID();
maximbolduc 38:b5352d6f8166 1177 Config_Save();
maximbolduc 38:b5352d6f8166 1178 */
jhedmonton 27:9ac59b261d87 1179 boom1.attach_asserted_held( &boom1PressedHeld );
jhedmonton 27:9ac59b261d87 1180 boom1.attach_deasserted_held( &boom1ReleasedHeld );
jhedmonton 27:9ac59b261d87 1181 boom1.setSampleFrequency(); //default = 20 ms
jhedmonton 27:9ac59b261d87 1182 boom1.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1183 boom1.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1184 boom2.attach_asserted_held( &boom2PressedHeld );
jhedmonton 27:9ac59b261d87 1185 boom2.attach_deasserted_held( &boom2ReleasedHeld );
jhedmonton 27:9ac59b261d87 1186 boom2.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1187 boom2.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1188 boom2.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1189 boom3.attach_asserted_held( &boom3PressedHeld );
jhedmonton 27:9ac59b261d87 1190 boom3.attach_deasserted_held( &boom3ReleasedHeld );
jhedmonton 27:9ac59b261d87 1191 boom3.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1192 boom3.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1193 boom3.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1194 boom4.attach_asserted_held( &boom4PressedHeld );
jhedmonton 27:9ac59b261d87 1195 boom4.attach_deasserted_held( &boom4ReleasedHeld );
jhedmonton 27:9ac59b261d87 1196 boom4.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1197 boom4.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1198 boom4.setSampleFrequency();
maximbolduc 35:f9caeb8ca31e 1199
maximbolduc 26:dc00998140af 1200 motor_switch.setSampleFrequency(10000);
maximbolduc 26:dc00998140af 1201 motor_switch.attach_asserted_held( &keyPressedHeld );
maximbolduc 26:dc00998140af 1202 motor_switch.attach_deasserted_held( &keyReleasedHeld );
maximbolduc 35:f9caeb8ca31e 1203
maximbolduc 36:8e84b5ade03e 1204 initializeAccelerometer();
maximbolduc 36:8e84b5ade03e 1205 calibrateAccelerometer();
maximbolduc 36:8e84b5ade03e 1206 initializeGyroscope();
maximbolduc 36:8e84b5ade03e 1207 calibrateGyroscope();
maximbolduc 36:8e84b5ade03e 1208
maximbolduc 30:3afafa1ef16b 1209 accelerometerTicker.attach(&sampleAccelerometer, 0.005);
maximbolduc 30:3afafa1ef16b 1210 gyroscopeTicker.attach(&sampleGyroscope, 0.005);
maximbolduc 30:3afafa1ef16b 1211 filterTicker.attach(&filter, FILTER_RATE);
maximbolduc 30:3afafa1ef16b 1212 angle_print.attach(&toprint,0.2);
maximbolduc 26:dc00998140af 1213 activate_antenna();
maximbolduc 36:8e84b5ade03e 1214 autosteer_timeout.start();
maximbolduc 38:b5352d6f8166 1215
maximbolduc 39:6767d4c840f9 1216 //setTunings(0.25, 5, 1);
maximbolduc 38:b5352d6f8166 1217
maximbolduc 35:f9caeb8ca31e 1218 while(1) {
jhedmonton 27:9ac59b261d87 1219 //JH send version information every 10 seconds to keep Bluetooth alive
maximbolduc 35:f9caeb8ca31e 1220 if ((vTimer.read_ms()-lastsend_version)>25000) {
jhedmonton 27:9ac59b261d87 1221 pc.puts(version);
jhedmonton 27:9ac59b261d87 1222 bluetooth.puts(version);
jhedmonton 27:9ac59b261d87 1223 vTimer.reset();
jhedmonton 27:9ac59b261d87 1224 lastsend_version=vTimer.read_ms();
maximbolduc 35:f9caeb8ca31e 1225 }
maximbolduc 35:f9caeb8ca31e 1226
maximbolduc 36:8e84b5ade03e 1227 if ( autosteer_timeout.read_ms() > 5000 || autosteer_timeout.read_ms() > time_till_stop ) {
maximbolduc 36:8e84b5ade03e 1228 autosteer_timeout.reset();
maximbolduc 36:8e84b5ade03e 1229 enable_motor = 0;
maximbolduc 36:8e84b5ade03e 1230 }
maximbolduc 35:f9caeb8ca31e 1231 if ( antenna_active == 1 && gps.readable()) {
maximbolduc 35:f9caeb8ca31e 1232 if (getline(false)) {
maximbolduc 35:f9caeb8ca31e 1233 if ( validate_checksum(msg)) {
maximbolduc 38:b5352d6f8166 1234 //pc.puts(msg);
maximbolduc 33:3e71c418e90d 1235 gps_analyse(msg);
maximbolduc 35:f9caeb8ca31e 1236 } else {
maximbolduc 33:3e71c418e90d 1237 pc.puts("INVALID!!!!\r\n");
maximbolduc 32:c57bc701d65c 1238 }
maximbolduc 26:dc00998140af 1239 }
maximbolduc 26:dc00998140af 1240 }
maximbolduc 35:f9caeb8ca31e 1241 if ( bluetooth.readable()) {
maximbolduc 35:f9caeb8ca31e 1242 if (getline2()) {
maximbolduc 35:f9caeb8ca31e 1243 btTimer.reset();
maximbolduc 35:f9caeb8ca31e 1244 lastgetBT= btTimer.read_ms();
maximbolduc 38:b5352d6f8166 1245 // pc.puts(msg2);
maximbolduc 26:dc00998140af 1246 pc_analyse(msg2);
maximbolduc 26:dc00998140af 1247 }
maximbolduc 26:dc00998140af 1248 }
maximbolduc 35:f9caeb8ca31e 1249 if ( btTimer.read_ms()-lastgetBT>1000) {
maximbolduc 35:f9caeb8ca31e 1250 //we did not get any commands over BT
maximbolduc 35:f9caeb8ca31e 1251 ledRED=1; //turn red
maximbolduc 35:f9caeb8ca31e 1252 } else ledRED=0;
maximbolduc 35:f9caeb8ca31e 1253
maximbolduc 35:f9caeb8ca31e 1254 if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) {
maximbolduc 26:dc00998140af 1255 bluetooth.puts(motor_enable_state);
maximbolduc 38:b5352d6f8166 1256 // pc.puts(motor_enable_state);
jhedmonton 28:5905886c76ee 1257 motTimer.reset();
jhedmonton 28:5905886c76ee 1258 lastsend_motorstate=motTimer.read_ms();
jhedmonton 28:5905886c76ee 1259 lastmotor_enable=motor_enable;
maximbolduc 26:dc00998140af 1260 }
maximbolduc 35:f9caeb8ca31e 1261 if (boom18!=lastboom18) {
maximbolduc 35:f9caeb8ca31e 1262 boomstate[4]=boom18 | 0x80; //
maximbolduc 35:f9caeb8ca31e 1263 bluetooth.puts(boomstate);
maximbolduc 38:b5352d6f8166 1264 // pc.puts(boomstate);
maximbolduc 35:f9caeb8ca31e 1265 lastboom18=boom18;
jhedmonton 27:9ac59b261d87 1266 }
maximbolduc 38:b5352d6f8166 1267 if ( print_euler == 1 && angle_send == 1 ) { //&& reading == 0){
maximbolduc 38:b5352d6f8166 1268 lastpitch = 0.8*lastpitch + 0.2 * (toDegrees(get_pitch()*3.5));
maximbolduc 38:b5352d6f8166 1269 lastroll = 0.8 * lastroll + 0.2 * toDegrees(get_roll()*3.5);
maximbolduc 38:b5352d6f8166 1270
maximbolduc 38:b5352d6f8166 1271 double dps = - last_yaw;
maximbolduc 38:b5352d6f8166 1272 last_yaw =toDegrees( imuFilter.getYaw()) * -1;
maximbolduc 38:b5352d6f8166 1273 dps = (dps + last_yaw) * 5; // update every 200 ms, so for dps need *5
maximbolduc 38:b5352d6f8166 1274
maximbolduc 38:b5352d6f8166 1275 sprintf(output,"$EULER,%f,%f,%f\r\n",lastroll,lastpitch,dps);
maximbolduc 30:3afafa1ef16b 1276 bluetooth.puts(output);
maximbolduc 26:dc00998140af 1277 angle_send = 0;
maximbolduc 38:b5352d6f8166 1278 counter++;
maximbolduc 38:b5352d6f8166 1279 if ( bear == false && counter > 3 ) { //reinitialise the gyro after 600ms for the reference to be changed.
maximbolduc 38:b5352d6f8166 1280 imuFilter.reset();
maximbolduc 38:b5352d6f8166 1281 bear = true;
maximbolduc 38:b5352d6f8166 1282 }
jhedmonton 27:9ac59b261d87 1283 }
maximbolduc 26:dc00998140af 1284 }
maximbolduc 35:f9caeb8ca31e 1285 }