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

utilities.cpp@55:8561b35130fc, 2015-03-27 (annotated)

Committer:
maximbolduc
Date:
Fri Mar 27 22:44:29 2015 +0000
Revision:
55:8561b35130fc
Child:
56:456d454d9ced
added dynamic bias

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 55:8561b35130fc 1 #include "utilities.h"
maximbolduc 55:8561b35130fc 2 #include "Point.h"
maximbolduc 55:8561b35130fc 3
maximbolduc 55:8561b35130fc 4 double heading_err(double angle1, double angle2)
maximbolduc 55:8561b35130fc 5 {
maximbolduc 55:8561b35130fc 6 double angle = ((int)((angle2-90 )* -1 + 360) + 180) % 360 - 180;
maximbolduc 55:8561b35130fc 7 double diff_angle = angle1 - angle;
maximbolduc 55:8561b35130fc 8 diff_angle = ((int)diff_angle + 180) % 360 - 180;
maximbolduc 55:8561b35130fc 9 if ( abs(diff_angle) > 90 ) {
maximbolduc 55:8561b35130fc 10 if ( (abs(360 - diff_angle)) > 90 ) {
maximbolduc 55:8561b35130fc 11 diff_angle = abs(360 - diff_angle);
maximbolduc 55:8561b35130fc 12 }
maximbolduc 55:8561b35130fc 13 }
maximbolduc 55:8561b35130fc 14 if ( abs(diff_angle) > 90 ) {
maximbolduc 55:8561b35130fc 15 diff_angle = 0;
maximbolduc 55:8561b35130fc 16 }
maximbolduc 55:8561b35130fc 17 return diff_angle;
maximbolduc 55:8561b35130fc 18 }
maximbolduc 55:8561b35130fc 19
maximbolduc 55:8561b35130fc 20 double calculateDifferenceBetweenAngles(double firstAngle, double secondAngle)
maximbolduc 55:8561b35130fc 21 {
maximbolduc 55:8561b35130fc 22 double difference = secondAngle - firstAngle;
maximbolduc 55:8561b35130fc 23 while (difference < -180) difference += 360;
maximbolduc 55:8561b35130fc 24 while (difference > 180) difference -= 360;
maximbolduc 55:8561b35130fc 25 return difference;
maximbolduc 55:8561b35130fc 26 }
maximbolduc 55:8561b35130fc 27
maximbolduc 55:8561b35130fc 28
maximbolduc 55:8561b35130fc 29 double ToDeg(double val)
maximbolduc 55:8561b35130fc 30 {
maximbolduc 55:8561b35130fc 31 return val * 57.2957795131;
maximbolduc 55:8561b35130fc 32 }
maximbolduc 55:8561b35130fc 33
maximbolduc 55:8561b35130fc 34 double ToRad(double val)
maximbolduc 55:8561b35130fc 35 {
maximbolduc 55:8561b35130fc 36 return val / 57.2957795131;
maximbolduc 55:8561b35130fc 37 }
maximbolduc 55:8561b35130fc 38
maximbolduc 55:8561b35130fc 39 /*double map(double variable, double min, double max, double newmin, double newmax)
maximbolduc 55:8561b35130fc 40 {
maximbolduc 55:8561b35130fc 41 variable = variable / ( max - min );
maximbolduc 55:8561b35130fc 42 variable *= ( newmax - newmin );
maximbolduc 55:8561b35130fc 43 variable += newmin;
maximbolduc 55:8561b35130fc 44 return variable;
maximbolduc 55:8561b35130fc 45 }*/
maximbolduc 55:8561b35130fc 46 double map(double x, double in_min, double in_max, double out_min, double out_max)
maximbolduc 55:8561b35130fc 47 {
maximbolduc 55:8561b35130fc 48 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
maximbolduc 55:8561b35130fc 49 }
maximbolduc 55:8561b35130fc 50
maximbolduc 55:8561b35130fc 51 double constrains(double variable, double min, double max)
maximbolduc 55:8561b35130fc 52 {
maximbolduc 55:8561b35130fc 53 if ( variable < min )
maximbolduc 55:8561b35130fc 54 variable = min;
maximbolduc 55:8561b35130fc 55 if ( variable > max )
maximbolduc 55:8561b35130fc 56 variable = max;
maximbolduc 55:8561b35130fc 57 return variable;
maximbolduc 55:8561b35130fc 58 }
maximbolduc 55:8561b35130fc 59
maximbolduc 55:8561b35130fc 60 double dead_band(double variable, double cut_at)
maximbolduc 55:8561b35130fc 61 {
maximbolduc 55:8561b35130fc 62 int sign = (int)pow( variable, 0 );
maximbolduc 55:8561b35130fc 63 variable = abs( variable );
maximbolduc 55:8561b35130fc 64 if ( variable < cut_at )
maximbolduc 55:8561b35130fc 65 variable = cut_at;
maximbolduc 55:8561b35130fc 66 variable *= sign;
maximbolduc 55:8561b35130fc 67 return variable;
maximbolduc 55:8561b35130fc 68 }
maximbolduc 55:8561b35130fc 69
maximbolduc 55:8561b35130fc 70 double head_err(Point pos, Point ls, Point le, Point forward, double track_north, double lookA)
maximbolduc 55:8561b35130fc 71 {
maximbolduc 55:8561b35130fc 72 double m = ( le.GetY() - ls.GetY() ) / ( le.GetX() - ls.GetX() );
maximbolduc 55:8561b35130fc 73 double b = ls.GetY() - m * ls.GetY();
maximbolduc 55:8561b35130fc 74 double bsec = forward.GetX()/m+forward.GetY();
maximbolduc 55:8561b35130fc 75 double x = (forward.GetX()+m*forward.GetY()-b*m )/(2*m*m);
maximbolduc 55:8561b35130fc 76 double y = m * x + b;
maximbolduc 55:8561b35130fc 77 double theta = (ToDeg((1/sin((y-pos.GetY())/(x-pos.GetX()))))-90)*-1;
maximbolduc 55:8561b35130fc 78 return heading_err(theta,track_north);
maximbolduc 55:8561b35130fc 79 }