Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
1:1e3318a30ddd 
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 volatile Flags F;
gke 2:90292f8bd179 24 int8 r; // global I2C
gke 0:62a1c91a859a 25
gke 0:62a1c91a859a 26 int main(void) {
gke 0:62a1c91a859a 27
gke 0:62a1c91a859a 28 InitMisc();
gke 2:90292f8bd179 29 InitI2C();
gke 0:62a1c91a859a 30 InitHarness();
gke 1:1e3318a30ddd 31
gke 0:62a1c91a859a 32 InitRC();
gke 0:62a1c91a859a 33 InitTimersAndInterrupts();
gke 0:62a1c91a859a 34 InitLEDs();
gke 2:90292f8bd179 35
gke 2:90292f8bd179 36 /*
gke 2:90292f8bd179 37 Delay1mS(500);
gke 2:90292f8bd179 38 InitADXL345Acc();
gke 2:90292f8bd179 39 MCP4725_ID_Actual = FORCE_BARO_ID;
gke 2:90292f8bd179 40 while (1) {
gke 2:90292f8bd179 41 DebugPin = 1;
gke 2:90292f8bd179 42 SetFreescaleMCP4725(1);
gke 2:90292f8bd179 43 DebugPin = 0;
gke 2:90292f8bd179 44 Delay1mS(1);
gke 2:90292f8bd179 45 ReadADXL345Acc();
gke 2:90292f8bd179 46 }
gke 2:90292f8bd179 47 */
gke 0:62a1c91a859a 48 InitParameters();
gke 0:62a1c91a859a 49 ReadStatsPX();
gke 0:62a1c91a859a 50 InitMotors();
gke 0:62a1c91a859a 51 InitBattery();
gke 0:62a1c91a859a 52
gke 0:62a1c91a859a 53 LEDYellow_ON;
gke 0:62a1c91a859a 54 InitAccelerometers();
gke 0:62a1c91a859a 55 InitGyros();
gke 0:62a1c91a859a 56 InitIRSensors();
gke 2:90292f8bd179 57
gke 0:62a1c91a859a 58 InitCompass();
gke 0:62a1c91a859a 59 InitRangefinder();
gke 0:62a1c91a859a 60
gke 0:62a1c91a859a 61 InitGPS();
gke 0:62a1c91a859a 62 InitNavigation();
gke 0:62a1c91a859a 63
gke 0:62a1c91a859a 64 InitTemperature();
gke 0:62a1c91a859a 65 InitBarometer();
gke 0:62a1c91a859a 66
gke 0:62a1c91a859a 67 ShowSetup(true);
gke 1:1e3318a30ddd 68
gke 0:62a1c91a859a 69 I2C0.frequency(MinI2CRate);
gke 0:62a1c91a859a 70
gke 0:62a1c91a859a 71 FirstPass = true;
gke 0:62a1c91a859a 72
gke 0:62a1c91a859a 73 while ( true ) {
gke 0:62a1c91a859a 74 StopMotors();
gke 0:62a1c91a859a 75
gke 0:62a1c91a859a 76 LightsAndSirens(); // Check for Rx signal, disarmed on power up, throttle closed, gyros ONLINE
gke 2:90292f8bd179 77
gke 1:1e3318a30ddd 78 GetAttitude();
gke 1:1e3318a30ddd 79 MixAndLimitCam();
gke 0:62a1c91a859a 80
gke 0:62a1c91a859a 81 State = Starting;
gke 0:62a1c91a859a 82 F.FirstArmed = false;
gke 0:62a1c91a859a 83
gke 0:62a1c91a859a 84 while ( Armed.read() ) { // no command processing while the Quadrocopter is armed
gke 0:62a1c91a859a 85
gke 0:62a1c91a859a 86 UpdateGPS();
gke 0:62a1c91a859a 87 if ( F.RCNewValues )
gke 0:62a1c91a859a 88 UpdateControls();
gke 0:62a1c91a859a 89
gke 0:62a1c91a859a 90 if ( ( F.Signal ) && ( FailState == MonitoringRx ) ) {
gke 0:62a1c91a859a 91 switch ( State ) {
gke 0:62a1c91a859a 92 case Starting: // this state executed once only after arming
gke 0:62a1c91a859a 93
gke 0:62a1c91a859a 94 LEDYellow_OFF;
gke 0:62a1c91a859a 95
gke 0:62a1c91a859a 96 CreateLogfile();
gke 0:62a1c91a859a 97
gke 0:62a1c91a859a 98 if ( !F.FirstArmed ) {
gke 0:62a1c91a859a 99 mS[StartTime] = mSClock();
gke 0:62a1c91a859a 100 F.FirstArmed = true;
gke 0:62a1c91a859a 101 }
gke 0:62a1c91a859a 102
gke 0:62a1c91a859a 103 InitControl();
gke 2:90292f8bd179 104 DesiredHeading = Heading;
gke 2:90292f8bd179 105 Angle[Yaw] = 0.0;
gke 0:62a1c91a859a 106 CaptureTrims();
gke 0:62a1c91a859a 107 InitGPS();
gke 0:62a1c91a859a 108 InitNavigation();
gke 0:62a1c91a859a 109
gke 0:62a1c91a859a 110 DesiredThrottle = 0;
gke 0:62a1c91a859a 111 ErectGyros(); // DO NOT MOVE AIRCRAFT!
gke 2:90292f8bd179 112 InitAttitude();
gke 0:62a1c91a859a 113 ZeroStats();
gke 0:62a1c91a859a 114 DoStartingBeeps(3);
gke 2:90292f8bd179 115
gke 1:1e3318a30ddd 116 SendParameters(ParamSet);
gke 0:62a1c91a859a 117
gke 0:62a1c91a859a 118 mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS;
gke 0:62a1c91a859a 119 mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS;
gke 2:90292f8bd179 120 ControlUpdateTimeuS = uSClock();
gke 0:62a1c91a859a 121 F.ForceFailsafe = F.LostModel = false;
gke 0:62a1c91a859a 122
gke 0:62a1c91a859a 123 State = Landed;
gke 0:62a1c91a859a 124 break;
gke 0:62a1c91a859a 125 case Landed:
gke 0:62a1c91a859a 126 DesiredThrottle = 0;
gke 2:90292f8bd179 127 DesiredHeading = Heading;
gke 2:90292f8bd179 128 Angle[Yaw] = 0.0;
gke 0:62a1c91a859a 129 if ( mSClock() > mS[ArmedTimeout] )
gke 0:62a1c91a859a 130 DoShutdown();
gke 0:62a1c91a859a 131 else
gke 0:62a1c91a859a 132 if ( StickThrottle < IdleThrottle ) {
gke 0:62a1c91a859a 133 SetGPSOrigin();
gke 0:62a1c91a859a 134 GetHeading();
gke 0:62a1c91a859a 135 if ( F.NewCommands )
gke 0:62a1c91a859a 136 F.LostModel = F.ForceFailsafe;
gke 0:62a1c91a859a 137 } else {
gke 0:62a1c91a859a 138 #ifdef SIMULATE
gke 0:62a1c91a859a 139 FakeBaroRelAltitude = 0;
gke 0:62a1c91a859a 140 #endif // SIMULATE
gke 0:62a1c91a859a 141 LEDPattern = 0;
gke 0:62a1c91a859a 142 mS[NavActiveTime] = mSClock() + NAV_ACTIVE_DELAY_MS;
gke 0:62a1c91a859a 143 Stats[RCGlitchesS] = RCGlitches; // start of flight
gke 0:62a1c91a859a 144 SaveLEDs();
gke 0:62a1c91a859a 145 if ( ParameterSanityCheck() )
gke 0:62a1c91a859a 146 State = InFlight;
gke 0:62a1c91a859a 147 else
gke 0:62a1c91a859a 148 ALL_LEDS_ON;
gke 0:62a1c91a859a 149 }
gke 0:62a1c91a859a 150 break;
gke 0:62a1c91a859a 151 case Landing:
gke 2:90292f8bd179 152 DesiredHeading = Heading;
gke 2:90292f8bd179 153 Angle[Yaw] = 0.0;
gke 0:62a1c91a859a 154 if ( StickThrottle > IdleThrottle ) {
gke 0:62a1c91a859a 155 DesiredThrottle = 0;
gke 0:62a1c91a859a 156 State = InFlight;
gke 0:62a1c91a859a 157 } else
gke 0:62a1c91a859a 158 if ( mSClock() < mS[ThrottleIdleTimeout] )
gke 0:62a1c91a859a 159 DesiredThrottle = IdleThrottle;
gke 0:62a1c91a859a 160 else {
gke 0:62a1c91a859a 161 DesiredThrottle = 0; // to catch cycles between Rx updates
gke 0:62a1c91a859a 162 F.MotorsArmed = false;
gke 0:62a1c91a859a 163 Stats[RCGlitchesS] = RCGlitches - Stats[RCGlitchesS];
gke 0:62a1c91a859a 164 WriteStatsPX();
gke 0:62a1c91a859a 165 WritePXImagefile();
gke 0:62a1c91a859a 166 mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS;
gke 0:62a1c91a859a 167 State = Landed;
gke 0:62a1c91a859a 168 }
gke 0:62a1c91a859a 169 break;
gke 0:62a1c91a859a 170 case Shutdown:
gke 0:62a1c91a859a 171 // wait until arming switch is cycled
gke 0:62a1c91a859a 172 F.LostModel = true;
gke 0:62a1c91a859a 173 DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = 0;
gke 0:62a1c91a859a 174 StopMotors();
gke 0:62a1c91a859a 175 break;
gke 0:62a1c91a859a 176 case InFlight:
gke 0:62a1c91a859a 177 F.MotorsArmed = true;
gke 0:62a1c91a859a 178 DoNavigation();
gke 0:62a1c91a859a 179 LEDChaser();
gke 0:62a1c91a859a 180
gke 0:62a1c91a859a 181 DesiredThrottle = SlewLimit(DesiredThrottle, StickThrottle, 1);
gke 0:62a1c91a859a 182
gke 0:62a1c91a859a 183 if ( StickThrottle < IdleThrottle ) {
gke 0:62a1c91a859a 184 mS[ThrottleIdleTimeout] = mSClock() + THROTTLE_LOW_DELAY_MS;
gke 0:62a1c91a859a 185 RestoreLEDs();
gke 0:62a1c91a859a 186 State = Landing;
gke 0:62a1c91a859a 187 }
gke 0:62a1c91a859a 188 break;
gke 0:62a1c91a859a 189 } // Switch State
gke 0:62a1c91a859a 190 mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS;
gke 0:62a1c91a859a 191 FailState = MonitoringRx;
gke 0:62a1c91a859a 192 } else
gke 0:62a1c91a859a 193 DoPPMFailsafe();
gke 0:62a1c91a859a 194
gke 2:90292f8bd179 195 while ( uSClock() < ControlUpdateTimeuS ) {}; // CAUTION: uS clock wraps at about an hour
gke 2:90292f8bd179 196 ControlUpdateTimeuS = uSClock() + PID_CYCLE_US;
gke 2:90292f8bd179 197
gke 0:62a1c91a859a 198 DoControl();
gke 0:62a1c91a859a 199
gke 0:62a1c91a859a 200 MixAndLimitMotors();
gke 1:1e3318a30ddd 201 OutSignals();
gke 2:90292f8bd179 202
gke 0:62a1c91a859a 203 MixAndLimitCam();
gke 2:90292f8bd179 204
gke 2:90292f8bd179 205 GetTemperature();
gke 0:62a1c91a859a 206 GetBattery();
gke 0:62a1c91a859a 207 CheckAlarms();
gke 2:90292f8bd179 208
gke 0:62a1c91a859a 209 CheckTelemetry();
gke 0:62a1c91a859a 210
gke 0:62a1c91a859a 211 SensorTrace();
gke 0:62a1c91a859a 212
gke 0:62a1c91a859a 213 } // flight while armed
gke 0:62a1c91a859a 214 }
gke 0:62a1c91a859a 215 } // main
gke 0:62a1c91a859a 216