From 392160314ebe7b835c5a29a38e4f9c3f0e0c1af4 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Thu, 13 Oct 2011 22:22:03 +0800 Subject: [PATCH] Sensor hil fix's --- ArduPlane/ArduPlane.pde | 2 +- ArduPlane/GCS_Mavlink.pde | 4 +- ArduPlane/config.h | 18 --------- ArduPlane/sensors.pde | 2 +- libraries/APM_BMP085/APM_BMP085.cpp | 42 +-------------------- libraries/APM_BMP085/APM_BMP085.h | 19 +--------- libraries/APM_BMP085/APM_BMP085_hil.cpp | 50 +++++++++++++++++++++++++ libraries/APM_BMP085/APM_BMP085_hil.h | 24 ++++++++++++ libraries/AP_IMU/AP_IMU_Shim.h | 2 +- 9 files changed, 82 insertions(+), 81 deletions(-) create mode 100644 libraries/APM_BMP085/APM_BMP085_hil.cpp create mode 100644 libraries/APM_BMP085/APM_BMP085_hil.h diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f3651d3da0..729d2e9ddf 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -483,7 +483,7 @@ static void fast_loop() #if HIL_MODE == HIL_MODE_SENSORS // update hil before dcm update - hil.update(); + gcs_update(); #endif dcm.update_DCM(); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 77da01321e..d9149c0b06 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1401,8 +1401,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // TODO: check scaling for temp/absPress float temp = 70; float absPress = 1; - // Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc); - // Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro); + Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc); + Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro); // rad/sec Vector3f gyros; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index be76ce2426..be0e3729f9 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -83,24 +83,6 @@ # define GPS_PROTOCOL GPS_PROTOCOL_NONE #endif - -// If we are in XPlane, diasble the mag -#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode - - // check xplane settings - #if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE - - // MAGNETOMETER not supported by XPLANE - # undef MAGNETOMETER - # define MAGNETOMETER DISABLED - - # if HIL_MODE != HIL_MODE_ATTITUDE - # error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE - # endif - - #endif -#endif - ////////////////////////////////////////////////////////////////////////////// // GPS_PROTOCOL // diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index aeaeb6aacc..a4da47bba4 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -22,7 +22,7 @@ static void init_barometer(void) for(int i = 0; i < 30; i++){ // We take some readings... #if HIL_MODE == HIL_MODE_SENSORS - hil.update(); // look for inbound hil packets + gcs_update(); // look for inbound hil packets #endif barometer.Read(); // Get initial data from absolute pressure sensor diff --git a/libraries/APM_BMP085/APM_BMP085.cpp b/libraries/APM_BMP085/APM_BMP085.cpp index 511784240b..22b6806b57 100644 --- a/libraries/APM_BMP085/APM_BMP085.cpp +++ b/libraries/APM_BMP085/APM_BMP085.cpp @@ -286,44 +286,4 @@ void APM_BMP085_Class::Calculate() x1 = (x1 * 3038) >> 16; x2 = (-7357 * p) >> 16; Press = p + ((x1 + x2 + 3791) >> 4); -} - -// Constructors //////////////////////////////////////////////////////////////// -APM_BMP085_HIL_Class::APM_BMP085_HIL_Class() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void APM_BMP085_HIL_Class::Init(int initialiseWireLib) -{ - BMP085_State=1; -} - - -// Read the sensor. This is a state machine -// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5) -uint8_t APM_BMP085_HIL_Class::Read() -{ - uint8_t result = 0; - - if (BMP085_State == 1){ - BMP085_State++; - }else{ - - if (BMP085_State == 5){ - BMP085_State = 1; // Start again from state = 1 - result = 1; // New pressure reading - }else{ - BMP085_State++; - result = 1; // New pressure reading - } - } - return(result); -} - -void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press) -{ - // TODO: map floats to raw - Temp = _Temp; - Press = _Press; -} +} \ No newline at end of file diff --git a/libraries/APM_BMP085/APM_BMP085.h b/libraries/APM_BMP085/APM_BMP085.h index e8a5085d0f..7b75c7c865 100644 --- a/libraries/APM_BMP085/APM_BMP085.h +++ b/libraries/APM_BMP085/APM_BMP085.h @@ -4,6 +4,8 @@ #define TEMP_FILTER_SIZE 16 #define PRESS_FILTER_SIZE 10 +#include + class APM_BMP085_Class { public: @@ -43,21 +45,4 @@ class APM_BMP085_Class void Calculate(); }; -class APM_BMP085_HIL_Class -{ - private: - uint8_t BMP085_State; - public: - int32_t RawPress; - int32_t RawTemp; - int16_t Temp; - int32_t Press; - //int Altitude; - uint8_t oss; - APM_BMP085_HIL_Class(); // Constructor - void Init(int initialiseWireLib = 1); - uint8_t Read(); - void setHIL(float Temp, float Press); -}; - #endif diff --git a/libraries/APM_BMP085/APM_BMP085_hil.cpp b/libraries/APM_BMP085/APM_BMP085_hil.cpp new file mode 100644 index 0000000000..21fc3dce5a --- /dev/null +++ b/libraries/APM_BMP085/APM_BMP085_hil.cpp @@ -0,0 +1,50 @@ + + +extern "C" { + // AVR LibC Includes + #include + #include + #include "WConstants.h" +} + +#include "APM_BMP085_hil.h" + +// Constructors //////////////////////////////////////////////////////////////// +APM_BMP085_HIL_Class::APM_BMP085_HIL_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void APM_BMP085_HIL_Class::Init(int initialiseWireLib) +{ + BMP085_State=1; +} + + +// Read the sensor. This is a state machine +// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5) +uint8_t APM_BMP085_HIL_Class::Read() +{ + uint8_t result = 0; + + if (BMP085_State == 1){ + BMP085_State++; + }else{ + + if (BMP085_State == 5){ + BMP085_State = 1; // Start again from state = 1 + result = 1; // New pressure reading + }else{ + BMP085_State++; + result = 1; // New pressure reading + } + } + return(result); +} + +void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press) +{ + // TODO: map floats to raw + Temp = _Temp; + Press = _Press; +} diff --git a/libraries/APM_BMP085/APM_BMP085_hil.h b/libraries/APM_BMP085/APM_BMP085_hil.h new file mode 100644 index 0000000000..9a316b64a4 --- /dev/null +++ b/libraries/APM_BMP085/APM_BMP085_hil.h @@ -0,0 +1,24 @@ +#ifndef APM_BMP085_hil_h +#define APM_BMP085_hil_h + +#define TEMP_FILTER_SIZE 16 +#define PRESS_FILTER_SIZE 10 + +class APM_BMP085_HIL_Class +{ + private: + uint8_t BMP085_State; + public: + APM_BMP085_HIL_Class(); // Constructor + int32_t RawPress; + int32_t RawTemp; + int16_t Temp; + int32_t Press; + //int Altitude; + uint8_t oss; + void Init(int initialiseWireLib = 1); + uint8_t Read(); + void setHIL(float Temp, float Press); +}; + +#endif diff --git a/libraries/AP_IMU/AP_IMU_Shim.h b/libraries/AP_IMU/AP_IMU_Shim.h index 6995a22b79..57bf298414 100644 --- a/libraries/AP_IMU/AP_IMU_Shim.h +++ b/libraries/AP_IMU/AP_IMU_Shim.h @@ -13,7 +13,7 @@ public: /// @name IMU protocol //@{ - virtual void init(Start_style style, void (*callback)(unsigned long t)) {} + virtual void init(Start_style style = COLD_START, void (*callback)(unsigned long t) = delay) {}; virtual void init_accel(void (*callback)(unsigned long t) = delay) {}; virtual void init_gyro(void (*callback)(unsigned long t) = delay) {}; virtual bool update(void) {