From ac3a677626b7e5c00519e3acced224f73679031c Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Fri, 28 Aug 2015 12:18:09 -0300 Subject: [PATCH] AP_InertialSensor: remove param rotate_and_correct from publish functions Once that parameter is always false. --- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 12 ++---------- .../AP_InertialSensor/AP_InertialSensor_Backend.h | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_Flymaple.cpp | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_MPU9150.cpp | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_MPU9250.cpp | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_Oilpan.cpp | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_PX4.cpp | 4 ++-- 10 files changed, 20 insertions(+), 28 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 4ec9a60bb5..457c0dd396 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -47,14 +47,10 @@ void AP_InertialSensor_Backend::_publish_delta_angle(uint8_t instance, const Vec /* rotate gyro vector and add the gyro offset */ -void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro, bool rotate_and_correct) +void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro) { _imu._gyro[instance] = gyro; _imu._gyro_healthy[instance] = true; - - if (rotate_and_correct) { - _rotate_and_correct_gyro(instance, _imu._gyro[instance]); - } } void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity, float dt) @@ -68,14 +64,10 @@ void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const /* rotate accel vector, scale and add the accel offset */ -void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct) +void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel) { _imu._accel[instance] = accel; _imu._accel_healthy[instance] = true; - - if (rotate_and_correct) { - _rotate_and_correct_accel(instance, _imu._accel[instance]); - } } void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 4938cec233..b71f840130 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -83,10 +83,10 @@ protected: void _publish_delta_angle(uint8_t instance, const Vector3f &delta_angle); // rotate gyro vector, offset and publish - void _publish_gyro(uint8_t instance, const Vector3f &gyro, bool rotate_and_correct = true); + void _publish_gyro(uint8_t instance, const Vector3f &gyro); // rotate accel vector, scale, offset and publish - void _publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct = true); + void _publish_accel(uint8_t instance, const Vector3f &accel); // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index c5f34474a4..4dbe25995b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -175,8 +175,8 @@ bool AP_InertialSensor_Flymaple::update(void) _have_accel_sample = false; hal.scheduler->resume_timer_procs(); - _publish_accel(_accel_instance, accel, false); - _publish_gyro(_gyro_instance, gyro, false); + _publish_accel(_accel_instance, accel); + _publish_gyro(_gyro_instance, gyro); if (_last_filter_hz != _accel_filter_cutoff()) { _set_filter_frequency(_accel_filter_cutoff()); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 967980da1f..0a2bb8924b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -271,8 +271,8 @@ bool AP_InertialSensor_L3G4200D::update(void) _have_sample = false; pthread_spin_unlock(&_data_lock); - _publish_accel(_accel_instance, accel, false); - _publish_gyro(_gyro_instance, gyro, false); + _publish_accel(_accel_instance, accel); + _publish_gyro(_gyro_instance, gyro); if (_last_filter_hz != _accel_filter_cutoff()) { _set_filter_frequency(_accel_filter_cutoff()); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 4192859864..6d7c784e98 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -765,8 +765,8 @@ bool AP_InertialSensor_LSM9DS0::update() _accel_sample_available = false; _gyro_sample_available = false; - _publish_gyro(_gyro_instance, _gyro_filtered, false); - _publish_accel(_accel_instance, _accel_filtered, false); + _publish_gyro(_gyro_instance, _gyro_filtered); + _publish_accel(_accel_instance, _accel_filtered); if (_last_accel_filter_hz != _accel_filter_cutoff()) { _set_accel_filter(_accel_filter_cutoff()); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 04dc90b94e..94b94b5834 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -666,8 +666,8 @@ bool AP_InertialSensor_MPU6000::update( void ) gyro /= num_samples; accel /= num_samples; - _publish_accel(_accel_instance, accel, false); - _publish_gyro(_gyro_instance, gyro, false); + _publish_accel(_accel_instance, accel); + _publish_gyro(_gyro_instance, gyro); #if MPU6000_FAST_SAMPLING if (_last_accel_filter_hz != _accel_filter_cutoff()) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp index 90095a8b04..b73ec3a6fa 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp @@ -1116,8 +1116,8 @@ bool AP_InertialSensor_MPU9150::update(void) _have_sample_available = false; hal.scheduler->resume_timer_procs(); - _publish_accel(_accel_instance, accel, false); - _publish_gyro(_gyro_instance, gyro, false); + _publish_accel(_accel_instance, accel); + _publish_gyro(_gyro_instance, gyro); if (_last_accel_filter_hz != _accel_filter_cutoff()) { _set_accel_filter_frequency(_accel_filter_cutoff()); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 9a70c2fa4a..613c851e3d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -330,8 +330,8 @@ bool AP_InertialSensor_MPU9250::update( void ) _have_sample_available = false; - _publish_gyro(_gyro_instance, gyro, false); - _publish_accel(_accel_instance, accel, false); + _publish_gyro(_gyro_instance, gyro); + _publish_accel(_accel_instance, accel); if (_last_accel_filter_hz != _accel_filter_cutoff()) { _set_accel_filter(_accel_filter_cutoff()); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 8ad731131f..361a8a9a6f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -105,7 +105,7 @@ bool AP_InertialSensor_Oilpan::update() _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y, _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z); _rotate_and_correct_gyro(_gyro_instance, v); - _publish_gyro(_gyro_instance, v, false); + _publish_gyro(_gyro_instance, v); // copy accels to frontend v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET), @@ -113,7 +113,7 @@ bool AP_InertialSensor_Oilpan::update() _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET)); v *= OILPAN_ACCEL_SCALE_1G; _rotate_and_correct_accel(_accel_instance, v); - _publish_accel(_accel_instance, v, false); + _publish_accel(_accel_instance, v); return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 98b154e697..2f15333060 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -216,7 +216,7 @@ bool AP_InertialSensor_PX4::update(void) // calling _publish_accel sets the sensor healthy, // so we only want to do this if we have new data from it if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) { - _publish_accel(_accel_instance[k], accel, false); + _publish_accel(_accel_instance[k], accel); _publish_delta_velocity(_accel_instance[k], _delta_velocity_accumulator[k], _delta_velocity_dt[k]); _last_accel_update_timestamp[k] = _last_accel_timestamp[k]; } @@ -227,7 +227,7 @@ bool AP_InertialSensor_PX4::update(void) // calling _publish_accel sets the sensor healthy, // so we only want to do this if we have new data from it if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) { - _publish_gyro(_gyro_instance[k], gyro, false); + _publish_gyro(_gyro_instance[k], gyro); _publish_delta_angle(_gyro_instance[k], _delta_angle_accumulator[k]); _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k]; }