From 94f956a0ca183a667d83c9116db4712a84181611 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 15 Sep 2012 16:47:18 +0800 Subject: [PATCH] fix AP attitude hil - with randys help --- ArduPlane/system.pde | 5 ++--- libraries/AP_AHRS/AP_AHRS_HIL.h | 2 ++ libraries/AP_IMU/AP_IMU_Shim.cpp | 6 ++++++ libraries/AP_IMU/AP_IMU_Shim.h | 36 ++++++++++++++++++++++++-------- 4 files changed, 37 insertions(+), 12 deletions(-) create mode 100644 libraries/AP_IMU/AP_IMU_Shim.cpp diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index c7d54d92ca..7cfcd5042a 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -414,7 +414,6 @@ static void check_short_failsafe() static void startup_IMU_ground(bool force_accel_level) { -#if HIL_MODE != HIL_MODE_ATTITUDE gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); mavlink_delay(500); @@ -434,6 +433,7 @@ static void startup_IMU_ground(bool force_accel_level) ahrs.set_fly_forward(true); ahrs.reset(); +#if HIL_MODE != HIL_MODE_ATTITUDE // read Baro pressure at ground //----------------------------- init_barometer(); @@ -446,8 +446,7 @@ static void startup_IMU_ground(bool force_accel_level) gcs_send_text_P(SEVERITY_LOW,PSTR("NO airspeed")); } -#endif // HIL_MODE_ATTITUDE - +#endif digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate IMU ready digitalWrite(A_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF); diff --git a/libraries/AP_AHRS/AP_AHRS_HIL.h b/libraries/AP_AHRS/AP_AHRS_HIL.h index 67544e6709..469f799bff 100644 --- a/libraries/AP_AHRS/AP_AHRS_HIL.h +++ b/libraries/AP_AHRS/AP_AHRS_HIL.h @@ -22,7 +22,9 @@ public: // Methods void update(void) { + _imu->update(); } + void setHil(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate); diff --git a/libraries/AP_IMU/AP_IMU_Shim.cpp b/libraries/AP_IMU/AP_IMU_Shim.cpp new file mode 100644 index 0000000000..7cc8a91874 --- /dev/null +++ b/libraries/AP_IMU/AP_IMU_Shim.cpp @@ -0,0 +1,6 @@ + +#include "AP_IMU_Shim.h" + +uint16_t AP_IMU_Shim::_count; // number of samples captured +uint32_t AP_IMU_Shim::_first_sample_time_micros; // time first sample began (equal to the last sample time of the previous iteration) +uint32_t AP_IMU_Shim::_last_sample_time_micros; // time that the latest sample was captured diff --git a/libraries/AP_IMU/AP_IMU_Shim.h b/libraries/AP_IMU/AP_IMU_Shim.h index b51da1642f..5100735525 100644 --- a/libraries/AP_IMU/AP_IMU_Shim.h +++ b/libraries/AP_IMU/AP_IMU_Shim.h @@ -8,11 +8,15 @@ #include "IMU.h" +#define AP_IMU_SHIM_UPDATE_MICROS 10000 // 10000 micrxos = 10 ms = 100hz + class AP_IMU_Shim : public IMU { public: AP_IMU_Shim(void) { _product_id = AP_PRODUCT_ID_NONE; + _first_sample_time_micros = 0; + _last_sample_time_micros = micros(); } @@ -22,6 +26,7 @@ public: void (*callback)(unsigned long t) = delay, void (*flash_leds_cb)(bool on) = NULL, AP_PeriodicProcess * scheduler = NULL) { + scheduler->register_process( AP_IMU_Shim::read ); }; virtual void init_accel(void (*callback)(unsigned long t) = delay, void (*flash_leds_cb)(bool on) = NULL) { @@ -32,19 +37,21 @@ public: virtual bool update(void) { bool updated = _updated; _updated = false; - - // return number of microseconds since last call - uint32_t us = micros(); - uint32_t ret = us - last_ch6_micros; - last_ch6_micros = us; - - _sample_time = ret; + _count = 0; + _sample_time = _last_sample_time_micros - _first_sample_time_micros; + _first_sample_time_micros = _last_sample_time_micros; return updated; } //@} virtual bool new_data_available(void) { - return true; + return _count > 0; + } + + /// Get number of samples read from the sensors + /// @returns number of samples read from the sensors + uint16_t num_samples_available(void) { + return _count; } float gx() { @@ -101,9 +108,20 @@ public: private: + + // read function that pretends to capture new data + static void read(uint32_t tnow) { + _last_sample_time_micros = tnow; + _count++; + } + + static uint16_t _count; // number of samples captured + static uint32_t _first_sample_time_micros; // time first sample began (equal to the last sample time of the previous iteration) + static uint32_t _last_sample_time_micros; // time that the latest sample was captured + /// set true when new data is delivered bool _updated; - uint32_t last_ch6_micros; + }; #endif