dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

WIP - sensors: enable multi airspeed

This commit is contained in:
fury1895 2024-02-09 17:26:12 +01:00
parent 8243b4f474
commit f554f6bf95
3 changed files with 144 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/logger/logged_topics.cpp
View File

@ -48,7 +48,6 @@ void LoggedTopics::add_default_topics()
add_topic("action_request");
add_topic("actuator_armed");
add_optional_topic("actuator_controls_status_0", 300);
add_topic("airspeed", 1000);
add_optional_topic("airspeed_validated", 200);
add_optional_topic("autotune_attitude_control_status", 100);
add_optional_topic("camera_capture");
@ -139,6 +138,7 @@ void LoggedTopics::add_default_topics()
// multi topics
add_optional_topic_multi("actuator_outputs", 100, 3);
add_optional_topic_multi("airspeed", 1000, 3);
add_optional_topic_multi("airspeed_wind", 1000, 4);
add_optional_topic_multi("control_allocator_status", 200, 2);
add_optional_topic_multi("rate_ctrl_status", 200, 2);

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

@ -59,6 +59,8 @@ Sensors::Sensors(bool hil_enabled) :
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
InitializeVehicleDifferentialPressure();
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
@ -67,8 +69,11 @@ Sensors::Sensors(bool hil_enabled) :
_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
_airspeed_validator.set_timeout(300000);
_airspeed_validator.set_equal_value_threshold(100);
for (int i = 0; i < _number_of_differential_pressure_sensors; i++) {
_diff_pres_data[i].airspeed_validator.set_timeout(300000);
_diff_pres_data[i].airspeed_validator.set_equal_value_threshold(100);
}
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
@ -260,106 +265,113 @@ int Sensors::parameters_update()
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
void Sensors::diff_pres_poll()
{
differential_pressure_s diff_pres{};
vehicle_air_data_s air_data{};
_vehicle_air_data_sub.copy(&air_data);
if (_diff_pres_sub.update(&diff_pres)) {
for (int i = 0; i < _number_of_differential_pressure_sensors; i++) {
if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
// ignore invalid data and reset accumulated
// poll raw differential sensor topic of the i-th sensor
differential_pressure_s diff_pres{};
// reset
_diff_pres_timestamp_sum = 0;
_diff_pres_pressure_sum = 0;
_diff_pres_temperature_sum = 0;
_baro_pressure_sum = 0;
_diff_pres_count = 0;
return;
}
if (_differential_pressure_subs[i].update(&diff_pres)) {
vehicle_air_data_s air_data{};
_vehicle_air_data_sub.copy(&air_data);
if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
// ignore invalid data and reset accumulated
float air_temperature_celsius = NAN;
// assume anything outside of a (generous) operating range of -40C to 125C is invalid
if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {
air_temperature_celsius = diff_pres.temperature;
} else {
// differential pressure temperature invalid, check barometer
if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
&& (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
}
}
// push raw data into validator
float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f };
_airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
// accumulate average for publication
_diff_pres_timestamp_sum += diff_pres.timestamp_sample;
_diff_pres_pressure_sum += diff_pres.differential_pressure_pa;
_diff_pres_temperature_sum += air_temperature_celsius;
_baro_pressure_sum += air_data.baro_pressure_pa;
_diff_pres_count++;
if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 50_ms) {
// average data and apply calibration offset (SENS_DPRES_OFF)
const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count;
const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa;
const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count;
const float temperature = _diff_pres_temperature_sum / _diff_pres_count;
// reset
_diff_pres_timestamp_sum = 0;
_diff_pres_pressure_sum = 0;
_diff_pres_temperature_sum = 0;
_baro_pressure_sum = 0;
_diff_pres_count = 0;
enum AIRSPEED_SENSOR_MODEL smodel;
switch ((diff_pres.device_id >> 16) & 0xFF) {
case DRV_DIFF_PRESS_DEVTYPE_SDP31:
// fallthrough
case DRV_DIFF_PRESS_DEVTYPE_SDP32:
// fallthrough
case DRV_DIFF_PRESS_DEVTYPE_SDP33:
smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
break;
default:
smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
break;
// reset
_diff_pres_data[i].diff_pres_timestamp_sum = 0;
_diff_pres_data[i].diff_pres_pressure_sum = 0;
_diff_pres_data[i].diff_pres_temperature_sum = 0;
_diff_pres_data[i].baro_pressure_sum = 0;
_diff_pres_data[i].diff_pres_count = 0;
return;
}
float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
differential_pressure_pa, baro_pressure_pa, temperature);
float air_temperature_celsius = NAN;
// assume that CAS = IAS as we don't have an CAS-scale here
float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature);
// assume anything outside of a (generous) operating range of -40C to 125C is invalid
if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {
if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
air_temperature_celsius = diff_pres.temperature;
airspeed_s airspeed;
airspeed.timestamp_sample = timestamp_sample;
airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s;
airspeed.true_airspeed_m_s = true_airspeed_m_s;
airspeed.air_temperature_celsius = temperature;
airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
airspeed.timestamp = hrt_absolute_time();
_airspeed_pub.publish(airspeed);
} else {
// differential pressure temperature invalid, check barometer
if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
&& (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
_airspeed_last_publish = airspeed.timestamp;
// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
}
}
// push raw data into validator
float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f };
_diff_pres_data[i].airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count,
100); // TODO: real priority?
// accumulate average for publication
_diff_pres_data[i].diff_pres_timestamp_sum += diff_pres.timestamp_sample;
_diff_pres_data[i].diff_pres_pressure_sum += diff_pres.differential_pressure_pa;
_diff_pres_data[i].diff_pres_temperature_sum += air_temperature_celsius;
_diff_pres_data[i].baro_pressure_sum += air_data.baro_pressure_pa;
_diff_pres_data[i].diff_pres_count++;
if ((_diff_pres_data[i].diff_pres_count > 0) && hrt_elapsed_time(&_diff_pres_data[i].airspeed_last_publish) >= 50_ms) {
// average data and apply calibration offset (SENS_DPRES_OFF)
const uint64_t timestamp_sample = _diff_pres_data[i].diff_pres_timestamp_sum / _diff_pres_data[i].diff_pres_count;
const float differential_pressure_pa = _diff_pres_data[i].diff_pres_pressure_sum / _diff_pres_data[i].diff_pres_count -
_parameters.diff_pres_offset_pa;
const float baro_pressure_pa = _diff_pres_data[i].baro_pressure_sum / _diff_pres_data[i].diff_pres_count;
const float temperature = _diff_pres_data[i].diff_pres_temperature_sum / _diff_pres_data[i].diff_pres_count;
// reset
_diff_pres_data[i].diff_pres_timestamp_sum = 0;
_diff_pres_data[i].diff_pres_pressure_sum = 0;
_diff_pres_data[i].diff_pres_temperature_sum = 0;
_diff_pres_data[i].baro_pressure_sum = 0;
_diff_pres_data[i].diff_pres_count = 0;
enum AIRSPEED_SENSOR_MODEL smodel;
switch ((diff_pres.device_id >> 16) & 0xFF) {
case DRV_DIFF_PRESS_DEVTYPE_SDP31:
// fallthrough
case DRV_DIFF_PRESS_DEVTYPE_SDP32:
// fallthrough
case DRV_DIFF_PRESS_DEVTYPE_SDP33:
smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
break;
default:
smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
break;
}
float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
differential_pressure_pa, baro_pressure_pa, temperature);
// assume that CAS = IAS as we don't have an CAS-scale here
float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature);
if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
airspeed_s airspeed;
airspeed.timestamp_sample = timestamp_sample;
airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s;
airspeed.true_airspeed_m_s = true_airspeed_m_s;
airspeed.air_temperature_celsius = temperature;
airspeed.confidence = _diff_pres_data[i].airspeed_validator.confidence(hrt_absolute_time());
airspeed.timestamp = hrt_absolute_time();
_airspeed_pubs[i].publish(airspeed);
_diff_pres_data[i].airspeed_last_publish = airspeed.timestamp;
}
}
}
}
@ -432,6 +444,22 @@ void Sensors::InitializeVehicleAirData()
}
#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
void Sensors::InitializeVehicleDifferentialPressure()
{
// check for connected differential pressure sensors
for (int i = 0; i < _differential_pressure_subs.size(); i++) {
if (!_differential_pressure_subs[i].advertised()) {
break;
}
_airspeed_pubs[i].advertise();
_number_of_differential_pressure_sensors = i + 1;
}
}
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
void Sensors::InitializeVehicleGPSPosition()
{
@ -708,9 +736,16 @@ int Sensors::print_status()
#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
PX4_INFO_RAW("\n");
PX4_INFO_RAW("Airspeed status:\n");
_airspeed_validator.print();
for (int i = 0; i < _number_of_differential_pressure_sensors; i++) {
PX4_INFO_RAW("Instance #%i:\n", i);
_diff_pres_data[i].airspeed_validator.print();
PX4_INFO_RAW("\n");
}
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)

