fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:33:34 +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/voted_sensors_update.cpp
+12 -103
View File
@@ -84,7 +84,6 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
initializeSensors();
_corrections_changed = true; //make sure to initially publish the corrections topic
_selection_changed = true;
return 0;
@@ -132,76 +131,6 @@ void VotedSensorsUpdate::parametersUpdate()
_mag_rotation[topic_instance] = _board_rotation;
}
/* Load & apply the sensor calibrations.
* IMPORTANT: we assume all sensor drivers are running and published sensor data at this point
*/
/* temperature compensation */
_temperature_compensation.parameters_update(_hil_enabled);
/* gyro */
for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_gyro_integrated_s> report{ORB_ID(sensor_gyro_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "gyro", report.get().device_id,
topic_instance);
_corrections.gyro_mapping[topic_instance] = 0;
_corrections.gyro_device_ids[topic_instance] = 0;
} else {
_corrections.gyro_mapping[topic_instance] = temp;
_corrections.gyro_device_ids[topic_instance] = report.get().device_id;
}
}
/* accel */
for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_accel_integrated_s> report{ORB_ID(sensor_accel_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "accel", report.get().device_id,
topic_instance);
_corrections.accel_mapping[topic_instance] = 0;
_corrections.accel_device_ids[topic_instance] = 0;
} else {
_corrections.accel_mapping[topic_instance] = temp;
_corrections.accel_device_ids[topic_instance] = report.get().device_id;
}
}
/* baro */
for (uint8_t topic_instance = 0; topic_instance < _baro.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_baro_s> report{ORB_ID(sensor_baro), topic_instance};
int temp = _temperature_compensation.set_sensor_id_baro(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "baro", report.get().device_id,
topic_instance);
_corrections.baro_mapping[topic_instance] = 0;
_corrections.baro_device_ids[topic_instance] = 0;
} else {
_corrections.baro_mapping[topic_instance] = temp;
_corrections.baro_device_ids[topic_instance] = report.get().device_id;
}
}
/* set offset parameters to new values */
bool failed = false;
@@ -601,11 +530,10 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
accel_data(0) = (accel_data(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X
accel_data(1) = (accel_data(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y
accel_data(2) = (accel_data(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z
// rotate corrected measurements from sensor to body frame
accel_data = _board_rotation * accel_data;
@@ -631,8 +559,6 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
if (best_index != _accel.last_best_vote) {
_accel.last_best_vote = (uint8_t)best_index;
_corrections.selected_accel_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.accel_device_id != _accel_device_id[best_index]) {
@@ -685,11 +611,10 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
gyro_rate(0) = (gyro_rate(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X
gyro_rate(1) = (gyro_rate(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y
gyro_rate(2) = (gyro_rate(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z
// rotate corrected measurements from sensor to body frame
gyro_rate = _board_rotation * gyro_rate;
@@ -716,8 +641,6 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
if (_gyro.last_best_vote != best_index) {
_gyro.last_best_vote = (uint8_t)best_index;
_corrections.selected_gyro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.gyro_device_id != _gyro_device_id[best_index]) {
@@ -814,11 +737,8 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
// Convert from millibar to Pa
float corrected_pressure = 100.0f * baro_report.pressure;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
corrected_pressure = (corrected_pressure - *offsets[uorb_index]) * *scales[uorb_index];
// First publication with data
if (_baro.priority[uorb_index] == 0) {
@@ -850,8 +770,6 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
if (_baro.last_best_vote != best_index) {
_baro.last_best_vote = (uint8_t)best_index;
_corrections.selected_baro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.baro_device_id != _baro_device_id[best_index]) {
@@ -1001,27 +919,18 @@ void VotedSensorsUpdate::printStatus()
_mag.voter.print();
PX4_INFO("baro status:");
_baro.voter.print();
_temperature_compensation.print_status();
}
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata,
vehicle_magnetometer_s &magnetometer)
{
_corrections_sub.update(&_corrections);
accelPoll(raw);
gyroPoll(raw);
magPoll(magnetometer);
baroPoll(airdata);
// publish sensor corrections if necessary
if (_corrections_changed) {
_corrections.timestamp = hrt_absolute_time();
_sensor_correction_pub.publish(_corrections);
_corrections_changed = false;
}
// publish sensor selection if changed
if (_selection_changed) {
_selection.timestamp = hrt_absolute_time();