Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

autonomous.c@0:62a1c91a859a, 2011-02-18 (annotated)

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
2:90292f8bd179
First release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gke 0:62a1c91a859a 1 // ===============================================================================================
gke 0:62a1c91a859a 2 // = UAVXArm Quadrocopter Controller =
gke 0:62a1c91a859a 3 // = Copyright (c) 2008 by Prof. Greg Egan =
gke 0:62a1c91a859a 4 // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
gke 0:62a1c91a859a 5 // = http://code.google.com/p/uavp-mods/ http://uavp.ch =
gke 0:62a1c91a859a 6 // ===============================================================================================
gke 0:62a1c91a859a 7
gke 0:62a1c91a859a 8 // This is part of UAVXArm.
gke 0:62a1c91a859a 9
gke 0:62a1c91a859a 10 // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
gke 0:62a1c91a859a 11 // General Public License as published by the Free Software Foundation, either version 3 of the
gke 0:62a1c91a859a 12 // License, or (at your option) any later version.
gke 0:62a1c91a859a 13
gke 0:62a1c91a859a 14 // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
gke 0:62a1c91a859a 15 // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
gke 0:62a1c91a859a 16 // See the GNU General Public License for more details.
gke 0:62a1c91a859a 17
gke 0:62a1c91a859a 18 // You should have received a copy of the GNU General Public License along with this program.
gke 0:62a1c91a859a 19 // If not, see http://www.gnu.org/licenses/
gke 0:62a1c91a859a 20
gke 0:62a1c91a859a 21 #include "UAVXArm.h"
gke 0:62a1c91a859a 22
gke 0:62a1c91a859a 23 void DoShutdown(void);
gke 0:62a1c91a859a 24 void DoPolarOrientation(void);
gke 0:62a1c91a859a 25 void Navigate(int32, int32);
gke 0:62a1c91a859a 26 void SetDesiredAltitude(int16);
gke 0:62a1c91a859a 27 void DoFailsafeLanding(void);
gke 0:62a1c91a859a 28 void AcquireHoldPosition(void);
gke 0:62a1c91a859a 29 void NavGainSchedule(int16);
gke 0:62a1c91a859a 30 void DoNavigation(void);
gke 0:62a1c91a859a 31 void DoPPMFailsafe(void);
gke 0:62a1c91a859a 32 void UAVXNavCommand(void);
gke 0:62a1c91a859a 33 void GetWayPointPX(int8);
gke 0:62a1c91a859a 34 void InitNavigation(void);
gke 0:62a1c91a859a 35
gke 0:62a1c91a859a 36 real32 NavCorr[3], NavCorrp[3];
gke 0:62a1c91a859a 37 real32 NavE[3], NavEp[3], NavIntE[3];
gke 0:62a1c91a859a 38 int16 NavYCorrLimit;
gke 0:62a1c91a859a 39
gke 0:62a1c91a859a 40 #ifdef SIMULATE
gke 0:62a1c91a859a 41 int16 FakeDesiredRoll, FakeDesiredPitch, FakeDesiredYaw, FakeHeading;
gke 0:62a1c91a859a 42 #endif // SIMULATE
gke 0:62a1c91a859a 43
gke 0:62a1c91a859a 44 typedef union {
gke 0:62a1c91a859a 45 uint8 b[256];
gke 0:62a1c91a859a 46 struct {
gke 0:62a1c91a859a 47 uint8 u0;
gke 0:62a1c91a859a 48 int16 u1;
gke 0:62a1c91a859a 49 int8 u3;
gke 0:62a1c91a859a 50 int8 u4;
gke 0:62a1c91a859a 51 uint16 u5;
gke 0:62a1c91a859a 52 uint16 u6;
gke 0:62a1c91a859a 53 uint8 NoOfWPs;
gke 0:62a1c91a859a 54 uint8 ProximityAltitude;
gke 0:62a1c91a859a 55 uint8 ProximityRadius;
gke 0:62a1c91a859a 56 int16 OriginAltitude;
gke 0:62a1c91a859a 57 int32 OriginLatitude;
gke 0:62a1c91a859a 58 int32 OriginLongitude;
gke 0:62a1c91a859a 59 int16 RTHAltitude;
gke 0:62a1c91a859a 60 struct {
gke 0:62a1c91a859a 61 int32 Latitude;
gke 0:62a1c91a859a 62 int32 Longitude;
gke 0:62a1c91a859a 63 int16 Altitude;
gke 0:62a1c91a859a 64 int8 Loiter;
gke 0:62a1c91a859a 65 } WP[23];
gke 0:62a1c91a859a 66 };
gke 0:62a1c91a859a 67 } UAVXNav;
gke 0:62a1c91a859a 68
gke 0:62a1c91a859a 69 uint8 BufferPX[256];
gke 0:62a1c91a859a 70
gke 0:62a1c91a859a 71 int8 CurrWP;
gke 0:62a1c91a859a 72 int8 NoOfWayPoints;
gke 0:62a1c91a859a 73 int16 WPAltitude;
gke 0:62a1c91a859a 74 int32 WPLatitude, WPLongitude;
gke 0:62a1c91a859a 75 int24 WPLoiter;
gke 0:62a1c91a859a 76
gke 0:62a1c91a859a 77 real32 WayHeading;
gke 0:62a1c91a859a 78 real32 NavPolarRadius, NavNeutralRadius, NavProximityRadius, NavProximityAltitude;
gke 0:62a1c91a859a 79 int24 NavRTHTimeoutmS;
gke 0:62a1c91a859a 80
gke 0:62a1c91a859a 81 int8 NavState;
gke 0:62a1c91a859a 82 int16 NavSensitivity, RollPitchMax;
gke 0:62a1c91a859a 83 int16 AltSum;
gke 0:62a1c91a859a 84
gke 0:62a1c91a859a 85 void DoShutdown(void)
gke 0:62a1c91a859a 86 {
gke 0:62a1c91a859a 87 State = Shutdown;
gke 0:62a1c91a859a 88 DesiredThrottle = 0;
gke 0:62a1c91a859a 89 StopMotors();
gke 0:62a1c91a859a 90 } // DoShutdown
gke 0:62a1c91a859a 91
gke 0:62a1c91a859a 92 void SetDesiredAltitude(int16 NewDesiredAltitude) { // Metres
gke 0:62a1c91a859a 93 if ( F.AllowNavAltitudeHold && F.AltHoldEnabled ) {
gke 0:62a1c91a859a 94 AltSum = 0;
gke 0:62a1c91a859a 95 DesiredAltitude = NewDesiredAltitude * 10L; // Decimetres
gke 0:62a1c91a859a 96 }
gke 0:62a1c91a859a 97 } // SetDesiredAltitude
gke 0:62a1c91a859a 98
gke 0:62a1c91a859a 99 void DoFailsafeLanding(void) {
gke 0:62a1c91a859a 100 // InTheAir micro switch RC0 Pin 11 to ground when landed
gke 0:62a1c91a859a 101
gke 0:62a1c91a859a 102 const boolean InTheAir = true;
gke 0:62a1c91a859a 103
gke 0:62a1c91a859a 104 DesiredAltitude = -20.0;
gke 0:62a1c91a859a 105 if ( F.BaroAltitudeValid )
gke 0:62a1c91a859a 106 {
gke 0:62a1c91a859a 107 if ( Altitude > LAND_M )
gke 0:62a1c91a859a 108 mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
gke 0:62a1c91a859a 109
gke 0:62a1c91a859a 110 if ( !InTheAir || (( mSClock() > mS[NavStateTimeout] )
gke 0:62a1c91a859a 111 && ( F.ForceFailsafe || ( NavState == Touchdown ) || (FailState == Terminated))) )
gke 0:62a1c91a859a 112 DoShutdown();
gke 0:62a1c91a859a 113 else
gke 0:62a1c91a859a 114 DesiredThrottle = CruiseThrottle;
gke 0:62a1c91a859a 115 }
gke 0:62a1c91a859a 116 else
gke 0:62a1c91a859a 117 {
gke 0:62a1c91a859a 118 if ( mSClock() > mS[DescentUpdate] )
gke 0:62a1c91a859a 119 {
gke 0:62a1c91a859a 120 mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS;
gke 0:62a1c91a859a 121 DesiredThrottle = CruiseThrottle - DescentComp;
gke 0:62a1c91a859a 122 if ( DesiredThrottle < IdleThrottle )
gke 0:62a1c91a859a 123 StopMotors();
gke 0:62a1c91a859a 124 else
gke 0:62a1c91a859a 125 if ( DescentComp < CruiseThrottle )
gke 0:62a1c91a859a 126 DescentComp++;
gke 0:62a1c91a859a 127 }
gke 0:62a1c91a859a 128 }
gke 0:62a1c91a859a 129 } // DoFailsafeLanding
gke 0:62a1c91a859a 130
gke 0:62a1c91a859a 131 void FailsafeHoldPosition(void) {
gke 0:62a1c91a859a 132 DesiredRoll = DesiredPitch = DesiredYaw = 0;
gke 0:62a1c91a859a 133 if ( F.GPSValid && F.CompassValid )
gke 0:62a1c91a859a 134 Navigate(HoldLatitude, HoldLongitude);
gke 0:62a1c91a859a 135 } // FailsafeHoldPosition
gke 0:62a1c91a859a 136
gke 0:62a1c91a859a 137 void AcquireHoldPosition(void) {
gke 0:62a1c91a859a 138 static int8 i;
gke 0:62a1c91a859a 139
gke 0:62a1c91a859a 140 for ( i = 0; i < (uint8)3; i++ )
gke 0:62a1c91a859a 141 NavCorr[i] = NavCorrp[i] = 0;
gke 0:62a1c91a859a 142
gke 0:62a1c91a859a 143 F.NavComputed = false;
gke 0:62a1c91a859a 144
gke 0:62a1c91a859a 145 HoldLatitude = GPSLatitude;
gke 0:62a1c91a859a 146 HoldLongitude = GPSLongitude;
gke 0:62a1c91a859a 147 F.WayPointAchieved = F.WayPointCentred = F.AcquireNewPosition = false;
gke 0:62a1c91a859a 148
gke 0:62a1c91a859a 149 NavState = HoldingStation;
gke 0:62a1c91a859a 150 } // AcquireHoldPosition
gke 0:62a1c91a859a 151
gke 0:62a1c91a859a 152 void DoPolarOrientation(void) {
gke 0:62a1c91a859a 153 static real32 EastDiff, NorthDiff, Radius;
gke 0:62a1c91a859a 154 static real32 DesiredRelativeHeading;
gke 0:62a1c91a859a 155 static int16 P;
gke 0:62a1c91a859a 156
gke 0:62a1c91a859a 157 F.UsingPolar = F.UsingPolarCoordinates && F.NavValid && ( NavState == HoldingStation );
gke 0:62a1c91a859a 158
gke 0:62a1c91a859a 159 if ( F.UsingPolar ) { // needs rethink - probably arm using RTH switch
gke 0:62a1c91a859a 160 EastDiff = (OriginLongitude - GPSLongitude) * GPSLongitudeCorrection;
gke 0:62a1c91a859a 161 NorthDiff = OriginLatitude - GPSLatitude;
gke 0:62a1c91a859a 162
gke 0:62a1c91a859a 163 Radius = Max(abs(EastDiff), abs(NorthDiff));
gke 0:62a1c91a859a 164 if ( Radius > NavPolarRadius ) {
gke 0:62a1c91a859a 165 DesiredRelativeHeading = Make2Pi(atan2((real32)EastDiff, (real32)NorthDiff) - PI - Heading );
gke 0:62a1c91a859a 166
gke 0:62a1c91a859a 167 P = ( (int24)DesiredRelativeHeading * 24L + HALFPI )/ PI + Orientation;
gke 0:62a1c91a859a 168
gke 0:62a1c91a859a 169 while ( P > 24 ) P -=24;
gke 0:62a1c91a859a 170 while ( P < 0 ) P +=24;
gke 0:62a1c91a859a 171
gke 0:62a1c91a859a 172 } else
gke 0:62a1c91a859a 173 P = 0;
gke 0:62a1c91a859a 174 } else
gke 0:62a1c91a859a 175 P = 0;
gke 0:62a1c91a859a 176
gke 0:62a1c91a859a 177 PolarOrientation = P;;
gke 0:62a1c91a859a 178
gke 0:62a1c91a859a 179 } // DoPolarOrientation
gke 0:62a1c91a859a 180
gke 0:62a1c91a859a 181 void Navigate(int32 NavLatitude, int32 NavLongitude ) {
gke 0:62a1c91a859a 182 // F.GPSValid must be true immediately prior to entry
gke 0:62a1c91a859a 183 // This routine does not point the quadrocopter at the destination
gke 0:62a1c91a859a 184 // waypoint. It simply rolls/pitches towards the destination.
gke 0:62a1c91a859a 185 // GPS coordinates MUST be int32 to allow sufficient range - real32 is insufficient.
gke 0:62a1c91a859a 186
gke 0:62a1c91a859a 187 static real32 RelHeadingSin, RelHeadingCos;
gke 0:62a1c91a859a 188 static real32 Radius;
gke 0:62a1c91a859a 189 static real32 AltE;
gke 0:62a1c91a859a 190 static real32 EastDiff, NorthDiff;
gke 0:62a1c91a859a 191 static real32 RelHeading;
gke 0:62a1c91a859a 192 static uint8 a;
gke 0:62a1c91a859a 193 static real32 Diff, NavP, NavI, NavD;
gke 0:62a1c91a859a 194
gke 0:62a1c91a859a 195 DoPolarOrientation();
gke 0:62a1c91a859a 196
gke 0:62a1c91a859a 197 DesiredLatitude = NavLatitude;
gke 0:62a1c91a859a 198 DesiredLongitude = NavLongitude;
gke 0:62a1c91a859a 199
gke 0:62a1c91a859a 200 EastDiff = (real32)(DesiredLongitude - GPSLongitude) * GPSLongitudeCorrection;
gke 0:62a1c91a859a 201 NorthDiff = (real32)(DesiredLatitude - GPSLatitude);
gke 0:62a1c91a859a 202
gke 0:62a1c91a859a 203 Radius = sqrt( Sqr(EastDiff) + Sqr(NorthDiff) );
gke 0:62a1c91a859a 204
gke 0:62a1c91a859a 205 F.WayPointCentred = Radius < NavNeutralRadius;
gke 0:62a1c91a859a 206 AltE = DesiredAltitude - Altitude;
gke 0:62a1c91a859a 207 F.WayPointAchieved = ( Radius < NavProximityRadius ) && ( abs(AltE) < NavProximityAltitude );
gke 0:62a1c91a859a 208
gke 0:62a1c91a859a 209 WayHeading = Make2Pi(atan2((real32)EastDiff, (real32)NorthDiff));
gke 0:62a1c91a859a 210 RelHeading = MakePi(WayHeading - Heading); // make +/- MilliPi
gke 0:62a1c91a859a 211
gke 0:62a1c91a859a 212 if ( ( NavSensitivity > NAV_SENS_THRESHOLD ) && !F.WayPointCentred ) {
gke 0:62a1c91a859a 213 #ifdef NAV_WING
gke 0:62a1c91a859a 214
gke 0:62a1c91a859a 215 // no Nav for conventional aircraft - yet!
gke 0:62a1c91a859a 216 NavCorr[Pitch] = -5; // always moving forward
gke 0:62a1c91a859a 217 // Just use simple rudder only for now.
gke 0:62a1c91a859a 218 if ( !F.WayPointAchieved ) {
gke 0:62a1c91a859a 219 NavCorr[Yaw] = -(RelHeading * NAV_YAW_LIMIT) / HALFPI;
gke 0:62a1c91a859a 220 NavCorr[Yaw] = Limit(NavCorr[Yaw], -NAV_YAW_LIMIT, NAV_YAW_LIMIT); // gently!
gke 0:62a1c91a859a 221 }
gke 0:62a1c91a859a 222
gke 0:62a1c91a859a 223 #else // MULTICOPTER
gke 0:62a1c91a859a 224
gke 0:62a1c91a859a 225 // revert to original simpler version from UAVP->UAVX transition
gke 0:62a1c91a859a 226
gke 0:62a1c91a859a 227 RelHeadingSin = sin(RelHeading);
gke 0:62a1c91a859a 228 RelHeadingCos = cos(RelHeading);
gke 0:62a1c91a859a 229
gke 0:62a1c91a859a 230 NavE[Roll] = Radius * RelHeadingSin;
gke 0:62a1c91a859a 231 NavE[Pitch] = -Radius * RelHeadingCos;
gke 0:62a1c91a859a 232
gke 0:62a1c91a859a 233 // Roll & Pitch
gke 0:62a1c91a859a 234
gke 0:62a1c91a859a 235 for ( a = 0; a < (uint8)2 ; a++ ) {
gke 0:62a1c91a859a 236 NavP = Limit(NavE[a], -NAV_MAX_ROLL_PITCH, NAV_MAX_ROLL_PITCH);
gke 0:62a1c91a859a 237
gke 0:62a1c91a859a 238 NavIntE[a] += NavE[a];
gke 0:62a1c91a859a 239 NavIntE[a] = Limit(NavIntE[a], -K[NavIntLimit], K[NavIntLimit]);
gke 0:62a1c91a859a 240 NavI = NavIntE[a] * K[NavKi] * GPSdT;
gke 0:62a1c91a859a 241 NavIntE[a] = Decay1(NavIntE[a]);
gke 0:62a1c91a859a 242
gke 0:62a1c91a859a 243 Diff = (NavEp[a] - NavE[a]);
gke 0:62a1c91a859a 244 Diff = Limit(Diff, -256, 256);
gke 0:62a1c91a859a 245 NavD = Diff * K[NavKd] * GPSdTR;
gke 0:62a1c91a859a 246 NavD = Limit(NavD, -NAV_DIFF_LIMIT, NAV_DIFF_LIMIT);
gke 0:62a1c91a859a 247
gke 0:62a1c91a859a 248 NavEp[a] = NavE[a];
gke 0:62a1c91a859a 249
gke 0:62a1c91a859a 250 NavCorr[a] = (NavP + NavI + NavD ) * NavSensitivity;
gke 0:62a1c91a859a 251
gke 0:62a1c91a859a 252 NavCorr[a] = SlewLimit(NavCorrp[a], NavCorr[a], NAV_CORR_SLEW_LIMIT);
gke 0:62a1c91a859a 253 NavCorr[a] = Limit(NavCorr[a], -NAV_MAX_ROLL_PITCH, NAV_MAX_ROLL_PITCH);
gke 0:62a1c91a859a 254
gke 0:62a1c91a859a 255 NavCorrp[a] = NavCorr[a];
gke 0:62a1c91a859a 256 }
gke 0:62a1c91a859a 257
gke 0:62a1c91a859a 258 // Yaw
gke 0:62a1c91a859a 259 if ( F.AllowTurnToWP && !F.WayPointAchieved ) {
gke 0:62a1c91a859a 260 RelHeading = MakePi(WayHeading - Heading); // make +/- MilliPi
gke 0:62a1c91a859a 261 NavCorr[Yaw] = -(RelHeading * NavYCorrLimit) / HALFPI;
gke 0:62a1c91a859a 262 NavCorr[Yaw] = Limit(NavCorr[Yaw], -NavYCorrLimit, NavYCorrLimit); // gently!
gke 0:62a1c91a859a 263 } else
gke 0:62a1c91a859a 264 NavCorr[Yaw] = 0;
gke 0:62a1c91a859a 265
gke 0:62a1c91a859a 266 #endif // NAV_WING
gke 0:62a1c91a859a 267 } else {
gke 0:62a1c91a859a 268 // Neutral Zone - no GPS influence
gke 0:62a1c91a859a 269 NavCorr[Pitch] = DecayX(NavCorr[Pitch], 2);
gke 0:62a1c91a859a 270 NavCorr[Roll] = DecayX(NavCorr[Roll], 2);
gke 0:62a1c91a859a 271 NavCorr[Yaw] = 0;
gke 0:62a1c91a859a 272 NavIntE[Roll] = NavIntE[Pitch] = NavEp[Roll] = NavEp[Pitch] = 0;
gke 0:62a1c91a859a 273 }
gke 0:62a1c91a859a 274
gke 0:62a1c91a859a 275 F.NavComputed = true;
gke 0:62a1c91a859a 276
gke 0:62a1c91a859a 277 } // Navigate
gke 0:62a1c91a859a 278
gke 0:62a1c91a859a 279 void DoNavigation(void) {
gke 0:62a1c91a859a 280
gke 0:62a1c91a859a 281 F.NavigationActive = F.GPSValid && F.CompassValid && ( mSClock() > mS[NavActiveTime]);
gke 0:62a1c91a859a 282
gke 0:62a1c91a859a 283 if ( F.NavigationActive ) {
gke 0:62a1c91a859a 284
gke 0:62a1c91a859a 285 F.LostModel = F.ForceFailsafe = false;
gke 0:62a1c91a859a 286
gke 0:62a1c91a859a 287 if ( !F.NavComputed )
gke 0:62a1c91a859a 288 switch ( NavState ) { // most case last - switches in C18 are IF chains not branch tables!
gke 0:62a1c91a859a 289 case Touchdown:
gke 0:62a1c91a859a 290 Navigate(OriginLatitude, OriginLongitude);
gke 0:62a1c91a859a 291 DoFailsafeLanding();
gke 0:62a1c91a859a 292 break;
gke 0:62a1c91a859a 293 case Descending:
gke 0:62a1c91a859a 294 Navigate( OriginLatitude, OriginLongitude );
gke 0:62a1c91a859a 295 if ( F.ReturnHome || F.Navigate )
gke 0:62a1c91a859a 296 #ifdef NAV_WING
gke 0:62a1c91a859a 297 {
gke 0:62a1c91a859a 298 // needs more thought - runway direction?
gke 0:62a1c91a859a 299 DoFailsafeLanding();
gke 0:62a1c91a859a 300 }
gke 0:62a1c91a859a 301 #else
gke 0:62a1c91a859a 302 if ( Altitude < LAND_M ) {
gke 0:62a1c91a859a 303 mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
gke 0:62a1c91a859a 304 NavState = Touchdown;
gke 0:62a1c91a859a 305 } else
gke 0:62a1c91a859a 306 DoFailsafeLanding();
gke 0:62a1c91a859a 307 #endif // NAV_WING
gke 0:62a1c91a859a 308 else
gke 0:62a1c91a859a 309 AcquireHoldPosition();
gke 0:62a1c91a859a 310 break;
gke 0:62a1c91a859a 311 case AtHome:
gke 0:62a1c91a859a 312 Navigate(OriginLatitude, OriginLongitude);
gke 0:62a1c91a859a 313 SetDesiredAltitude((int16)P[NavRTHAlt]);
gke 0:62a1c91a859a 314 if ( F.ReturnHome || F.Navigate )
gke 0:62a1c91a859a 315 if ( F.WayPointAchieved ) { // check still @ Home
gke 0:62a1c91a859a 316 if ( F.UsingRTHAutoDescend && ( mSClock() > mS[NavStateTimeout] ) )
gke 0:62a1c91a859a 317 NavState = Descending;
gke 0:62a1c91a859a 318 } else
gke 0:62a1c91a859a 319 NavState = ReturningHome;
gke 0:62a1c91a859a 320 else
gke 0:62a1c91a859a 321 AcquireHoldPosition();
gke 0:62a1c91a859a 322 break;
gke 0:62a1c91a859a 323 case ReturningHome:
gke 0:62a1c91a859a 324 Navigate(OriginLatitude, OriginLongitude);
gke 0:62a1c91a859a 325 SetDesiredAltitude((int16)P[NavRTHAlt]);
gke 0:62a1c91a859a 326 if ( F.ReturnHome || F.Navigate ) {
gke 0:62a1c91a859a 327 if ( F.WayPointAchieved ) {
gke 0:62a1c91a859a 328 mS[NavStateTimeout] = mSClock() + NavRTHTimeoutmS;
gke 0:62a1c91a859a 329 NavState = AtHome;
gke 0:62a1c91a859a 330 }
gke 0:62a1c91a859a 331 } else
gke 0:62a1c91a859a 332 AcquireHoldPosition();
gke 0:62a1c91a859a 333 break;
gke 0:62a1c91a859a 334 case Loitering:
gke 0:62a1c91a859a 335 Navigate(WPLatitude, WPLongitude);
gke 0:62a1c91a859a 336 SetDesiredAltitude(WPAltitude);
gke 0:62a1c91a859a 337 if ( F.Navigate ) {
gke 0:62a1c91a859a 338 if ( F.WayPointAchieved && (mSClock() > mS[NavStateTimeout]) )
gke 0:62a1c91a859a 339 if ( CurrWP == NoOfWayPoints ) {
gke 0:62a1c91a859a 340 CurrWP = 1;
gke 0:62a1c91a859a 341 NavState = ReturningHome;
gke 0:62a1c91a859a 342 } else {
gke 0:62a1c91a859a 343 GetWayPointPX(++CurrWP);
gke 0:62a1c91a859a 344 NavState = Navigating;
gke 0:62a1c91a859a 345 }
gke 0:62a1c91a859a 346 } else
gke 0:62a1c91a859a 347 AcquireHoldPosition();
gke 0:62a1c91a859a 348 break;
gke 0:62a1c91a859a 349 case Navigating:
gke 0:62a1c91a859a 350 Navigate(WPLatitude, WPLongitude);
gke 0:62a1c91a859a 351 SetDesiredAltitude(WPAltitude);
gke 0:62a1c91a859a 352 if ( F.Navigate ) {
gke 0:62a1c91a859a 353 if ( F.WayPointAchieved ) {
gke 0:62a1c91a859a 354 mS[NavStateTimeout] = mSClock() + WPLoiter;
gke 0:62a1c91a859a 355 NavState = Loitering;
gke 0:62a1c91a859a 356 }
gke 0:62a1c91a859a 357 } else
gke 0:62a1c91a859a 358 AcquireHoldPosition();
gke 0:62a1c91a859a 359 break;
gke 0:62a1c91a859a 360 case HoldingStation:
gke 0:62a1c91a859a 361 if ( F.AttitudeHold ) {
gke 0:62a1c91a859a 362 if ( F.AcquireNewPosition && !( F.Ch5Active & F.UsingPositionHoldLock ) ) {
gke 0:62a1c91a859a 363 F.AllowTurnToWP = SaveAllowTurnToWP;
gke 0:62a1c91a859a 364 AcquireHoldPosition();
gke 0:62a1c91a859a 365 #ifdef NAV_ACQUIRE_BEEPER
gke 0:62a1c91a859a 366 if ( !F.BeeperInUse ) {
gke 0:62a1c91a859a 367 mS[BeeperTimeout] = mSClock() + 500L;
gke 0:62a1c91a859a 368 Beeper_ON;
gke 0:62a1c91a859a 369 }
gke 0:62a1c91a859a 370 #endif // NAV_ACQUIRE_BEEPER
gke 0:62a1c91a859a 371 }
gke 0:62a1c91a859a 372 } else
gke 0:62a1c91a859a 373 F.AcquireNewPosition = true;
gke 0:62a1c91a859a 374
gke 0:62a1c91a859a 375 Navigate(HoldLatitude, HoldLongitude);
gke 0:62a1c91a859a 376
gke 0:62a1c91a859a 377 if ( F.NavValid && F.NearLevel ) // Origin must be valid for ANY navigation!
gke 0:62a1c91a859a 378 if ( F.Navigate ) {
gke 0:62a1c91a859a 379 GetWayPointPX(CurrWP); // resume from previous WP
gke 0:62a1c91a859a 380 SetDesiredAltitude(WPAltitude);
gke 0:62a1c91a859a 381 NavState = Navigating;
gke 0:62a1c91a859a 382 } else
gke 0:62a1c91a859a 383 if ( F.ReturnHome )
gke 0:62a1c91a859a 384 NavState = ReturningHome;
gke 0:62a1c91a859a 385 break;
gke 0:62a1c91a859a 386 } // switch NavState
gke 0:62a1c91a859a 387 } else
gke 0:62a1c91a859a 388 if ( F.ForceFailsafe && F.NewCommands )
gke 0:62a1c91a859a 389 {
gke 0:62a1c91a859a 390 F.AltHoldEnabled = F.AllowNavAltitudeHold = true;
gke 0:62a1c91a859a 391 F.LostModel = true;
gke 0:62a1c91a859a 392 DoFailsafeLanding();
gke 0:62a1c91a859a 393 }
gke 0:62a1c91a859a 394 else // kill nav correction immediately
gke 0:62a1c91a859a 395 NavCorr[Pitch] = NavCorr[Roll] = NavCorr[Yaw] = 0; // zzz
gke 0:62a1c91a859a 396
gke 0:62a1c91a859a 397 F.NewCommands = false; // Navigate modifies Desired Roll, Pitch and Yaw values.
gke 0:62a1c91a859a 398
gke 0:62a1c91a859a 399 } // DoNavigation
gke 0:62a1c91a859a 400
gke 0:62a1c91a859a 401 void CheckFailsafeAbort(void) {
gke 0:62a1c91a859a 402 if ( mSClock() > mS[AbortTimeout] ) {
gke 0:62a1c91a859a 403 if ( F.Signal ) {
gke 0:62a1c91a859a 404 LEDGreen_ON;
gke 0:62a1c91a859a 405 mS[NavStateTimeout] = 0;
gke 0:62a1c91a859a 406 mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS; // may be redundant?
gke 0:62a1c91a859a 407 NavState = HoldingStation;
gke 0:62a1c91a859a 408 FailState = MonitoringRx;
gke 0:62a1c91a859a 409 }
gke 0:62a1c91a859a 410 } else
gke 0:62a1c91a859a 411 mS[AbortTimeout] += ABORT_UPDATE_MS;
gke 0:62a1c91a859a 412 } // CheckFailsafeAbort
gke 0:62a1c91a859a 413
gke 0:62a1c91a859a 414 void DoPPMFailsafe(void) { // only relevant to PPM Rx or Quad NOT synchronising with Rx
gke 0:62a1c91a859a 415
gke 0:62a1c91a859a 416 if ( State == InFlight )
gke 0:62a1c91a859a 417 switch ( FailState ) { // FailStates { MonitoringRx, Aborting, Terminating, Terminated }
gke 0:62a1c91a859a 418 case Terminated: // Basic assumption is that aircraft is being flown over a safe area!
gke 0:62a1c91a859a 419 FailsafeHoldPosition();
gke 0:62a1c91a859a 420 DoFailsafeLanding();
gke 0:62a1c91a859a 421 break;
gke 0:62a1c91a859a 422 case Terminating:
gke 0:62a1c91a859a 423 FailsafeHoldPosition();
gke 0:62a1c91a859a 424 if ( Altitude < LAND_M ) {
gke 0:62a1c91a859a 425 mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
gke 0:62a1c91a859a 426 NavState = Touchdown;
gke 0:62a1c91a859a 427 FailState = Terminated;
gke 0:62a1c91a859a 428 }
gke 0:62a1c91a859a 429 DoFailsafeLanding();
gke 0:62a1c91a859a 430 break;
gke 0:62a1c91a859a 431 case Aborting:
gke 0:62a1c91a859a 432 FailsafeHoldPosition();
gke 0:62a1c91a859a 433 F.AltHoldEnabled = true;
gke 0:62a1c91a859a 434 SetDesiredAltitude((int16)P[NavRTHAlt]);
gke 0:62a1c91a859a 435 if ( mSClock() > mS[NavStateTimeout] ) {
gke 0:62a1c91a859a 436 F.LostModel = true;
gke 0:62a1c91a859a 437 LEDGreen_OFF;
gke 0:62a1c91a859a 438 LEDRed_ON;
gke 0:62a1c91a859a 439
gke 0:62a1c91a859a 440 mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
gke 0:62a1c91a859a 441 NavState = Descending;
gke 0:62a1c91a859a 442 FailState = Terminating;
gke 0:62a1c91a859a 443 } else
gke 0:62a1c91a859a 444 CheckFailsafeAbort();
gke 0:62a1c91a859a 445 break;
gke 0:62a1c91a859a 446 case MonitoringRx:
gke 0:62a1c91a859a 447 if ( mSClock() > mS[FailsafeTimeout] ) {
gke 0:62a1c91a859a 448 // use last "good" throttle
gke 0:62a1c91a859a 449 Stats[RCFailsafesS]++;
gke 0:62a1c91a859a 450 if ( F.GPSValid && F.CompassValid )
gke 0:62a1c91a859a 451 mS[NavStateTimeout] = mSClock() + ABORT_TIMEOUT_GPS_MS;
gke 0:62a1c91a859a 452 else
gke 0:62a1c91a859a 453 mS[NavStateTimeout] = mSClock() + ABORT_TIMEOUT_NO_GPS_MS;
gke 0:62a1c91a859a 454 mS[AbortTimeout] = mSClock() + ABORT_UPDATE_MS;
gke 0:62a1c91a859a 455 FailState = Aborting;
gke 0:62a1c91a859a 456 }
gke 0:62a1c91a859a 457 break;
gke 0:62a1c91a859a 458 } // Switch FailState
gke 0:62a1c91a859a 459 else
gke 0:62a1c91a859a 460 DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = 0;
gke 0:62a1c91a859a 461
gke 0:62a1c91a859a 462 } // DoPPMFailsafe
gke 0:62a1c91a859a 463
gke 0:62a1c91a859a 464 void UAVXNavCommand(void) {
gke 0:62a1c91a859a 465
gke 0:62a1c91a859a 466 static int16 b;
gke 0:62a1c91a859a 467 static uint8 c, d, csum;
gke 0:62a1c91a859a 468
gke 0:62a1c91a859a 469 c = RxChar();
gke 0:62a1c91a859a 470 LEDBlue_ON;
gke 0:62a1c91a859a 471
gke 0:62a1c91a859a 472 switch ( c ) {
gke 0:62a1c91a859a 473 case '0': // hello
gke 0:62a1c91a859a 474 TxChar(ACK);
gke 0:62a1c91a859a 475 break;
gke 0:62a1c91a859a 476 case '1': // write
gke 0:62a1c91a859a 477 csum = 0;
gke 0:62a1c91a859a 478 for ( b = 0; b < 256; b++) { // cannot write fast enough so buffer
gke 0:62a1c91a859a 479 d = RxChar();
gke 0:62a1c91a859a 480 csum ^= d;
gke 0:62a1c91a859a 481 BufferPX[b] = d;
gke 0:62a1c91a859a 482 }
gke 0:62a1c91a859a 483 if ( csum == (uint8)0 ) {
gke 0:62a1c91a859a 484 for ( b = 0; b < 256; b++)
gke 0:62a1c91a859a 485 WritePX(NAV_ADDR_PX + b, BufferPX[b]);
gke 0:62a1c91a859a 486 TxChar(ACK);
gke 0:62a1c91a859a 487 } else
gke 0:62a1c91a859a 488 TxChar(NAK);
gke 0:62a1c91a859a 489
gke 0:62a1c91a859a 490 InitNavigation();
gke 0:62a1c91a859a 491
gke 0:62a1c91a859a 492 break;
gke 0:62a1c91a859a 493 case '2':
gke 0:62a1c91a859a 494 csum = 0;
gke 0:62a1c91a859a 495 for ( b = 0; b < 255; b++) {
gke 0:62a1c91a859a 496 d = ReadPX(NAV_ADDR_PX + b);
gke 0:62a1c91a859a 497 csum ^= d;
gke 0:62a1c91a859a 498 BufferPX[b] = d;
gke 0:62a1c91a859a 499 }
gke 0:62a1c91a859a 500 BufferPX[255] = csum;
gke 0:62a1c91a859a 501 for ( b = 0; b < 256; b++)
gke 0:62a1c91a859a 502 TxChar(BufferPX[b]);
gke 0:62a1c91a859a 503 TxChar(ACK);
gke 0:62a1c91a859a 504 break;
gke 0:62a1c91a859a 505 case '3':
gke 0:62a1c91a859a 506 csum = 0;
gke 0:62a1c91a859a 507 for ( b = 0; b < 63; b++) {
gke 0:62a1c91a859a 508 d = ReadPX(STATS_ADDR_PX + b);
gke 0:62a1c91a859a 509 csum ^= d;
gke 0:62a1c91a859a 510 BufferPX[b] = d;
gke 0:62a1c91a859a 511 }
gke 0:62a1c91a859a 512 BufferPX[63] = csum;
gke 0:62a1c91a859a 513 for ( b = 0; b < 64; b++)
gke 0:62a1c91a859a 514 TxChar(BufferPX[b]);
gke 0:62a1c91a859a 515 TxChar(ACK);
gke 0:62a1c91a859a 516 break;
gke 0:62a1c91a859a 517 default:
gke 0:62a1c91a859a 518 break;
gke 0:62a1c91a859a 519 } // switch
gke 0:62a1c91a859a 520
gke 0:62a1c91a859a 521 WritePXImagefile();
gke 0:62a1c91a859a 522 LEDBlue_OFF;
gke 0:62a1c91a859a 523
gke 0:62a1c91a859a 524 } // UAVXNavCommand
gke 0:62a1c91a859a 525
gke 0:62a1c91a859a 526 void GetWayPointPX(int8 wp) {
gke 0:62a1c91a859a 527 static uint16 w;
gke 0:62a1c91a859a 528
gke 0:62a1c91a859a 529 if ( wp > NoOfWayPoints )
gke 0:62a1c91a859a 530 CurrWP = wp = 0;
gke 0:62a1c91a859a 531 if ( wp == 0 ) { // force to Origin
gke 0:62a1c91a859a 532 WPLatitude = OriginLatitude;
gke 0:62a1c91a859a 533 WPLongitude = OriginLongitude;
gke 0:62a1c91a859a 534 WPAltitude = (int16)P[NavRTHAlt];
gke 0:62a1c91a859a 535 WPLoiter = 30000; // mS
gke 0:62a1c91a859a 536 } else {
gke 0:62a1c91a859a 537 w = NAV_WP_START + (wp-1) * WAYPOINT_REC_SIZE;
gke 0:62a1c91a859a 538 WPLatitude = Read32PX(w + 0);
gke 0:62a1c91a859a 539 WPLongitude = Read32PX(w + 4);
gke 0:62a1c91a859a 540 WPAltitude = Read16PX(w + 8);
gke 0:62a1c91a859a 541
gke 0:62a1c91a859a 542 #ifdef NAV_ENFORCE_ALTITUDE_CEILING
gke 0:62a1c91a859a 543 if ( WPAltitude > NAV_CEILING )
gke 0:62a1c91a859a 544 WPAltitude = NAV_CEILING;
gke 0:62a1c91a859a 545 #endif // NAV_ENFORCE_ALTITUDE_CEILING
gke 0:62a1c91a859a 546 WPLoiter = (int16)ReadPX(w + 10) * 1000L; // mS
gke 0:62a1c91a859a 547 }
gke 0:62a1c91a859a 548
gke 0:62a1c91a859a 549 F.WayPointCentred = F.WayPointAchieved = false;
gke 0:62a1c91a859a 550
gke 0:62a1c91a859a 551 } // GetWaypointPX
gke 0:62a1c91a859a 552
gke 0:62a1c91a859a 553 void InitNavigation(void) {
gke 0:62a1c91a859a 554 static uint8 i;
gke 0:62a1c91a859a 555
gke 0:62a1c91a859a 556 HoldLatitude = HoldLongitude = WayHeading = 0;
gke 0:62a1c91a859a 557
gke 0:62a1c91a859a 558 for ( i = 0; i < (uint8)3; i++ )
gke 0:62a1c91a859a 559 NavEp[i] = NavIntE[i] = NavCorr[i] = NavCorrp[i] = 0;
gke 0:62a1c91a859a 560
gke 0:62a1c91a859a 561 NavState = HoldingStation;
gke 0:62a1c91a859a 562 AttitudeHoldResetCount = 0;
gke 0:62a1c91a859a 563 CurrMaxRollPitch = 0;
gke 0:62a1c91a859a 564 F.WayPointAchieved = F.WayPointCentred = false;
gke 0:62a1c91a859a 565 F.NavComputed = false;
gke 0:62a1c91a859a 566
gke 0:62a1c91a859a 567 if ( ReadPX(NAV_NO_WP) <= 0 ) {
gke 0:62a1c91a859a 568 NavProximityRadius = ConvertMToGPS(NAV_PROXIMITY_RADIUS);
gke 0:62a1c91a859a 569 NavProximityAltitude = NAV_PROXIMITY_ALTITUDE * 10L; // Decimetres
gke 0:62a1c91a859a 570 } else {
gke 0:62a1c91a859a 571 // need minimum values in UAVXNav?
gke 0:62a1c91a859a 572 NavProximityRadius = ConvertMToGPS(ReadPX(NAV_PROX_RADIUS));
gke 0:62a1c91a859a 573 NavProximityAltitude = ReadPX(NAV_PROX_ALT) * 10L; // Decimetres
gke 0:62a1c91a859a 574 }
gke 0:62a1c91a859a 575
gke 0:62a1c91a859a 576 NoOfWayPoints = ReadPX(NAV_NO_WP);
gke 0:62a1c91a859a 577
gke 0:62a1c91a859a 578 if ( NoOfWayPoints <= 0 )
gke 0:62a1c91a859a 579 CurrWP = 0;
gke 0:62a1c91a859a 580 else
gke 0:62a1c91a859a 581 CurrWP = 1;
gke 0:62a1c91a859a 582 GetWayPointPX(0);
gke 0:62a1c91a859a 583
gke 0:62a1c91a859a 584 } // InitNavigation
gke 0:62a1c91a859a 585
gke 0:62a1c91a859a 586