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@56:456d454d9ced, 2015-03-29 (annotated)

Committer:
maximbolduc
Date:
Sun Mar 29 16:03:18 2015 +0000
Revision:
56:456d454d9ced
Parent:
55:8561b35130fc
Child:
57:0299098b2d0e
more settings for 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 56:456d454d9ced 20 double angle_normal(double angle, double min, double max)
maximbolduc 56:456d454d9ced 21 {
maximbolduc 56:456d454d9ced 22 while (angle < min) angle += (max-min);
maximbolduc 56:456d454d9ced 23 while (angle > max) angle -= (max-min);
maximbolduc 56:456d454d9ced 24 return angle;
maximbolduc 56:456d454d9ced 25 }
maximbolduc 56:456d454d9ced 26
maximbolduc 55:8561b35130fc 27 double calculateDifferenceBetweenAngles(double firstAngle, double secondAngle)
maximbolduc 55:8561b35130fc 28 {
maximbolduc 55:8561b35130fc 29 double difference = secondAngle - firstAngle;
maximbolduc 55:8561b35130fc 30 while (difference < -180) difference += 360;
maximbolduc 55:8561b35130fc 31 while (difference > 180) difference -= 360;
maximbolduc 55:8561b35130fc 32 return difference;
maximbolduc 55:8561b35130fc 33 }
maximbolduc 55:8561b35130fc 34
maximbolduc 55:8561b35130fc 35
maximbolduc 55:8561b35130fc 36 double ToDeg(double val)
maximbolduc 55:8561b35130fc 37 {
maximbolduc 55:8561b35130fc 38 return val * 57.2957795131;
maximbolduc 55:8561b35130fc 39 }
maximbolduc 55:8561b35130fc 40
maximbolduc 55:8561b35130fc 41 double ToRad(double val)
maximbolduc 55:8561b35130fc 42 {
maximbolduc 55:8561b35130fc 43 return val / 57.2957795131;
maximbolduc 55:8561b35130fc 44 }
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 56:456d454d9ced 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 56:456d454d9ced 79 }
maximbolduc 56:456d454d9ced 80
maximbolduc 56:456d454d9ced 81 double arr_avg(double *arr, int amount)
maximbolduc 56:456d454d9ced 82 {
maximbolduc 56:456d454d9ced 83 return arr_sum(amount,arr)/amount;
maximbolduc 56:456d454d9ced 84 }
maximbolduc 56:456d454d9ced 85
maximbolduc 56:456d454d9ced 86 double arr_degangleavg(double *a, int numb)
maximbolduc 56:456d454d9ced 87 {
maximbolduc 56:456d454d9ced 88 double x = 0;
maximbolduc 56:456d454d9ced 89 double y = 0;
maximbolduc 56:456d454d9ced 90 for ( int i = 0; i < numb; i++ ) {
maximbolduc 56:456d454d9ced 91 x += cos(ToRad(a[i]));
maximbolduc 56:456d454d9ced 92 y += sin(ToRad(a[i]));
maximbolduc 56:456d454d9ced 93 }
maximbolduc 56:456d454d9ced 94 return ToDeg(atan2(y, x));
maximbolduc 56:456d454d9ced 95 }
maximbolduc 56:456d454d9ced 96
maximbolduc 56:456d454d9ced 97 double arr_sum(int a, double *arr)
maximbolduc 56:456d454d9ced 98 {
maximbolduc 56:456d454d9ced 99 double cummul = 0;
maximbolduc 56:456d454d9ced 100 for ( int i = 0; i < a; i++ ) {
maximbolduc 56:456d454d9ced 101 cummul += arr[i];
maximbolduc 56:456d454d9ced 102 }
maximbolduc 56:456d454d9ced 103 return cummul;
maximbolduc 56:456d454d9ced 104 }
maximbolduc 56:456d454d9ced 105
maximbolduc 56:456d454d9ced 106 double arr_radangleavg(double *a, int numb)
maximbolduc 56:456d454d9ced 107 {
maximbolduc 56:456d454d9ced 108 double x = 0;
maximbolduc 56:456d454d9ced 109 double y = 0;
maximbolduc 56:456d454d9ced 110 for ( int i = 0; i < numb; i++ ) {
maximbolduc 56:456d454d9ced 111 x += cos(a[i]);
maximbolduc 56:456d454d9ced 112 y += sin(a[i]);
maximbolduc 56:456d454d9ced 113 }
maximbolduc 56:456d454d9ced 114 return atan2(y, x);
maximbolduc 56:456d454d9ced 115 }
maximbolduc 56:456d454d9ced 116
maximbolduc 56:456d454d9ced 117 void arr_pushback(double new_num, double *arr, int elements)
maximbolduc 56:456d454d9ced 118 {
maximbolduc 56:456d454d9ced 119 for( int i = 0; i < elements; i++ ) {
maximbolduc 56:456d454d9ced 120 arr[i+1] = arr[i];
maximbolduc 56:456d454d9ced 121 }
maximbolduc 56:456d454d9ced 122 arr[0] = new_num;
maximbolduc 55:8561b35130fc 123 }