File content as of revision 2:90292f8bd179:

// Commissioning defines

//#define SERIAL_TELEMETRY GPSSerial                // Select the one you want Steve
#define SERIAL_TELEMETRY TelemetrySerial

#define MAX_PID_CYCLE_HZ    200                     // PID cycle rate do not exceed
#define PID_CYCLE_US        (1000000/MAX_PID_CYCLE_HZ)

#define PWM_UPDATE_HZ       200                     // reduced for Turnigys - I2C runs at PID loop rate always
                                                    // MUST BE LESS THAN OR EQUAL TO 450HZ
// LP filter cutoffs for sensors

//#define SUPPRESS_ROLL_PITCH_GYRO_FILTERS                           
#define ROLL_PITCH_FREQ     (0.5*PWM_UPDATE_HZ)     // must be <= ITG3200 LP filter
#define ATTITUDE_ANGLE_LIMIT QUARTERPI              // set to PI for aerobatics
#define GYRO_PROP_NOISE     0.1                     // largely prop coupled
                    
//#define SUPPRESS_ACC_FILTERS
#define ACC_FREQ            5                       // could be lower again?

//#define SUPPRESS_YAW_GYRO_FILTERS
//#define USE_FIXED_YAW_FILTER                      // does not rescale LP cutoff with yaw stick  
#define MAX_YAW_FREQ            10.0                // <= 10Hz
#define COMPASS_SANITY_CHECK_RAD_S  TWOPI           // changes greater than this rate ignored

// DCM Attitude Estimation

//#define DCM_YAW_COMP
//#define USE_ANGLE_DERIVED_RATE                    // Gyros have periodic prop noise - define to use rate derived from angle

// The pitch/roll angle should track CLOSELY with the noise "mostly" tuned out.
// Jitter in the artificial horizon gives part of the story but better to use the UAVXFC logs.

// Assumes normalised gravity vector
#define TAU_S               5.0                                 //  1-10
#define Kp_RollPitch        5.0 //(2.0/TAU_S)                   //1.0      // 5.0
#define Ki_RollPitch        0.005//((1.0/TAU_S)*(1.0/TAU_S))    //0.001    // 0.005
//#define Ki_RollPitch      (1.0/(TAU_S*TAU_S)) ?
#define Kp_Yaw 1.2
#define Ki_Yaw 0.00002

/*
                Kp      Ki          KpYaw   KiYaw
Arducopter      0.0014  0.00002     1.0     0.00002 (200Hz)
Arducopter      5.0     0.005       1.2     0.00002
Ihlein          0.2     0.01        0.02    0.01    (50Hz)
Premerlani      0.0755  0.00943                     (50Hz)
Bascom          0.02    0.00002     1.2     0.00002
Matrixpilot     0.04    0.008       0.016   0.0005
Superstable     0.0014  0.00000015  1.2     0.00005 (200Hz)

*/

//#define DISABLE_LED_DRIVER                             // disables the PCA driver and also the BUZZER

#define DISABLE_EXTRAS                      // suppress altitude hold, position hold and inertial compensation
#define SUPPRESS_SDCARD                     //DO NOT RE-ENABLE - MOTOR INTERMITTENTS WILL OCCUR

//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 BATTERY_VOLTS_SCALE   57.85         // 51.8144    // Volts scaling for internal divider

#define SW_I2C                                      // define for software I2C 400KHz 

//#define INC_ALL_SCHEMES                            // runs all attitude control schemes - use "custom" telemetry

#define I2C_MAX_RATE_HZ     400000

#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/                               =
// ===============================================================================================

//    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 THIRDPI         (PI/3.0)
#define HALFPI          (PI*0.5)
#define ONEANDHALFPI    (PI * 1.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    100L    // mS. flight control only
#define CUSTOM_TEL_INTERVAL_MS          125L    // mS.
#define FAST_CUSTOM_TEL_INTERVAL_MS     5L      // 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.008     // scaling of stick units for attitude angle control 

