Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

sensors temp compensation: do an orb_copy to get the driver ID

Browse Source 
This removes the necessity, that the driver class ID matches the uorb topic
instance.

Also improve error handling & reporting
This commit is contained in:
Beat Küng
2017-01-19 08:26:41 +01:00
committed by Lorenz Meier
parent 41d220ac2d
commit ab9e0aa524
4 changed files with 82 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/temperature_compensation.cpp
+30 -6
View File
@@ -315,16 +315,28 @@ bool TemperatureCompensation::calc_thermal_offsets_3D(SensorCalData3D &coef, flo
int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance)
{
if (_parameters.gyro_tc_enable != 1) {
return 0;
}
return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data);
}
int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance)
{
if (_parameters.accel_tc_enable != 1) {
return 0;
}
return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data);
}
int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance)
{
if (_parameters.baro_tc_enable != 1) {
return 0;
}
return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data);
}
@@ -346,10 +358,14 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc
int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature,
float *offsets, float *scales)
{
if (_parameters.gyro_tc_enable != 1) {
return 0;
}
uint8_t mapping = _gyro_data.device_mapping[topic_instance];
if (mapping == 255 || _parameters.gyro_tc_enable != 1) {
return 0;
if (mapping == 255) {
return -1;
}
calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets);
@@ -373,10 +389,14 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Ve
int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data,
float temperature, float *offsets, float *scales)
{
if (_parameters.accel_tc_enable != 1) {
return 0;
}
uint8_t mapping = _accel_data.device_mapping[topic_instance];
if (mapping == 255 || _parameters.accel_tc_enable != 1) {
return 0;
if (mapping == 255) {
return -1;
}
calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
@@ -400,10 +420,14 @@ int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::V
int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
float *offsets, float *scales)
{
if (_parameters.baro_tc_enable != 1) {
return 0;
}
uint8_t mapping = _baro_data.device_mapping[topic_instance];
if (mapping == 255 || _parameters.baro_tc_enable != 1) {
return 0;
if (mapping == 255) {
return -1;
}
calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets);
src/modules/sensors/temperature_compensation.h
+2 -1
View File
@@ -78,7 +78,8 @@ public:
* @param temperature measured current temperature
* @param offsets returns offsets that were applied (length = 3, except for baro), depending on return value
* @param scales returns scales that were applied (length = 3), depending on return value
* @return 0: no changes (correction not enabled),
* @return -1: error: correction enabled, but no sensor mapping set (@see set_sendor_id_gyro)
* 0: no changes (correction not enabled),
* 1: corrections applied but no changes to offsets & scales,
* 2: corrections applied and offsets & scales updated
*/
src/modules/sensors/voted_sensors_update.cpp
+48 -20
View File
@@ -140,8 +140,56 @@ void VotedSensorsUpdate::parameters_update()
_board_rotation = board_rotation_offset * _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();
for (unsigned topic_instance = 0; topic_instance < SENSOR_COUNT_MAX; ++topic_instance) {
/* gyro */
if (topic_instance < _gyro.subscription_count) {
// valid subscription, so get the driver id by getting the published sensor data
struct gyro_report report;
if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) {
if (_temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance) < 0) {
PX4_ERR("gyro temp compensation init: failed to find device ID %u for instance %i",
report.device_id, topic_instance);
}
}
}
/* accel */
if (topic_instance < _accel.subscription_count) {
// valid subscription, so get the driver id by getting the published sensor data
struct accel_report report;
if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) {
if (_temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance) < 0) {
PX4_ERR("accel temp compensation init: failed to find device ID %u for instance %i",
report.device_id, topic_instance);
}
}
}
/* baro */
if (topic_instance < _baro.subscription_count) {
// valid subscription, so get the driver id by getting the published sensor data
struct baro_report report;
if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) {
if (_temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance) < 0) {
PX4_ERR("baro temp compensation init: failed to find device ID %u for instance %i",
report.device_id, topic_instance);
}
}
}
}
/* set offset parameters to new values */
bool failed;
char str[30];
@@ -162,8 +210,6 @@ void VotedSensorsUpdate::parameters_update()
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
_temperature_compensation.set_sensor_id_gyro(driver_device_id, s);
bool config_ok = false;
/* run through all stored calibrations that are applied at the driver level*/
@@ -230,8 +276,6 @@ void VotedSensorsUpdate::parameters_update()
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
_temperature_compensation.set_sensor_id_accel(driver_device_id, s);
bool config_ok = false;
/* run through all stored calibrations */
@@ -305,7 +349,6 @@ void VotedSensorsUpdate::parameters_update()
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
bool config_ok = false;
/* run through all stored calibrations */
@@ -421,21 +464,6 @@ void VotedSensorsUpdate::parameters_update()
}
}
/* run through all baro sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
(void)sprintf(str, "%s%u", BARO_BASE_DEVICE_PATH, s);
DevHandle h;
DevMgr::getHandle(str, h);
if (!h.isValid()) {
continue;
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
_temperature_compensation.set_sensor_id_baro(driver_device_id, s);
}
}
void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
src/modules/sensors/voted_sensors_update.h
+2 -1
View File
@@ -100,7 +100,8 @@ public:
float baro_pressure() const { return _last_best_baro_pressure; }
/**
* call this whenever parameters got updated
* call this whenever parameters got updated. Make sure to have initialize_sensors() called at least
* once before calling this.
*/
void parameters_update();