Diff: UAVXArm.c

Revision:

2:90292f8bd179

Parent:

1:1e3318a30ddd

--- 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();