28
src/modules/sensors/sensors.hpp
View File

@ -49,6 +49,7 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include "voted_sensors_update.h"
#include "vehicle_imu/VehicleIMU.hpp"
@ -124,6 +125,8 @@ private:
void InitializeVehicleAirData();
void InitializeVehicleDifferentialPressure();
void InitializeVehicleGPSPosition();
void InitializeVehicleIMU();
@ -179,21 +182,22 @@ private:
*/
void adc_poll();
uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)};
uORB::SubscriptionMultiArray < differential_pressure_s, MAX_SENSOR_COUNT - 1 > _differential_pressure_subs{ORB_ID::differential_pressure};
int _number_of_differential_pressure_sensors{0};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
uORB::PublicationMulti<airspeed_s> _airspeed_pubs[MAX_SENSOR_COUNT - 1] {ORB_ID(airspeed), ORB_ID(airspeed), ORB_ID(airspeed)};
DataValidator _airspeed_validator; /**< data validator to monitor airspeed */
float _diff_pres_pressure_sum{0.f};
float _diff_pres_temperature_sum{0.f};
float _baro_pressure_sum{0.f};
int _diff_pres_count{0};
uint64_t _airspeed_last_publish{0};
uint64_t _diff_pres_timestamp_sum{0};
struct DifferentialPressureData {
DataValidator airspeed_validator;
float diff_pres_pressure_sum;
float diff_pres_temperature_sum;
int diff_pres_count;
uint64_t airspeed_last_publish;
uint64_t diff_pres_timestamp_sum;
float baro_pressure_sum;
} _diff_pres_data[MAX_SENSOR_COUNT - 1] {};
# ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};