// Enable "Dynamic mass" compensation Roll and/or Pitch
// Normally disabled for pitch only
//#define ENABLE_DYNAMIC_MASS_COMP_ROLL
//#define ENABLE_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

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 Limit1(i,l)            (((i) < -(l)) ? -(l) : (((i) > (l)) ? (l) : (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           32 // 64 bytes

// 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 { Roll, Pitch, 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,
UnusedGPSAlt: // 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,
AccMagnitudeOK:
        1;
    };
} Flags;

enum FlightStates { Starting, Landing, Landed, Shutdown, InFlight};
extern volatile Flags F;
extern int8 State;
extern int8 r;

//______________________________________________________________________________________________

// accel.c

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_WR           ADXL345_ID
#define ADXL345_RD           (ADXL345_ID+1)

extern void ReadADXL345Acc(void);
extern void InitADXL345Acc(void);
extern boolean ADXL345AccActive(void);

// LIS3LV02DG 3-Axis Accelerometer 400KHz

#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

#define LISL_WR         LISL_ID
#define LISL_RD         (LISL_ID+1)

extern boolean WriteLISL(uint8, uint8);
extern void ReadLISLAcc(void);
extern void InitLISLAcc(void);
extern boolean LISLAccActive(void);

// other accelerometers

extern real32 Vel[3], AccADC[3], AccNoise[3], Acc[3], AccNeutral[3], Accp[3];
extern int16 NewAccNeutral[3];
extern uint8 AccelerometerType;
extern real32 GravityR; // recip gravity scaling

//______________________________________________________________________________________________

// analog.c

extern real32 ADC(uint8);
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 { Integrator, Wolferl,  Complementary, Kalman, PremerlaniDCM,  MadgwickIMU,
                MadgwickIMU2, MadgwickAHRS, MultiWii,  MaxAttitudeScheme};

extern void AdaptiveYawLPFreq(void);
extern void GetAttitude(void);
extern void DoLegacyYawComp(uint8);
extern void NormaliseAccelerations(void);
extern void AttitudeTest(void);
extern void InitAttitude(void);

extern real32 dT, dTOn2, dTR, dTmS;
extern real32 YawFilterLPFreq;
extern uint32 PrevDCMUpdate;
extern uint8 AttitudeMethod;

extern real32 EstAngle[3][MaxAttitudeScheme];
extern real32 EstRate[3][MaxAttitudeScheme];

extern real32 HE;

// SimpleIntegrator

extern void DoIntegrator(void);

// Wolferl

extern real32 Correction[2];

extern void DoWolferl(void);

// 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 void DoDCM(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 DoMadgwickIMU(real32, real32, real32, real32, real32, real32);
extern void DoMadgwickIMU2(real32, real32, real32, real32, real32, real32);
extern void DoMadgwickAHRS(real32, real32, real32, real32, real32, real32, real32, real32, real32);
extern void MadgwickEulerAngles(uint8);

extern real32 q0, q1, q2, q3;    // quaternion elements representing the estimated orientation

// Kalman
extern void DoKalman(void);

// Complementary
extern void DoCF(void);

// MultiWii
extern  void DoMultiWii();

//______________________________________________________________________________________________

// 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 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          ADS7823_ID      // ADS7823 ADC
#define ADS7823_RD          (ADS7823_ID+1)  // ADS7823 ADC
#define ADS7823_CMD         0x00

extern uint8 MCP4725_ID_Actual;

#define MCP4725_MAX         4095    // 12 bits
#define MCP4725_ID_0xC8     0xc8
#define MCP4725_ID_0xCC     0xcc
#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 boolean IdentifyMCP4725(void);
extern void InitFreescaleBarometer(void);

#define BOSCH_ID_BMP085         0x55
#define BOSCH_ID                0xee
#define BOSCH_WR                BOSCH_ID_BMP085
#define BOSCH_RD                (BOSCH_ID_BMP085+1)
#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 SUPPRESS_COMPASS_FILTER

extern real32 AdaptiveCompassFreq(void);
extern void ReadCompass(void);
extern void GetHeading(void);
extern real32 MinimumTurn(real32);
extern void ShowCompassType(void);
extern void DoCompassTest(void);
extern void CalibrateCompass(void);
extern void InitCompass(void);

// HMC5843 Compass

#define HMC5843_DR          6       // 50Hz
#define HMC5843_UPDATE_S    0.02

//#define HMC5843_DR            5    // 20Hz
//#define HMC5843_UPDATE_S      0.05

#define HMC5843_ID      0x3C        // 0x1E 9DOF
#define HMC5843_WR      HMC5843_ID
#define HMC5843_RD      (HMC5843_ID+1)

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_UPDATE_S    0.05    // 20Hz

#define HMC6352_ID       0x42
#define HMC6352_WR       HMC6352_ID
#define HMC6352_RD       (HMC6352_ID+1)

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, HeadingP, FakeHeading;
extern real32 CompassMaxSlew;
extern real32 HeadingSin, HeadingCos;
extern uint8 CompassType;

//______________________________________________________________________________________________

// control.c

extern void DoAltitudeHold(void);
extern void UpdateAltitudeSource(void);
extern real32 AltitudeCF( real32, real32);
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], YawRateEp;
extern real32 AccAltComp, AltComp;
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, DesiredCamPitchTrim;
extern real32 DesiredHeading;
extern real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP;
extern real32 CameraRollAngle, CameraPitchAngle;
extern int16 CurrMaxRollPitch;
extern int16 Trim[3];

