fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: refactor common corrections and rotation code

Browse Source
This commit is contained in:
Daniel Agar
2020-02-04 09:44:40 -05:00
parent ca96b6b0ea
commit 36c6e36b9d
14 changed files with 403 additions and 322 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+14 -67
View File
@@ -37,11 +37,14 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
namespace sensors
{
VehicleAngularVelocity::VehicleAngularVelocity() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_corrections(this, SensorCorrections::SensorType::Gyroscope)
{
_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
_notch_filter_velocity.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
@@ -150,46 +153,6 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force)
}
}
void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections{};
_sensor_correction_sub.copy(&corrections);
// selected sensor has changed, find updated index
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.gyro_device_ids[i] == _selected_sensor_device_id) {
_corrections_selected_instance = i;
}
}
}
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.gyro_offset_0};
_scale = Vector3f{corrections.gyro_scale_0};
break;
case 1:
_offset = Vector3f{corrections.gyro_offset_1};
_scale = Vector3f{corrections.gyro_scale_1};
break;
case 2:
_offset = Vector3f{corrections.gyro_offset_2};
_scale = Vector3f{corrections.gyro_scale_2};
break;
default:
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
{
if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
@@ -216,11 +179,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
// force corrections reselection
_corrections_selected_instance = -1;
_corrections.set_device_id(report.device_id);
// reset sample rate monitor
_sample_rate_incorrect_count = 0;
@@ -249,16 +209,7 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
_corrections.ParametersUpdate();
}
}
@@ -267,7 +218,7 @@ void VehicleAngularVelocity::Run()
// update corrections first to set _selected_sensor
bool selection_updated = SensorSelectionUpdate();
SensorCorrectionsUpdate(selection_updated);
_corrections.SensorCorrectionsUpdate(selection_updated);
SensorBiasUpdate(selection_updated);
ParametersUpdate();
@@ -290,10 +241,10 @@ void VehicleAngularVelocity::Run()
_timestamp_sample_prev = sensor_data.timestamp_sample;
// get the sensor data and correct for thermal errors (apply offsets and scale)
Vector3f angular_velocity_raw{(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z} - _offset).emult(_scale)};
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// rotate corrected measurements from sensor to body frame
angular_velocity_raw = _board_rotation * angular_velocity_raw;
// correct for in-run bias errors
Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias;
// correct for in-run bias errors
angular_velocity_raw -= _bias;
@@ -352,14 +303,10 @@ void VehicleAngularVelocity::PrintStatus()
{
PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter_velocity.get_cutoff_freq());
if (_notch_filter_velocity.getNotchFreq() > 0.0f) {
PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter_velocity.getNotchFreq(),
(double)_notch_filter_velocity.getBandwidth());
}
_corrections.PrintStatus();
}
} // namespace sensors