diff --git a/EKF/control.cpp b/EKF/control.cpp index 19e57815c1..4d3c0961ac 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -125,7 +125,7 @@ void Ekf::controlFusionModes() _range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth); // update range sensor angle parameters in case they have changed - _range_sensor.setTiltOffset(_params.rng_sens_pitch); + _range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index b70dbda89d..529af9db39 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -75,7 +75,7 @@ void Ekf::reset() _filter_initialised = false; _terrain_initialised = false; - _range_sensor.setTiltOffset(_params.rng_sens_pitch); + _range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _control_status.value = 0; diff --git a/EKF/sensor_range_finder.cpp b/EKF/sensor_range_finder.cpp index afa8a2b263..5fc8630d6a 100644 --- a/EKF/sensor_range_finder.cpp +++ b/EKF/sensor_range_finder.cpp @@ -55,7 +55,7 @@ void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth) { // calculate 2,2 element of rotation matrix from sensor frame to earth frame // this is required for use of range finder and flow data - _cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_tilt_rng + R_to_earth(2, 2) * _cos_tilt_rng; + _cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset; } void SensorRangeFinder::updateValidity(uint64_t current_time_us) diff --git a/EKF/sensor_range_finder.hpp b/EKF/sensor_range_finder.hpp index 6366b30bf5..4d226a2da9 100644 --- a/EKF/sensor_range_finder.hpp +++ b/EKF/sensor_range_finder.hpp @@ -67,12 +67,12 @@ public: // TODO: move the ring buffer here rangeSample* getSampleAddress() { return &_sample; } - void setTiltOffset(float new_tilt_offset) + void setPitchOffset(float new_pitch_offset) { - if (fabsf(_tilt_offset_rad - new_tilt_offset) > FLT_EPSILON) { - _sin_tilt_rng = sinf(new_tilt_offset); - _cos_tilt_rng = cosf(new_tilt_offset); - _tilt_offset_rad = new_tilt_offset; + if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) { + _sin_pitch_offset = sinf(new_pitch_offset); + _cos_pitch_offset = cosf(new_pitch_offset); + _pitch_offset_rad = new_pitch_offset; } } @@ -132,9 +132,9 @@ private: */ float _cos_tilt_rng_to_earth{}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame float _range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data - float _tilt_offset_rad{3.14f}; ///< range finder tilt rotation about the Y body axis - float _sin_tilt_rng{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis - float _cos_tilt_rng{-1.0f}; ///< cosine of the range finder tilt rotation about the Y body axis + float _pitch_offset_rad{3.14f}; ///< range finder tilt rotation about the Y body axis + float _sin_pitch_offset{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis + float _cos_pitch_offset{-1.0f}; ///< cosine of the range finder tilt rotation about the Y body axis /* * Range check diff --git a/test/test_SensorRangeFinder.cpp b/test/test_SensorRangeFinder.cpp index a39cb9b151..35fbe73a63 100644 --- a/test/test_SensorRangeFinder.cpp +++ b/test/test_SensorRangeFinder.cpp @@ -47,7 +47,7 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - _range_finder.setTiltOffset(0.f); + _range_finder.setPitchOffset(0.f); _range_finder.setCosMaxTilt(0.707f); _range_finder.setLimits(_min_range, _max_range); }