extern real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd;

extern int16 AttitudeHoldResetCount;
extern real32 AltDiffSum, AltD, AltDSum;
extern real32 DesiredAltitude, Altitude, AltitudeP;
extern real32 ROC;
extern boolean FirstPass;

extern uint32 AltuSp;
extern uint32 ControlUpdateTimeuS;
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_WR       ITG3200_ID
#define ITG3200_RD       (ITG3200_ID+1)

extern void ReadITG3200Gyro(void);
extern uint8 ReadByteITG3200(uint8);
extern boolean WriteByteITG3200(uint8, uint8);
extern void InitITG3200Gyro(void);
extern void ITG3200Test(void);
extern boolean ITG3200GyroActive(void);

extern const real32 GyroToRadian[];
extern real32 GyroADC[3], GyroADCp[3], GyroNoise[3], GyroNeutral[3], Gyrop[3], Gyro[3]; // Radians
extern uint8 GyroType;

//______________________________________________________________________________________________

// harness.c

extern void UpdateRTC(void);
extern void InitHarness(void);

extern LocalFileSystem Flash;

extern uint8 I2C0SDAPin, I2C0SCLPin;

// 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

enum ADCValues { ADCPitch, ADCRoll, ADCYaw, ADCRangefinder, ADCBatteryCurrent, ADCBatteryVolts };

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

#ifdef SW_I2C

class MyI2C {

private:

    boolean waitclock(void);

public:

    void frequency(uint32 f);
    void start(void);
    void stop(void);
    uint8 blockread(uint8 r, char* b, uint8);
    uint8 read(uint8 r);
    boolean blockwrite(uint8 a, const char* b, uint8 l);
    uint8 write(uint8 d);
};

extern MyI2C I2C0;                    // 27, 28
extern PortInOut I2C0SCL;
extern PortInOut I2C0SDA;
#else

extern I2C I2C0;                    // 27, 28
#define blockread   read
#define blockwrite  write

#endif // SW_I2C

extern DigitalIn  RCIn;             // 29 CAN
extern InterruptIn RCInterrupt;

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 char RTCString[], RTCLogfile[];
extern struct tm* RTCTime;

