Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
Diff: UAVXArm.h
- Revision:
- 1:1e3318a30ddd
- Parent:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
--- a/UAVXArm.h Fri Feb 18 22:28:05 2011 +0000 +++ b/UAVXArm.h Fri Feb 25 01:35:24 2011 +0000 @@ -1,25 +1,27 @@ // Commissioning defines -#define I2C_MAX_RATE_HZ 400000 +#define SW_I2C // define for software I2C - TRAGICALLY SLOW + +#define MAGIC 1.0 // rescales the sensitivity of all PID loop params -#define PWM_UPDATE_HZ 200 // MUST BE LESS THAN OR EAUAL TO 450HZ +#define I2C_MAX_RATE_HZ 400000 -#define MCP4725_ID 0xc8 // or 0xcc +#define PWM_UPDATE_HZ 200 // MUST BE LESS THAN OR EQUAL TO 450HZ +#define DISABLE_EXTRAS // suppress altitude hold, position hold and inertial compensation #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 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 @@ -593,7 +595,8 @@ #define ADXL345_ID 0x53 #endif // 6DOF -#define ADXL345_W ADXL345_ID +#define ADXL345_WR ADXL345_ID +#define ADXL345_RD (ADXL345_ID+1) extern const float GRAVITY_ADXL345; @@ -647,6 +650,7 @@ // analog.c +extern real32 ADC(uint8); extern void GetBattery(void); extern void BatteryTest(void); extern void InitBattery(void); @@ -766,14 +770,15 @@ #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_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 -#define MCP4725_WR MCP4725_ID -#define MCP4725_RD (MCP4725_ID+1) +#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 @@ -783,10 +788,13 @@ 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 @@ -845,6 +853,8 @@ // HMC5843 Compass #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); @@ -855,7 +865,9 @@ // HMC6352 -#define HMC6352_ID 0x42 +#define HMC6352_ID 0x42 +#define HMC6352_WR HMC6352_ID +#define HMC6352_RD (HMC6352_ID+1) extern void ReadHMC6352(void); extern uint8 WriteByteHMC6352(uint8); @@ -865,7 +877,10 @@ extern void InitHMC6352(void); extern boolean HMC6352Active(void); -typedef struct { real32 V; real32 Offset; } MagStruct; +typedef struct { + real32 V; + real32 Offset; +} MagStruct; extern MagStruct Mag[3]; extern real32 MagDeviation, CompassOffset; extern real32 MagHeading, Heading, FakeHeading; @@ -876,6 +891,8 @@ // control.c +extern real32 PTerm, ITerm, DTerm; //zzz + extern void DoAltitudeHold(void); extern void UpdateAltitudeSource(void); extern void AltitudeHold(void); @@ -898,6 +915,7 @@ extern int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle; extern int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredHeading, DesiredCamPitchTrim; extern real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP; +extern real32 CameraRollAngle, CameraPitchAngle; extern int16 CurrMaxRollPitch; extern int16 AttitudeHoldResetCount; @@ -1014,8 +1032,8 @@ #define ITG3200_ID 0xD2 #endif // 6DOF -#define ITG3200_R (ITG3200_ID+1) -#define ITG3200_W ITG3200_ID +#define ITG3200_WR ITG3200_ID +#define ITG3200_RD (ITG3200_ID+1) extern void ReadITG3200Gyro(void); extern uint8 ReadByteITG3200(uint8); @@ -1032,7 +1050,6 @@ // harness.c - extern void UpdateRTC(void); extern void InitHarness(void); @@ -1061,6 +1078,8 @@ 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 @@ -1068,9 +1087,37 @@ //extern PwmOut Out4; // 25 //extern PwmOut Out5; // 26 -extern DigitalOut DebugPin; // 25 +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); + void blockwrite(uint8 a, const char* b, uint8 l); + uint8 write(uint8 d); +}; + +extern MyI2C I2C0; // 27, 28 +extern DigitalInOut I2C0SCL; +extern DigitalInOut I2C0SDA; +#else extern I2C I2C0; // 27, 28 +#define blockread read +#define blockwrite write + +#endif // SW_I2C extern DigitalIn RCIn; // 29 CAN extern DigitalOut PWMCamRoll; // 30 CAN @@ -1094,9 +1141,13 @@ #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 @@ -1193,9 +1244,18 @@ // 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 +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); @@ -1359,16 +1419,21 @@ 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, BackC, RightC, LeftC }; // order is important for X3D & Holger ESCs +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 {FrontLeftC=0, FrontRightC}; -enum PWMY6Tags {FrontTC=0, LeftTC, RightTC, FrontBC, LeftBC, RightBC }; -enum PWMHexTags {FrontHC=0, FrontLeftHC, FrontRightHC, BackLeftHC, BackRightHC,BackHC }; +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; @@ -1593,7 +1658,8 @@ extern void SendNavPacket(void); extern void SendControlPacket(void); extern void SendStatsPacket(void); -extern void SendParamPacket(uint8); +extern void SendParamPacket(uint8, uint8); +extern void SendParameters(uint8); extern void SendMinPacket(void); extern void SendArduStation(void); extern void SendCustom(void); @@ -1606,7 +1672,8 @@ enum PacketTags {UnknownPacketTag = 0, LevPacketTag, NavPacketTag, MicropilotPacketTag, WayPacketTag, AirframePacketTag, NavUpdatePacketTag, BasicPacketTag, RestartPacketTag, TrimblePacketTag, MessagePacketTag, EnvironmentPacketTag, BeaconPacketTag, UAVXFlightPacketTag, - UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamsPacketTag, UAVXMinPacketTag, FrSkyPacketTag + UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamPacketTag, UAVXMinPacketTag, + UAVXArmParamPacketTag, UAVXStickPacketTag, FrSkyPacketTag = 99 }; enum TelemetryTypes { NoTelemetry, GPSTelemetry, UAVXTelemetry, UAVXControlTelemetry, UAVXMinTelemetry,