David Spillman / Mbed 2 deprecated GPSNavigationNew

Crude navigation

Dependencies:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

navigation.h@17:ba45eaae96b3, 2015-05-09 (annotated)

Committer:
Spilly
Date:
Sat May 09 22:29:01 2015 +0000
Revision:
17:ba45eaae96b3
Parent:
13:17f04a55c6e2 
Fix for SD card GPS waypoints

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Spilly 8:c77ab7615b21 1 /*************************************************************************************************************************************************************/
Spilly 5:40ac894e0fa7 2 // Created by: Ryan Spillman
Spilly 5:40ac894e0fa7 3 //
Spilly 13:17f04a55c6e2 4 // Last updated 4/29/2015
Spilly 5:40ac894e0fa7 5 //
Spilly 5:40ac894e0fa7 6 // Contains function that outputs difference in compass heading(magHead) and heading required(calcHead)
Spilly 8:c77ab7615b21 7 /*************************************************************************************************************************************************************/
Spilly 5:40ac894e0fa7 8
Spilly 5:40ac894e0fa7 9 #define navigation_h
Spilly 5:40ac894e0fa7 10
Spilly 5:40ac894e0fa7 11 //Tolerance for heading actual and heading needed
Spilly 5:40ac894e0fa7 12 #define HEADDIFF 0.000000000000000000f
Spilly 5:40ac894e0fa7 13
Spilly 8:c77ab7615b21 14 /*************************************************************************************************************************************************************/
Spilly 8:c77ab7615b21 15 // whichWay
Spilly 8:c77ab7615b21 16 //
Spilly 8:c77ab7615b21 17 // Outputs difference in compass heading(magHead) and heading required(calcHead)
Spilly 8:c77ab7615b21 18 // negative is left and positive is right
Spilly 8:c77ab7615b21 19 /*************************************************************************************************************************************************************/
Spilly 12:273479524c71 20 //TODO: Check for redundancy
Spilly 5:40ac894e0fa7 21 float whichWay(float magHead, float calcHead)
Spilly 5:40ac894e0fa7 22 {
Spilly 5:40ac894e0fa7 23 float magDiff;
Spilly 5:40ac894e0fa7 24
Spilly 5:40ac894e0fa7 25 float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
Spilly 5:40ac894e0fa7 26
Spilly 5:40ac894e0fa7 27 //Is the heading off enough to care?
Spilly 5:40ac894e0fa7 28 if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360 - HEADDIFF)))
Spilly 5:40ac894e0fa7 29 //if(1)
Spilly 5:40ac894e0fa7 30 {
Spilly 5:40ac894e0fa7 31 //quadrant I
Spilly 5:40ac894e0fa7 32 if(calcHead < 90)
Spilly 5:40ac894e0fa7 33 {
Spilly 5:40ac894e0fa7 34 if(calcHead < magHead)
Spilly 5:40ac894e0fa7 35 {
Spilly 5:40ac894e0fa7 36 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 37 {
Spilly 5:40ac894e0fa7 38 //turn left need negative
Spilly 5:40ac894e0fa7 39 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 40 }
Spilly 5:40ac894e0fa7 41 else
Spilly 5:40ac894e0fa7 42 {
Spilly 5:40ac894e0fa7 43 //turn right need positive
Spilly 5:40ac894e0fa7 44 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 45 }
Spilly 5:40ac894e0fa7 46 }
Spilly 5:40ac894e0fa7 47 else
Spilly 5:40ac894e0fa7 48 {
Spilly 5:40ac894e0fa7 49 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 50 {
Spilly 5:40ac894e0fa7 51 //turn left need negative
Spilly 5:40ac894e0fa7 52 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 53 }
Spilly 5:40ac894e0fa7 54 else
Spilly 5:40ac894e0fa7 55 {
Spilly 5:40ac894e0fa7 56 //turn right need positive
Spilly 5:40ac894e0fa7 57 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 58 }
Spilly 5:40ac894e0fa7 59 }
Spilly 5:40ac894e0fa7 60 }
Spilly 5:40ac894e0fa7 61 //quadrant II
Spilly 5:40ac894e0fa7 62 else if(calcHead < 180)
Spilly 5:40ac894e0fa7 63 {
Spilly 5:40ac894e0fa7 64 if(calcHead < magHead)
Spilly 5:40ac894e0fa7 65 {
Spilly 5:40ac894e0fa7 66 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 67 {
Spilly 5:40ac894e0fa7 68 //turn left need negative
Spilly 5:40ac894e0fa7 69 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 70 }
Spilly 5:40ac894e0fa7 71 else
Spilly 5:40ac894e0fa7 72 {
Spilly 5:40ac894e0fa7 73 //turn right need positive
Spilly 5:40ac894e0fa7 74 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 75 }
Spilly 5:40ac894e0fa7 76 }
Spilly 5:40ac894e0fa7 77 else
Spilly 5:40ac894e0fa7 78 {
Spilly 5:40ac894e0fa7 79 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 80 {
Spilly 5:40ac894e0fa7 81 //turn left need negative
Spilly 5:40ac894e0fa7 82 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 83 }
Spilly 5:40ac894e0fa7 84 else
Spilly 5:40ac894e0fa7 85 {
Spilly 5:40ac894e0fa7 86 //turn right need positive
Spilly 5:40ac894e0fa7 87 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 88 }
Spilly 5:40ac894e0fa7 89 }
Spilly 5:40ac894e0fa7 90 }
Spilly 5:40ac894e0fa7 91 //quadrant III
Spilly 5:40ac894e0fa7 92 else if(calcHead < 270)
Spilly 5:40ac894e0fa7 93 {
Spilly 5:40ac894e0fa7 94 if(calcHead < magHead)
Spilly 5:40ac894e0fa7 95 {
Spilly 5:40ac894e0fa7 96 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 97 {
Spilly 5:40ac894e0fa7 98 //turn left need negative
Spilly 5:40ac894e0fa7 99 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 100 }
Spilly 5:40ac894e0fa7 101 else
Spilly 5:40ac894e0fa7 102 {
Spilly 5:40ac894e0fa7 103 //turn right need positive
Spilly 5:40ac894e0fa7 104 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 105 }
Spilly 5:40ac894e0fa7 106 }
Spilly 5:40ac894e0fa7 107 else
Spilly 5:40ac894e0fa7 108 {
Spilly 5:40ac894e0fa7 109 /*
Spilly 5:40ac894e0fa7 110 if(magHead < (180 + calcHead))
Spilly 5:40ac894e0fa7 111 {
Spilly 5:40ac894e0fa7 112 //turn left need negative
Spilly 5:40ac894e0fa7 113 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 114 }
Spilly 5:40ac894e0fa7 115 else
Spilly 5:40ac894e0fa7 116 {
Spilly 5:40ac894e0fa7 117
Spilly 5:40ac894e0fa7 118 //turn right need positive
Spilly 5:40ac894e0fa7 119 magDiff = calcHead - magHead + 360;
Spilly 5:40ac894e0fa7 120 }
Spilly 5:40ac894e0fa7 121 */
Spilly 5:40ac894e0fa7 122 magDiff = calcHead - magHead - 90;
Spilly 5:40ac894e0fa7 123 }
Spilly 5:40ac894e0fa7 124 }
Spilly 5:40ac894e0fa7 125 //quadrant IV
Spilly 5:40ac894e0fa7 126 else
Spilly 5:40ac894e0fa7 127 {
Spilly 5:40ac894e0fa7 128
Spilly 5:40ac894e0fa7 129 if(calcHead < magHead)
Spilly 5:40ac894e0fa7 130 {
Spilly 5:40ac894e0fa7 131 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 132 }
Spilly 5:40ac894e0fa7 133 else
Spilly 5:40ac894e0fa7 134 {
Spilly 5:40ac894e0fa7 135 if(magHead < (calcHead - 180))
Spilly 5:40ac894e0fa7 136 {
Spilly 5:40ac894e0fa7 137
Spilly 5:40ac894e0fa7 138 magDiff = calcHead - 360 - magHead;
Spilly 5:40ac894e0fa7 139 }
Spilly 5:40ac894e0fa7 140 else
Spilly 5:40ac894e0fa7 141 {
Spilly 5:40ac894e0fa7 142 //turn right need positive
Spilly 5:40ac894e0fa7 143 magDiff = calcHead - magHead;
Spilly 5:40ac894e0fa7 144 }
Spilly 5:40ac894e0fa7 145 }
Spilly 5:40ac894e0fa7 146 }
Spilly 5:40ac894e0fa7 147 return magDiff;
Spilly 5:40ac894e0fa7 148 }
Spilly 5:40ac894e0fa7 149 return 0;
Spilly 5:40ac894e0fa7 150 }