dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

change naming from tiltOffset to pitchOffset

This commit is contained in:
Kamil Ritz 2020-04-03 21:32:54 +02:00 committed by Mathieu Bresciani
parent 572ad2df0a
commit d6b6276cdc
5 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/control.cpp
View File

@ -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);
}

2
EKF/ekf.cpp
View File

@ -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;

2
EKF/sensor_range_finder.cpp
View File

@ -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)

16
EKF/sensor_range_finder.hpp
View File

@ -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

2
test/test_SensorRangeFinder.cpp
View File

@ -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);
}