fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: no temperature compensation in HITL

Browse Source 
Temperature compensation is not supported in HITL. Therefore, don't try
and fail. This basically removes the confusing error message that we get
in HITL.
This commit is contained in:
Julian Oes 2018-07-24 13:40:03 +02:00 committed by Lorenz Meier
parent ee3e34cd06
commit 80cac6561f
4 changed files with 35 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/sensors/sensors.cpp
View File

@ -95,6 +95,7 @@
using namespace DriverFramework;
using namespace sensors;
using namespace time_literals;
/**
* Analog layout:
@ -584,13 +585,11 @@ Sensors::run()
}
sensor_combined_s raw = {};
sensor_preflight_s preflt = {};
vehicle_air_data_s airdata = {};
vehicle_magnetometer_s magnetometer = {};
struct sensor_preflight_s preflt = {};
_rc_update.init();
_voted_sensors_update.init(raw);
/* (re)load params and calibration */
@ -600,11 +599,8 @@ Sensors::run()
* do subscriptions
*/
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
/* get a set of initial values */
@ -614,18 +610,10 @@ Sensors::run()
_rc_update.rc_parameter_map_poll(_parameter_handles, true /* forced */);
/* advertise the sensor_preflight topic and make the initial publication */
preflt.accel_inconsistency_m_s_s = 0.0f;
preflt.gyro_inconsistency_rad_s = 0.0f;
preflt.mag_inconsistency_ga = 0.0f;
_sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt);
/* wakeup source */
px4_pollfd_struct_t poll_fds = {};
poll_fds.events = POLLIN;
uint64_t last_config_update = hrt_absolute_time();
@ -704,7 +692,7 @@ Sensors::run()
/* keep adding sensors as long as we are not armed,
* when not adding sensors poll for param updates
*/
if (!_armed && hrt_elapsed_time(&last_config_update) > 500 * 1000) {
if (!_armed && hrt_elapsed_time(&last_config_update) > 500_ms) {
_voted_sensors_update.initialize_sensors();
last_config_update = hrt_absolute_time();
@ -846,7 +834,7 @@ Sensors *Sensors::instantiate(int argc, char *argv[])
return nullptr;
}
return new Sensors(hil_enabled);;
return new Sensors(hil_enabled);
}
int sensors_main(int argc, char *argv[])

23
src/modules/sensors/temperature_compensation.cpp
View File

@ -151,7 +151,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
return PX4_OK;
}
int TemperatureCompensation::parameters_update()
int TemperatureCompensation::parameters_update(bool hil_enabled)
{
int ret = 0;
@ -163,7 +163,12 @@ int TemperatureCompensation::parameters_update()
}
/* rate gyro calibration parameters */
param_get(parameter_handles.gyro_tc_enable, &(_parameters.gyro_tc_enable));
if (!hil_enabled) {
param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable);
} else {
_parameters.gyro_tc_enable = 0;
}
if (_parameters.gyro_tc_enable == 1) {
for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) {
@ -196,7 +201,12 @@ int TemperatureCompensation::parameters_update()
}
/* accelerometer calibration parameters */
param_get(parameter_handles.accel_tc_enable, &(_parameters.accel_tc_enable));
if (!hil_enabled) {
param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable);
} else {
_parameters.accel_tc_enable = 0;
}
if (_parameters.accel_tc_enable == 1) {
for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
@ -229,7 +239,12 @@ int TemperatureCompensation::parameters_update()
}
/* barometer calibration parameters */
param_get(parameter_handles.baro_tc_enable, &(_parameters.baro_tc_enable));
if (!hil_enabled) {
param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable);
} else {
_parameters.baro_tc_enable = 0;
}
if (_parameters.baro_tc_enable == 1) {
for (unsigned j = 0; j < BARO_COUNT_MAX; j++) {

2
src/modules/sensors/temperature_compensation.h
View File

@ -68,7 +68,7 @@ class TemperatureCompensation
public:
/** (re)load the parameters. Make sure to call this on startup as well */
int parameters_update();
int parameters_update(bool hil_enabled = false);
/** supply information which device_id matches a specific uORB topic_instance
* (needed if a system has multiple sensors of the same type)

28
src/modules/sensors/voted_sensors_update.cpp
View File

@ -148,7 +148,7 @@ void VotedSensorsUpdate::parameters_update()
*/
/* temperature compensation */
_temperature_compensation.parameters_update();
_temperature_compensation.parameters_update(_hil_enabled);
/* gyro */
for (int topic_instance = 0; topic_instance < GYRO_COUNT_MAX; ++topic_instance) {
@ -602,11 +602,9 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
}
// handle temperature compensation
if (!_hil_enabled) {
if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// rotate corrected measurements from sensor to body frame
@ -711,11 +709,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
}
// handle temperature compensation
if (!_hil_enabled) {
if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// rotate corrected measurements from sensor to body frame
@ -829,11 +825,9 @@ void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata)
float corrected_pressure = 100.0f * baro_report.pressure;
// handle temperature compensation
if (!_hil_enabled) {
if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// First publication with data
@ -1080,7 +1074,7 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw, vehicle_air_data_s
baro_poll(airdata);
// publish sensor corrections if necessary
if (!_hil_enabled && _corrections_changed) {
if (_corrections_changed) {
_corrections.timestamp = hrt_absolute_time();
if (_sensor_correction_pub == nullptr) {