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

sensors: add baro calibration and cleanup

Browse Source 
- sensor_baro.msg use SI (pressure in Pascals)
 - update all barometer drivers to publish directly and remove PX4Barometer helper
 - introduce baro cal (offset) mainly as a mechanism to adjust
relative priority
 - commander: add simple baro cal that sets baro offsets to align with
GPS altitude (if available)
 - create new sensors_status.msg to generalize sensor reporting
This commit is contained in:
Daniel Agar
2022-03-26 16:08:41 -04:00
parent 5800c417c8
commit 0c31f63896
91 changed files with 1287 additions and 511 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/sensors/baro.cpp
+41 -13
View File
@@ -39,7 +39,8 @@
#include "baro.hpp"
#include <math.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/geo/geo.h> // For CONSTANTS_*
const char *const UavcanBarometerBridge::NAME = "baro";
@@ -72,12 +73,24 @@ int UavcanBarometerBridge::init()
void UavcanBarometerBridge::air_temperature_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
{
last_temperature_kelvin = msg.static_temperature;
if (msg.static_temperature >= 0.f) {
_last_temperature_kelvin = msg.static_temperature;
} else if (msg.static_temperature < 0) {
// handle previous incorrect temperature conversion to Kelvin where 273 was subtracted instead of added (https://github.com/PX4/PX4-Autopilot/pull/19061)
float temperature_c = msg.static_temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
if (temperature_c > -40.f && temperature_c < 120.f) {
_last_temperature_kelvin = temperature_c - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
}
}
}
void UavcanBarometerBridge::air_pressure_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
{
const hrt_abstime timestamp_sample = hrt_absolute_time();
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
if (channel == nullptr) {
@@ -86,31 +99,46 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
}
// Cast our generic CDev pointer to the sensor-specific driver class
PX4Barometer *baro = (PX4Barometer *)channel->h_driver;
uORB::PublicationMulti<sensor_baro_s> *baro = static_cast<uORB::PublicationMulti<sensor_baro_s> *>(channel->h_driver);
if (baro == nullptr) {
return;
}
baro->set_temperature(last_temperature_kelvin + CONSTANTS_ABSOLUTE_NULL_CELSIUS);
baro->update(hrt_absolute_time(), msg.static_pressure / 100.0f); // Convert pressure to millibar
}
int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel)
{
// update device id as we now know our device node_id
DeviceId device_id{_device_id};
DeviceId device_id{};
device_id.devid_s.bus = 0;
device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN;
device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
channel->h_driver = new PX4Barometer(device_id.devid);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = device_id.devid;
sensor_baro.pressure = msg.static_pressure;
if (PX4_ISFINITE(_last_temperature_kelvin) && (_last_temperature_kelvin >= 0.f)) {
sensor_baro.temperature = _last_temperature_kelvin + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
} else {
sensor_baro.temperature = NAN;
}
sensor_baro.error_count = 0;
sensor_baro.timestamp = hrt_absolute_time();
baro->publish(sensor_baro);
}
int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel)
{
channel->h_driver = new uORB::PublicationMulti<sensor_baro_s>(ORB_ID(sensor_baro));
if (channel->h_driver == nullptr) {
return PX4_ERROR;
}
PX4Barometer *baro = (PX4Barometer *)channel->h_driver;
uORB::PublicationMulti<sensor_baro_s> *baro = static_cast<uORB::PublicationMulti<sensor_baro_s> *>(channel->h_driver);
channel->instance = baro->get_instance();
src/drivers/uavcan/sensors/baro.hpp
+1 -1
View File
@@ -73,6 +73,6 @@ private:
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
float last_temperature_kelvin{0.0f};
float _last_temperature_kelvin{NAN};
};