autonomous.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 void DoShutdown(void);
00024 void DoPolarOrientation(void);
00025 void Navigate(int32, int32);
00026 void SetDesiredAltitude(int16);
00027 void DoFailsafeLanding(void);
00028 void AcquireHoldPosition(void);
00029 void DoNavigation(void);
00030 void DoPPMFailsafe(void);
00031 void UAVXNavCommand(void);
00032 void GetWayPointPX(int8);
00033 void InitNavigation(void);
00034 
00035 real32 NavCorr[3], NavCorrp[3];
00036 real32 NavE[3], NavEp[3], NavIntE[3];
00037 int16 NavYCorrLimit;
00038 
00039 #ifdef SIMULATE
00040 int16 FakeDesiredRoll, FakeDesiredPitch, FakeDesiredYaw, FakeHeading;
00041 #endif // SIMULATE
00042 
00043 typedef union {
00044     uint8 b[256];
00045     struct {
00046         uint8 u0;
00047         int16 u1;
00048         int8 u3;
00049         int8 u4;
00050         uint16 u5;
00051         uint16 u6;
00052         uint8 NoOfWPs;
00053         uint8 ProximityAltitude;
00054         uint8 ProximityRadius;
00055         int16 OriginAltitude;
00056         int32 OriginLatitude;
00057         int32 OriginLongitude;
00058         int16 RTHAltitude;
00059         struct {
00060             int32 Latitude;
00061             int32 Longitude;
00062             int16 Altitude;
00063             int8 Loiter;
00064         } WP[23];
00065     };
00066 } UAVXNav;
00067 
00068 uint8 BufferPX[256];
00069 
00070 int8     CurrWP;
00071 int8     NoOfWayPoints;
00072 int16    WPAltitude;
00073 int32    WPLatitude, WPLongitude;
00074 int24    WPLoiter;
00075 
00076 real32   WayHeading;
00077 real32   NavPolarRadius, NavNeutralRadius, NavProximityRadius, NavProximityAltitude;
00078 int24    NavRTHTimeoutmS;
00079 
00080 int8     NavState;
00081 int16    NavSensitivity, RollPitchMax;
00082 int16    AltSum;
00083 
00084 void DoShutdown(void)
00085 {
00086     State = Shutdown;
00087     DesiredThrottle = 0;
00088     StopMotors();
00089 } // DoShutdown
00090 
00091 void SetDesiredAltitude(int16 NewDesiredAltitude) { // Metres
00092     if ( F.AllowNavAltitudeHold && F.AltHoldEnabled ) {
00093         AltSum = 0;
00094         DesiredAltitude = NewDesiredAltitude * 10L; // Decimetres
00095     }
00096 } // SetDesiredAltitude
00097 
00098 void DoFailsafeLanding(void) {
00099     // InTheAir micro switch RC0 Pin 11 to ground when landed
00100     
00101     const boolean InTheAir = true;
00102 
00103     DesiredAltitude = -20.0;
00104 if ( F.BaroAltitudeValid )
00105     {
00106         if ( Altitude > LAND_M )
00107             mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
00108 
00109         if ( !InTheAir || (( mSClock() > mS[NavStateTimeout] ) 
00110             && ( F.ForceFailsafe || ( NavState == Touchdown ) || (FailState == Terminated))) )
00111             DoShutdown();
00112         else
00113             DesiredThrottle = CruiseThrottle;
00114     }
00115     else
00116     {
00117         if ( mSClock() > mS[DescentUpdate] )
00118         {
00119             mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS;
00120             DesiredThrottle = CruiseThrottle - DescentComp;
00121             if ( DesiredThrottle < IdleThrottle )
00122                 StopMotors();
00123             else
00124                 if ( DescentComp < CruiseThrottle )
00125                     DescentComp++;        
00126         }
00127     }
00128 } // DoFailsafeLanding
00129 
00130 void FailsafeHoldPosition(void) {
00131     DesiredRoll = DesiredPitch = DesiredYaw = 0;
00132     if ( F.GPSValid && F.CompassValid )
00133         Navigate(HoldLatitude, HoldLongitude);
00134 } // FailsafeHoldPosition
00135 
00136 void AcquireHoldPosition(void) {
00137     static int8 i;
00138 
00139     for ( i = 0; i < (uint8)3; i++ )
00140         NavCorr[i] = NavCorrp[i]  =  0;
00141 
00142     F.NavComputed = false;
00143 
00144     HoldLatitude = GPSLatitude;
00145     HoldLongitude = GPSLongitude;
00146     F.WayPointAchieved = F.WayPointCentred = F.AcquireNewPosition = false;
00147 
00148     NavState = HoldingStation;
00149 } // AcquireHoldPosition
00150 
00151 void DoPolarOrientation(void) {
00152     static real32 EastDiff, NorthDiff, Radius;
00153     static real32 DesiredRelativeHeading;
00154     static int16 P;
00155 
00156     F.UsingPolar = F.UsingPolarCoordinates && F.NavValid && ( NavState == HoldingStation );
00157 
00158     if ( F.UsingPolar ) { // needs rethink - probably arm using RTH switch
00159         EastDiff = (OriginLongitude - GPSLongitude) * GPSLongitudeCorrection;
00160         NorthDiff = OriginLatitude - GPSLatitude;
00161 
00162         Radius = Max(abs(EastDiff), abs(NorthDiff));
00163         if ( Radius > NavPolarRadius ) {
00164             DesiredRelativeHeading = Make2Pi(atan2((real32)EastDiff, (real32)NorthDiff) - PI - Heading );
00165 
00166             P = ( (int24)DesiredRelativeHeading * 24L + HALFPI )/ PI + Orientation;
00167 
00168             while ( P > 24 ) P -=24;
00169             while ( P < 0 ) P +=24;
00170 
00171         } else
00172             P = 0;
00173     } else
00174         P = 0;
00175 
00176     PolarOrientation = P;;
00177 
00178 } // DoPolarOrientation
00179 
00180 void Navigate(int32 NavLatitude, int32 NavLongitude ) {
00181     // F.GPSValid must be true immediately prior to entry
00182     // This routine does not point the quadrocopter at the destination
00183     // waypoint. It simply rolls/pitches towards the destination.
00184     // GPS coordinates MUST be int32 to allow sufficient range - real32 is insufficient.
00185 
00186     static real32 RelHeadingSin, RelHeadingCos;
00187     static real32 Radius;
00188     static real32 AltE;
00189     static real32 EastDiff, NorthDiff;
00190     static real32 RelHeading;
00191     static uint8 a;
00192     static real32 Diff, NavP, NavI, NavD;
00193 
00194     DoPolarOrientation();
00195 
00196     DesiredLatitude = NavLatitude;
00197     DesiredLongitude = NavLongitude;
00198 
00199     EastDiff = (real32)(DesiredLongitude - GPSLongitude) * GPSLongitudeCorrection;
00200     NorthDiff = (real32)(DesiredLatitude - GPSLatitude);
00201 
00202     Radius = sqrt( Sqr(EastDiff) + Sqr(NorthDiff) );
00203 
00204     F.WayPointCentred =  Radius < NavNeutralRadius;
00205     AltE = DesiredAltitude - Altitude;
00206     F.WayPointAchieved = ( Radius < NavProximityRadius ) && ( abs(AltE) < NavProximityAltitude );
00207 
00208     WayHeading = Make2Pi(atan2((real32)EastDiff, (real32)NorthDiff));
00209     RelHeading = MakePi(WayHeading - Heading); // make +/- MilliPi
00210 
00211     if ( ( NavSensitivity > NAV_SENS_THRESHOLD ) && !F.WayPointCentred ) {
00212 #ifdef NAV_WING
00213 
00214         // no Nav for conventional aircraft - yet!
00215         NavCorr[Pitch] = -5; // always moving forward
00216         // Just use simple rudder only for now.
00217         if ( !F.WayPointAchieved ) {
00218             NavCorr[Yaw] = -(RelHeading * NAV_YAW_LIMIT) / HALFPI;
00219             NavCorr[Yaw] = Limit1(NavCorr[Yaw], NAV_YAW_LIMIT); // gently!
00220         }
00221 
00222 #else // MULTICOPTER
00223 
00224         // revert to original simpler version from UAVP->UAVX transition
00225 
00226         RelHeadingSin = sin(RelHeading);
00227         RelHeadingCos = cos(RelHeading);
00228 
00229         NavE[Roll] = Radius * RelHeadingSin;
00230         NavE[Pitch] = -Radius * RelHeadingCos;
00231 
00232         // Roll & Pitch
00233 
00234         for ( a = 0; a < (uint8)2 ; a++ ) {
00235             NavP = Limit1(NavE[a], NAV_MAX_ROLL_PITCH);
00236 
00237             NavIntE[a] += NavE[a];
00238             NavIntE[a] = Limit1(NavIntE[a], K[NavIntLimit]);
00239             NavI = NavIntE[a] * K[NavKi] * GPSdT;
00240             NavIntE[a] = Decay1(NavIntE[a]);
00241 
00242             Diff = (NavEp[a] - NavE[a]);
00243             Diff = Limit1(Diff, 256);
00244             NavD = Diff * K[NavKd] * GPSdTR;
00245             NavD = Limit1(NavD, NAV_DIFF_LIMIT);
00246 
00247             NavEp[a] = NavE[a];
00248 
00249             NavCorr[a] = (NavP + NavI + NavD ) * NavSensitivity;
00250 
00251             NavCorr[a] = SlewLimit(NavCorrp[a], NavCorr[a], NAV_CORR_SLEW_LIMIT);
00252             NavCorr[a] = Limit1(NavCorr[a], NAV_MAX_ROLL_PITCH);
00253 
00254             NavCorrp[a] = NavCorr[a];
00255         }
00256 
00257         // Yaw
00258         if ( F.AllowTurnToWP && !F.WayPointAchieved ) {
00259             RelHeading = MakePi(WayHeading - Heading); // make +/- MilliPi
00260             NavCorr[Yaw] = -(RelHeading * NavYCorrLimit) / HALFPI;
00261             NavCorr[Yaw] = Limit1(NavCorr[Yaw], NavYCorrLimit); // gently!
00262         } else
00263             NavCorr[Yaw] = 0;
00264 
00265 #endif // NAV_WING    
00266     } else {
00267         // Neutral Zone - no GPS influence
00268         NavCorr[Pitch] = DecayX(NavCorr[Pitch], 2);
00269         NavCorr[Roll] = DecayX(NavCorr[Roll], 2);
00270         NavCorr[Yaw] = 0;
00271         NavIntE[Roll] = NavIntE[Pitch] = NavEp[Roll] = NavEp[Pitch] = 0;
00272     }
00273 
00274     F.NavComputed = true;
00275 
00276 } // Navigate
00277 
00278 void DoNavigation(void) {
00279 
00280     F.NavigationActive = F.GPSValid && F.CompassValid && ( mSClock() > mS[NavActiveTime]);
00281 
00282     if ( F.NavigationActive ) {
00283     
00284             F.LostModel = F.ForceFailsafe = false;
00285 
00286         if ( !F.NavComputed )
00287             switch ( NavState ) { // most case last - switches in C18 are IF chains not branch tables!
00288                 case Touchdown:
00289                     Navigate(OriginLatitude, OriginLongitude);
00290                     DoFailsafeLanding();
00291                     break;
00292                 case Descending:
00293                     Navigate( OriginLatitude, OriginLongitude );
00294                     if ( F.ReturnHome || F.Navigate )
00295 #ifdef NAV_WING
00296                     {
00297                         // needs more thought - runway direction?
00298                         DoFailsafeLanding();
00299                     }
00300 #else
00301                         if ( Altitude < LAND_M ) {
00302                             mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
00303                             NavState = Touchdown;
00304                         } else
00305                             DoFailsafeLanding();
00306 #endif // NAV_WING
00307                     else
00308                         AcquireHoldPosition();
00309                     break;
00310                 case AtHome:
00311                     Navigate(OriginLatitude, OriginLongitude);
00312                     SetDesiredAltitude((int16)P[NavRTHAlt]);
00313                     if ( F.ReturnHome || F.Navigate )
00314                         if ( F.WayPointAchieved ) { // check still @ Home
00315                             if ( F.UsingRTHAutoDescend && ( mSClock() > mS[NavStateTimeout] ) )
00316                                 NavState = Descending;
00317                         } else
00318                             NavState = ReturningHome;
00319                     else
00320                         AcquireHoldPosition();
00321                     break;
00322                 case ReturningHome:
00323                     Navigate(OriginLatitude, OriginLongitude);
00324                     SetDesiredAltitude((int16)P[NavRTHAlt]);
00325                     if ( F.ReturnHome || F.Navigate ) {
00326                         if ( F.WayPointAchieved ) {
00327                             mS[NavStateTimeout] = mSClock() + NavRTHTimeoutmS;
00328                             NavState = AtHome;
00329                         }
00330                     } else
00331                         AcquireHoldPosition();
00332                     break;
00333                 case Loitering:
00334                     Navigate(WPLatitude, WPLongitude);
00335                     SetDesiredAltitude(WPAltitude);
00336                     if ( F.Navigate ) {
00337                         if ( F.WayPointAchieved && (mSClock() > mS[NavStateTimeout]) )
00338                             if ( CurrWP == NoOfWayPoints ) {
00339                                 CurrWP = 1;
00340                                 NavState = ReturningHome;
00341                             } else {
00342                                 GetWayPointPX(++CurrWP);
00343                                 NavState = Navigating;
00344                             }
00345                     } else
00346                         AcquireHoldPosition();
00347                     break;
00348                 case Navigating:
00349                     Navigate(WPLatitude, WPLongitude);
00350                     SetDesiredAltitude(WPAltitude);
00351                     if ( F.Navigate ) {
00352                         if ( F.WayPointAchieved ) {
00353                             mS[NavStateTimeout] = mSClock() + WPLoiter;
00354                             NavState = Loitering;
00355                         }
00356                     } else
00357                         AcquireHoldPosition();
00358                     break;
00359                 case HoldingStation:
00360                     if ( F.AttitudeHold ) {
00361                         if ( F.AcquireNewPosition && !( F.Ch5Active & F.UsingPositionHoldLock ) ) {
00362                             F.AllowTurnToWP = SaveAllowTurnToWP;
00363                             AcquireHoldPosition();
00364                             #ifdef NAV_ACQUIRE_BEEPER
00365                             DoBeeperPulse1mS(500);
00366                             #endif // NAV_ACQUIRE_BEEPER
00367                         }
00368                     } else
00369                         F.AcquireNewPosition = true;
00370 
00371                     Navigate(HoldLatitude, HoldLongitude);
00372 
00373                     if ( F.NavValid && F.NearLevel )  // Origin must be valid for ANY navigation!
00374                         if ( F.Navigate ) {
00375                             GetWayPointPX(CurrWP); // resume from previous WP
00376                             SetDesiredAltitude(WPAltitude);
00377                             NavState = Navigating;
00378                         } else
00379                             if ( F.ReturnHome )
00380                                 NavState = ReturningHome;
00381                     break;
00382             } // switch NavState
00383     } else
00384         if ( F.ForceFailsafe && F.NewCommands )
00385         {
00386             F.AltHoldEnabled = F.AllowNavAltitudeHold = true;
00387             F.LostModel = true;
00388             DoFailsafeLanding();
00389         }
00390         else // kill nav correction immediately
00391              NavCorr[Pitch] = NavCorr[Roll] = NavCorr[Yaw] = 0; // zzz
00392 
00393     F.NewCommands = false;    // Navigate modifies Desired Roll, Pitch and Yaw values.
00394 
00395 } // DoNavigation
00396 
00397 void CheckFailsafeAbort(void) {
00398     if ( mSClock() > mS[AbortTimeout] ) {
00399         if ( F.Signal ) {
00400             LEDGreen_ON;
00401             mS[NavStateTimeout] = 0;
00402             mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS; // may be redundant?
00403             NavState = HoldingStation;
00404             FailState = MonitoringRx;
00405         }
00406     } else
00407         mS[AbortTimeout] += ABORT_UPDATE_MS;
00408 } // CheckFailsafeAbort
00409 
00410 void DoPPMFailsafe(void) { // only relevant to PPM Rx or Quad NOT synchronising with Rx
00411 
00412     if ( State == InFlight )
00413         switch ( FailState ) { // FailStates { MonitoringRx, Aborting, Terminating, Terminated }
00414             case Terminated: // Basic assumption is that aircraft is being flown over a safe area!
00415                 FailsafeHoldPosition();
00416                 DoFailsafeLanding();
00417                 break;
00418             case Terminating:
00419                 FailsafeHoldPosition();
00420                 if ( Altitude < LAND_M ) {
00421                     mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
00422                     NavState = Touchdown;
00423                     FailState = Terminated;
00424                 }
00425                 DoFailsafeLanding();
00426                 break;
00427             case Aborting:
00428                 FailsafeHoldPosition();
00429                 F.AltHoldEnabled = true;
00430                 SetDesiredAltitude((int16)P[NavRTHAlt]);
00431                 if ( mSClock() > mS[NavStateTimeout] ) {
00432                     F.LostModel = true;
00433                     LEDGreen_OFF;
00434                     LEDRed_ON;
00435 
00436                     mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
00437                     NavState = Descending;
00438                     FailState = Terminating;
00439                 } else
00440                     CheckFailsafeAbort();
00441                 break;
00442             case MonitoringRx:
00443                 if ( mSClock() > mS[FailsafeTimeout] ) {
00444                     // use last "good" throttle
00445                     Stats[RCFailsafesS]++;
00446                     if ( F.GPSValid && F.CompassValid )
00447                         mS[NavStateTimeout] = mSClock() + ABORT_TIMEOUT_GPS_MS;
00448                     else
00449                         mS[NavStateTimeout] = mSClock() + ABORT_TIMEOUT_NO_GPS_MS;
00450                     mS[AbortTimeout] = mSClock() + ABORT_UPDATE_MS;
00451                     FailState = Aborting;
00452                 }
00453                 break;
00454         } // Switch FailState
00455     else
00456         DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = 0;
00457 
00458 } // DoPPMFailsafe
00459 
00460 void UAVXNavCommand(void) {
00461 
00462     static int16 b;
00463     static uint8 c, d, csum;
00464 
00465     c = RxChar();
00466     LEDBlue_ON;
00467 
00468     switch ( c ) {
00469         case '0': // hello
00470             TxChar(ACK);
00471             break;
00472         case '1': // write
00473             csum = 0;
00474             for ( b = 0; b < 256; b++) { // cannot write fast enough so buffer
00475                 d = RxChar();
00476                 csum ^= d;
00477                 BufferPX[b] = d;
00478             }
00479             if ( csum == (uint8)0 ) {
00480                 for ( b = 0; b < 256; b++)
00481                     WritePX(NAV_ADDR_PX + b, BufferPX[b]);
00482                 TxChar(ACK);
00483             } else
00484                 TxChar(NAK);
00485 
00486             InitNavigation();
00487 
00488             break;
00489         case '2':
00490             csum = 0;
00491             for ( b = 0; b < 255; b++) {
00492                 d = ReadPX(NAV_ADDR_PX + b);
00493                 csum ^= d;
00494                 BufferPX[b] = d;
00495             }
00496             BufferPX[255] = csum;
00497             for ( b = 0; b < 256; b++)
00498                 TxChar(BufferPX[b]);
00499             TxChar(ACK);
00500             break;
00501         case '3':
00502             csum = 0;
00503             for ( b = 0; b < 63; b++) {
00504                 d = ReadPX(STATS_ADDR_PX + b);
00505                 csum ^= d;
00506                 BufferPX[b] = d;
00507             }
00508             BufferPX[63] = csum;
00509             for ( b = 0; b < 64; b++)
00510                 TxChar(BufferPX[b]);
00511             TxChar(ACK);
00512             break;
00513         default:
00514             break;
00515     } // switch
00516 
00517     WritePXImagefile();
00518     LEDBlue_OFF;
00519 
00520 } // UAVXNavCommand
00521 
00522 void GetWayPointPX(int8 wp) {
00523     static uint16 w;
00524 
00525     if ( wp > NoOfWayPoints )
00526         CurrWP = wp = 0;
00527     if ( wp == 0 ) { // force to Origin
00528         WPLatitude = OriginLatitude;
00529         WPLongitude = OriginLongitude;
00530         WPAltitude = (int16)P[NavRTHAlt];
00531         WPLoiter = 30000; // mS
00532     } else {
00533         w = NAV_WP_START + (wp-1) * WAYPOINT_REC_SIZE;
00534         WPLatitude = Read32PX(w + 0);
00535         WPLongitude = Read32PX(w + 4);
00536         WPAltitude = Read16PX(w + 8);
00537 
00538 #ifdef NAV_ENFORCE_ALTITUDE_CEILING
00539         if ( WPAltitude > NAV_CEILING )
00540             WPAltitude = NAV_CEILING;
00541 #endif // NAV_ENFORCE_ALTITUDE_CEILING
00542         WPLoiter = (int16)ReadPX(w + 10) * 1000L; // mS
00543     }
00544 
00545     F.WayPointCentred =  F.WayPointAchieved = false;
00546 
00547 } // GetWaypointPX
00548 
00549 void InitNavigation(void) {
00550     static uint8 i;
00551 
00552     HoldLatitude = HoldLongitude = WayHeading = 0;
00553 
00554     for ( i = 0; i < (uint8)3; i++ )
00555         NavEp[i] = NavIntE[i] = NavCorr[i] = NavCorrp[i] = 0;
00556 
00557     NavState = HoldingStation;
00558     AttitudeHoldResetCount = 0;
00559     CurrMaxRollPitch = 0;
00560     F.WayPointAchieved = F.WayPointCentred = false;
00561     F.NavComputed = false;
00562 
00563     if ( ReadPX(NAV_NO_WP) <= 0 ) {
00564         NavProximityRadius = ConvertMToGPS(NAV_PROXIMITY_RADIUS);
00565         NavProximityAltitude = NAV_PROXIMITY_ALTITUDE * 10L; // Decimetres
00566     } else {
00567         // need minimum values in UAVXNav?
00568         NavProximityRadius = ConvertMToGPS(ReadPX(NAV_PROX_RADIUS));
00569         NavProximityAltitude = ReadPX(NAV_PROX_ALT) * 10L; // Decimetres
00570     }
00571 
00572     NoOfWayPoints = ReadPX(NAV_NO_WP);
00573 
00574     if ( NoOfWayPoints <= 0 )
00575         CurrWP = 0;
00576     else
00577         CurrWP = 1;
00578     GetWayPointPX(0);
00579 
00580 } // InitNavigation
00581 
00582