#define I2CTEMP I2C0
#define I2CBARO I2C0
#define I2CBAROAddressResponds I2C0AddressResponds
#define I2CGYRO I2C0
#define I2CGYROAddressResponds I2C0AddressResponds
#define I2CACC I2C0
#define I2CACCAddressResponds I2C0AddressResponds
#define I2CCOMPASS I2C0
#define I2CCOMPASSAddressResponds I2C0AddressResponds
#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);

extern void TurnBeeperOff(void);
extern void DoBeeperPulse1mS(int16);

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;

extern Timer timer;
extern Timeout CamRollTimeout, CamPitchTimeout;
extern Ticker CameraTicker;
extern Timeout RCTimeout;

//______________________________________________________________________________________________

// 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  true
#define I2C_NACK false

extern boolean I2C0AddressResponds(uint8);
#ifdef HAVE_I2C1
extern boolean I2C1AddressResponds(uint8);
#endif // HAVE_I2C1
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 void InitI2C(void);

extern uint32 MinI2CRate;
extern uint16 I2CError[256];

//______________________________________________________________________________________________

// 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[];

//______________________________________________________________________________________________

// NXP1768pins.c

extern boolean PinRead(uint8);
extern void PinWrite(uint8, boolean);
extern void PinSetOutput(uint8, boolean);

extern const uint8 mbed1768Pins[];

//______________________________________________________________________________________________

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

// Using clockwise numbering - NOT the same as UAVXPIC
enum PWMCamTags { CamRollC = 6, CamPitchC = 7 };
enum PWMQuadTags {FrontC=0, RightC, BackC, LeftC }; // order is important for X3D & Holger ESCs
enum PWMConvTags {ThrottleC=0, AileronC, ElevatorC, RudderC, RTHC };
enum PWMWingTags3 {RightElevonC=1, LeftElevonC=2};
enum PWMVTTags {FrontRightC=0, FrontLeftC};
enum PWMY6Tags {FrontTC=0, RightTC, LeftTC, FrontBC, RightBC, LeftBC };
enum PWMHexTags {FrontHC=0, FrontRightHC, BackRightHC, BackHC, BackLeftHC,FrontLeftHC };

//#define NoOfPWMOutputs            4
#ifdef HEXACOPTER
#define NoOfI2CESCOutputs         6
#else
#define NoOfI2CESCOutputs         4
#endif // HEXACOPTER

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
    Unused04,           // 04c HorizDampKp
    RollIntLimit,          // 05
    PitchKp,               // 06
    PitchKi,               // 07
    PitchKd,               // 08
    AltKp,                 // 09c
    PitchIntLimit,         // 10

    YawKp,                 // 11
    YawKi,                 // 12
    Unused13,                 // 13 YawKd
    YawLimit,              // 14
    YawIntLimit,           // 15
    ConfigBits,            // 16c
    Unused17 ,             // 17 TimeSlots
    LowVoltThres,          // 18c
    CamRollKp,             // 19
    PercentCruiseThr,      // 20c

    VertDamp,            // 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
    Unused42 ,             // 42  VertDampDecay
    Unused43,              // 43  HorizDampDecay
    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

#define RC_INIT_FRAMES                  32      // number of initial RC frames to allow filters to settle

#define RC_MIN_WIDTH_US                 1000    // temporarily to prevent wraparound 900
#define RC_MAX_WIDTH_US                 2100

#define RC_NO_CHANGE_TIMEOUT_MS         20000L

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 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, uint8);
extern void SendParameters(uint8);
extern void SendMinPacket(void);
extern void SendArduStation(void);
extern void SendPIDTuning(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, UAVXParamPacketTag, UAVXMinPacketTag,
                 UAVXArmParamPacketTag, UAVXStickPacketTag,  UAVXCustomPacketTag, FrSkyPacketTag = 99
                };

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 real32 LPFilter(real32, real32, real32);
extern void CheckAlarms(void);
extern void Timing(uint8, uint32);

typedef struct {
    uint32 T;
    uint32 Count;
} TimingRec;

extern uint32 BeeperOnTime, BeeperOffTime;

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