Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
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?
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 | 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 |