Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
Diff: UAVXArm.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
diff -r 1e3318a30ddd -r 90292f8bd179 UAVXArm.c --- a/UAVXArm.c Fri Feb 25 01:35:24 2011 +0000 +++ b/UAVXArm.c Tue Apr 26 12:12:29 2011 +0000 @@ -2,7 +2,7 @@ // = UAVXArm Quadrocopter Controller = // = Copyright (c) 2008 by Prof. Greg Egan = // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = -// = http://code.google.com/p/uavp-mods/ http://uavp.ch = +// = http://code.google.com/p/uavp-mods/ = // =============================================================================================== // This is part of UAVXArm. @@ -21,16 +21,30 @@ #include "UAVXArm.h" volatile Flags F; +int8 r; // global I2C int main(void) { InitMisc(); + InitI2C(); InitHarness(); InitRC(); InitTimersAndInterrupts(); InitLEDs(); - + + /* + Delay1mS(500); + InitADXL345Acc(); + MCP4725_ID_Actual = FORCE_BARO_ID; + while (1) { + DebugPin = 1; + SetFreescaleMCP4725(1); + DebugPin = 0; + Delay1mS(1); + ReadADXL345Acc(); + } + */ InitParameters(); ReadStatsPX(); InitMotors(); @@ -40,7 +54,7 @@ InitAccelerometers(); InitGyros(); InitIRSensors(); - + InitCompass(); InitRangefinder(); @@ -60,7 +74,7 @@ StopMotors(); LightsAndSirens(); // Check for Rx signal, disarmed on power up, throttle closed, gyros ONLINE - + GetAttitude(); MixAndLimitCam(); @@ -87,25 +101,31 @@ } InitControl(); + DesiredHeading = Heading; + Angle[Yaw] = 0.0; CaptureTrims(); InitGPS(); InitNavigation(); DesiredThrottle = 0; ErectGyros(); // DO NOT MOVE AIRCRAFT! + InitAttitude(); ZeroStats(); DoStartingBeeps(3); - + SendParameters(ParamSet); mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS; mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS; + ControlUpdateTimeuS = uSClock(); F.ForceFailsafe = F.LostModel = false; State = Landed; break; case Landed: DesiredThrottle = 0; + DesiredHeading = Heading; + Angle[Yaw] = 0.0; if ( mSClock() > mS[ArmedTimeout] ) DoShutdown(); else @@ -129,6 +149,8 @@ } break; case Landing: + DesiredHeading = Heading; + Angle[Yaw] = 0.0; if ( StickThrottle > IdleThrottle ) { DesiredThrottle = 0; State = InFlight; @@ -170,16 +192,20 @@ } else DoPPMFailsafe(); + while ( uSClock() < ControlUpdateTimeuS ) {}; // CAUTION: uS clock wraps at about an hour + ControlUpdateTimeuS = uSClock() + PID_CYCLE_US; + DoControl(); MixAndLimitMotors(); OutSignals(); - + MixAndLimitCam(); - - GetTemperature(); + + GetTemperature(); GetBattery(); CheckAlarms(); + CheckTelemetry(); SensorTrace();