Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

autonomous.c@2:90292f8bd179, 2011-04-26 (annotated)

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
0:62a1c91a859a 
Not flightworthy. Posted for others to make use of the I2C SW code.

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