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

create temperature_compensation module

Browse Source 
- this is a new module for temperature compensation that consolidates the functionality previously handled in the sensors module (calculating runtime thermal corrections) and the events module (online thermal calibration)
 - by collecting this functionality into a single module we can optionally disable it on systems where it's not used and save some flash (if disabled at build time) or memory (disabled at run time)
This commit is contained in:
Daniel Agar
2020-01-20 21:42:42 -05:00
committed by GitHub
parent dedb4e8267
commit dc05ceaad2
87 changed files with 978 additions and 739 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+37 -28
View File
@@ -37,6 +37,7 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
VehicleAcceleration::VehicleAcceleration() :
ModuleParams(nullptr),
@@ -53,11 +54,15 @@ bool VehicleAcceleration::Start()
{
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
// sensor_selection needed to change the active sensor if the primary stops updating
if (!_sensor_selection_sub.registerCallback()) {
PX4_ERR("sensor_selection callback registration failed");
return false;
}
ScheduleNow();
return true;
}
void VehicleAcceleration::Stop()
@@ -97,7 +102,7 @@ void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
_sensor_correction_sub.copy(&corrections);
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_accel_instance) || force) {
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
@@ -122,8 +127,8 @@ void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
_scale = Vector3f{corrections.accel_scale_2};
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
@@ -154,8 +159,11 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
// force corrections reselection
_corrections_selected_instance = -1;
return true;
}
@@ -182,13 +190,13 @@ void VehicleAcceleration::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
math::radians(_param_sens_board_x_off.get()),
math::radians(_param_sens_board_y_off.get()),
math::radians(_param_sens_board_z_off.get())));
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;
}
@@ -204,28 +212,29 @@ void VehicleAcceleration::Run()
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_accel_s sensor_data;
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
Vector3f accel{(val - _offset).emult(_scale)};
// apply offsets and scale
Vector3f accel{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
accel = _board_rotation * accel;
// rotate corrected measurements from sensor to body frame
accel = _board_rotation * accel;
// correct for in-run bias errors
accel -= _bias;
// correct for in-run bias errors
accel -= _bias;
// publish
vehicle_acceleration_s out;
// publish
vehicle_acceleration_s out;
out.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
out.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(out);
_vehicle_acceleration_pub.publish(out);
}
}
}
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+1 -1
View File
@@ -96,9 +96,9 @@ private:
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
matrix::Vector3f _bias{0.f, 0.f, 0.f};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};