UAVXArm.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 volatile Flags     F;
00024 int8 r; // global I2C
00025 
00026 int main(void) {
00027 
00028     InitMisc();
00029     InitI2C();
00030     InitHarness();
00031 
00032     InitRC();
00033     InitTimersAndInterrupts();
00034     InitLEDs();
00035 
00036     /*
00037     Delay1mS(500);
00038             InitADXL345Acc();
00039                 MCP4725_ID_Actual = FORCE_BARO_ID;
00040         while (1) {
00041                DebugPin = 1;
00042            SetFreescaleMCP4725(1);
00043            DebugPin = 0;
00044             Delay1mS(1);
00045             ReadADXL345Acc();
00046         }
00047     */
00048     InitParameters();
00049     ReadStatsPX();
00050     InitMotors();
00051     InitBattery();
00052 
00053     LEDYellow_ON;
00054     InitAccelerometers();
00055     InitGyros();
00056     InitIRSensors();
00057 
00058     InitCompass();
00059     InitRangefinder();
00060 
00061     InitGPS();
00062     InitNavigation();
00063 
00064     InitTemperature();
00065     InitBarometer();
00066 
00067     ShowSetup(true);
00068 
00069     I2C0.frequency(MinI2CRate);
00070 
00071     FirstPass = true;
00072 
00073     while ( true ) {
00074         StopMotors();
00075 
00076         LightsAndSirens();    // Check for Rx signal, disarmed on power up, throttle closed, gyros ONLINE
00077 
00078         GetAttitude();
00079         MixAndLimitCam();
00080 
00081         State = Starting;
00082         F.FirstArmed = false;
00083 
00084         while ( Armed.read() ) { // no command processing while the Quadrocopter is armed
00085 
00086             UpdateGPS();
00087             if ( F.RCNewValues )
00088                 UpdateControls();
00089 
00090             if ( ( F.Signal ) && ( FailState == MonitoringRx ) ) {
00091                 switch ( State  ) {
00092                     case Starting:    // this state executed once only after arming
00093 
00094                         LEDYellow_OFF;
00095 
00096                         CreateLogfile();
00097 
00098                         if ( !F.FirstArmed ) {
00099                             mS[StartTime] = mSClock();
00100                             F.FirstArmed = true;
00101                         }
00102 
00103                         InitControl();
00104                         DesiredHeading = Heading;
00105                         Angle[Yaw] = 0.0;
00106                         CaptureTrims();
00107                         InitGPS();
00108                         InitNavigation();
00109 
00110                         DesiredThrottle = 0;
00111                         ErectGyros(); // DO NOT MOVE AIRCRAFT!
00112                         InitAttitude();
00113                         ZeroStats();
00114                         DoStartingBeeps(3);
00115 
00116                         SendParameters(ParamSet);
00117 
00118                         mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS;
00119                         mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS;
00120                         ControlUpdateTimeuS = uSClock();
00121                         F.ForceFailsafe = F.LostModel = false;
00122 
00123                         State = Landed;
00124                         break;
00125                     case Landed:
00126                         DesiredThrottle = 0;
00127                         DesiredHeading = Heading;
00128                         Angle[Yaw] = 0.0;
00129                         if ( mSClock() > mS[ArmedTimeout] )
00130                             DoShutdown();
00131                         else
00132                             if ( StickThrottle < IdleThrottle ) {
00133                                 SetGPSOrigin();
00134                                 GetHeading();
00135                                 if ( F.NewCommands )
00136                                     F.LostModel = F.ForceFailsafe;
00137                             } else {
00138 #ifdef SIMULATE
00139                                 FakeBaroRelAltitude = 0;
00140 #endif // SIMULATE                        
00141                                 LEDPattern = 0;
00142                                 mS[NavActiveTime] = mSClock() + NAV_ACTIVE_DELAY_MS;
00143                                 Stats[RCGlitchesS] = RCGlitches; // start of flight
00144                                 SaveLEDs();
00145                                 if ( ParameterSanityCheck() )
00146                                     State = InFlight;
00147                                 else
00148                                     ALL_LEDS_ON;
00149                             }
00150                         break;
00151                     case Landing:
00152                         DesiredHeading = Heading;
00153                         Angle[Yaw] = 0.0;
00154                         if ( StickThrottle > IdleThrottle ) {
00155                             DesiredThrottle = 0;
00156                             State = InFlight;
00157                         } else
00158                             if ( mSClock() < mS[ThrottleIdleTimeout] )
00159                                 DesiredThrottle = IdleThrottle;
00160                             else {
00161                                 DesiredThrottle = 0; // to catch cycles between Rx updates
00162                                 F.MotorsArmed = false;
00163                                 Stats[RCGlitchesS] = RCGlitches - Stats[RCGlitchesS];
00164                                 WriteStatsPX();
00165                                 WritePXImagefile();
00166                                 mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS;
00167                                 State = Landed;
00168                             }
00169                         break;
00170                     case Shutdown:
00171                         // wait until arming switch is cycled
00172                         F.LostModel = true;
00173                         DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = 0;
00174                         StopMotors();
00175                         break;
00176                     case InFlight:
00177                         F.MotorsArmed = true;
00178                         DoNavigation();
00179                         LEDChaser();
00180 
00181                         DesiredThrottle = SlewLimit(DesiredThrottle, StickThrottle, 1);
00182 
00183                         if ( StickThrottle < IdleThrottle ) {
00184                             mS[ThrottleIdleTimeout] = mSClock() + THROTTLE_LOW_DELAY_MS;
00185                             RestoreLEDs();
00186                             State = Landing;
00187                         }
00188                         break;
00189                 } // Switch State
00190                 mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS;
00191                 FailState = MonitoringRx;
00192             } else
00193                 DoPPMFailsafe();
00194 
00195             while ( uSClock() < ControlUpdateTimeuS ) {}; // CAUTION: uS clock wraps at about an hour
00196             ControlUpdateTimeuS = uSClock() + PID_CYCLE_US;
00197 
00198             DoControl();
00199 
00200             MixAndLimitMotors();
00201             OutSignals();
00202 
00203             MixAndLimitCam();
00204 
00205             GetTemperature();
00206             GetBattery();
00207             CheckAlarms();
00208 
00209             CheckTelemetry();
00210 
00211             SensorTrace();
00212 
00213         } // flight while armed
00214     }
00215 } // main
00216