From d11f952ef1de849e839fb9db8a71484cb9e00a31 Mon Sep 17 00:00:00 2001 From: Mike McCauley Date: Fri, 27 Sep 2013 07:59:44 +1000 Subject: [PATCH] AP_InertialSensor: Formalise and document Flymaple Gyro scaling factor --- libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index ac32539da4..d0be2ecf82 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -153,6 +153,8 @@ bool AP_InertialSensor_Flymaple::update(void) } Vector3f accel_scale = _accel_scale.get(); +// FIXME: +hal.gpio->write(4, 1); hal.scheduler->suspend_timer_procs(); // base the time on the gyro timestamp, as that is what is @@ -188,6 +190,9 @@ bool AP_InertialSensor_Flymaple::update(void) _gyro *= FLYMAPLE_GYRO_SCALE_R_S; _gyro -= _gyro_offset; +// FIXME: +hal.gpio->write(4, 0); + #if 0 // whats this all about???? if (_last_filter_hz != _mpu6000_filter) { @@ -251,8 +256,6 @@ void AP_InertialSensor_Flymaple::_accumulate(void) _gyro_sum_count++; _last_gyro_timestamp = hal.scheduler->micros(); } -// FIXME: -hal.gpio->write(4, 0); _in_accumulate = false; }