From 8dce5e11d678002d3df53b9beac5b54ca79ecbbc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 May 2015 08:42:37 +1000 Subject: [PATCH] HAL_SITL: removed earth-frame rates --- libraries/AP_HAL_SITL/sitl_ins.cpp | 12 +++--------- libraries/AP_HAL_SITL/sitl_optical_flow.cpp | 12 +++--------- 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp index ec9f3e5b55..37a352aee4 100644 --- a/libraries/AP_HAL_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_SITL/sitl_ins.cpp @@ -151,17 +151,11 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative double xAccel, double yAccel, double zAccel, // Local to plane float airspeed, float altitude) { - double p, q, r; - if (_ins == NULL) { // no inertial sensor in this sketch return; } - SITL::convert_body_frame(roll, pitch, - rollRate, pitchRate, yawRate, - &p, &q, &r); - // minimum noise levels are 2 bits, but averaged over many // samples, giving around 0.01 m/s/s float accel_noise = 0.01f; @@ -191,9 +185,9 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative _ins->set_accel(0, Vector3f(xAccel1, yAccel1, zAccel1) + _ins->get_accel_offsets(0)); _ins->set_accel(1, Vector3f(xAccel2, yAccel2, zAccel2) + _ins->get_accel_offsets(1)); - p += _gyro_drift(); - q += _gyro_drift(); - r += _gyro_drift(); + float p = radians(rollRate) + _gyro_drift(); + float q = radians(pitchRate) + _gyro_drift(); + float r = radians(yawRate) + _gyro_drift(); float p1 = p + gyro_noise * _rand_float(); float q1 = q + gyro_noise * _rand_float(); diff --git a/libraries/AP_HAL_SITL/sitl_optical_flow.cpp b/libraries/AP_HAL_SITL/sitl_optical_flow.cpp index 3af71ea111..06c06ce01f 100644 --- a/libraries/AP_HAL_SITL/sitl_optical_flow.cpp +++ b/libraries/AP_HAL_SITL/sitl_optical_flow.cpp @@ -33,7 +33,6 @@ static OpticalFlow::OpticalFlow_state optflow_data[MAX_OPTFLOW_DELAY]; */ void SITL_State::_update_flow(void) { - double p, q, r; Vector3f gyro; static uint32_t last_flow_ms; @@ -49,14 +48,9 @@ void SITL_State::_update_flow(void) } last_flow_ms = now; - // convert roll rates to body frame - SITL::convert_body_frame(_sitl->state.rollDeg, - _sitl->state.pitchDeg, - _sitl->state.rollRate, - _sitl->state.pitchRate, - _sitl->state.yawRate, - &p, &q, &r); - gyro(p, q, r); + gyro(radians(_sitl->state.rollRate), + radians(_sitl->state.pitchRate), + radians(_sitl->state.yawRate)); OpticalFlow::OpticalFlow_state state;