Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
UAVXArm.c@1:1e3318a30ddd, 2011-02-25 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 25 01:35:24 2011 +0000
- Revision:
- 1:1e3318a30ddd
- Parent:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.
Who changed what in which revision?
User | Revision | Line number | New 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 | volatile Flags F; |
gke | 0:62a1c91a859a | 24 | |
gke | 0:62a1c91a859a | 25 | int main(void) { |
gke | 0:62a1c91a859a | 26 | |
gke | 0:62a1c91a859a | 27 | InitMisc(); |
gke | 0:62a1c91a859a | 28 | InitHarness(); |
gke | 1:1e3318a30ddd | 29 | |
gke | 0:62a1c91a859a | 30 | InitRC(); |
gke | 0:62a1c91a859a | 31 | InitTimersAndInterrupts(); |
gke | 0:62a1c91a859a | 32 | InitLEDs(); |
gke | 1:1e3318a30ddd | 33 | |
gke | 0:62a1c91a859a | 34 | InitParameters(); |
gke | 0:62a1c91a859a | 35 | ReadStatsPX(); |
gke | 0:62a1c91a859a | 36 | InitMotors(); |
gke | 0:62a1c91a859a | 37 | InitBattery(); |
gke | 0:62a1c91a859a | 38 | |
gke | 0:62a1c91a859a | 39 | LEDYellow_ON; |
gke | 0:62a1c91a859a | 40 | InitAccelerometers(); |
gke | 0:62a1c91a859a | 41 | InitGyros(); |
gke | 0:62a1c91a859a | 42 | InitIRSensors(); |
gke | 1:1e3318a30ddd | 43 | |
gke | 0:62a1c91a859a | 44 | InitCompass(); |
gke | 0:62a1c91a859a | 45 | InitRangefinder(); |
gke | 0:62a1c91a859a | 46 | |
gke | 0:62a1c91a859a | 47 | InitGPS(); |
gke | 0:62a1c91a859a | 48 | InitNavigation(); |
gke | 0:62a1c91a859a | 49 | |
gke | 0:62a1c91a859a | 50 | InitTemperature(); |
gke | 0:62a1c91a859a | 51 | InitBarometer(); |
gke | 0:62a1c91a859a | 52 | |
gke | 0:62a1c91a859a | 53 | ShowSetup(true); |
gke | 1:1e3318a30ddd | 54 | |
gke | 0:62a1c91a859a | 55 | I2C0.frequency(MinI2CRate); |
gke | 0:62a1c91a859a | 56 | |
gke | 0:62a1c91a859a | 57 | FirstPass = true; |
gke | 0:62a1c91a859a | 58 | |
gke | 0:62a1c91a859a | 59 | while ( true ) { |
gke | 0:62a1c91a859a | 60 | StopMotors(); |
gke | 0:62a1c91a859a | 61 | |
gke | 0:62a1c91a859a | 62 | LightsAndSirens(); // Check for Rx signal, disarmed on power up, throttle closed, gyros ONLINE |
gke | 1:1e3318a30ddd | 63 | |
gke | 1:1e3318a30ddd | 64 | GetAttitude(); |
gke | 1:1e3318a30ddd | 65 | MixAndLimitCam(); |
gke | 0:62a1c91a859a | 66 | |
gke | 0:62a1c91a859a | 67 | State = Starting; |
gke | 0:62a1c91a859a | 68 | F.FirstArmed = false; |
gke | 0:62a1c91a859a | 69 | |
gke | 0:62a1c91a859a | 70 | while ( Armed.read() ) { // no command processing while the Quadrocopter is armed |
gke | 0:62a1c91a859a | 71 | |
gke | 0:62a1c91a859a | 72 | UpdateGPS(); |
gke | 0:62a1c91a859a | 73 | if ( F.RCNewValues ) |
gke | 0:62a1c91a859a | 74 | UpdateControls(); |
gke | 0:62a1c91a859a | 75 | |
gke | 0:62a1c91a859a | 76 | if ( ( F.Signal ) && ( FailState == MonitoringRx ) ) { |
gke | 0:62a1c91a859a | 77 | switch ( State ) { |
gke | 0:62a1c91a859a | 78 | case Starting: // this state executed once only after arming |
gke | 0:62a1c91a859a | 79 | |
gke | 0:62a1c91a859a | 80 | LEDYellow_OFF; |
gke | 0:62a1c91a859a | 81 | |
gke | 0:62a1c91a859a | 82 | CreateLogfile(); |
gke | 0:62a1c91a859a | 83 | |
gke | 0:62a1c91a859a | 84 | if ( !F.FirstArmed ) { |
gke | 0:62a1c91a859a | 85 | mS[StartTime] = mSClock(); |
gke | 0:62a1c91a859a | 86 | F.FirstArmed = true; |
gke | 0:62a1c91a859a | 87 | } |
gke | 0:62a1c91a859a | 88 | |
gke | 0:62a1c91a859a | 89 | InitControl(); |
gke | 0:62a1c91a859a | 90 | CaptureTrims(); |
gke | 0:62a1c91a859a | 91 | InitGPS(); |
gke | 0:62a1c91a859a | 92 | InitNavigation(); |
gke | 0:62a1c91a859a | 93 | |
gke | 0:62a1c91a859a | 94 | DesiredThrottle = 0; |
gke | 0:62a1c91a859a | 95 | ErectGyros(); // DO NOT MOVE AIRCRAFT! |
gke | 0:62a1c91a859a | 96 | ZeroStats(); |
gke | 0:62a1c91a859a | 97 | DoStartingBeeps(3); |
gke | 0:62a1c91a859a | 98 | |
gke | 1:1e3318a30ddd | 99 | SendParameters(ParamSet); |
gke | 0:62a1c91a859a | 100 | |
gke | 0:62a1c91a859a | 101 | mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 102 | mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 103 | F.ForceFailsafe = F.LostModel = false; |
gke | 0:62a1c91a859a | 104 | |
gke | 0:62a1c91a859a | 105 | State = Landed; |
gke | 0:62a1c91a859a | 106 | break; |
gke | 0:62a1c91a859a | 107 | case Landed: |
gke | 0:62a1c91a859a | 108 | DesiredThrottle = 0; |
gke | 0:62a1c91a859a | 109 | if ( mSClock() > mS[ArmedTimeout] ) |
gke | 0:62a1c91a859a | 110 | DoShutdown(); |
gke | 0:62a1c91a859a | 111 | else |
gke | 0:62a1c91a859a | 112 | if ( StickThrottle < IdleThrottle ) { |
gke | 0:62a1c91a859a | 113 | SetGPSOrigin(); |
gke | 0:62a1c91a859a | 114 | GetHeading(); |
gke | 0:62a1c91a859a | 115 | if ( F.NewCommands ) |
gke | 0:62a1c91a859a | 116 | F.LostModel = F.ForceFailsafe; |
gke | 0:62a1c91a859a | 117 | } else { |
gke | 0:62a1c91a859a | 118 | #ifdef SIMULATE |
gke | 0:62a1c91a859a | 119 | FakeBaroRelAltitude = 0; |
gke | 0:62a1c91a859a | 120 | #endif // SIMULATE |
gke | 0:62a1c91a859a | 121 | LEDPattern = 0; |
gke | 0:62a1c91a859a | 122 | mS[NavActiveTime] = mSClock() + NAV_ACTIVE_DELAY_MS; |
gke | 0:62a1c91a859a | 123 | Stats[RCGlitchesS] = RCGlitches; // start of flight |
gke | 0:62a1c91a859a | 124 | SaveLEDs(); |
gke | 0:62a1c91a859a | 125 | if ( ParameterSanityCheck() ) |
gke | 0:62a1c91a859a | 126 | State = InFlight; |
gke | 0:62a1c91a859a | 127 | else |
gke | 0:62a1c91a859a | 128 | ALL_LEDS_ON; |
gke | 0:62a1c91a859a | 129 | } |
gke | 0:62a1c91a859a | 130 | break; |
gke | 0:62a1c91a859a | 131 | case Landing: |
gke | 0:62a1c91a859a | 132 | if ( StickThrottle > IdleThrottle ) { |
gke | 0:62a1c91a859a | 133 | DesiredThrottle = 0; |
gke | 0:62a1c91a859a | 134 | State = InFlight; |
gke | 0:62a1c91a859a | 135 | } else |
gke | 0:62a1c91a859a | 136 | if ( mSClock() < mS[ThrottleIdleTimeout] ) |
gke | 0:62a1c91a859a | 137 | DesiredThrottle = IdleThrottle; |
gke | 0:62a1c91a859a | 138 | else { |
gke | 0:62a1c91a859a | 139 | DesiredThrottle = 0; // to catch cycles between Rx updates |
gke | 0:62a1c91a859a | 140 | F.MotorsArmed = false; |
gke | 0:62a1c91a859a | 141 | Stats[RCGlitchesS] = RCGlitches - Stats[RCGlitchesS]; |
gke | 0:62a1c91a859a | 142 | WriteStatsPX(); |
gke | 0:62a1c91a859a | 143 | WritePXImagefile(); |
gke | 0:62a1c91a859a | 144 | mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 145 | State = Landed; |
gke | 0:62a1c91a859a | 146 | } |
gke | 0:62a1c91a859a | 147 | break; |
gke | 0:62a1c91a859a | 148 | case Shutdown: |
gke | 0:62a1c91a859a | 149 | // wait until arming switch is cycled |
gke | 0:62a1c91a859a | 150 | F.LostModel = true; |
gke | 0:62a1c91a859a | 151 | DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = 0; |
gke | 0:62a1c91a859a | 152 | StopMotors(); |
gke | 0:62a1c91a859a | 153 | break; |
gke | 0:62a1c91a859a | 154 | case InFlight: |
gke | 0:62a1c91a859a | 155 | F.MotorsArmed = true; |
gke | 0:62a1c91a859a | 156 | DoNavigation(); |
gke | 0:62a1c91a859a | 157 | LEDChaser(); |
gke | 0:62a1c91a859a | 158 | |
gke | 0:62a1c91a859a | 159 | DesiredThrottle = SlewLimit(DesiredThrottle, StickThrottle, 1); |
gke | 0:62a1c91a859a | 160 | |
gke | 0:62a1c91a859a | 161 | if ( StickThrottle < IdleThrottle ) { |
gke | 0:62a1c91a859a | 162 | mS[ThrottleIdleTimeout] = mSClock() + THROTTLE_LOW_DELAY_MS; |
gke | 0:62a1c91a859a | 163 | RestoreLEDs(); |
gke | 0:62a1c91a859a | 164 | State = Landing; |
gke | 0:62a1c91a859a | 165 | } |
gke | 0:62a1c91a859a | 166 | break; |
gke | 0:62a1c91a859a | 167 | } // Switch State |
gke | 0:62a1c91a859a | 168 | mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 169 | FailState = MonitoringRx; |
gke | 0:62a1c91a859a | 170 | } else |
gke | 0:62a1c91a859a | 171 | DoPPMFailsafe(); |
gke | 0:62a1c91a859a | 172 | |
gke | 0:62a1c91a859a | 173 | DoControl(); |
gke | 0:62a1c91a859a | 174 | |
gke | 0:62a1c91a859a | 175 | MixAndLimitMotors(); |
gke | 1:1e3318a30ddd | 176 | OutSignals(); |
gke | 1:1e3318a30ddd | 177 | |
gke | 0:62a1c91a859a | 178 | MixAndLimitCam(); |
gke | 1:1e3318a30ddd | 179 | |
gke | 1:1e3318a30ddd | 180 | GetTemperature(); |
gke | 0:62a1c91a859a | 181 | GetBattery(); |
gke | 0:62a1c91a859a | 182 | CheckAlarms(); |
gke | 0:62a1c91a859a | 183 | CheckTelemetry(); |
gke | 0:62a1c91a859a | 184 | |
gke | 0:62a1c91a859a | 185 | SensorTrace(); |
gke | 0:62a1c91a859a | 186 | |
gke | 0:62a1c91a859a | 187 | } // flight while armed |
gke | 0:62a1c91a859a | 188 | } |
gke | 0:62a1c91a859a | 189 | } // main |
gke | 0:62a1c91a859a | 190 |