Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: UAVXArm.h
- Revision:
- 0:62a1c91a859a
- Child:
- 1:1e3318a30ddd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UAVXArm.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,1683 @@
+
+// Commissioning defines
+
+#define I2C_MAX_RATE_HZ 400000
+
+#define PWM_UPDATE_HZ 200 // MUST BE LESS THAN OR EAUAL TO 450HZ
+
+#define MCP4725_ID 0xc8 // or 0xcc
+
+#define SUPPRESS_SDCARD // no logging to check if buffering backup is an issue
+
+//BMP occasional returns bad results - changes outside this rate are deemed sensor/buss errors
+#define BARO_SANITY_CHECK_MPS 10.0 // dm/S 20,40,60,80 or 100
+
+#define SIX_DOF // effects acc and gyro addresses
+
+#define SOFTWARE_CAM_PWM
+
+#define BATTERY_VOLTS_SCALE 57.85 // 51.8144 // Volts scaling for internal divider
+
+//#define DCM_YAW_COMP
+//#define DISABLE_EXTRAS
+
+#define USING_MBED
+
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// This is part of UAVXArm.
+
+// UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
+// General Public License as published by the Free Software Foundation, either version 3 of the
+// License, or (at your option) any later version.
+
+// UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
+// even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+// See the GNU General Public License for more details.
+
+// You should have received a copy of the GNU General Public License along with this program.
+// If not, see http://www.gnu.org/licenses/
+
+#include "mbed.h"
+#include "SDFileSystem.h"
+#include "SerialBuffered.h" // used in preference to MODSERIAL
+
+// Additional Types
+typedef uint8_t uint8 ;
+typedef int8_t int8;
+typedef uint16_t uint16;
+typedef int16_t int16;
+typedef int32_t int24;
+typedef uint32_t uint24;
+typedef int32_t int32;
+typedef uint32_t uint32;
+typedef uint8_t boolean;
+typedef float real32;
+
+extern Timer timer;
+
+//________________________________________________________________________________________
+
+
+// Useful Constants
+#define NUL 0
+#define SOH 1
+#define EOT 4
+#define ACK 6
+#define HT 9
+#define LF 10
+#define CR 13
+#define NAK 21
+#define ESC 27
+#define true 1
+#define false 0
+
+#define PI 3.141592654
+#define HALFPI (PI*0.5)
+#define QUARTERPI (PI*0.25)
+#define SIXTHPI (PI/6.0)
+#define TWOPI (PI*2.0)
+#define RADDEG (180.0/PI)
+#define MILLIANGLE (180000.0/PI)
+#define DEGRAD (PI/180.0)
+
+#define MILLIRAD 18
+#define CENTIRAD 2
+
+#define MAXINT32 0x7fffffff
+#define MAXINT16 0x7fff
+
+//#define BATCHMODE
+
+#ifndef BATCHMODE
+//#define RX6CH
+//#define EXPERIMENTAL
+//#define TESTING
+//#define RX6CH // 6ch Receivers
+//#define SIMULATE
+//#define VTCOPTER
+//#define Y6COPTER
+#define QUADROCOPTER
+//#define TRICOPTER
+//#define HELICOPTER
+//#define AILERON
+// #define ELEVON
+#endif // !BATCHMODE
+
+//________________________________________________________________________________________________
+
+#define USE_PPM_FAILSAFE
+
+// Airframe
+
+#if ( defined TRICOPTER | defined QUADROCOPTER | defined VTCOPTER | defined Y6COPTER )
+#define MULTICOPTER
+#endif
+
+#if ( defined HELICOPTER | defined AILERON | defined ELEVON )
+#if ( defined AILERON | defined ELEVON )
+#define NAV_WING
+#endif
+#endif
+
+#ifdef QUADROCOPTER
+#define AF_TYPE QuadAF
+#endif
+#ifdef TRICOPTER
+#define AF_TYPE TriAF
+#endif
+#ifdef VTCOPTER
+#define AF_TYPE VTAF
+#endif
+#ifdef Y6COPTER
+#define AF_TYPE Y6AF
+#endif
+#ifdef HELICOPTER
+#define AF_TYPE HeliAF
+#endif
+#ifdef ELEVON
+#define AF_TYPE ElevAF
+#endif
+#ifdef AILERON
+#define AF_TYPE AilAF
+#endif
+
+#define GPS_INC_GROUNDSPEED // GPS groundspeed is not used for flight but may be of interest
+
+// Timeouts and Update Intervals
+
+#define FAILSAFE_TIMEOUT_MS 1000L // mS. hold last "good" settings and then restore flight or abort
+#define ABORT_TIMEOUT_GPS_MS 5000L // mS. go to descend on position hold if GPS valid.
+#define ABORT_TIMEOUT_NO_GPS_MS 0L // mS. go to descend on position hold if GPS valid.
+#define ABORT_UPDATE_MS 1000L // mS. retry period for RC Signal and restore Pilot in Control
+#define ARMED_TIMEOUT_MS 150000L // mS. automatic disarming if armed for this long and landed
+
+#define ALT_DESCENT_UPDATE_MS 1000L // mS time between throttle reduction clicks in failsafe descent without baro
+
+#define RC_STICK_MOVEMENT 5L // minimum to be recognised as a stick input change without triggering failsafe
+
+#define THROTTLE_LOW_DELAY_MS 1000L // mS. that motor runs at idle after the throttle is closed
+#define THROTTLE_UPDATE_MS 3000L // mS. constant throttle time for altitude hold
+
+#define NAV_ACTIVE_DELAY_MS 10000L // mS. after throttle exceeds idle that Nav becomes active
+#define NAV_RTH_LAND_TIMEOUT_MS 10000L // mS. Shutdown throttle if descent lasts too long
+
+#define UAVX_TEL_INTERVAL_MS 125L // mS. emit an interleaved telemetry packet
+#define ARDU_TEL_INTERVAL_MS 200L // mS. Ardustation
+#define UAVX_CONTROL_TEL_INTERVAL_MS 10L // mS. flight control only
+#define CUSTOM_TEL_INTERVAL_MS 250L // mS.
+#define UAVX_MIN_TEL_INTERVAL_MS 1000L // mS. emit minimum FPV telemetry packet slow rate for example to FrSky
+
+#define GPS_TIMEOUT_MS 2000L // mS.
+
+#define ALT_UPDATE_HZ 20L // Hz based on 50mS update time for Baro
+
+#ifdef MULTICOPTER
+//#define PWM_UPDATE_HZ 450L // PWM motor update rate must be <450 and >100
+#else
+#define PWM_UPDATE_HZ 40L // standard RC servo update rate
+#endif // MULTICOPTER
+
+// Accelerometers
+
+#define DAMP_HORIZ_LIMIT 3L // equivalent stick units - no larger than 5
+#define DAMP_VERT_LIMIT_LOW -5L // maximum throttle reduction
+#define DAMP_VERT_LIMIT_HIGH 20L // maximum throttle increase
+
+// Gyros
+
+#define ATTITUDE_FF_DIFF (24.0/16.0) // 0 - 32 max feedforward speeds up roll/pitch recovery on fast stick change
+
+#define ATTITUDE_ENABLE_DECAY // enables decay to zero angle when roll/pitch is not in fact zero!
+// unfortunately there seems to be a leak which cause the roll/pitch
+// to increase without the decay.
+
+#define ATTITUDE_SCALE 0.5 // scaling of stick units for attitude angle control
+
+// Enable "Dynamic mass" compensation Roll and/or Pitch
+// Normally disabled for pitch only
+//#define DISABLE_DYNAMIC_MASS_COMP_ROLL
+//#define DISABLE_DYNAMIC_MASS_COMP_PITCH
+
+// Altitude Hold
+
+
+#define ALT_HOLD_MAX_ROC_MPS 0.5 // Must be changing altitude at less than this for alt. hold to be detected
+
+// the range within which throttle adjustment is proportional to altitude error
+#define ALT_BAND_M 5.0 // Metres
+
+#define LAND_M 3.0 // Metres deemed to have landed when below this height
+
+#define ALT_MAX_THR_COMP 40L // Stick units was 32
+
+#define ALT_INT_WINDUP_LIMIT 16L
+
+#define ALT_RF_ENABLE_M 5.0 // altitude below which the rangefiner is selected as the altitude source
+#define ALT_RF_DISABLE_M 6.0 // altitude above which the rangefiner is deselected as the altitude source
+
+// Navigation
+
+#define NAV_ACQUIRE_BEEPER
+
+//#define ATTITUDE_NO_LIMITS // full stick range is available otherwise it is scaled to Nav sensitivity
+
+#define NAV_RTH_LOCKOUT ((10.0*PI)/180.0) // first number is degrees
+
+#define NAV_MAX_ROLL_PITCH 25L // Rx stick units
+#define NAV_CONTROL_HEADROOM 10L // at least this much stick control headroom above Nav control
+#define NAV_DIFF_LIMIT 24L // Approx double NAV_INT_LIMIT
+#define NAV_INT_WINDUP_LIMIT 64L // ???
+
+#define NAV_ENFORCE_ALTITUDE_CEILING // limit all autonomous altitudes
+#define NAV_CEILING 120L // 400 feet
+#define NAV_MAX_NEUTRAL_RADIUS 3L // Metres also minimum closing radius
+#define NAV_MAX_RADIUS 99L // Metres
+
+#ifdef NAV_WING
+#define NAV_PROXIMITY_RADIUS 20.0 // Metres if there are no WPs
+#define NAV_PROXIMITY_ALTITUDE 5.0 // Metres
+#else
+#define NAV_PROXIMITY_RADIUS 5.0 // Metres if there are no WPs
+#define NAV_PROXIMITY_ALTITUDE 3.0 // Metres
+#endif // NAV_WING
+
+// reads $GPGGA sentence - all others discarded
+
+#define GPS_MIN_SATELLITES 6 // preferably > 5 for 3D fix
+#define GPS_MIN_FIX 1 // must be 1 or 2
+#define GPS_ORIGIN_SENTENCES 30L // Number of sentences needed to obtain reasonable Origin
+#define GPS_MIN_HDILUTE 130L // HDilute * 100
+
+#define NAV_SENS_THRESHOLD 40L // Navigation disabled if Ch7 is less than this
+#define NAV_SENS_ALTHOLD_THRESHOLD 20L // Altitude hold disabled if Ch7 is less than this
+#define NAV_SENS_6CH 80L // Low GPS gain for 6ch Rx
+
+#define NAV_YAW_LIMIT 10L // yaw slew rate for RTH
+#define NAV_MAX_TRIM 20L // max trim offset for altitude hold
+#define NAV_CORR_SLEW_LIMIT 1L // *5L maximum change in roll or pitch correction per GPS update
+
+#define ATTITUDE_HOLD_LIMIT 8L // dead zone for roll/pitch stick for position hold
+#define ATTITUDE_HOLD_RESET_INTERVAL 25L // number of impulse cycles before GPS position is re-acquired
+
+//#define NAV_PPM_FAILSAFE_RTH // PPM signal failure causes RTH with Signal sampled periodically
+
+// Throttle
+
+#define THROTTLE_MAX_CURRENT 40L // Amps total current at full throttle for estimated mAH
+#define CURRENT_SENSOR_MAX 50L // Amps range of current sensor - used for estimated consumption - no actual sensor yet.
+#define THROTTLE_CURRENT_SCALE ((THROTTLE_MAX_CURRENT * 1024L)/(200L * CURRENT_SENSOR_MAX ))
+
+#define THROTTLE_SLEW_LIMIT 0 // limits the rate at which the throttle can change (=0 no slew limit, 5 OK)
+#define THROTTLE_MIDDLE 10 // throttle stick dead zone for baro
+#define THROTTLE_MIN_ALT_HOLD 75 // min throttle stick for altitude lock
+
+// RC
+
+#define RC_INIT_FRAMES 32 // number of initial RC frames to allow filters to settle
+
+#define RC_MIN_WIDTH_US 900
+#define RC_MAX_WIDTH_US 2100
+
+#define RC_NO_CHANGE_TIMEOUT_MS 20000L // mS.
+
+typedef union {
+ int16 i16;
+ uint16 u16;
+ struct {
+ uint8 b0;
+ uint8 b1;
+ };
+ struct {
+ int8 pad;
+ int8 i1;
+ };
+} i16u;
+
+typedef union {
+ int24 i24;
+ uint24 u24;
+ struct {
+ uint8 b0;
+ uint8 b1;
+ uint8 b2;
+ };
+ struct {
+ uint8 pad;
+ int16 i2_1;
+ };
+} i24u;
+
+typedef union {
+ int32 i32;
+ uint32 u32;
+ struct {
+ uint8 b0;
+ uint8 b1;
+ uint8 b2;
+ uint8 b3;
+ };
+ struct {
+ uint16 w0;
+ uint16 w1;
+ };
+ struct {
+ int16 pad;
+ int16 iw1;
+ };
+
+ struct {
+ uint8 padding;
+ int24 i3_1;
+ };
+} i32u;
+
+#define TX_BUFF_MASK 511
+#define RX_BUFF_MASK 511
+
+typedef struct { // PPM
+ uint8 Head;
+ int16 B[4][8];
+} int16x8x4Q;
+
+typedef struct { // Baro
+ uint8 Head, Tail;
+ int24 B[8];
+} int24x8Q;
+
+typedef struct { // GPS
+ uint8 Head, Tail;
+ int32 Lat[4], Lon[4];
+} int32x4Q;
+
+// Macros
+
+#define Sign(i) (((i)<0) ? -1 : 1)
+
+#define Max(i,j) ((i<j) ? j : i)
+#define Min(i,j) ((i<j) ? i : j )
+#define Decay1(i) (((i) < 0) ? (i+1) : (((i) > 0) ? (i-1) : 0))
+#define Limit(i,l,u) (((i) < l) ? l : (((i) > u) ? u : (i)))
+#define Sqr(v) ( v * v )
+
+// To speed up NMEA sentence processing
+// must have a positive argument
+#define ConvertDDegToMPi(d) (((int32)d * 3574L)>>11)
+#define ConvertMPiToDDeg(d) (((int32)d * 2048L)/3574L)
+
+#define ToPercent(n, m) (((n)*100L)/m)
+
+// Simple filters using weighted averaging
+#define VerySoftFilter(O,N) (SRS16((O)+(N)*3, 2))
+#define SoftFilter(O,N) (SRS16((O)+(N), 1))
+#define MediumFilter(O,N) (SRS16((O)*3+(N), 2))
+#define HardFilter(O,N) (SRS16((O)*7+(N), 3))
+
+// Unsigned
+#define VerySoftFilterU(O,N) (((O)+(N)*3+2)>>2)
+#define SoftFilterU(O,N) (((O)+(N)+1)>>1)
+#define MediumFilterU(O,N) (((O)*3+(N)+2)>>2)
+#define HardFilterU(O,N) (((O)*7+(N)+4)>>3)
+
+#define NoFilter(O,N) (N)
+
+#define DisableInterrupts (INTCONbits.GIEH=0)
+#define EnableInterrupts (INTCONbits.GIEH=1)
+#define InterruptsEnabled (INTCONbits.GIEH)
+
+// PARAMS
+
+#define PARAMS_ADDR_PX 0 // code assumes zero!
+#define MAX_PARAMETERS 64 // parameters in PXPROM start at zero
+
+#define STATS_ADDR_PX ( PARAMS_ADDR_PX + (MAX_PARAMETERS *2) )
+#define MAX_STATS 64
+
+// uses second Page of PXPROM
+#define NAV_ADDR_PX 256L
+// 0 - 8 not used
+
+#define NAV_NO_WP (NAV_ADDR_PX + 9)
+#define NAV_PROX_ALT (NAV_ADDR_PX + 10)
+#define NAV_PROX_RADIUS (NAV_ADDR_PX + 11)
+#define NAV_ORIGIN_ALT (NAV_ADDR_PX + 12)
+#define NAV_ORIGIN_LAT (NAV_ADDR_PX + 14)
+#define NAV_ORIGIN_LON (NAV_ADDR_PX + 18)
+#define NAV_RTH_ALT_HOLD (NAV_ADDR_PX + 22) // P[NavRTHAlt]
+#define NAV_WP_START (NAV_ADDR_PX + 24)
+
+#define WAYPOINT_REC_SIZE (4 + 4 + 2 + 1) // Lat + Lon + Alt + Loiter
+#define NAV_MAX_WAYPOINTS ((256 - 24 - 1)/WAYPOINT_REC_SIZE)
+
+//______________________________________________________________________________________________
+
+// main.c
+
+enum Directions {BF, LR, UD, Alt }; // x forward, y left and z down
+enum Angles { Pitch, Roll, Yaw }; // X, Y, Z
+
+#define FLAG_BYTES 10
+#define TELEMETRY_FLAG_BYTES 6
+typedef union {
+ uint8 AllFlags[FLAG_BYTES];
+ struct { // Order of these flags subject to change
+ uint8
+AltHoldEnabled:
+ 1,
+AllowTurnToWP:
+ 1, // stick programmed
+GyroFailure:
+ 1,
+LostModel:
+ 1,
+NearLevel:
+ 1,
+LowBatt:
+ 1,
+GPSValid:
+ 1,
+NavValid:
+ 1,
+
+BaroFailure:
+ 1,
+AccFailure:
+ 1,
+CompassFailure:
+ 1,
+GPSFailure:
+ 1,
+AttitudeHold:
+ 1,
+ThrottleMoving:
+ 1,
+HoldingAlt:
+ 1,
+Navigate:
+ 1,
+
+ReturnHome:
+ 1,
+WayPointAchieved:
+ 1,
+WayPointCentred:
+ 1,
+Unused2: // was UsingGPSAlt:
+ 1,
+UsingRTHAutoDescend:
+ 1,
+BaroAltitudeValid:
+ 1,
+RangefinderAltitudeValid:
+ 1,
+UsingRangefinderAlt:
+ 1,
+
+ // internal flags not really useful externally
+
+AllowNavAltitudeHold:
+ 1, // stick programmed
+UsingPositionHoldLock:
+ 1,
+Ch5Active:
+ 1,
+Simulation:
+ 1,
+AcquireNewPosition:
+ 1,
+MotorsArmed:
+ 1,
+NavigationActive:
+ 1,
+ForceFailsafe:
+ 1,
+
+Signal:
+ 1,
+RCFrameOK:
+ 1,
+ParametersValid:
+ 1,
+RCNewValues:
+ 1,
+NewCommands:
+ 1,
+AccelerationsValid:
+ 1,
+CompassValid:
+ 1,
+CompassMissRead:
+ 1,
+
+UsingPolarCoordinates:
+ 1,
+UsingAngleControl:
+ 1,
+GPSPacketReceived:
+ 1,
+NavComputed:
+ 1,
+AltitudeValid:
+ 1,
+UsingSerialPPM:
+ 1,
+UsingTxMode2:
+ 1,
+UsingAltOrientation:
+ 1,
+
+ // outside telemetry flags
+
+UsingTelemetry:
+ 1,
+TxToBuffer:
+ 1,
+NewBaroValue:
+ 1,
+BeeperInUse:
+ 1,
+RFInInches:
+ 1,
+FirstArmed:
+
+ 1,
+HaveGPRMC:
+ 1,
+UsingPolar:
+ 1,
+RTCValid:
+ 1,
+SDCardValid:
+ 1,
+PXImageStale:
+ 1,
+UsingLEDDriver:
+ 1,
+Using9DOF:
+ 1,
+HaveBatterySensor:
+ 1;
+ };
+} Flags;
+
+enum FlightStates { Starting, Landing, Landed, Shutdown, InFlight};
+extern volatile Flags F;
+extern int8 State;
+
+//______________________________________________________________________________________________
+
+// accel.c
+
+#define ACC_FREQ 50 // Hz must be less than 100Hz
+const real32 OneOnTwoPiAccF = ( 1.0 / ( TWOPI * ACC_FREQ ));
+
+enum AccelerometerTypes { LISLAcc, ADXL345Acc, AccUnknown };
+
+extern void ShowAccType(void);
+extern void ReadAccelerometers(void);
+extern void GetNeutralAccelerations(void);
+extern void GetAccelerations(void);
+extern void AccelerometerTest(void);
+extern void InitAccelerometers(void);
+
+// ADXL345 3-Axis Accelerometer
+
+#ifdef SIX_DOF
+#define ADXL345_ID 0xA6
+#else
+#define ADXL345_ID 0x53
+#endif // 6DOF
+
+#define ADXL345_W ADXL345_ID
+
+extern const float GRAVITY_ADXL345;
+
+extern void ReadADXL345Acc(void);
+extern void InitADXL345Acc(void);
+extern boolean ADXL345AccActive(void);
+
+// LIS3LV02DG 3-Axis Accelerometer 400KHz
+
+extern const float GRAVITY_LISL;
+
+#define LISL_WHOAMI 0x0f
+#define LISL_OFFSET_X 0x16
+#define LISL_OFFSET_Y 0x17
+#define LISL_OFFSET_Z 0x18
+#define LISL_GAIN_X 0x19
+#define LISL_GAIN_Y 0x1A
+#define LISL_GAIN_Z 0x1B
+#define LISL_CTRLREG_1 0x20
+#define LISL_CTRLREG_2 0x21
+#define LISL_CTRLREG_3 0x22
+#define LISL_STATUS 0x27
+#define LISL_OUTX_L 0x28
+#define LISL_OUTX_H 0x29
+#define LISL_OUTY_L 0x2A
+#define LISL_OUTY_H 0x2B
+#define LISL_OUTZ_L 0x2C
+#define LISL_OUTZ_H 0x2D
+#define LISL_FF_CFG 0x30
+#define LISL_FF_SRC 0x31
+#define LISL_FF_ACK 0x32
+#define LISL_FF_THS_L 0x34
+#define LISL_FF_THS_H 0x35
+#define LISL_FF_DUR 0x36
+#define LISL_DD_CFG 0x38
+#define LISL_INCR_ADDR 0x40
+#define LISL_READ 0x80
+#define LISL_ID 0x3a
+
+extern void WriteLISL(uint8, uint8);
+extern void ReadLISLAcc(void);
+extern boolean LISLAccActive(void);
+
+// other accelerometers
+
+extern real32 Vel[3], Acc[3], AccNeutral[3];
+extern int16 NewAccNeutral[3];
+extern uint8 AccelerometerType;
+
+//______________________________________________________________________________________________
+
+// analog.c
+
+extern void GetBattery(void);
+extern void BatteryTest(void);
+extern void InitBattery(void);
+
+extern void GetRangefinderAltitude(void);
+extern void InitRangefinder(void);
+
+extern real32 BatteryVolts, BatteryCurrentADCEstimated, BatteryChargeUsedAH;
+extern real32 BatteryCharge, BatteryCurrent;
+extern real32 BatteryVoltsScale;
+
+extern real32 RangefinderAltitude;
+
+//______________________________________________________________________________________________
+
+// attitude.c
+
+enum AttitudeMethods { PremerlaniDCM, MadgwickIMU, MadgwickAHRS};
+
+extern void GetAttitude(void);
+extern void DoLegacyYawComp(void);
+extern void AttitudeTest(void);
+extern void InitAttitude(void);
+
+extern real32 dT, dTR;
+extern uint32 PrevDCMUpdate;
+extern uint8 AttitudeMethod;
+
+// DCM Premerlani
+
+extern void DCMNormalise(void);
+extern void DCMDriftCorrection(void);
+extern void AccAdjust(void);
+extern void DCMMotionCompensation(void);
+extern void DCMUpdate(void);
+extern void DCMEulerAngles(void);
+
+extern real32 RollPitchError[3];
+extern real32 AccV[3];
+extern real32 GyroV[3];
+extern real32 OmegaV[3];
+extern real32 OmegaP[3];
+extern real32 OmegaI[3];
+extern real32 Omega[3];
+extern real32 DCM[3][3];
+extern real32 U[3][3];
+extern real32 TempM[3][3];
+
+// IMU & AHRS S.O.H. Madgwick
+
+extern void IMUupdate(real32, real32, real32, real32, real32, real32);
+extern void AHRSupdate(real32, real32, real32, real32, real32, real32, real32, real32, real32);
+extern void EulerAngles(void);
+
+extern real32 q0, q1, q2, q3; // quaternion elements representing the estimated orientation
+
+//______________________________________________________________________________________________
+
+// autonomous.c
+
+extern void DoShutdown(void);
+extern void FailsafeHoldPosition(void);
+extern void DoPolarOrientation(void);
+extern void Navigate(int32, int32);
+extern void SetDesiredAltitude(int16);
+extern void DoFailsafeLanding(void);
+extern void AcquireHoldPosition(void);
+extern void NavGainSchedule(int16);
+extern void DoNavigation(void);
+extern void FakeFlight(void);
+extern void DoPPMFailsafe(void);
+extern void WriteWayPointPX(uint8, int32, int32, int16, uint8);
+extern void UAVXNavCommand(void);
+extern void GetWayPointPX(int8);
+extern void InitNavigation(void);
+
+typedef struct {
+ int32 E, N;
+ int16 A;
+ uint8 L;
+} WayPoint;
+
+enum NavStates { HoldingStation, ReturningHome, AtHome, Descending, Touchdown, Navigating, Loitering};
+enum FailStates { MonitoringRx, Aborting, Terminating, Terminated };
+
+extern real32 NavCorr[3], NavCorrp[3];
+extern real32 NavE[3], NavEp[3], NavIntE[3];
+extern int16 NavYCorrLimit;
+
+extern int8 FailState;
+extern WayPoint WP;
+extern int8 CurrWP;
+extern int8 NoOfWayPoints;
+extern int16 WPAltitude;
+extern int32 WPLatitude, WPLongitude;
+
+extern real32 WayHeading;
+extern real32 NavPolarRadius, NavNeutralRadius, NavProximityRadius, NavProximityAltitude;
+
+extern int24 NavRTHTimeoutmS;
+extern int8 NavState;
+extern int16 NavSensitivity, RollPitchMax;
+extern int16 AltSum;
+
+extern int16 EffNavSensitivity;
+extern int16 EastP, EastDiffSum, EastI, EastCorr, NorthP, NorthDiffSum, NorthI, NorthCorr;
+extern int24 EastD, EastDiffP, NorthD, NorthDiffP;
+
+//______________________________________________________________________________________________
+
+// baro.c
+
+#define BARO_INIT_RETRIES 10 // max number of initialisation retries
+
+enum BaroTypes { BaroBMP085, BaroSMD500, BaroMPX4115, BaroUnknown };
+
+#define ADS7823_TIME_MS 50 // 20Hz
+#define ADS7823_MAX 4095 // 12 bits
+#define ADS7823_ID 0x90 // ADS7823 ADC
+#define ADS7823_WR 0x90 // ADS7823 ADC
+#define ADS7823_RD 0x91 // ADS7823 ADC
+#define ADS7823_CMD 0x00
+
+#define MCP4725_MAX 4095 // 12 bits
+//#define MCP4725_ID 0xC8
+#define MCP4725_WR MCP4725_ID
+#define MCP4725_RD (MCP4725_ID+1)
+#define MCP4725_CMD 0x40 // write to DAC registor in next 2 bytes
+#define MCP4725_EPROM 0x60 // write to DAC registor and eprom
+
+extern void SetFreescaleMCP4725(int16);
+extern void SetFreescaleOffset(void);
+extern void ReadFreescaleBaro(void);
+extern real32 FreescaleToDM(int24);
+extern void GetFreescaleBaroAltitude(void);
+extern boolean IsFreescaleBaroActive(void);
+extern void InitFreescaleBarometer(void);
+
+#define BOSCH_ID_BMP085 0x55
+#define BOSCH_ID 0xee
+#define BOSCH_TEMP_BMP085 0x2e
+#define BOSCH_TEMP_SMD500 0x6e
+#define BOSCH_PRESS 0xf4
+#define BOSCH_CTL 0xf4 // OSRS=3 for BMP085 25.5mS, SMD500 34mS
+#define BOSCH_ADC_MSB 0xf6
+#define BOSCH_ADC_LSB 0xf7
+#define BOSCH_ADC_XLSB 0xf8 // BMP085
+#define BOSCH_TYPE 0xd0
+
+extern void StartBoschBaroADC(boolean);
+extern void ReadBoschBaro(void);
+extern int24 CompensatedBoschPressure(uint16, uint16);
+extern void GetBoschBaroAltitude(void);
+extern boolean IsBoschBaroActive(void);
+extern void InitBoschBarometer(void);
+
+extern void GetBaroAltitude(void);
+extern void InitBarometer(void);
+
+extern void ShowBaroType(void);
+extern void BaroTest(void);
+
+extern int32 OriginBaroPressure, CompBaroPressure;
+extern uint16 BaroPressure, BaroTemperature;
+extern boolean AcquiringPressure;
+extern real32 BaroRelAltitude, BaroRelAltitudeP;
+extern i16u BaroVal;
+extern int8 BaroType;
+extern int16 AltitudeUpdateRate;
+extern int8 BaroRetries;
+
+extern real32 FakeBaroRelAltitude;
+
+//______________________________________________________________________________________________
+
+// compass.c
+
+enum CompassTypes { HMC5843, HMC6352, NoCompass };
+
+#define COMPASS_UPDATE_MS 50
+#define COMPASS_UPDATE_S (real32)(COMPASS_UPDATE_MS * 0.001)
+#define COMPASS_FREQ 10 // Hz must be less than 10Hz
+
+#define COMPASS_MAXDEV 30 // maximum yaw compensation of compass heading
+#define COMPASS_MIDDLE 10 // yaw stick neutral dead zone
+
+const real32 OneOnTwoPiCompassF = ( 1.0 / ( TWOPI * COMPASS_FREQ ));
+
+extern void ReadCompass(void);
+extern void GetHeading(void);
+extern void ShowCompassType(void);
+extern void DoCompassTest(void);
+extern void CalibrateCompass(void);
+extern void InitCompass(void);
+
+// HMC5843 Compass
+
+#define HMC5843_ID 0x3C // 0x1E 9DOF
+
+extern void ReadHMC5843(void);
+extern void GetHMC5843Parameters(void);
+extern void DoHMC5843Test(void);
+extern void CalibrateHMC5843(void);
+extern void InitHMC5843(void);
+extern boolean IsHMC5843Active(void);
+
+// HMC6352
+
+#define HMC6352_ID 0x42
+
+extern void ReadHMC6352(void);
+extern uint8 WriteByteHMC6352(uint8);
+extern void GetHMC6352Parameters(void);
+extern void DoHMC6352Test(void);
+extern void CalibrateHMC6352(void);
+extern void InitHMC6352(void);
+extern boolean HMC6352Active(void);
+
+typedef struct { real32 V; real32 Offset; } MagStruct;
+extern MagStruct Mag[3];
+extern real32 MagDeviation, CompassOffset;
+extern real32 MagHeading, Heading, FakeHeading;
+extern real32 HeadingSin, HeadingCos;
+extern uint8 CompassType;
+
+//______________________________________________________________________________________________
+
+// control.c
+
+extern void DoAltitudeHold(void);
+extern void UpdateAltitudeSource(void);
+extern void AltitudeHold(void);
+
+extern void LimitYawSum(void);
+extern void InertialDamping(void);
+extern void DoOrientationTransform(void);
+extern void DoControl(void);
+
+extern void LightsAndSirens(void);
+extern void InitControl(void);
+
+extern real32 Angle[3], Anglep[3], Rate[3], Ratep[3];
+extern real32 Comp[4];
+extern real32 DescentComp;
+extern real32 Rl, Pl, Yl, Ylp;
+extern real32 GS;
+
+extern int16 HoldYaw, YawSlewLimit;
+extern int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle;
+extern int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredHeading, DesiredCamPitchTrim;
+extern real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP;
+extern int16 CurrMaxRollPitch;
+
+extern int16 AttitudeHoldResetCount;
+extern real32 AltDiffSum, AltD, AltDSum;
+extern real32 DesiredAltitude, Altitude, AltitudeP;
+extern real32 ROC;
+extern boolean FirstPass;
+
+extern uint32 AltuSp;
+extern int16 DescentLimiter;
+
+extern int16 FakeDesiredPitch, FakeDesiredRoll, FakeDesiredYaw;
+
+//______________________________________________________________________________________________
+
+// gps.c
+
+extern void UpdateField(void);
+extern int32 ConvertGPSToM(int32);
+extern int32 ConvertMToGPS(int32);
+extern int24 ConvertInt(uint8, uint8);
+extern real32 ConvertReal(uint8, uint8);
+extern int32 ConvertLatLonM(uint8, uint8);
+extern void ConvertUTime(uint8, uint8);
+extern void ConvertUDate(uint8, uint8);
+extern void ParseGPGGASentence(void);
+extern void ParseGPRMCSentence(void);
+extern void SetGPSOrigin(void);
+extern void ParseGPSSentence(void);
+extern void RxGPSPacket(uint8);
+extern void SetGPSOrigin(void);
+extern void DoGPS(void);
+extern void GPSTest(void);
+extern void UpdateGPS(void);
+extern void InitGPS(void);
+
+#define MAXTAGINDEX 4
+#define GPSRXBUFFLENGTH 80
+typedef struct {
+ uint8 s[GPSRXBUFFLENGTH];
+ uint8 length;
+} NMEAStruct;
+
+#define MAX_NMEA_SENTENCES 2
+#define NMEA_TAG_INDEX 4
+
+enum GPSPackeType { GPGGAPacketTag, GPRMCPacketTag, GPSUnknownPacketTag };
+extern NMEAStruct NMEA;
+extern const uint8 NMEATags[MAX_NMEA_SENTENCES][5];
+
+extern uint8 GPSPacketTag;
+extern tm GPSTime;
+extern int32 GPSStartTime, GPSSeconds;
+extern int32 GPSLatitude, GPSLongitude;
+extern int32 OriginLatitude, OriginLongitude;
+extern real32 GPSAltitude, GPSRelAltitude, GPSOriginAltitude;
+extern int32 DesiredLatitude, DesiredLongitude;
+extern int32 LatitudeP, LongitudeP, HoldLatitude, HoldLongitude;
+extern real32 GPSLongitudeCorrection;
+extern real32 GPSHeading, GPSMagHeading, GPSMagDeviation, GPSVel, GPSVelp, GPSROC;
+extern int8 GPSNoOfSats;
+extern int8 GPSFix;
+extern int16 GPSHDilute;
+
+extern real32 GPSdT, GPSdTR;
+extern uint32 GPSuSp;
+
+extern int32 FakeGPSLongitude, FakeGPSLatitude;
+
+extern uint8 ll, tt, ss, RxCh;
+extern uint8 GPSCheckSumChar, GPSTxCheckSum;
+
+//______________________________________________________________________________________________
+
+// gyro.c
+
+enum GyroTypes { MLX90609Gyro, ADXRS150Gyro, IDG300Gyro, LY530Gyro, ADXRS300Gyro, ITG3200Gyro,
+ IRSensors, UnknownGyro
+ };
+
+extern void ReadGyros(void);
+extern void GetGyroRates(void);
+extern void CheckGyroFault(uint8, uint8, uint8);
+extern void ErectGyros(void);
+extern void GyroTest(void);
+extern void InitGyros(void);
+extern void ShowGyroType(void);
+
+// Generic Analog Gyrso
+extern void ReadAnalogGyros(void);
+extern void InitAnalogGyros(void);
+extern void CheckAnalogGyroFault(uint8, uint8, uint8);
+extern void AnalogGyroTest(void);
+
+// ITG3200 3 Axis Gyro
+
+#define ITG3200_WHO 0x00
+#define ITG3200_SMPL 0x15
+#define ITG3200_DLPF 0x16
+#define ITG3200_INT_C 0x17
+#define ITG3200_TMP_H 0x18
+#define ITG3200_TMP_L 0x1C
+#define ITG3200_GX_H 0x1D
+#define ITG3200_GX_L 0x1E
+#define ITG3200_GY_H 0x1F
+#define ITG3200_GY_L 0x20
+#define ITG3200_GZ_H 0x21
+#define ITG3200_GZ_L 0x22
+#define ITG3200_PWR_M 0x3E
+
+#ifdef SIX_DOF
+#define ITG3200_ID 0xD0
+#else
+#define ITG3200_ID 0xD2
+#endif // 6DOF
+
+#define ITG3200_R (ITG3200_ID+1)
+#define ITG3200_W ITG3200_ID
+
+extern void ReadITG3200Gyro(void);
+extern uint8 ReadByteITG3200(uint8);
+extern void WriteByteITG3200(uint8, uint8);
+extern void InitITG3200Gyro(void);
+extern void ITG3200Test(void);
+extern boolean ITG3200GyroActive(void);
+
+extern const real32 GyroToRadian[];
+extern real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians
+extern uint8 GyroType;
+
+//______________________________________________________________________________________________
+
+// harness.c
+
+
+extern void UpdateRTC(void);
+extern void InitHarness(void);
+
+extern LocalFileSystem Flash;
+
+// 1 GND
+// 2 4.5-9V
+// 3 VBat
+// 4 NReset
+
+//extern SPI SPI0; // 5 SPI MOSI, 6 SPI MOSO, 7 SPI CLK
+//extern DigitalOut SPICS; // 8
+extern SDFileSystem SDCard;
+
+//extern I2C I2C1; // 9, 10
+extern SerialBuffered TelemetrySerial;
+extern DigitalIn Armed; // 11 SPI MOSI
+extern DigitalOut PWMCamPitch; // 12 SPI MOSO // 12 SPI MOSO
+extern Serial GPSSerial; // 13 Tx1 / SPI CLK, 14 Rx1
+
+extern AnalogIn PitchADC; // 15 AN0
+extern AnalogIn RollADC; // 16 AN1
+extern AnalogIn YawADC; // 17 AN2
+
+extern AnalogIn RangefinderADC; // 18 AN3
+extern AnalogIn BatteryCurrentADC; // 19 AN4
+extern AnalogIn BatteryVoltsADC; // 20 AN5
+
+extern PwmOut Out0; // 21
+extern PwmOut Out1; // 22
+extern PwmOut Out2; // 23
+extern PwmOut Out3; // 24
+
+//extern PwmOut Out4; // 25
+//extern PwmOut Out5; // 26
+extern DigitalOut DebugPin; // 25
+
+extern I2C I2C0; // 27, 28
+
+extern DigitalIn RCIn; // 29 CAN
+extern DigitalOut PWMCamRoll; // 30 CAN
+
+//extern Serial TelemetrySerial;
+// 31 USB +, 32 USB -
+// 34 -37 Ethernet
+// 38 IF +
+// 39 IF -
+// 40 3.3V Out
+
+extern DigitalOut BlueLED;
+extern DigitalOut GreenLED;
+extern DigitalOut RedLED;
+extern DigitalOut YellowLED;
+
+extern InterruptIn RCInterrupt;
+
+extern char RTCString[], RTCLogfile[];
+extern struct tm* RTCTime;
+
+#define I2CTEMP I2C0
+#define I2CBARO I2C0
+#define I2CGYRO I2C0
+#define I2CACC I2C0
+#define I2CCOMPASS I2C0
+#define I2CESC I2C0
+#define I2CLED I2C0
+
+#define SPIACC SPI0
+
+#define mSClock timer.read_ms
+#define uSClock timer.read_us
+
+//______________________________________________________________________________________________
+
+// irq.c
+
+#define CONTROLS 7
+#define MAX_CONTROLS 12 // maximum Rx channels
+
+#define RxFilter MediumFilterU
+//#define RxFilter SoftFilterU
+//#define RxFilter NoFilter
+
+#define RC_GOOD_BUCKET_MAX 20
+#define RC_GOOD_RATIO 4
+
+#define RC_MINIMUM 0
+
+#define RC_MAXIMUM 238
+
+#define RC_NEUTRAL ((RC_MAXIMUM-RC_MINIMUM+1)/2)
+
+#define RC_MAX_ROLL_PITCH (170)
+
+#define RC_THRES_STOP ((6L*RC_MAXIMUM)/100)
+#define RC_THRES_START ((10L*RC_MAXIMUM)/100)
+
+#define RC_FRAME_TIMEOUT_MS 25
+#define RC_SIGNAL_TIMEOUT_MS (5L*RC_FRAME_TIMEOUT_MS)
+#define RC_THR_MAX RC_MAXIMUM
+
+#define MAX_ROLL_PITCH RC_NEUTRAL // Rx stick units - rely on Tx Rate/Exp
+
+#ifdef RX6CH
+#define RC_CONTROLS 5
+#else
+#define RC_CONTROLS CONTROLS
+#endif //RX6CH
+
+extern void InitTimersAndInterrupts(void);
+
+extern void enableTxIrq0(void);
+extern void disableTxIrq0(void);
+extern void enableTxIrq1(void);
+extern void disableTxIrq1(void);
+
+extern void RCISR(void);
+extern void RCNullISR(void);
+extern void RCTimeoutISR(void);
+extern void GPSInISR(void);
+extern void GPSOutISR(void);
+extern void TelemetryInISR(void);
+extern void TelemetryOutISR(void);
+extern void RazorInISR(void);
+extern void RazorOutISR(void);
+
+enum { Clock, GeneralCountdown, UpdateTimeout, RCSignalTimeout, BeeperTimeout, ThrottleIdleTimeout,
+ FailsafeTimeout, AbortTimeout, RxFailsafeTimeout, DescentUpdate, StickChangeUpdate, NavStateTimeout, LastValidRx,
+ LastGPS, StartTime, GPSTimeout, LEDChaserUpdate, LastBattery, TelemetryUpdate, NavActiveTime, BeeperUpdate,
+ ArmedTimeout, ThrottleUpdate, RazorUpdate, VerticalDampingUpdate, CamUpdate, BaroUpdate, CompassUpdate
+ };
+
+enum WaitPacketStates { WaitSentinel, WaitTag, WaitBody, WaitCheckSum};
+
+extern uint32 mS[];
+extern int16 PPM[];
+extern int8 PPM_Index;
+extern uint32 PrevEdge, CurrEdge;
+extern uint8 Intersection, PrevPattern, CurrPattern;
+extern uint32 Width;
+extern uint8 RxState;
+extern boolean WaitingForSync;
+
+extern int8 SignalCount;
+extern uint16 RCGlitches;
+
+//______________________________________________________________________________________________
+
+// ir.c
+
+extern void GetIRAttitude(void);
+extern void TrackIRMaxMin(uint8);
+extern void InitIRSensors(void);
+
+extern real32 IR[3], IRMax, IRMin, IRSwing;
+
+//______________________________________________________________________________________________
+
+// i2c.c
+
+#define I2C_ACK 1
+#define I2C_NACK 0
+
+extern void TrackMinI2CRate(uint32);
+extern void ShowI2CDeviceName(uint8);
+extern uint8 ScanI2CBus(void);
+extern boolean ESCWaitClkHi(void);
+extern void ProgramSlaveAddress(uint8);
+extern void ConfigureESCs(void);
+
+extern uint32 MinI2CRate;
+
+//______________________________________________________________________________________________
+
+// leds.c
+
+#define PCA9551_ID 0xc0
+
+#define DRV0M 0x0001
+#define DRV1M 0x0002
+#define DRV2M 0x0004
+#define DRV3M 0x0008
+
+#define AUX0M 0x0010
+#define AUX1M 0x0020
+#define AUX2M 0x0040
+#define AUX3M 0x0080
+
+#define BeeperM DRV0M
+
+#define YellowM 0x0100
+#define RedM 0x0200
+#define GreenM 0x0400
+#define BlueM 0x0800
+
+#define ALL_LEDS_ON LEDsOn(BlueM|RedM|GreenM|YellowM)
+#define ALL_LEDS_OFF LEDsOff(BlueM|RedM|GreenM|YellowM)
+
+#define AUX_LEDS_ON LEDsOn(AUX0M|AUX1M|AUX2M|AUX3M)
+#define AUX_LEDS_OFF LEDsOff(AUX0M|AUX1M|AUX2M|AUX3M)
+
+#define AUX_DRVS_ON LEDsOn(DRV0M|DRV1M|DRV2M|DRV3M)
+#define AUX_DRVS_OFF LEDsOff(DRV0M|DRV1M|DRV2M|DRV3M)
+
+#define ALL_LEDS_ARE_OFF ( (LEDShadow&(BlueM|RedM|GreenM|YellowM))== (uint8)0 )
+
+#define BEEPER_IS_ON ( (LEDShadow & BeeperM) != (uint8)0 )
+#define BEEPER_IS_OFF ( (LEDShadow & BeeperM) == (uint8)0 )
+
+#define LEDRed_ON LEDsOn(RedM)
+#define LEDBlue_ON LEDsOn(BlueM)
+#define LEDGreen_ON LEDsOn(GreenM)
+#define LEDYellow_ON LEDsOn(YellowM)
+#define LEDAUX1_ON LEDsOn(AUX1M)
+#define LEDAUX2_ON LEDsOn(AUX2M)
+#define LEDAUX3_ON LEDsOn(AUX3M)
+#define LEDRed_OFF LEDsOff(RedM)
+#define LEDBlue_OFF LEDsOff(BlueM)
+#define LEDGreen_OFF LEDsOff(GreenM)
+#define LEDYellow_OFF LEDsOff(YellowM)
+#define LEDYellow_TOG if( (LEDShadow&YellowM) == (uint8)0 ) LEDsOn(YellowM); else LEDsOff(YellowM)
+#define LEDRed_TOG if( (LEDShadow&RedM) == (uint8)0 ) LEDsOn(RedM); else LEDsOff(RedM)
+#define LEDBlue_TOG if( (LEDShadow&BlueM) == (uint8)0 ) LEDsOn(BlueM); else LEDsOff(BlueM)
+#define LEDGreen_TOG if( (LEDShadow&GreenM) == (uint8)0 ) LEDsOn(GreenM); else LEDsOff(GreenM)
+#define Beeper_OFF LEDsOff(BeeperM)
+#define Beeper_ON LEDsOn(BeeperM)
+#define Beeper_TOG if( (LEDShadow&BeeperM) == (uint8)0 ) LEDsOn(BeeperM); else LEDsOff(BeeperM)
+
+extern void SendLEDs(void);
+extern void SaveLEDs(void);
+extern void RestoreLEDs(void);
+extern void LEDsOn(uint16);
+extern void LEDsOff(uint16);
+extern void LEDChaser(void);
+extern void DoLEDs(void);
+extern void PowerOutput(int8);
+extern void LEDsAndBuzzer(void);
+
+extern void PCA9551Test(void);
+extern void WritePCA9551(uint8);
+extern boolean IsPCA9551Active(void);
+
+extern void InitLEDs(void);
+
+extern uint16 LEDShadow, PrevLEDShadow, SavedLEDs, LEDPattern;
+extern uint8 PrevPCA9551LEDShadow;
+
+//______________________________________________________________________________________________
+
+// math.c
+
+extern int16 SRS16(int16, uint8);
+extern int32 SRS32(int32, uint8);
+extern real32 Make2Pi(real32);
+extern real32 MakePi(real32);
+extern int16 Table16(int16, const int16 *);
+
+extern real32 VDot(real32 v1[3], real32 v2[3]);
+extern void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]);
+extern void VScale(real32 VOut[3], real32 v[3], real32 s);
+extern void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]);
+extern void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]);
+
+//______________________________________________________________________________________________
+
+// menu.c
+
+extern void ShowPrompt(void);
+extern void ShowRxSetup(void);
+extern void ShowSetup(boolean);
+extern uint8 MakeUpper(uint8);
+extern void ProcessCommand(void);
+
+extern const uint8 SerHello[];
+extern const uint8 SerSetup[];
+extern const uint8 SerPrompt[];
+
+extern const uint8 RxChMnem[];
+
+//______________________________________________________________________________________________
+
+// nonvolatiles.c
+
+extern void CheckSDCardValid(void);
+
+extern void CreateLogfile(void);
+extern void CloseLogfile(void);
+extern void TxLogChar(uint8);
+
+extern void WritePXImagefile(void);
+extern boolean ReadPXImagefile(void);
+extern int8 ReadPX(uint16);
+extern int16 Read16PX(uint16);
+extern int32 Read32PX(uint16);
+extern void WritePX(uint16, int8);
+extern void Write16PX(uint16, int16);
+extern void Write32PX(uint16, int32);
+
+extern FILE *pxfile;
+extern FILE *newpxfile;
+
+extern const int PX_LENGTH;
+extern int8 PX[], PXNew[];
+
+//______________________________________________________________________________________________
+
+// outputs.c
+
+#define OUT_MINIMUM 1.0 // Required for PPM timing loops
+#define OUT_MAXIMUM 200.0 // to reduce Rx capture and servo pulse output interaction
+#define OUT_NEUTRAL 105.0 // 1.503mS @ 105 16MHz
+#define OUT_HOLGER_MAXIMUM 225.0
+#define OUT_YGEI2C_MAXIMUM 240.0
+#define OUT_X3D_MAXIMUM 200.0
+
+extern uint8 SaturInt(int16);
+extern void DoMulticopterMix(real32 CurrThrottle);
+extern void CheckDemand(real32 CurrThrottle);
+extern void MixAndLimitMotors(void);
+extern void MixAndLimitCam(void);
+extern void OutSignals(void);
+extern void InitI2CESCs(void);
+extern void StopMotors(void);
+extern void ExercisePWM(void);
+extern void InitMotors(void);
+
+enum PWMCamTags { CamRollC = 6, CamPitchC = 7 };
+enum PWMQuadTags {FrontC=0, BackC, RightC, LeftC }; // order is important for X3D & Holger ESCs
+enum PWMConvTags {ThrottleC=0, AileronC, ElevatorC, RudderC, RTHC };
+enum PWMWingTags3 {RightElevonC=1, LeftElevonC=2};
+enum PWMVTTags {FrontLeftC=0, FrontRightC};
+enum PWMY6Tags {FrontTC=0, LeftTC, RightTC, FrontBC, LeftBC, RightBC };
+enum PWMHexTags {FrontHC=0, FrontLeftHC, FrontRightHC, BackLeftHC, BackRightHC,BackHC };
+
+//#define NoOfPWMOutputs 4
+#define NoOfI2CESCOutputs 4
+
+extern const real32 PWMScale;
+
+extern real32 PWM[8];
+extern real32 PWMSense[8];
+extern int16 ESCI2CFail[6];
+extern int16 CurrThrottle;
+extern int16 CamRollPulseWidth, CamPitchPulseWidth;
+extern int16 ESCMin, ESCMax;
+
+//______________________________________________________________________________________________
+
+// params.c
+
+extern void ReadParameters(void);
+extern void UseDefaultParameters(void);
+extern void UpdateParamSetChoice(void);
+extern boolean ParameterSanityCheck(void);
+extern void InitParameters(void);
+
+enum TxRxTypes {
+ FutabaCh3, FutabaCh2, FutabaDM8, JRPPM, JRDM9, JRDXS12,
+ DX7AR7000, DX7AR6200, FutabaCh3_6_7, DX7AR6000, GraupnerMX16s, DX6iAR6200, FutabaCh3_R617FS,
+ DX7aAR7000, ExternalDecoder, FrSkyDJT_D8R, UnknownTxRx, CustomTxRx
+};
+enum RCControls {ThrottleRC, RollRC, PitchRC, YawRC, RTHRC, CamPitchRC, NavGainRC};
+enum ESCTypes { ESCPPM, ESCHolger, ESCX3D, ESCYGEI2C };
+enum AFs { QuadAF, TriAF, VTAF, Y6AF, HeliAF, ElevAF, AilAF };
+
+enum Params { // MAX 64
+ RollKp, // 01
+ RollKi, // 02
+ RollKd, // 03
+ HorizDampKp, // 04c
+ RollIntLimit, // 05
+ PitchKp, // 06
+ PitchKi, // 07
+ PitchKd, // 08
+ AltKp, // 09c
+ PitchIntLimit, // 10
+
+ YawKp, // 11
+ YawKi, // 12
+ YawKd, // 13
+ YawLimit, // 14
+ YawIntLimit, // 15
+ ConfigBits, // 16c
+ unused17 , // 17 TimeSlots
+ LowVoltThres, // 18c
+ CamRollKp, // 19
+ PercentCruiseThr, // 20c
+
+ VertDampKp, // 21c
+ MiddleUD, // 22c
+ PercentIdleThr, // 23c
+ MiddleLR, // 24c
+ MiddleBF, // 25c
+ CamPitchKp, // 26
+ CompassKp, // 27
+ AltKi, // 28c
+ unused29 , // 29 NavRadius
+ NavKi, // 30
+
+ GSThrottle, // 31
+ Acro, // 32
+ NavRTHAlt, // 33
+ NavMagVar, // 34c
+ GyroRollPitchType, // 35
+ ESCType, // 36
+ TxRxType, // 37
+ NeutralRadius, // 38
+ PercentNavSens6Ch, // 39
+ CamRollTrim, // 40
+ NavKd, // 41
+ VertDampDecay, // 42
+ HorizDampDecay, // 43
+ BaroScale, // 44
+ TelemetryType, // 45
+ MaxDescentRateDmpS, // 46
+ DescentDelayS, // 47
+ NavIntLimit, // 48
+ AltIntLimit, // 49
+ unused50, // 50 GravComp
+ unused51 , // 51 CompSteps
+ ServoSense, // 52
+ CompassOffsetQtr, // 53
+ BatteryCapacity, // 54
+ unused55, // 55 GyroYawType
+ AltKd, // 56
+ Orient, // 57
+ NavYawLimit, // 58
+ Balance // 59
+
+ // 56 - 64 unused currently
+};
+
+//#define FlyXMode 0
+//#define FlyAltOrientationMask 0x01
+
+#define UsePositionHoldLock 0
+#define UsePositionHoldLockMask 0x01
+
+#define UseRTHDescend 1
+#define UseRTHDescendMask 0x02
+
+#define TxMode2 2
+#define TxMode2Mask 0x04
+
+#define RxSerialPPM 3
+#define RxSerialPPMMask 0x08
+
+#define RFInches 4
+#define RFInchesMask 0x10
+
+// bit 4 is pulse polarity for 3.15
+
+#define UseUseAngleControl 5
+#define UseAngleControlMask 0x20
+
+#define UsePolar 6
+#define UsePolarMask 0x40
+
+// bit 7 unusable in UAVPSet
+
+extern const int8 DefaultParams[MAX_PARAMETERS][2];
+
+extern const uint8 ESCLimits [];
+
+extern real32 OSin[], OCos[];
+extern uint8 Orientation, PolarOrientation;
+
+extern uint8 ParamSet;
+extern boolean ParametersChanged, SaveAllowTurnToWP;
+extern int8 P[];
+extern real32 K[MAX_PARAMETERS]; // Arm rescaled legacy parameters as appropriate
+
+extern uint8 UAVXAirframe;
+
+//__________________________________________________________________________________________
+
+// rc.c
+
+extern void DoRxPolarity(void);
+extern void InitRC(void);
+extern void MapRC(void);
+extern void CheckSticksHaveChanged(void);
+extern void UpdateControls(void);
+extern void CaptureTrims(void);
+extern void CheckThrottleMoved(void);
+extern void ReceiverTest(void);
+
+extern const boolean PPMPosPolarity[];
+extern const uint8 Map[CustomTxRx+1][CONTROLS];
+extern int8 RMap[];
+
+#define PPMQMASK 3
+extern int16 PPMQSum[];
+extern int16x8x4Q PPMQ;
+extern boolean RCPositiveEdge;
+extern int16 RC[], RCp[];
+extern int16 Trim[3];
+extern int16 ThrLow, ThrNeutral, ThrHigh;
+
+//__________________________________________________________________________________________
+
+// serial.c
+
+extern void TxString(const uint8*);
+extern void TxChar(uint8);
+extern void TxValU(uint8);
+extern void TxValS(int8);
+extern void TxBin8(uint8);
+extern void TxNextLine(void);
+extern void TxNibble(uint8);
+extern void TxValH( uint8);
+extern void TxValH16(uint16);
+extern uint8 RxChar(void);
+extern uint8 PollRxChar(void);
+extern uint8 RxNumU(void);
+extern int8 RxNumS(void);
+extern void TxVal32(int32, int8, uint8);
+extern void TxChar(uint8);
+extern void TxESCu8(uint8);
+extern void Sendi16(int16);
+extern void TxESCi8(int8);
+extern void TxESCi16(int16);
+extern void TxESCi24(int24);
+extern void TxESCi32(int32);
+
+//______________________________________________________________________________________________
+
+// stats.c
+
+extern void ZeroStats(void);
+extern void ReadStatsPX(void);
+extern void WriteStatsPX(void);
+extern void ShowStats(void);
+
+enum Statistics {
+ GPSAltitudeS, BaroRelAltitudeS, ESCI2CFailS, GPSMinSatsS, MinROCS, MaxROCS, GPSVelS,
+ AccFailS, CompassFailS, BaroFailS, GPSInvalidS, GPSMaxSatsS, NavValidS,
+ MinHDiluteS, MaxHDiluteS, RCGlitchesS, GPSBaroScaleS, GyroFailS, RCFailsafesS, I2CFailS, MinTempS, MaxTempS, BadS, BadNumS
+}; // NO MORE THAN 32 or 64 bytes
+
+extern int16 Stats[];
+
+//______________________________________________________________________________________________
+
+// telemetry.c
+
+extern void RxTelemetryPacket(uint8);
+extern void InitTelemetryPacket(void);
+extern void BuildTelemetryPacket(uint8);
+
+extern void SendPacketHeader(void);
+extern void SendPacketTrailer(void);
+
+extern void SendTelemetry(void);
+extern void SendUAVX(void);
+extern void SendUAVXControl(void);
+extern void SendFlightPacket(void);
+extern void SendNavPacket(void);
+extern void SendControlPacket(void);
+extern void SendStatsPacket(void);
+extern void SendParamPacket(uint8);
+extern void SendMinPacket(void);
+extern void SendArduStation(void);
+extern void SendCustom(void);
+extern void SensorTrace(void);
+extern void CheckTelemetry(void);
+
+enum TelemetryStates { WaitRxSentinel, WaitRxESC, WaitRxBody };
+
+
+enum PacketTags {UnknownPacketTag = 0, LevPacketTag, NavPacketTag, MicropilotPacketTag, WayPacketTag,
+ AirframePacketTag, NavUpdatePacketTag, BasicPacketTag, RestartPacketTag, TrimblePacketTag,
+ MessagePacketTag, EnvironmentPacketTag, BeaconPacketTag, UAVXFlightPacketTag,
+ UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamsPacketTag, UAVXMinPacketTag, FrSkyPacketTag
+ };
+
+enum TelemetryTypes { NoTelemetry, GPSTelemetry, UAVXTelemetry, UAVXControlTelemetry, UAVXMinTelemetry,
+ ArduStationTelemetry, CustomTelemetry
+ };
+
+extern uint8 UAVXCurrPacketTag;
+extern uint8 RxPacketLength, RxPacketByteCount;
+extern uint8 RxCheckSum;
+extern uint8 RxPacketTag, ReceivedPacketTag;
+extern uint8 PacketRxState;
+extern boolean CheckSumError, TelemetryPacketReceived;
+
+extern int16 RxLengthErrors, RxTypeErrors, RxCheckSumErrors;
+
+extern uint8 TxCheckSum;
+
+extern FILE *logfile;
+extern boolean EchoToLogFile, LogfileIsOpen;
+extern uint32 LogChars;
+
+//______________________________________________________________________________________________
+
+// temperature.c
+
+#define TMP100_MAX_ADC 4095 // 12 bits
+#define TMP100_ID 0x96
+#define TMP100_WR 0x96 // Write
+#define TMP100_RD 0x97 // Read
+#define TMP100_TMP 0x00 // Temperature
+#define TMP100_CMD 0x01
+#define TMP100_LOW 0x02 // Alarm low limit
+#define TMP100_HI 0x03 // Alarm high limit
+#define TMP100_CFG 0 // 0.5 deg resolution continuous
+
+extern void GetTemperature(void);
+extern void InitTemperature(void);
+
+extern i16u AmbientTemperature;
+
+//______________________________________________________________________________________________
+
+// utils.c
+
+extern void InitMisc(void);
+extern void Delay1mS(int16);
+extern void Delay100mS(int16);
+extern void DoBeep100mS(uint8, uint8);
+extern void DoStartingBeeps(uint8);
+extern real32 SlewLimit(real32, real32, real32);
+extern real32 DecayX(real32, real32);
+extern void LPFilter(real32*, real32*, real32, real32);
+extern void CheckAlarms(void);
+extern void Timing(uint8, uint32);
+
+typedef struct {
+ uint32 T;
+ uint32 Count;
+} TimingRec;
+
+enum Timed { GetAttitudeT , UnknownT };
+extern TimingRec Times[];
+
+#define TimeS uint32 TStart;TStart=timer.read_us();
+#define TimeF(w, tf) Time(w, tf)
+
+//______________________________________________________________________________________________
+
+
+// Sanity checks
+
+#if (( defined TRICOPTER + defined QUADROCOPTER + defined VTCOPTER + defined Y6COPTER + defined HELICOPTER + defined AILERON + defined ELEVON ) != 1)
+#error None or more than one aircraft configuration defined !
+#endif