Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
Diff: UAVXArm.h
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
--- a/UAVXArm.h Fri Feb 25 01:35:24 2011 +0000 +++ b/UAVXArm.h Tue Apr 26 12:12:29 2011 +0000 @@ -1,27 +1,74 @@ // Commissioning defines -#define SW_I2C // define for software I2C - TRAGICALLY SLOW +//#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 MAGIC 1.0 // rescales the sensitivity of all PID loop params +//#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 -#define I2C_MAX_RATE_HZ 400000 +// 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 -#define PWM_UPDATE_HZ 200 // MUST BE LESS THAN OR EQUAL TO 450HZ +/* + 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_EXTRAS // suppress altitude hold, position hold and inertial compensation -#define SUPPRESS_SDCARD // no logging to check if buffering backup is an issue +*/ + +//#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 SOFTWARE_CAM_PWM - #define BATTERY_VOLTS_SCALE 57.85 // 51.8144 // Volts scaling for internal divider -//#define DCM_YAW_COMP +#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 @@ -29,7 +76,7 @@ // = UAVXArm Quadrocopter Controller = // = Copyright (c) 2008 by Prof. Greg Egan = // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = -// = http://code.google.com/p/uavp-mods/ http://uavp.ch = +// = http://code.google.com/p/uavp-mods/ = // =============================================================================================== // This is part of UAVXArm. @@ -80,7 +127,9 @@ #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) @@ -171,8 +220,9 @@ #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_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. @@ -199,16 +249,15 @@ // 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 +#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 DISABLE_DYNAMIC_MASS_COMP_ROLL -//#define DISABLE_DYNAMIC_MASS_COMP_PITCH +//#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 @@ -279,15 +328,6 @@ #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; @@ -365,6 +405,7 @@ #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 @@ -398,7 +439,7 @@ #define MAX_PARAMETERS 64 // parameters in PXPROM start at zero #define STATS_ADDR_PX ( PARAMS_ADDR_PX + (MAX_PARAMETERS *2) ) -#define MAX_STATS 64 +#define MAX_STATS 32 // 64 bytes // uses second Page of PXPROM #define NAV_ADDR_PX 256L @@ -421,7 +462,7 @@ // main.c enum Directions {BF, LR, UD, Alt }; // x forward, y left and z down -enum Angles { Pitch, Roll, Yaw }; // X, Y, Z +enum Angles { Roll, Pitch, Yaw }; // X, Y, Z #define FLAG_BYTES 10 #define TELEMETRY_FLAG_BYTES 6 @@ -469,7 +510,7 @@ 1, WayPointCentred: 1, -Unused2: // was UsingGPSAlt: +UnusedGPSAlt: // was UsingGPSAlt: 1, UsingRTHAutoDescend: 1, @@ -563,6 +604,8 @@ Using9DOF: 1, HaveBatterySensor: + 1, +AccMagnitudeOK: 1; }; } Flags; @@ -570,14 +613,12 @@ enum FlightStates { Starting, Landing, Landed, Shutdown, InFlight}; extern volatile Flags F; extern int8 State; +extern int8 r; //______________________________________________________________________________________________ // 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); @@ -598,16 +639,12 @@ #define ADXL345_WR ADXL345_ID #define ADXL345_RD (ADXL345_ID+1) -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 @@ -636,15 +673,20 @@ #define LISL_READ 0x80 #define LISL_ID 0x3a -extern void WriteLISL(uint8, uint8); +#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], Acc[3], AccNeutral[3]; +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 //______________________________________________________________________________________________ @@ -668,17 +710,36 @@ // attitude.c -enum AttitudeMethods { PremerlaniDCM, MadgwickIMU, MadgwickAHRS}; +enum AttitudeMethods { Integrator, Wolferl, Complementary, Kalman, PremerlaniDCM, MadgwickIMU, + MadgwickIMU2, MadgwickAHRS, MultiWii, MaxAttitudeScheme}; +extern void AdaptiveYawLPFreq(void); extern void GetAttitude(void); -extern void DoLegacyYawComp(void); +extern void DoLegacyYawComp(uint8); +extern void NormaliseAccelerations(void); extern void AttitudeTest(void); extern void InitAttitude(void); -extern real32 dT, dTR; +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); @@ -687,6 +748,7 @@ extern void DCMMotionCompensation(void); extern void DCMUpdate(void); extern void DCMEulerAngles(void); +extern void DoDCM(void); extern real32 RollPitchError[3]; extern real32 AccV[3]; @@ -701,12 +763,22 @@ // 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 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 @@ -718,7 +790,6 @@ 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); @@ -834,17 +905,12 @@ 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 SUPPRESS_COMPASS_FILTER -#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 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); @@ -852,6 +918,12 @@ // 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) @@ -865,6 +937,8 @@ // HMC6352 +#define HMC6352_UPDATE_S 0.05 // 20Hz + #define HMC6352_ID 0x42 #define HMC6352_WR HMC6352_ID #define HMC6352_RD (HMC6352_ID+1) @@ -883,7 +957,8 @@ } MagStruct; extern MagStruct Mag[3]; extern real32 MagDeviation, CompassOffset; -extern real32 MagHeading, Heading, FakeHeading; +extern real32 MagHeading, Heading, HeadingP, FakeHeading; +extern real32 CompassMaxSlew; extern real32 HeadingSin, HeadingCos; extern uint8 CompassType; @@ -891,10 +966,9 @@ // control.c -extern real32 PTerm, ITerm, DTerm; //zzz - extern void DoAltitudeHold(void); extern void UpdateAltitudeSource(void); +extern real32 AltitudeCF( real32, real32); extern void AltitudeHold(void); extern void LimitYawSum(void); @@ -905,18 +979,22 @@ extern void LightsAndSirens(void); extern void InitControl(void); -extern real32 Angle[3], Anglep[3], Rate[3], Ratep[3]; -extern real32 Comp[4]; +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, DesiredHeading, DesiredCamPitchTrim; +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; @@ -925,6 +1003,7 @@ extern boolean FirstPass; extern uint32 AltuSp; +extern uint32 ControlUpdateTimeuS; extern int16 DescentLimiter; extern int16 FakeDesiredPitch, FakeDesiredRoll, FakeDesiredYaw; @@ -1037,13 +1116,13 @@ extern void ReadITG3200Gyro(void); extern uint8 ReadByteITG3200(uint8); -extern void WriteByteITG3200(uint8, 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], GyroNeutral[3], Gyro[3]; // Radians +extern real32 GyroADC[3], GyroADCp[3], GyroNoise[3], GyroNeutral[3], Gyrop[3], Gyro[3]; // Radians extern uint8 GyroType; //______________________________________________________________________________________________ @@ -1055,6 +1134,8 @@ extern LocalFileSystem Flash; +extern uint8 I2C0SDAPin, I2C0SCLPin; + // 1 GND // 2 4.5-9V // 3 VBat @@ -1104,13 +1185,13 @@ void stop(void); uint8 blockread(uint8 r, char* b, uint8); uint8 read(uint8 r); - void blockwrite(uint8 a, const char* b, uint8 l); + boolean blockwrite(uint8 a, const char* b, uint8 l); uint8 write(uint8 d); }; extern MyI2C I2C0; // 27, 28 -extern DigitalInOut I2C0SCL; -extern DigitalInOut I2C0SDA; +extern PortInOut I2C0SCL; +extern PortInOut I2C0SDA; #else extern I2C I2C0; // 27, 28 @@ -1120,6 +1201,8 @@ #endif // SW_I2C extern DigitalIn RCIn; // 29 CAN +extern InterruptIn RCInterrupt; + extern DigitalOut PWMCamRoll; // 30 CAN //extern Serial TelemetrySerial; @@ -1134,8 +1217,6 @@ extern DigitalOut RedLED; extern DigitalOut YellowLED; -extern InterruptIn RCInterrupt; - extern char RTCString[], RTCLogfile[]; extern struct tm* RTCTime; @@ -1210,6 +1291,9 @@ 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, @@ -1230,6 +1314,11 @@ extern int8 SignalCount; extern uint16 RCGlitches; +extern Timer timer; +extern Timeout CamRollTimeout, CamPitchTimeout; +extern Ticker CameraTicker; +extern Timeout RCTimeout; + //______________________________________________________________________________________________ // ir.c @@ -1244,13 +1333,8 @@ // i2c.c -#ifdef SW_I2C -#define I2C_ACK 0 -#define I2C_NACK 1 -#else -#define I2C_ACK 1 -#define I2C_NACK 0 -#endif // SW_I2C +#define I2C_ACK true +#define I2C_NACK false extern boolean I2C0AddressResponds(uint8); #ifdef HAVE_I2C1 @@ -1262,8 +1346,10 @@ extern boolean ESCWaitClkHi(void); extern void ProgramSlaveAddress(uint8); extern void ConfigureESCs(void); +extern void InitI2C(void); extern uint32 MinI2CRate; +extern uint16 I2CError[256]; //______________________________________________________________________________________________ @@ -1399,6 +1485,16 @@ //______________________________________________________________________________________________ +// 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 @@ -1467,7 +1563,7 @@ RollKp, // 01 RollKi, // 02 RollKd, // 03 - HorizDampKp, // 04c + Unused04, // 04c HorizDampKp RollIntLimit, // 05 PitchKp, // 06 PitchKi, // 07 @@ -1477,16 +1573,16 @@ YawKp, // 11 YawKi, // 12 - YawKd, // 13 + Unused13, // 13 YawKd YawLimit, // 14 YawIntLimit, // 15 ConfigBits, // 16c - unused17 , // 17 TimeSlots + Unused17 , // 17 TimeSlots LowVoltThres, // 18c CamRollKp, // 19 PercentCruiseThr, // 20c - VertDampKp, // 21c + VertDamp, // 21c MiddleUD, // 22c PercentIdleThr, // 23c MiddleLR, // 24c @@ -1494,7 +1590,7 @@ CamPitchKp, // 26 CompassKp, // 27 AltKi, // 28c - unused29 , // 29 NavRadius + Unused29 , // 29 NavRadius NavKi, // 30 GSThrottle, // 31 @@ -1508,20 +1604,20 @@ PercentNavSens6Ch, // 39 CamRollTrim, // 40 NavKd, // 41 - VertDampDecay, // 42 - HorizDampDecay, // 43 + Unused42 , // 42 VertDampDecay + Unused43, // 43 HorizDampDecay BaroScale, // 44 TelemetryType, // 45 MaxDescentRateDmpS, // 46 DescentDelayS, // 47 NavIntLimit, // 48 AltIntLimit, // 49 - unused50, // 50 GravComp - unused51 , // 51 CompSteps + Unused50, // 50 GravComp + Unused51 , // 51 CompSteps ServoSense, // 52 CompassOffsetQtr, // 53 BatteryCapacity, // 54 - unused55, // 55 GyroYawType + Unused55, // 55 GyroYawType AltKd, // 56 Orient, // 57 NavYawLimit, // 58 @@ -1576,6 +1672,13 @@ // 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); @@ -1594,7 +1697,6 @@ extern int16x8x4Q PPMQ; extern boolean RCPositiveEdge; extern int16 RC[], RCp[]; -extern int16 Trim[3]; extern int16 ThrLow, ThrNeutral, ThrHigh; //__________________________________________________________________________________________ @@ -1635,8 +1737,7 @@ 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 + MinHDiluteS, MaxHDiluteS, RCGlitchesS, GPSBaroScaleS, GyroFailS, RCFailsafesS, I2CFailS, MinTempS, MaxTempS, BadS, BadNumS }; // NO MORE THAN 32 or 64 bytes extern int16 Stats[]; @@ -1662,6 +1763,7 @@ 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); @@ -1672,8 +1774,8 @@ enum PacketTags {UnknownPacketTag = 0, LevPacketTag, NavPacketTag, MicropilotPacketTag, WayPacketTag, AirframePacketTag, NavUpdatePacketTag, BasicPacketTag, RestartPacketTag, TrimblePacketTag, MessagePacketTag, EnvironmentPacketTag, BeaconPacketTag, UAVXFlightPacketTag, - UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamPacketTag, UAVXMinPacketTag, - UAVXArmParamPacketTag, UAVXStickPacketTag, FrSkyPacketTag = 99 + UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamPacketTag, UAVXMinPacketTag, + UAVXArmParamPacketTag, UAVXStickPacketTag, UAVXCustomPacketTag, FrSkyPacketTag = 99 }; enum TelemetryTypes { NoTelemetry, GPSTelemetry, UAVXTelemetry, UAVXControlTelemetry, UAVXMinTelemetry, @@ -1725,7 +1827,7 @@ extern void DoStartingBeeps(uint8); extern real32 SlewLimit(real32, real32, real32); extern real32 DecayX(real32, real32); -extern void LPFilter(real32*, real32*, real32, real32); +extern real32 LPFilter(real32, real32, real32); extern void CheckAlarms(void); extern void Timing(uint8, uint32); @@ -1734,6 +1836,8 @@ uint32 Count; } TimingRec; +extern uint32 BeeperOnTime, BeeperOffTime; + enum Timed { GetAttitudeT , UnknownT }; extern TimingRec Times[];