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@39:6767d4c840f9, 2015-03-05 (annotated)

Committer:
maximbolduc
Date:
Thu Mar 05 16:26:52 2015 +0000
Revision:
39:6767d4c840f9
Parent:
38:b5352d6f8166
Child:
40:a68cc1a1a1e7
Working clsoe on line and line inverting

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 39:6767d4c840f9 576 if ( position_dist < 0.50 ) {
maximbolduc 39:6767d4c840f9 577 if ( forward_sign == position_sign ) {
maximbolduc 39:6767d4c840f9 578 if ( position_dist > (dist_line + (0.5 * position_dist))) { // && abs(position_dist) <
maximbolduc 39:6767d4c840f9 579 error = error * (dist_line * filter_g - position_dist * phase_adv);
maximbolduc 39:6767d4c840f9 580 } else {
maximbolduc 39:6767d4c840f9 581 error = error * dist_line * filter_g;
maximbolduc 39:6767d4c840f9 582 }
maximbolduc 39:6767d4c840f9 583 } else { //
maximbolduc 39:6767d4c840f9 584 error = error * ((dist_line * filter_g)*0.5 - position_dist * phase_adv) * 0.5;//.8 in order to come back less on line than we came on it
maximbolduc 37:ac60a8a0ae8a 585 }
maximbolduc 39:6767d4c840f9 586 } else { //target coming back at 15-20 degrees on the line
maximbolduc 39:6767d4c840f9 587 error = error * (20 - heading_err) * 5;
maximbolduc 35:f9caeb8ca31e 588 }
maximbolduc 38:b5352d6f8166 589
maximbolduc 35:f9caeb8ca31e 590 error = error * scale;
maximbolduc 38:b5352d6f8166 591 if ( error > 0 ) {
maximbolduc 38:b5352d6f8166 592 error = error + center;
maximbolduc 38:b5352d6f8166 593 } else if (error < 0 ) {
maximbolduc 38:b5352d6f8166 594 error = error - center;
maximbolduc 38:b5352d6f8166 595 }
maximbolduc 35:f9caeb8ca31e 596 if ( error > 255 ) {
maximbolduc 35:f9caeb8ca31e 597 error = 255;
maximbolduc 35:f9caeb8ca31e 598 } else if ( error < -255 ) {
maximbolduc 35:f9caeb8ca31e 599 error = -255;
maximbolduc 35:f9caeb8ca31e 600 }
maximbolduc 38:b5352d6f8166 601
maximbolduc 35:f9caeb8ca31e 602 error = error + 255;
maximbolduc 35:f9caeb8ca31e 603 error = (int)(error / 2);
maximbolduc 35:f9caeb8ca31e 604
maximbolduc 35:f9caeb8ca31e 605 return (int)error;
maximbolduc 35:f9caeb8ca31e 606 }
maximbolduc 35:f9caeb8ca31e 607
maximbolduc 38:b5352d6f8166 608 char *strsep(char **stringp, char *delim)
maximbolduc 38:b5352d6f8166 609 {
maximbolduc 38:b5352d6f8166 610 char *s;
maximbolduc 38:b5352d6f8166 611 const char *spanp;
maximbolduc 38:b5352d6f8166 612 int c, sc;
maximbolduc 38:b5352d6f8166 613 char *tok;
maximbolduc 38:b5352d6f8166 614 if ((s = *stringp) == NULL)
maximbolduc 38:b5352d6f8166 615 return (NULL);
maximbolduc 38:b5352d6f8166 616 for (tok = s;;) {
maximbolduc 38:b5352d6f8166 617 c = *s++;
maximbolduc 38:b5352d6f8166 618 spanp = delim;
maximbolduc 38:b5352d6f8166 619 do {
maximbolduc 38:b5352d6f8166 620 if ((sc = *spanp++) == c) {
maximbolduc 38:b5352d6f8166 621 if (c == 0)
maximbolduc 38:b5352d6f8166 622 s = NULL;
maximbolduc 38:b5352d6f8166 623 else
maximbolduc 38:b5352d6f8166 624 s[-1] = 0;
maximbolduc 38:b5352d6f8166 625 *stringp = s;
maximbolduc 38:b5352d6f8166 626 return (tok);
maximbolduc 38:b5352d6f8166 627 }
maximbolduc 38:b5352d6f8166 628 } while (sc != 0);
maximbolduc 38:b5352d6f8166 629 }
maximbolduc 38:b5352d6f8166 630 /* NOTREACHED */
maximbolduc 38:b5352d6f8166 631 }
maximbolduc 38:b5352d6f8166 632
maximbolduc 38:b5352d6f8166 633 Point old_position;
maximbolduc 38:b5352d6f8166 634
maximbolduc 38:b5352d6f8166 635 //char rmc_cpy[256];
maximbolduc 26:dc00998140af 636 void nmea_rmc(char *s)
maximbolduc 26:dc00998140af 637 {
maximbolduc 26:dc00998140af 638 char *token;
maximbolduc 26:dc00998140af 639 int token_counter = 0;
maximbolduc 26:dc00998140af 640 char *time = (char *)NULL;
maximbolduc 26:dc00998140af 641 char *date = (char *)NULL;
maximbolduc 26:dc00998140af 642 char *stat = (char *)NULL;
maximbolduc 26:dc00998140af 643 char *vel = (char *)NULL;
maximbolduc 26:dc00998140af 644 char *trk = (char *)NULL;
maximbolduc 26:dc00998140af 645 char *magv = (char *)NULL;
maximbolduc 38:b5352d6f8166 646 char *magd = (char *)NULL;
maximbolduc 38:b5352d6f8166 647 char *latit = "";
maximbolduc 38:b5352d6f8166 648 char *longit = "";
maximbolduc 26:dc00998140af 649 char *latitude = (char *)NULL;
maximbolduc 26:dc00998140af 650 char *longitude = (char *)NULL;
maximbolduc 26:dc00998140af 651 char *lat_dir = (char *)NULL;
maximbolduc 26:dc00998140af 652 char *lon_dir = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 653
maximbolduc 38:b5352d6f8166 654 while ((token = strsep(&s, ",")) != NULL) {
maximbolduc 35:f9caeb8ca31e 655 switch (token_counter) {
maximbolduc 26:dc00998140af 656 case 1:
maximbolduc 26:dc00998140af 657 time = token;
maximbolduc 26:dc00998140af 658 break;
maximbolduc 26:dc00998140af 659 case 2:
maximbolduc 26:dc00998140af 660 stat = token;
maximbolduc 26:dc00998140af 661 break;
maximbolduc 34:c2bc9f9be7ff 662 case 3:
maximbolduc 39:6767d4c840f9 663 if ( token ) {
maximbolduc 38:b5352d6f8166 664 latit = token;
maximbolduc 38:b5352d6f8166 665 latitude = token;
maximbolduc 38:b5352d6f8166 666 }
maximbolduc 34:c2bc9f9be7ff 667 break;
maximbolduc 34:c2bc9f9be7ff 668 case 4:
maximbolduc 34:c2bc9f9be7ff 669 lat_dir = token;
maximbolduc 34:c2bc9f9be7ff 670 break;
maximbolduc 34:c2bc9f9be7ff 671 case 5:
maximbolduc 38:b5352d6f8166 672 longit = token;
maximbolduc 34:c2bc9f9be7ff 673 longitude = token;
maximbolduc 34:c2bc9f9be7ff 674 break;
maximbolduc 34:c2bc9f9be7ff 675 case 6:
maximbolduc 34:c2bc9f9be7ff 676 lon_dir = token;
maximbolduc 34:c2bc9f9be7ff 677 break;
maximbolduc 26:dc00998140af 678 case 7:
maximbolduc 26:dc00998140af 679 vel = token;
maximbolduc 26:dc00998140af 680 break;
maximbolduc 26:dc00998140af 681 case 8:
maximbolduc 26:dc00998140af 682 trk = token;
maximbolduc 26:dc00998140af 683 break;
maximbolduc 34:c2bc9f9be7ff 684 case 9:
maximbolduc 34:c2bc9f9be7ff 685 date = token;
maximbolduc 34:c2bc9f9be7ff 686 break;
maximbolduc 26:dc00998140af 687 case 10:
maximbolduc 26:dc00998140af 688 magv = token;
maximbolduc 26:dc00998140af 689 break;
maximbolduc 38:b5352d6f8166 690 case 11:
maximbolduc 38:b5352d6f8166 691 magd = token;
maximbolduc 38:b5352d6f8166 692 break;
maximbolduc 26:dc00998140af 693 }
maximbolduc 26:dc00998140af 694 token_counter++;
maximbolduc 26:dc00998140af 695 }
maximbolduc 38:b5352d6f8166 696
maximbolduc 38:b5352d6f8166 697 if (stat!= '\0' && date!= '\0' && time!= '\0') {
maximbolduc 26:dc00998140af 698 hour = (char)((time[0] - '0') * 10) + (time[1] - '0');
maximbolduc 26:dc00998140af 699 minute = (char)((time[2] - '0') * 10) + (time[3] - '0');
maximbolduc 26:dc00998140af 700 second = (char)((time[4] - '0') * 10) + (time[5] - '0');
maximbolduc 26:dc00998140af 701 day = (char)((date[0] - '0') * 10) + (date[1] - '0');
maximbolduc 26:dc00998140af 702 month = (char)((date[2] - '0') * 10) + (date[3] - '0');
maximbolduc 26:dc00998140af 703 year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
maximbolduc 26:dc00998140af 704 status = stat[0];
maximbolduc 26:dc00998140af 705 velocity = atof(vel);
maximbolduc 26:dc00998140af 706 speed_km = velocity * 1.852;
maximbolduc 26:dc00998140af 707 speed_m_s = speed_km * 3600.0 / 1000.0;
maximbolduc 26:dc00998140af 708 track = atof(trk);
maximbolduc 26:dc00998140af 709 magvar = atof(magv);
maximbolduc 35:f9caeb8ca31e 710 // magvar_dir = magd[0];
maximbolduc 26:dc00998140af 711 }
maximbolduc 39:6767d4c840f9 712
maximbolduc 39:6767d4c840f9 713 double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
maximbolduc 39:6767d4c840f9 714 double diff_angle = Freepilot_bearing - angle;
maximbolduc 39:6767d4c840f9 715 diff_angle = ((int)diff_angle + 180) % 360 - 180;
maximbolduc 39:6767d4c840f9 716
maximbolduc 39:6767d4c840f9 717 // pc.printf("%f %f %f\r\n",diff_angle, Freepilot_bearing, (track - 90) * -1);
maximbolduc 39:6767d4c840f9 718 if ( abs(diff_angle) > 90 ) {
maximbolduc 39:6767d4c840f9 719 if ( (abs(360 - diff_angle)) > 90 ) {
maximbolduc 39:6767d4c840f9 720 Point temp = line_end;
maximbolduc 39:6767d4c840f9 721 line_end = line_start;
maximbolduc 39:6767d4c840f9 722 line_start = temp;
maximbolduc 39:6767d4c840f9 723 Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
maximbolduc 39:6767d4c840f9 724 }
maximbolduc 39:6767d4c840f9 725 }
maximbolduc 38:b5352d6f8166 726 if ( longit != '\0' && latit != '\0' ) {
maximbolduc 38:b5352d6f8166 727 old_position = position;
maximbolduc 38:b5352d6f8166 728 position.SetX(lat_to_deg(latitude, lat_dir[0]));
maximbolduc 38:b5352d6f8166 729 position.SetY(lon_to_deg(longitude, lon_dir[0]));
maximbolduc 38:b5352d6f8166 730 cm_per_deg_lat = 11054000;
maximbolduc 38:b5352d6f8166 731 cm_per_deg_lon = 11132000 * cos(decimal_latitude);
maximbolduc 35:f9caeb8ca31e 732
maximbolduc 38:b5352d6f8166 733 // pitch_and_roll((track-90)*-1);// as to be real bearing
maximbolduc 35:f9caeb8ca31e 734
maximbolduc 38:b5352d6f8166 735 compensation.SetY(compensation.GetY() / cm_per_deg_lon);
maximbolduc 38:b5352d6f8166 736 compensation.SetX(compensation.GetX() / cm_per_deg_lat);
maximbolduc 38:b5352d6f8166 737
maximbolduc 38:b5352d6f8166 738 position = point_add(position,compensation);
maximbolduc 35:f9caeb8ca31e 739
maximbolduc 38:b5352d6f8166 740 double lookaheaddistance = lookaheadtime * speed_m_s;
maximbolduc 38:b5352d6f8166 741 get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,(track-90)*-1,look_ahead_lon,look_ahead_lat);
maximbolduc 38:b5352d6f8166 742 looked_ahead.SetX(look_ahead_lat);
maximbolduc 38:b5352d6f8166 743 looked_ahead.SetY(look_ahead_lon);
maximbolduc 38:b5352d6f8166 744 double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
maximbolduc 38:b5352d6f8166 745 distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;
maximbolduc 35:f9caeb8ca31e 746
maximbolduc 39:6767d4c840f9 747 int val = freepilot(position, looked_ahead, diff_angle, distance_to_line, scale, filterg , phaseadv,tcenter ,filtering);
maximbolduc 35:f9caeb8ca31e 748
maximbolduc 38:b5352d6f8166 749 char command[128];
maximbolduc 38:b5352d6f8166 750 sprintf(command,"$ASTEER,%i,%i\r\n",val,250); //(int)((((val/2)-127)/scale)+127),500);
maximbolduc 38:b5352d6f8166 751 pc.puts(command);
maximbolduc 38:b5352d6f8166 752 process_ASTEER(command);
maximbolduc 38:b5352d6f8166 753 }
maximbolduc 34:c2bc9f9be7ff 754 string rmc = "";
maximbolduc 38:b5352d6f8166 755 if(time!= '\0') {
maximbolduc 34:c2bc9f9be7ff 756 rmc = "$GPRMC,";
maximbolduc 34:c2bc9f9be7ff 757 rmc += string(time) + ",";
maximbolduc 34:c2bc9f9be7ff 758 } else {
maximbolduc 34:c2bc9f9be7ff 759 rmc = "$GPRMC,,";
maximbolduc 34:c2bc9f9be7ff 760 }
maximbolduc 38:b5352d6f8166 761 if(stat!= '\0') {
maximbolduc 34:c2bc9f9be7ff 762 rmc +=(string(stat) + ",");
maximbolduc 34:c2bc9f9be7ff 763 } else {
maximbolduc 34:c2bc9f9be7ff 764 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 765 }
maximbolduc 38:b5352d6f8166 766 if ( latit != '\0' && lat_dir!= '\0') {
maximbolduc 34:c2bc9f9be7ff 767 rmc +=( string(To_DMS(position.GetX())) + "," + string(lat_dir) +",");
maximbolduc 34:c2bc9f9be7ff 768 } else {
maximbolduc 34:c2bc9f9be7ff 769 rmc += ",,";
maximbolduc 34:c2bc9f9be7ff 770 }
maximbolduc 38:b5352d6f8166 771 if ( longit != '\0' && lon_dir!= '\0' ) {
maximbolduc 34:c2bc9f9be7ff 772 rmc += (string(To_DMS_lon(position.GetY())) + "," + string(lon_dir) + ",");
maximbolduc 34:c2bc9f9be7ff 773 } else {
maximbolduc 34:c2bc9f9be7ff 774 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 775 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 776 }
maximbolduc 38:b5352d6f8166 777 if (vel!= '\0') {
maximbolduc 38:b5352d6f8166 778 rmc += (string(vel) + ",");
maximbolduc 34:c2bc9f9be7ff 779 } else {
maximbolduc 34:c2bc9f9be7ff 780 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 781 }
maximbolduc 38:b5352d6f8166 782 if ((trk)!= '\0') {
maximbolduc 34:c2bc9f9be7ff 783 rmc += string(trk) + ",";
maximbolduc 34:c2bc9f9be7ff 784 } else {
maximbolduc 34:c2bc9f9be7ff 785 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 786 }
maximbolduc 38:b5352d6f8166 787 if (date!= '\0') {
maximbolduc 34:c2bc9f9be7ff 788 rmc += string(date) + ",";
maximbolduc 34:c2bc9f9be7ff 789 } else {
maximbolduc 34:c2bc9f9be7ff 790 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 791 }
maximbolduc 38:b5352d6f8166 792 if (magv!= '\0') {
maximbolduc 34:c2bc9f9be7ff 793 rmc += string(magv) + ",";
maximbolduc 34:c2bc9f9be7ff 794 } else {
maximbolduc 34:c2bc9f9be7ff 795 rmc += ",";
maximbolduc 34:c2bc9f9be7ff 796 }
maximbolduc 38:b5352d6f8166 797 if (magd!= '\0') {
maximbolduc 38:b5352d6f8166 798 rmc += string(magd) + ",W";
maximbolduc 38:b5352d6f8166 799 } else {
maximbolduc 38:b5352d6f8166 800 rmc += ",W";
maximbolduc 38:b5352d6f8166 801 }
maximbolduc 34:c2bc9f9be7ff 802
maximbolduc 34:c2bc9f9be7ff 803 char test[256];
maximbolduc 38:b5352d6f8166 804 sprintf(test,"%s*\0",rmc.c_str());
maximbolduc 38:b5352d6f8166 805 sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));
maximbolduc 39:6767d4c840f9 806
maximbolduc 34:c2bc9f9be7ff 807 bluetooth.puts(output);
maximbolduc 26:dc00998140af 808 }
maximbolduc 35:f9caeb8ca31e 809
maximbolduc 26:dc00998140af 810 void process_FGPSAB(char* ab)
maximbolduc 26:dc00998140af 811 {
maximbolduc 33:3e71c418e90d 812 char *token;
maximbolduc 33:3e71c418e90d 813 int token_counter = 0;
maximbolduc 33:3e71c418e90d 814 char *line_lat = (char *)NULL;
maximbolduc 33:3e71c418e90d 815 char *line_lon = (char *)NULL;
maximbolduc 33:3e71c418e90d 816 char *line_lat1 = (char *)NULL;
maximbolduc 33:3e71c418e90d 817 char *line_lon1 = (char *)NULL;
maximbolduc 38:b5352d6f8166 818 //char *bearing = (char *)NULL;
maximbolduc 38:b5352d6f8166 819 string bearing = "";
maximbolduc 33:3e71c418e90d 820 token = strtok(ab, ",");
maximbolduc 35:f9caeb8ca31e 821 while (token) {
maximbolduc 35:f9caeb8ca31e 822 switch (token_counter) {
maximbolduc 33:3e71c418e90d 823 case 1:
maximbolduc 33:3e71c418e90d 824 line_lat = token;
maximbolduc 33:3e71c418e90d 825 break;
maximbolduc 33:3e71c418e90d 826 case 2:
maximbolduc 33:3e71c418e90d 827 line_lon = token;
maximbolduc 33:3e71c418e90d 828 break;
maximbolduc 33:3e71c418e90d 829 case 3:
maximbolduc 33:3e71c418e90d 830 line_lat1 = token;
maximbolduc 33:3e71c418e90d 831 break;
maximbolduc 33:3e71c418e90d 832 case 4:
maximbolduc 33:3e71c418e90d 833 line_lon1 = token;
maximbolduc 33:3e71c418e90d 834 break;
maximbolduc 33:3e71c418e90d 835 case 5:
maximbolduc 38:b5352d6f8166 836 for (int n=0; n < sizeof(token); n++) {
maximbolduc 38:b5352d6f8166 837 if ( token[n] == '*' ) {
maximbolduc 38:b5352d6f8166 838 break;
maximbolduc 38:b5352d6f8166 839 } else {
maximbolduc 38:b5352d6f8166 840 bearing += token[n];
maximbolduc 38:b5352d6f8166 841 }
maximbolduc 38:b5352d6f8166 842 }
maximbolduc 33:3e71c418e90d 843 break;
maximbolduc 26:dc00998140af 844 }
maximbolduc 33:3e71c418e90d 845 token = strtok((char *)NULL, ",");
maximbolduc 33:3e71c418e90d 846 token_counter++;
maximbolduc 33:3e71c418e90d 847 }
maximbolduc 39:6767d4c840f9 848 double Freepilot_lon = atof(line_lon);
maximbolduc 39:6767d4c840f9 849 double Freepilot_lat = atof(line_lat);
maximbolduc 39:6767d4c840f9 850 double Freepilot_lon1 = atof(line_lon1);
maximbolduc 39:6767d4c840f9 851 double Freepilot_lat1 = atof(line_lat1);
maximbolduc 39:6767d4c840f9 852 Freepilot_bearing = atof(bearing.c_str()) + 360;
maximbolduc 39:6767d4c840f9 853 Freepilot_bearing = ((int)Freepilot_bearing+ 180) % 360 - 180;
maximbolduc 33:3e71c418e90d 854 line_start.SetX(Freepilot_lat);
maximbolduc 33:3e71c418e90d 855 line_start.SetY(Freepilot_lon);
maximbolduc 33:3e71c418e90d 856 line_end.SetX(Freepilot_lat1);
maximbolduc 33:3e71c418e90d 857 line_end.SetY(Freepilot_lon1);
maximbolduc 39:6767d4c840f9 858 /*
maximbolduc 39:6767d4c840f9 859 double diff_angle = Freepilot_bearing - track;
maximbolduc 39:6767d4c840f9 860 diff_angle = ((int)diff_angle + 180) % 360 - 180;
maximbolduc 38:b5352d6f8166 861
maximbolduc 39:6767d4c840f9 862 if ( abs(diff_angle) > 90 ) {
maximbolduc 39:6767d4c840f9 863 line_end.SetX(Freepilot_lat);
maximbolduc 39:6767d4c840f9 864 line_end.SetY(Freepilot_lon);
maximbolduc 39:6767d4c840f9 865 line_start.SetX(Freepilot_lat1);
maximbolduc 39:6767d4c840f9 866 line_start.SetY(Freepilot_lon1);
maximbolduc 39:6767d4c840f9 867 Freepilot_bearing = Freepilot_bearing + 180;
maximbolduc 39:6767d4c840f9 868 Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
maximbolduc 39:6767d4c840f9 869 } else {
maximbolduc 39:6767d4c840f9 870 line_start.SetX(Freepilot_lat);
maximbolduc 39:6767d4c840f9 871 line_start.SetY(Freepilot_lon);
maximbolduc 39:6767d4c840f9 872 line_end.SetX(Freepilot_lat1);
maximbolduc 39:6767d4c840f9 873 line_end.SetY(Freepilot_lon1);
maximbolduc 39:6767d4c840f9 874 Freepilot_bearing = Freepilot_bearing;
maximbolduc 39:6767d4c840f9 875 }
maximbolduc 39:6767d4c840f9 876 */
maximbolduc 38:b5352d6f8166 877 pc.puts(bearing.c_str());
maximbolduc 38:b5352d6f8166 878 pc.puts("\r\n");
maximbolduc 39:6767d4c840f9 879
maximbolduc 33:3e71c418e90d 880 active_AB = 1;
maximbolduc 35:f9caeb8ca31e 881
maximbolduc 38:b5352d6f8166 882 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 883 pc.puts(output);
maximbolduc 26:dc00998140af 884 }
maximbolduc 35:f9caeb8ca31e 885
maximbolduc 26:dc00998140af 886 void process_FGPSAUTO(char* FGPSAUTO)
maximbolduc 26:dc00998140af 887 {
maximbolduc 26:dc00998140af 888 char *token;
maximbolduc 26:dc00998140af 889 int token_counter = 0;
maximbolduc 26:dc00998140af 890 char *ahead = (char *)NULL;
maximbolduc 26:dc00998140af 891 char *center = (char *)NULL;
maximbolduc 26:dc00998140af 892 char *phase = (char *)NULL;
maximbolduc 26:dc00998140af 893 char *scl = (char *)NULL;
maximbolduc 26:dc00998140af 894 char *avg = (char *)NULL;
maximbolduc 26:dc00998140af 895 char *_kp = (char *)NULL;
maximbolduc 26:dc00998140af 896 char *_ki = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 897 char *fg = (char *)NULL;
maximbolduc 26:dc00998140af 898 char *_kd = (char *)NULL;
maximbolduc 39:6767d4c840f9 899
maximbolduc 26:dc00998140af 900 token = strtok(FGPSAUTO, ",");
maximbolduc 35:f9caeb8ca31e 901 while (token) {
maximbolduc 35:f9caeb8ca31e 902 switch (token_counter) {
maximbolduc 26:dc00998140af 903 case 1:
maximbolduc 26:dc00998140af 904 phase = token;
maximbolduc 26:dc00998140af 905 break;
maximbolduc 26:dc00998140af 906 case 2:
maximbolduc 26:dc00998140af 907 center = token;
maximbolduc 26:dc00998140af 908 break;
maximbolduc 34:c2bc9f9be7ff 909 case 3:
maximbolduc 34:c2bc9f9be7ff 910 fg = token;
maximbolduc 34:c2bc9f9be7ff 911 break;
maximbolduc 26:dc00998140af 912 case 4:
maximbolduc 26:dc00998140af 913 scl = token;
maximbolduc 26:dc00998140af 914 break;
maximbolduc 26:dc00998140af 915 case 5:
maximbolduc 26:dc00998140af 916 ahead = token;
maximbolduc 26:dc00998140af 917 break;
maximbolduc 26:dc00998140af 918 case 6:
maximbolduc 26:dc00998140af 919 avg = token;
maximbolduc 26:dc00998140af 920 break;
maximbolduc 26:dc00998140af 921 case 7:
maximbolduc 26:dc00998140af 922 _kp = token;
maximbolduc 26:dc00998140af 923 break;
maximbolduc 26:dc00998140af 924 case 8:
maximbolduc 26:dc00998140af 925 _ki = token;
maximbolduc 26:dc00998140af 926 break;
maximbolduc 26:dc00998140af 927 case 9:
maximbolduc 26:dc00998140af 928 _kd = token;
maximbolduc 26:dc00998140af 929 break;
maximbolduc 26:dc00998140af 930 }
maximbolduc 26:dc00998140af 931 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 932 token_counter++;
maximbolduc 26:dc00998140af 933 }
maximbolduc 35:f9caeb8ca31e 934 if ( _kp && _ki && _kd ) {
maximbolduc 26:dc00998140af 935 kp = atof(_kp);
maximbolduc 26:dc00998140af 936 ki = atof(_ki);
maximbolduc 26:dc00998140af 937 kd = atof(_kd);
maximbolduc 26:dc00998140af 938 }
maximbolduc 35:f9caeb8ca31e 939 if ( phase && center && scl && avg && ahead ) {
jhedmonton 28:5905886c76ee 940 lookaheadtime = atof(ahead);
maximbolduc 38:b5352d6f8166 941 scale = atof(scl); ///////////////////////////////////////////////////////////////////////////////
maximbolduc 26:dc00998140af 942 phaseadv = atof(phase);
jhedmonton 28:5905886c76ee 943 avgpos = atof(avg);
jhedmonton 28:5905886c76ee 944 tcenter = atof(center);
maximbolduc 34:c2bc9f9be7ff 945 filterg = atof(fg);
maximbolduc 38:b5352d6f8166 946
maximbolduc 39:6767d4c840f9 947 scale = scale * -1;
maximbolduc 26:dc00998140af 948 }
maximbolduc 26:dc00998140af 949 }
maximbolduc 35:f9caeb8ca31e 950
maximbolduc 26:dc00998140af 951 void pc_analyse(char* pc_string)
maximbolduc 26:dc00998140af 952 {
maximbolduc 35:f9caeb8ca31e 953 if (!strncmp(pc_string, "$ASTEER", 7)) {
maximbolduc 39:6767d4c840f9 954 //stop sending when already exists
maximbolduc 39:6767d4c840f9 955
maximbolduc 35:f9caeb8ca31e 956 } else if (!strncmp(pc_string, "$BANY",5)) {
jhedmonton 29:23ccb2a50b6f 957 _ID = Config_GetID();
jhedmonton 29:23ccb2a50b6f 958 Config_Save();
maximbolduc 35:f9caeb8ca31e 959 } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
maximbolduc 26:dc00998140af 960 process_GPSBAUD(pc_string);
maximbolduc 30:3afafa1ef16b 961 Config_Save();
jhedmonton 27:9ac59b261d87 962 sprintf(output,"%s %d\r\n",pc_string,gps_baud);
maximbolduc 38:b5352d6f8166 963 // pc.puts(output);
maximbolduc 35:f9caeb8ca31e 964 } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
maximbolduc 34:c2bc9f9be7ff 965 process_FGPSAUTO(pc_string);
maximbolduc 34:c2bc9f9be7ff 966 sprintf(output,"%s\r\n",pc_string);
maximbolduc 34:c2bc9f9be7ff 967 bluetooth.puts(output);
maximbolduc 34:c2bc9f9be7ff 968 Config_Save();
maximbolduc 35:f9caeb8ca31e 969 } else if (!strncmp(pc_string, "$FGPS,",6)) {
maximbolduc 35:f9caeb8ca31e 970 int i=5;
maximbolduc 35:f9caeb8ca31e 971 char c=pc_string[i];
maximbolduc 35:f9caeb8ca31e 972 while (c!=0) {
maximbolduc 35:f9caeb8ca31e 973 i++;
maximbolduc 35:f9caeb8ca31e 974 if (i>255) break; //protect msg buffer!
maximbolduc 35:f9caeb8ca31e 975 c=pc_string[i];
maximbolduc 35:f9caeb8ca31e 976 gps.putc(c);
maximbolduc 35:f9caeb8ca31e 977 pc.putc(c);
maximbolduc 35:f9caeb8ca31e 978 }
maximbolduc 36:8e84b5ade03e 979 } else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
maximbolduc 26:dc00998140af 980 process_GPSHEIGHT(pc_string);
maximbolduc 26:dc00998140af 981 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 982 bluetooth.puts(output);
maximbolduc 30:3afafa1ef16b 983 Config_Save();
maximbolduc 37:ac60a8a0ae8a 984 } else if (!strncmp(pc_string, "$FGPSAB",7)) {
maximbolduc 26:dc00998140af 985 process_FGPSAB(pc_string);
maximbolduc 35:f9caeb8ca31e 986 } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
maximbolduc 32:c57bc701d65c 987 calibrateGyroscope();
maximbolduc 30:3afafa1ef16b 988 calibrateAccelerometer();
maximbolduc 30:3afafa1ef16b 989 Config_Save();
maximbolduc 35:f9caeb8ca31e 990 } else if (!strncmp(pc_string, "$POSITION",9)) {
maximbolduc 32:c57bc701d65c 991 char* pointer;
maximbolduc 32:c57bc701d65c 992 char* Data[5];
maximbolduc 32:c57bc701d65c 993 int index = 0;
maximbolduc 32:c57bc701d65c 994 //Split data at commas
maximbolduc 32:c57bc701d65c 995 pointer = strtok(pc_string, ",");
maximbolduc 32:c57bc701d65c 996 if(pointer == NULL)
maximbolduc 32:c57bc701d65c 997 Data[0] = pc_string;
maximbolduc 35:f9caeb8ca31e 998 while(pointer != NULL) {
maximbolduc 32:c57bc701d65c 999 Data[index] = pointer;
maximbolduc 32:c57bc701d65c 1000 pointer = strtok(NULL, ",");
maximbolduc 32:c57bc701d65c 1001 index++;
maximbolduc 32:c57bc701d65c 1002 }
maximbolduc 32:c57bc701d65c 1003 gyro_pos = atoi(Data[1]);
maximbolduc 32:c57bc701d65c 1004 Config_Save();
maximbolduc 35:f9caeb8ca31e 1005 } else {
maximbolduc 26:dc00998140af 1006 }
maximbolduc 26:dc00998140af 1007 }
maximbolduc 35:f9caeb8ca31e 1008
maximbolduc 26:dc00998140af 1009 void gps_analyse(char* gps_string)
maximbolduc 26:dc00998140af 1010 {
maximbolduc 35:f9caeb8ca31e 1011 if (!strncmp(gps_string, "$GPRMC", 6)) {
maximbolduc 26:dc00998140af 1012 nmea_rmc(gps_string); //analyse and decompose the rmc string
maximbolduc 36:8e84b5ade03e 1013 } else {
maximbolduc 36:8e84b5ade03e 1014 bluetooth.puts(gps_string);
maximbolduc 33:3e71c418e90d 1015 }
maximbolduc 26:dc00998140af 1016 }
maximbolduc 35:f9caeb8ca31e 1017
jhedmonton 27:9ac59b261d87 1018 int i2 = 0;
jhedmonton 27:9ac59b261d87 1019 bool end2 = false;
jhedmonton 27:9ac59b261d87 1020 bool start2 = false;
maximbolduc 35:f9caeb8ca31e 1021
jhedmonton 27:9ac59b261d87 1022 bool getline2()
maximbolduc 26:dc00998140af 1023 {
jhedmonton 27:9ac59b261d87 1024 int gotstring=false;
maximbolduc 35:f9caeb8ca31e 1025 while (1) {
maximbolduc 35:f9caeb8ca31e 1026 if( !bluetooth.readable() ) {
jhedmonton 27:9ac59b261d87 1027 break;
jhedmonton 27:9ac59b261d87 1028 }
jhedmonton 27:9ac59b261d87 1029 char c = bluetooth.getc();
maximbolduc 35:f9caeb8ca31e 1030 if (c == 36 ) {
maximbolduc 33:3e71c418e90d 1031 start2=true;
maximbolduc 33:3e71c418e90d 1032 end2 = false;
maximbolduc 33:3e71c418e90d 1033 i2 = 0;
maximbolduc 33:3e71c418e90d 1034 }
maximbolduc 35:f9caeb8ca31e 1035 if ((start2) && (c == 10)) {
maximbolduc 33:3e71c418e90d 1036 end2=true;
jhedmonton 29:23ccb2a50b6f 1037 start2 = false;
jhedmonton 29:23ccb2a50b6f 1038 }
maximbolduc 35:f9caeb8ca31e 1039 if (start2) {
jhedmonton 27:9ac59b261d87 1040 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 1041 i2++;
jhedmonton 27:9ac59b261d87 1042 if (i2>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 1043 }
maximbolduc 35:f9caeb8ca31e 1044 if (end2) {
maximbolduc 33:3e71c418e90d 1045 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 1046 msg2[i2+1] = 0;
jhedmonton 27:9ac59b261d87 1047 start2 = false;
jhedmonton 27:9ac59b261d87 1048 gotstring = true;
jhedmonton 27:9ac59b261d87 1049 end2=false;
jhedmonton 27:9ac59b261d87 1050 i2=0;
jhedmonton 27:9ac59b261d87 1051 break;
maximbolduc 26:dc00998140af 1052 }
maximbolduc 26:dc00998140af 1053 }
jhedmonton 27:9ac59b261d87 1054 return gotstring;
maximbolduc 26:dc00998140af 1055 }
maximbolduc 35:f9caeb8ca31e 1056
maximbolduc 35:f9caeb8ca31e 1057
jhedmonton 27:9ac59b261d87 1058 int i=0;
jhedmonton 27:9ac59b261d87 1059 bool start=false;
jhedmonton 27:9ac59b261d87 1060 bool end=false;
maximbolduc 35:f9caeb8ca31e 1061
jhedmonton 27:9ac59b261d87 1062 bool getline(bool forward)
maximbolduc 26:dc00998140af 1063 {
maximbolduc 35:f9caeb8ca31e 1064 while (1) {
maximbolduc 35:f9caeb8ca31e 1065 if( !gps.readable() ) {
jhedmonton 27:9ac59b261d87 1066 break;
jhedmonton 27:9ac59b261d87 1067 }
jhedmonton 28:5905886c76ee 1068 char c = gps.getc();
maximbolduc 35:f9caeb8ca31e 1069 if (forward) { //simply forward all to Bluetooth
maximbolduc 35:f9caeb8ca31e 1070 bluetooth.putc(c);
jhedmonton 27:9ac59b261d87 1071 }
maximbolduc 35:f9caeb8ca31e 1072 if (c == 36 ) {
maximbolduc 35:f9caeb8ca31e 1073 start=true;
maximbolduc 35:f9caeb8ca31e 1074 end = false;
maximbolduc 35:f9caeb8ca31e 1075 i = 0;
maximbolduc 35:f9caeb8ca31e 1076 }
maximbolduc 35:f9caeb8ca31e 1077 if ((start) && (c == 10)) {
maximbolduc 35:f9caeb8ca31e 1078 end=true;
jhedmonton 29:23ccb2a50b6f 1079 start = false;
jhedmonton 29:23ccb2a50b6f 1080 }
maximbolduc 35:f9caeb8ca31e 1081 if (start) {
jhedmonton 27:9ac59b261d87 1082 msg[i]=c;
jhedmonton 27:9ac59b261d87 1083 i++;
jhedmonton 27:9ac59b261d87 1084 if (i>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 1085 }
maximbolduc 35:f9caeb8ca31e 1086 if (end) {
maximbolduc 35:f9caeb8ca31e 1087 msg[i]=c;
maximbolduc 26:dc00998140af 1088 msg[i+1] = 0;
jhedmonton 29:23ccb2a50b6f 1089 i=0;
jhedmonton 27:9ac59b261d87 1090 start = false;
jhedmonton 27:9ac59b261d87 1091 end = true;
jhedmonton 27:9ac59b261d87 1092 break;
maximbolduc 26:dc00998140af 1093 }
maximbolduc 26:dc00998140af 1094 }
jhedmonton 27:9ac59b261d87 1095 return end;
maximbolduc 26:dc00998140af 1096 }
maximbolduc 35:f9caeb8ca31e 1097
maximbolduc 26:dc00998140af 1098 void keyPressedHeld( void )
maximbolduc 26:dc00998140af 1099 {
maximbolduc 36:8e84b5ade03e 1100 motor_enable_state = "$ENABLE,0\r\n";
maximbolduc 37:ac60a8a0ae8a 1101 motor_enable = 0;
maximbolduc 37:ac60a8a0ae8a 1102 pwm1 = 0.0;
maximbolduc 37:ac60a8a0ae8a 1103 pwm2 = 0.0;
jhedmonton 28:5905886c76ee 1104 ledGREEN=1; //show green for being ready to steer
maximbolduc 26:dc00998140af 1105 }
maximbolduc 35:f9caeb8ca31e 1106
maximbolduc 26:dc00998140af 1107 void keyReleasedHeld( void )
maximbolduc 26:dc00998140af 1108 {
maximbolduc 34:c2bc9f9be7ff 1109 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 34:c2bc9f9be7ff 1110 motor_enable_state = "$ENABLE,1\r\n";
maximbolduc 37:ac60a8a0ae8a 1111 motor_enable = 1;
maximbolduc 37:ac60a8a0ae8a 1112 pwm1 = 0.0;
maximbolduc 37:ac60a8a0ae8a 1113 pwm2 = 0.0;
maximbolduc 35:f9caeb8ca31e 1114 ledGREEN=0;
jhedmonton 27:9ac59b261d87 1115 }
maximbolduc 35:f9caeb8ca31e 1116
jhedmonton 27:9ac59b261d87 1117 void boom1PressedHeld( void )
jhedmonton 27:9ac59b261d87 1118 {
maximbolduc 35:f9caeb8ca31e 1119 // ledGREEN=1;
maximbolduc 35:f9caeb8ca31e 1120 boom18=boom18 & 0xFE;
jhedmonton 27:9ac59b261d87 1121 }
maximbolduc 35:f9caeb8ca31e 1122
jhedmonton 27:9ac59b261d87 1123 void boom1ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1124 {
maximbolduc 35:f9caeb8ca31e 1125 //ledGREEN=0;
maximbolduc 35:f9caeb8ca31e 1126 boom18=boom18 | 0x01;
jhedmonton 27:9ac59b261d87 1127 }
maximbolduc 35:f9caeb8ca31e 1128
jhedmonton 27:9ac59b261d87 1129 void boom2PressedHeld( void )
maximbolduc 35:f9caeb8ca31e 1130 {
maximbolduc 35:f9caeb8ca31e 1131 boom18=boom18 & 0xFD;
maximbolduc 26:dc00998140af 1132 }
maximbolduc 35:f9caeb8ca31e 1133
jhedmonton 27:9ac59b261d87 1134 void boom2ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1135 {
maximbolduc 35:f9caeb8ca31e 1136 boom18=boom18 | 0x02;
jhedmonton 27:9ac59b261d87 1137 }
jhedmonton 27:9ac59b261d87 1138 void boom3PressedHeld( void )
jhedmonton 27:9ac59b261d87 1139 {
maximbolduc 35:f9caeb8ca31e 1140 boom18=boom18 & 0xFB;
jhedmonton 27:9ac59b261d87 1141 }
maximbolduc 35:f9caeb8ca31e 1142
jhedmonton 27:9ac59b261d87 1143 void boom3ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1144 {
maximbolduc 35:f9caeb8ca31e 1145 boom18=boom18 | 0x04;
jhedmonton 27:9ac59b261d87 1146 }
maximbolduc 35:f9caeb8ca31e 1147
jhedmonton 27:9ac59b261d87 1148 void boom4PressedHeld( void )
jhedmonton 27:9ac59b261d87 1149 {
maximbolduc 32:c57bc701d65c 1150 boom18=boom18 & 0xF7;
jhedmonton 27:9ac59b261d87 1151 }
maximbolduc 35:f9caeb8ca31e 1152
jhedmonton 27:9ac59b261d87 1153 void boom4ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 1154 {
maximbolduc 32:c57bc701d65c 1155 boom18=boom18 | 0x08;
jhedmonton 27:9ac59b261d87 1156 }
maximbolduc 35:f9caeb8ca31e 1157
maximbolduc 26:dc00998140af 1158 void toprint()
maximbolduc 26:dc00998140af 1159 {
maximbolduc 26:dc00998140af 1160 angle_send = 1;
maximbolduc 26:dc00998140af 1161 }
maximbolduc 35:f9caeb8ca31e 1162
maximbolduc 38:b5352d6f8166 1163 double last_yaw = 0;
maximbolduc 38:b5352d6f8166 1164 int counter = 0;
maximbolduc 38:b5352d6f8166 1165 bool bear = false;
maximbolduc 38:b5352d6f8166 1166 double lastroll = 0;
maximbolduc 38:b5352d6f8166 1167 double lastpitch = 0;
maximbolduc 38:b5352d6f8166 1168
maximbolduc 26:dc00998140af 1169 int main()
maximbolduc 26:dc00998140af 1170 {
jhedmonton 27:9ac59b261d87 1171 bluetooth.baud(115200);
jhedmonton 28:5905886c76ee 1172 gps.baud(38400);
maximbolduc 30:3afafa1ef16b 1173 pc.baud(38400);
maximbolduc 35:f9caeb8ca31e 1174
jhedmonton 27:9ac59b261d87 1175 //JH prepare and send version info
jhedmonton 27:9ac59b261d87 1176 vTimer.start();
jhedmonton 27:9ac59b261d87 1177 vTimer.reset();
maximbolduc 38:b5352d6f8166 1178
jhedmonton 28:5905886c76ee 1179 motTimer.start();
jhedmonton 28:5905886c76ee 1180 motTimer.reset();
jhedmonton 29:23ccb2a50b6f 1181 lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
maximbolduc 34:c2bc9f9be7ff 1182 motor_enable_state = "$ENABLE,1\r\n";
maximbolduc 36:8e84b5ade03e 1183
jhedmonton 28:5905886c76ee 1184 btTimer.start();
jhedmonton 28:5905886c76ee 1185 btTimer.reset();
maximbolduc 38:b5352d6f8166 1186 lastgetBT= btTimer.read_ms();
maximbolduc 35:f9caeb8ca31e 1187
jhedmonton 27:9ac59b261d87 1188 bluetooth.puts(version);
maximbolduc 38:b5352d6f8166 1189
jhedmonton 29:23ccb2a50b6f 1190 lastsend_version=vTimer.read_ms()-18000;
maximbolduc 38:b5352d6f8166 1191 // pc.puts("test\r\n");
maximbolduc 38:b5352d6f8166 1192 /* Config_Startup();
maximbolduc 38:b5352d6f8166 1193 _ID = Config_GetID();
maximbolduc 38:b5352d6f8166 1194 Config_Save();
maximbolduc 38:b5352d6f8166 1195 */
jhedmonton 27:9ac59b261d87 1196 boom1.attach_asserted_held( &boom1PressedHeld );
jhedmonton 27:9ac59b261d87 1197 boom1.attach_deasserted_held( &boom1ReleasedHeld );
jhedmonton 27:9ac59b261d87 1198 boom1.setSampleFrequency(); //default = 20 ms
jhedmonton 27:9ac59b261d87 1199 boom1.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1200 boom1.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1201 boom2.attach_asserted_held( &boom2PressedHeld );
jhedmonton 27:9ac59b261d87 1202 boom2.attach_deasserted_held( &boom2ReleasedHeld );
jhedmonton 27:9ac59b261d87 1203 boom2.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1204 boom2.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1205 boom2.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1206 boom3.attach_asserted_held( &boom3PressedHeld );
jhedmonton 27:9ac59b261d87 1207 boom3.attach_deasserted_held( &boom3ReleasedHeld );
jhedmonton 27:9ac59b261d87 1208 boom3.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1209 boom3.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1210 boom3.setSampleFrequency();
jhedmonton 27:9ac59b261d87 1211 boom4.attach_asserted_held( &boom4PressedHeld );
jhedmonton 27:9ac59b261d87 1212 boom4.attach_deasserted_held( &boom4ReleasedHeld );
jhedmonton 27:9ac59b261d87 1213 boom4.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 1214 boom4.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 1215 boom4.setSampleFrequency();
maximbolduc 35:f9caeb8ca31e 1216
maximbolduc 26:dc00998140af 1217 motor_switch.setSampleFrequency(10000);
maximbolduc 26:dc00998140af 1218 motor_switch.attach_asserted_held( &keyPressedHeld );
maximbolduc 26:dc00998140af 1219 motor_switch.attach_deasserted_held( &keyReleasedHeld );
maximbolduc 35:f9caeb8ca31e 1220
maximbolduc 36:8e84b5ade03e 1221 initializeAccelerometer();
maximbolduc 36:8e84b5ade03e 1222 calibrateAccelerometer();
maximbolduc 36:8e84b5ade03e 1223 initializeGyroscope();
maximbolduc 36:8e84b5ade03e 1224 calibrateGyroscope();
maximbolduc 36:8e84b5ade03e 1225
maximbolduc 30:3afafa1ef16b 1226 accelerometerTicker.attach(&sampleAccelerometer, 0.005);
maximbolduc 30:3afafa1ef16b 1227 gyroscopeTicker.attach(&sampleGyroscope, 0.005);
maximbolduc 30:3afafa1ef16b 1228 filterTicker.attach(&filter, FILTER_RATE);
maximbolduc 30:3afafa1ef16b 1229 angle_print.attach(&toprint,0.2);
maximbolduc 26:dc00998140af 1230 activate_antenna();
maximbolduc 36:8e84b5ade03e 1231 autosteer_timeout.start();
maximbolduc 38:b5352d6f8166 1232
maximbolduc 39:6767d4c840f9 1233 //setTunings(0.25, 5, 1);
maximbolduc 38:b5352d6f8166 1234
maximbolduc 35:f9caeb8ca31e 1235 while(1) {
jhedmonton 27:9ac59b261d87 1236 //JH send version information every 10 seconds to keep Bluetooth alive
maximbolduc 35:f9caeb8ca31e 1237 if ((vTimer.read_ms()-lastsend_version)>25000) {
jhedmonton 27:9ac59b261d87 1238 pc.puts(version);
jhedmonton 27:9ac59b261d87 1239 bluetooth.puts(version);
jhedmonton 27:9ac59b261d87 1240 vTimer.reset();
jhedmonton 27:9ac59b261d87 1241 lastsend_version=vTimer.read_ms();
maximbolduc 35:f9caeb8ca31e 1242 }
maximbolduc 35:f9caeb8ca31e 1243
maximbolduc 36:8e84b5ade03e 1244 if ( autosteer_timeout.read_ms() > 5000 || autosteer_timeout.read_ms() > time_till_stop ) {
maximbolduc 36:8e84b5ade03e 1245 autosteer_timeout.reset();
maximbolduc 36:8e84b5ade03e 1246 enable_motor = 0;
maximbolduc 36:8e84b5ade03e 1247 }
maximbolduc 35:f9caeb8ca31e 1248 if ( antenna_active == 1 && gps.readable()) {
maximbolduc 35:f9caeb8ca31e 1249 if (getline(false)) {
maximbolduc 35:f9caeb8ca31e 1250 if ( validate_checksum(msg)) {
maximbolduc 38:b5352d6f8166 1251 //pc.puts(msg);
maximbolduc 33:3e71c418e90d 1252 gps_analyse(msg);
maximbolduc 35:f9caeb8ca31e 1253 } else {
maximbolduc 33:3e71c418e90d 1254 pc.puts("INVALID!!!!\r\n");
maximbolduc 32:c57bc701d65c 1255 }
maximbolduc 26:dc00998140af 1256 }
maximbolduc 26:dc00998140af 1257 }
maximbolduc 35:f9caeb8ca31e 1258 if ( bluetooth.readable()) {
maximbolduc 35:f9caeb8ca31e 1259 if (getline2()) {
maximbolduc 35:f9caeb8ca31e 1260 btTimer.reset();
maximbolduc 35:f9caeb8ca31e 1261 lastgetBT= btTimer.read_ms();
maximbolduc 38:b5352d6f8166 1262 // pc.puts(msg2);
maximbolduc 26:dc00998140af 1263 pc_analyse(msg2);
maximbolduc 26:dc00998140af 1264 }
maximbolduc 26:dc00998140af 1265 }
maximbolduc 35:f9caeb8ca31e 1266 if ( btTimer.read_ms()-lastgetBT>1000) {
maximbolduc 35:f9caeb8ca31e 1267 //we did not get any commands over BT
maximbolduc 35:f9caeb8ca31e 1268 ledRED=1; //turn red
maximbolduc 35:f9caeb8ca31e 1269 } else ledRED=0;
maximbolduc 35:f9caeb8ca31e 1270
maximbolduc 35:f9caeb8ca31e 1271 if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) {
maximbolduc 26:dc00998140af 1272 bluetooth.puts(motor_enable_state);
maximbolduc 38:b5352d6f8166 1273 // pc.puts(motor_enable_state);
jhedmonton 28:5905886c76ee 1274 motTimer.reset();
jhedmonton 28:5905886c76ee 1275 lastsend_motorstate=motTimer.read_ms();
jhedmonton 28:5905886c76ee 1276 lastmotor_enable=motor_enable;
maximbolduc 26:dc00998140af 1277 }
maximbolduc 35:f9caeb8ca31e 1278 if (boom18!=lastboom18) {
maximbolduc 35:f9caeb8ca31e 1279 boomstate[4]=boom18 | 0x80; //
maximbolduc 35:f9caeb8ca31e 1280 bluetooth.puts(boomstate);
maximbolduc 38:b5352d6f8166 1281 // pc.puts(boomstate);
maximbolduc 35:f9caeb8ca31e 1282 lastboom18=boom18;
jhedmonton 27:9ac59b261d87 1283 }
maximbolduc 38:b5352d6f8166 1284 if ( print_euler == 1 && angle_send == 1 ) { //&& reading == 0){
maximbolduc 38:b5352d6f8166 1285 lastpitch = 0.8*lastpitch + 0.2 * (toDegrees(get_pitch()*3.5));
maximbolduc 38:b5352d6f8166 1286 lastroll = 0.8 * lastroll + 0.2 * toDegrees(get_roll()*3.5);
maximbolduc 38:b5352d6f8166 1287
maximbolduc 38:b5352d6f8166 1288 double dps = - last_yaw;
maximbolduc 38:b5352d6f8166 1289 last_yaw =toDegrees( imuFilter.getYaw()) * -1;
maximbolduc 38:b5352d6f8166 1290 dps = (dps + last_yaw) * 5; // update every 200 ms, so for dps need *5
maximbolduc 38:b5352d6f8166 1291
maximbolduc 38:b5352d6f8166 1292 sprintf(output,"$EULER,%f,%f,%f\r\n",lastroll,lastpitch,dps);
maximbolduc 30:3afafa1ef16b 1293 bluetooth.puts(output);
maximbolduc 26:dc00998140af 1294 angle_send = 0;
maximbolduc 38:b5352d6f8166 1295 counter++;
maximbolduc 38:b5352d6f8166 1296 if ( bear == false && counter > 3 ) { //reinitialise the gyro after 600ms for the reference to be changed.
maximbolduc 38:b5352d6f8166 1297 imuFilter.reset();
maximbolduc 38:b5352d6f8166 1298 bear = true;
maximbolduc 38:b5352d6f8166 1299 }
jhedmonton 27:9ac59b261d87 1300 }
maximbolduc 26:dc00998140af 1301 }
maximbolduc 35:f9caeb8ca31e 1302 }