sensors: add baro calibration and cleanup

- 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
+7 -4
View File
@@ -705,18 +705,21 @@ void quickCalibrate() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status || true"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick; sleep 1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_ACC*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro; sleep 2"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_GYRO*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level; sleep 2"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SENS*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick; sleep 1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_MAG*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate baro; sleep 5"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_BARO*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters after
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
}
+1
View File
@@ -152,6 +152,7 @@ set(msg_files
sensor_preflight_mag.msg
sensor_selection.msg
sensors_status_imu.msg
sensors_status.msg
system_power.msg
takeoff_status.msg
task_stack_info.msg
+5 -3
View File
@@ -3,8 +3,10 @@ uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 pressure # static pressure measurement in Pascals
float32 temperature # temperature in degrees Celsius
uint32 error_count
float32 pressure # static pressure measurement in millibar
float32 temperature # static temperature measurement in deg Celsius
uint8 ORB_QUEUE_LENGTH = 4
+15
View File
@@ -0,0 +1,15 @@
#
# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated.
#
uint64 timestamp # time since system start (microseconds)
uint32 device_id_primary # current primary device id for reference
uint32[4] device_ids
float32[4] inconsistency # magnitude of difference between sensor instance and mean
bool[4] healthy # sensor healthy
uint8[4] priority
bool[4] enabled
bool[4] external
# TOPICS sensors_status sensors_status_baro sensors_status_mag
+2
View File
@@ -10,3 +10,5 @@ float32 baro_temp_celcius # Temperature in degrees Celsius
float32 baro_pressure_pa # Absolute pressure in Pascals
float32 rho # air density
uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes.
+9 -6
View File
@@ -35,7 +35,6 @@
BMP280::BMP280(const I2CSPIDriverConfig &config, bmp280::IBMP280 *interface) :
I2CSPIDriver(config),
_px4_baro(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
@@ -172,11 +171,15 @@ BMP280::collect()
float pf = ((float) p_raw + x1) / x2;
const float P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7;
_px4_baro.set_error_count(perf_event_count(_comms_errors));
_px4_baro.set_temperature(T);
float pressure = P / 100.0f; // to mbar
_px4_baro.update(timestamp_sample, pressure);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = P;
sensor_baro.temperature = T;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
+3 -2
View File
@@ -39,7 +39,8 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
class BMP280 : public I2CSPIDriver<BMP280>
@@ -61,7 +62,7 @@ private:
int measure(); //start measure
int collect(); //get results and publish
PX4Barometer _px4_baro;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
bmp280::IBMP280 *_interface;
@@ -42,6 +42,5 @@ px4_add_module(
bmp280_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
@@ -40,6 +40,5 @@ px4_add_module(
bmp388.cpp
bmp388_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
+9 -6
View File
@@ -43,7 +43,6 @@
BMP388::BMP388(const I2CSPIDriverConfig &config, IBMP388 *interface) :
I2CSPIDriver(config),
_px4_baro(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
@@ -165,14 +164,18 @@ BMP388::collect()
return -EIO;
}
_px4_baro.set_error_count(perf_event_count(_comms_errors));
float temperature = (float)(data.temperature / 100.0f);
float pressure = (float)(data.pressure / 100.0f); // to Pascal
pressure = pressure / 100.0f; // to mbar
_px4_baro.set_temperature(temperature);
_px4_baro.update(timestamp_sample, pressure);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = pressure;
sensor_baro.temperature = temperature;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
+3 -2
View File
@@ -44,7 +44,8 @@
#include <perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include "board_config.h"
@@ -325,7 +326,7 @@ private:
static constexpr uint8_t odr{BMP3_ODR_50_HZ}; // output data rate (not used)
static constexpr uint8_t iir_coef{BMP3_IIR_FILTER_DISABLE}; // IIR coefficient
PX4Barometer _px4_baro;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
IBMP388 *_interface{nullptr};
unsigned _measure_interval{0}; // interval in microseconds needed to measure
@@ -40,6 +40,5 @@ px4_add_module(
DPS310_SPI.cpp
dps310_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
+9 -4
View File
@@ -48,7 +48,6 @@ static void getTwosComplement(T &raw, uint8_t length)
DPS310::DPS310(const I2CSPIDriverConfig &config, device::Device *interface) :
I2CSPIDriver(config),
_px4_barometer(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comm errors"))
@@ -233,9 +232,15 @@ DPS310::RunImpl()
const float Tcomp = c0 * 0.5f + c1 * Traw_sc;
_px4_barometer.set_error_count(perf_event_count(_comms_errors));
_px4_barometer.set_temperature(Tcomp);
_px4_barometer.update(timestamp_sample, Pcomp / 100.0f); // Pascals -> Millibar
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = Pcomp;
sensor_baro.temperature = Tcomp;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
}
+3 -2
View File
@@ -40,7 +40,8 @@
#pragma once
#include <drivers/device/Device.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -79,7 +80,7 @@ private:
static constexpr uint32_t SAMPLE_RATE{32};
PX4Barometer _px4_barometer;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
device::Device *_interface;
@@ -41,6 +41,5 @@ px4_add_module(
SPL06_SPI.cpp
spl06_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
+10 -5
View File
@@ -35,7 +35,6 @@
SPL06::SPL06(const I2CSPIDriverConfig &config, spl06::ISPL06 *interface) :
I2CSPIDriver(config),
_px4_baro(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
@@ -224,10 +223,16 @@ SPL06::collect()
float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3;
float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc;
_px4_baro.set_error_count(perf_event_count(_comms_errors));
_px4_baro.set_temperature(temperature);
_px4_baro.update(timestamp_sample, fp / 100.0f); // to millbar
//PX4_DEBUG("%d",(int)fp);
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = fp;
sensor_baro.temperature = temperature;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
return OK;
@@ -39,8 +39,9 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
class SPL06 : public I2CSPIDriver<SPL06>
{
@@ -62,7 +63,7 @@ private:
int collect(); //get results and publish
int calibrate();
PX4Barometer _px4_baro;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
spl06::ISPL06 *_interface;
spl06::data_s _data;
@@ -41,6 +41,5 @@ px4_add_module(
ICP10100.hpp
icp10100_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
@@ -37,8 +37,7 @@ using namespace time_literals;
ICP10100::ICP10100(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config),
_px4_baro(get_device_id())
I2CSPIDriver(config)
{
}
@@ -236,12 +235,17 @@ ICP10100::RunImpl()
const hrt_abstime nowx = hrt_absolute_time();
float temperature = _temperature_C;
float pressure = _pressure_Pa; // to Pascal
pressure = pressure / 100.0f; // to mbar
float pressure = _pressure_Pa;
_px4_baro.set_error_count(perf_event_count(_bad_transfer_perf));
_px4_baro.set_temperature(temperature);
_px4_baro.update(nowx, pressure);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = now;
sensor_baro.device_id = get_device_id();
sensor_baro.pressure = pressure;
sensor_baro.temperature = temperature;
sensor_baro.error_count = perf_event_count(_bad_transfer_perf);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
success = true;
@@ -37,7 +37,8 @@
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -69,7 +70,7 @@ private:
int send_command(Cmd cmd);
int send_command(Cmd cmd, uint8_t *data, uint8_t len);
PX4Barometer _px4_baro;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
@@ -41,6 +41,5 @@ px4_add_module(
ICP10111.hpp
icp10111_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
@@ -37,8 +37,7 @@ using namespace time_literals;
ICP10111::ICP10111(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config),
_px4_baro(get_device_id())
I2CSPIDriver(config)
{
}
@@ -234,14 +233,18 @@ ICP10111::RunImpl()
float b = (_pcal[0] - a) * (s1 + c);
float _pressure_Pa = a + b / (c + _raw_p);
const hrt_abstime nowx = hrt_absolute_time();
float temperature = _temperature_C;
float pressure = _pressure_Pa; // to Pascal
pressure = pressure / 100.0f; // to mbar
_px4_baro.set_error_count(perf_event_count(_bad_transfer_perf));
_px4_baro.set_temperature(temperature);
_px4_baro.update(nowx, pressure);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = now;
sensor_baro.device_id = get_device_id();
sensor_baro.pressure = pressure;
sensor_baro.temperature = temperature;
sensor_baro.error_count = perf_event_count(_bad_transfer_perf);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
success = true;
@@ -37,7 +37,8 @@
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -69,7 +70,7 @@ private:
int send_command(Cmd cmd);
int send_command(Cmd cmd, uint8_t *data, uint8_t len);
PX4Barometer _px4_baro;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
@@ -39,6 +39,5 @@ px4_add_module(
LPS22HB_I2C.cpp
LPS22HB_SPI.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
+11 -3
View File
@@ -44,7 +44,6 @@
LPS22HB::LPS22HB(const I2CSPIDriverConfig &config, device::Device *interface) :
I2CSPIDriver(config),
_px4_baro(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
@@ -152,14 +151,23 @@ int LPS22HB::collect()
uint32_t TEMP_OUT = report.TEMP_OUT_L + (report.TEMP_OUT_H << 8);
float temperature = 42.5f + (TEMP_OUT / 480.0f);
_px4_baro.set_temperature(temperature);
// To obtain the pressure in hPa, take the two’s complement of the complete word and then divide by 4096 LSB/hPa.
uint32_t P = report.PRESS_OUT_XL + (report.PRESS_OUT_L << 8) + (report.PRESS_OUT_H << 16);
/* Pressure and MSL in mBar */
float pressure = P / 4096.0f;
_px4_baro.update(timestamp_sample, pressure);
float pressure_pa = pressure * 100.f;
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = pressure_pa;
sensor_baro.temperature = temperature;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
return PX4_OK;
+4 -2
View File
@@ -39,7 +39,8 @@
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
static constexpr uint8_t WHO_AM_I = 0x0F;
static constexpr uint8_t LPS22HB_ID_WHO_AM_I = 0xB1;
@@ -92,7 +93,8 @@ public:
void RunImpl();
private:
PX4Barometer _px4_baro;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
device::Device *_interface;
bool _collect_phase{false};
@@ -40,6 +40,5 @@ px4_add_module(
lps25h_i2c.cpp
lps25h_spi.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
+10 -5
View File
@@ -35,7 +35,6 @@
LPS25H::LPS25H(const I2CSPIDriverConfig &config, device::Device *interface) :
I2CSPIDriver(config),
_px4_barometer(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors"))
@@ -162,19 +161,25 @@ int LPS25H::collect()
return ret;
}
_px4_barometer.set_error_count(perf_event_count(_comms_errors));
/* get measurements from the device */
float temperature = 42.5f + (report.t / 480);
_px4_barometer.set_temperature(temperature);
/* raw pressure */
uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16);
/* Pressure and MSL in mBar */
float pressure = raw / 4096.0f;
float pressure_pa = pressure * 100.f;
_px4_barometer.update(timestamp_sample, pressure);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = pressure_pa;
sensor_baro.temperature = temperature;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
return PX4_OK;
+4 -2
View File
@@ -43,7 +43,8 @@
#include <drivers/device/Device.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -176,7 +177,8 @@ private:
int measure();
int collect();
PX4Barometer _px4_barometer;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
device::Device *_interface;
unsigned _measure_interval{0};
@@ -40,6 +40,5 @@ px4_add_module(
lps33hw_spi.cpp
lps33hw_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
+10 -4
View File
@@ -49,7 +49,6 @@ static void getTwosComplement(T &raw, uint8_t length)
LPS33HW::LPS33HW(const I2CSPIDriverConfig &config, device::Device *interface) :
I2CSPIDriver(config),
_px4_barometer(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comm errors")),
@@ -177,10 +176,17 @@ LPS33HW::RunImpl()
int32_t Praw = (int32_t)data[1] | (data[2] << 8) | (data[3] << 16);
getTwosComplement(Praw, 24);
float pressure_hPa = Praw / 4096.f;
float pressure_pa = pressure_hPa * 100.f;
_px4_barometer.set_error_count(perf_event_count(_comms_errors));
_px4_barometer.set_temperature(temp);
_px4_barometer.update(timestamp_sample, pressure_hPa); // hPascals -> Millibar
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = pressure_pa;
sensor_baro.temperature = temp;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
ScheduleDelayed(1000000 / SAMPLE_RATE);
+3 -2
View File
@@ -40,7 +40,8 @@
#pragma once
#include <drivers/device/Device.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -82,7 +83,7 @@ private:
static constexpr uint32_t SAMPLE_RATE{75};
PX4Barometer _px4_barometer;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
device::Device *_interface;
@@ -41,6 +41,5 @@ px4_add_module(
MPC2520.cpp
MPC2520.hpp
DEPENDS
drivers_barometer
px4_work_queue
)
@@ -44,8 +44,7 @@ static constexpr int32_t combine(uint8_t h, uint8_t m, uint8_t l)
MPC2520::MPC2520(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config),
_px4_baro(get_device_id())
I2CSPIDriver(config)
{
//_debug_enabled = true;
}
@@ -232,7 +231,6 @@ void MPC2520::RunImpl()
static constexpr float kT = 7864320; // temperature 8 times oversampling
float Traw_sc = static_cast<float>(Traw) / kT;
float Tcomp = _prom.c0 * 0.5f + _prom.c1 * Traw_sc;
_px4_baro.set_temperature(Tcomp);
int32_t Praw = (int32_t)(buffer.PSR_B2 << 16) | (int32_t)(buffer.PSR_B1 << 8) | (int32_t)buffer.PSR_B1;
Praw = (Praw & 0x800000) ? (0xFF000000 | Praw) : Praw;
@@ -244,9 +242,15 @@ void MPC2520::RunImpl()
float Pcomp = _prom.c00 + Praw_sc * (_prom.c10 + Praw_sc * (_prom.c20 + Praw_sc * _prom.c30)) + Traw_sc * _prom.c01 +
Traw_sc * Praw_sc * (_prom.c11 + Praw_sc * _prom.c21);
float pressure_mbar = Pcomp / 100.0f; // convert to millibar
_px4_baro.update(now, pressure_mbar);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = now;
sensor_baro.device_id = get_device_id();
sensor_baro.pressure = Pcomp;
sensor_baro.temperature = Tcomp;
sensor_baro.error_count = perf_event_count(_bad_transfer_perf) + perf_event_count(_bad_register_perf);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
success = true;
@@ -37,7 +37,8 @@
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -76,7 +77,7 @@ private:
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
PX4Barometer _px4_baro;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
@@ -40,6 +40,5 @@ px4_add_module(
MPL3115A2.cpp
mpl3115a2_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
@@ -55,7 +55,6 @@
MPL3115A2::MPL3115A2(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config),
_px4_barometer(get_device_id()),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
@@ -260,9 +259,15 @@ int MPL3115A2::collect()
float T = (float) reading.temperature.b[1] + ((float)(reading.temperature.b[0]) / 16.0f);
float P = (float)(reading.pressure.q >> 8) + ((float)(reading.pressure.b[0]) / 4.0f);
_px4_barometer.set_error_count(perf_event_count(_comms_errors));
_px4_barometer.set_temperature(T);
_px4_barometer.update(timestamp_sample, P / 100.0f);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = get_device_id();
sensor_baro.pressure = P;
sensor_baro.temperature = T;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
@@ -42,7 +42,8 @@
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
@@ -76,7 +77,7 @@ private:
int RegisterRead(uint8_t reg, void *data, unsigned count = 1);
int RegisterWrite(uint8_t reg, uint8_t data);
PX4Barometer _px4_barometer;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
bool _collect_phase{false};
@@ -44,6 +44,5 @@ px4_add_module(
DEPENDS
cdev
drivers__device
drivers_barometer
px4_work_queue
)
+8 -2
View File
@@ -34,7 +34,8 @@
#pragma once
#include <drivers/device/device.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -113,7 +114,7 @@ public:
protected:
void print_status() override;
PX4Barometer _px4_barometer;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
device::Device *_interface;
@@ -127,6 +128,11 @@ protected:
int64_t _OFF{0};
int64_t _SENS{0};
bool _initialized{false};
float _last_pressure{NAN};
float _last_temperature{NAN};
perf_counter_t _sample_perf;
perf_counter_t _measure_perf;
perf_counter_t _comms_errors;
+17 -13
View File
@@ -43,7 +43,6 @@
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_barometer(interface->get_device_id()),
_interface(interface),
_prom(prom_buf.s),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
@@ -108,15 +107,13 @@ MS5611::init()
}
/* state machine will have generated a report, copy it out */
const sensor_baro_s &brp = _px4_barometer.get();
if (_device_type == MS5607_DEVICE) {
if (brp.pressure < 520.0f) {
if (_last_pressure < 52'000.f) {
/* This is likely not this device, abort */
ret = -EINVAL;
break;
} else if (brp.pressure > 1500.0f) {
} else if (_last_pressure > 150'000.f) {
/* This is likely not this device, abort */
ret = -EINVAL;
break;
@@ -129,12 +126,10 @@ MS5611::init()
/* fall through */
case MS5611_DEVICE:
_interface->set_device_type(DRV_BARO_DEVTYPE_MS5611);
_px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MS5611);
break;
case MS5607_DEVICE:
_interface->set_device_type(DRV_BARO_DEVTYPE_MS5607);
_px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MS5607);
break;
}
@@ -144,6 +139,7 @@ MS5611::init()
}
if (ret == 0) {
_initialized = true;
start();
}
@@ -235,8 +231,6 @@ MS5611::measure()
perf_count(_comms_errors);
}
_px4_barometer.set_error_count(perf_event_count(_comms_errors));
perf_end(_measure_perf);
return ret;
@@ -321,16 +315,26 @@ MS5611::collect()
}
}
float temperature = TEMP / 100.0f;
_px4_barometer.set_temperature(temperature);
_last_temperature = TEMP / 100.0f;
} else {
/* pressure calculation, result in Pa */
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
float pressure = P / 100.0f; /* convert to millibar */
_last_pressure = P;
// publish
if (_initialized && PX4_ISFINITE(_last_pressure) && PX4_ISFINITE(_last_temperature)) {
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = P;
sensor_baro.temperature = _last_temperature;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
}
_px4_barometer.update(timestamp_sample, pressure);
}
/* update the measurement state machine */
@@ -41,6 +41,5 @@ px4_add_module(
MS5837.cpp
MS5837.hpp
DEPENDS
drivers_barometer
px4_work_queue
)
+11 -12
View File
@@ -41,7 +41,6 @@
MS5837::MS5837(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config),
_px4_barometer(get_device_id()),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
@@ -96,7 +95,7 @@ int MS5837::init()
break;
}
_px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MS5837);
set_device_type(DRV_BARO_DEVTYPE_MS5837);
ret = OK;
@@ -259,8 +258,6 @@ int MS5837::_measure()
perf_count(_comms_errors);
}
_px4_barometer.set_error_count(perf_event_count(_comms_errors));
perf_end(_measure_perf);
return ret;
@@ -331,17 +328,22 @@ int MS5837::_collect()
_OFF -= OFF2;
_SENS -= SENS2;
float temperature = TEMP / 100.0f;
_px4_barometer.set_temperature(temperature);
_last_temperature = TEMP / 100.0f;
} else {
/* pressure calculation, result in Pa */
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 13;
float pressure = P / 10.0f; /* convert to millibar */
_px4_barometer.update(timestamp_sample, pressure);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = get_device_id();
sensor_baro.pressure = P;
sensor_baro.temperature = T;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
}
/* update the measurement state machine */
@@ -357,9 +359,6 @@ void MS5837::print_status()
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("pressure: %f\n", (double)_px4_barometer.get().pressure);
printf("temperature: %f\n", (double)_px4_barometer.get().temperature);
}
int MS5837::_read_prom()
+5 -2
View File
@@ -35,7 +35,8 @@
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -79,7 +80,7 @@ public:
private:
int probe() override;
PX4Barometer _px4_barometer;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
ms5837::prom_u _prom{};
@@ -90,6 +91,8 @@ private:
int64_t _OFF{0};
int64_t _SENS{0};
float _last_temperature{NAN};
perf_counter_t _sample_perf;
perf_counter_t _measure_perf;
perf_counter_t _comms_errors;
@@ -39,6 +39,5 @@ px4_add_module(
tcbp001ta_spi.cpp
tcbp001ta_main.cpp
DEPENDS
drivers_barometer
px4_work_queue
)
+9 -10
View File
@@ -45,13 +45,11 @@
TCBP001TA::TCBP001TA(tcbp001ta::ITCBP001TA *interface) :
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
_px4_baro(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
{
_px4_baro.set_device_type(DRV_BARO_DEVTYPE_TCBP001TA);
}
TCBP001TA::~TCBP001TA()
@@ -252,12 +250,15 @@ TCBP001TA::collect()
Praw_sc * _fcal.c01 +
Praw_sc * Praw_sc * (_fcal.c11 + Praw_sc * _fcal.c21);
_px4_baro.set_error_count(perf_event_count(_comms_errors));
_px4_baro.set_temperature(T);
float pressure = P / 100.0f; // to mbar
// PX4_INFO("press %f", double(pressure));
_px4_baro.update(timestamp_sample, pressure);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = _interface->get_device_id();
sensor_baro.pressure = P;
sensor_baro.temperature = T;
sensor_baro.error_count = perf_event_count(_comms_errors);
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_end(_sample_perf);
@@ -270,8 +271,6 @@ TCBP001TA::print_info()
perf_print_counter(_sample_perf);
perf_print_counter(_measure_perf);
perf_print_counter(_comms_errors);
_px4_baro.print_status();
}
uint32_t
@@ -38,7 +38,8 @@
#include <drivers/drv_hrt.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/perf/perf_counter.h>
class TCBP001TA : public px4::ScheduledWorkItem
@@ -63,7 +64,7 @@ private:
int measure(); //start measure
int collect(); //get results and publish
PX4Barometer _px4_baro;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
tcbp001ta::ITCBP001TA *_interface;
@@ -93,6 +93,7 @@ tcbp001ta_spi_interface(uint8_t busnum, uint32_t device)
TCBP001TA_SPI::TCBP001TA_SPI(uint8_t bus, uint32_t device) :
SPI("TCBP001TA_SPI", nullptr, bus, device, SPIDEV_MODE3, 10 * 1000 * 1000)
{
set_device_type(DRV_BARO_DEVTYPE_TCBP001TA);
}
uint8_t
@@ -100,7 +100,6 @@ ADIS16448::ADIS16448(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_drdy_gpio(config.drdy_gpio), // TODO: DRDY disabled
_px4_accel(get_device_id(), config.rotation),
_px4_baro(get_device_id()),
_px4_gyro(get_device_id(), config.rotation),
_px4_mag(get_device_id(), config.rotation)
{
@@ -445,11 +444,18 @@ void ADIS16448::RunImpl()
const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT;
_px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z);
_px4_baro.set_error_count(error_count);
_px4_baro.set_temperature(temperature);
float pressure_pa = buffer.BARO_OUT * 0.02f; // 20 μbar per LSB
_px4_baro.update(timestamp_sample, pressure_pa);
// publish baro
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp_sample;
sensor_baro.device_id = get_device_id();
sensor_baro.pressure = pressure_pa;
sensor_baro.temperature = temperature;
sensor_baro.error_count = error_count;
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
}
success = true;
@@ -45,9 +45,10 @@
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
#include <lib/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
@@ -98,10 +99,11 @@ private:
const spi_drdy_gpio_t _drdy_gpio;
PX4Accelerometer _px4_accel;
PX4Barometer _px4_baro;
PX4Gyroscope _px4_gyro;
PX4Magnetometer _px4_mag;
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
@@ -44,7 +44,6 @@ px4_add_module(
Analog_Devices_ADIS16448_registers.hpp
DEPENDS
drivers_accelerometer
drivers_barometer
drivers_gyroscope
drivers_magnetometer
px4_work_queue
+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();
+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};
};
-1
View File
@@ -33,7 +33,6 @@
add_subdirectory(accelerometer)
add_subdirectory(airspeed)
add_subdirectory(barometer)
add_subdirectory(device)
add_subdirectory(gyroscope)
add_subdirectory(led)
@@ -53,7 +53,6 @@ public:
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint32_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
void set_range(float range) { _range = range; UpdateClipLimit(); }
void set_scale(float scale);
void set_temperature(float temperature) { _temperature = temperature; }
-34
View File
@@ -1,34 +0,0 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(drivers_barometer PX4Barometer.cpp)
@@ -1,70 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "PX4Barometer.hpp"
#include <lib/drivers/device/Device.hpp>
PX4Barometer::PX4Barometer(uint32_t device_id)
{
_sensor_baro_pub.get().device_id = device_id;
}
PX4Barometer::~PX4Barometer()
{
_sensor_baro_pub.unadvertise();
}
void PX4Barometer::set_device_type(uint8_t devtype)
{
// current DeviceStructure
union device::Device::DeviceId device_id;
device_id.devid = _sensor_baro_pub.get().device_id;
// update to new device type
device_id.devid_s.devtype = devtype;
// copy back to report
_sensor_baro_pub.get().device_id = device_id.devid;
}
void PX4Barometer::update(const hrt_abstime &timestamp_sample, float pressure)
{
sensor_baro_s &report = _sensor_baro_pub.get();
report.timestamp_sample = timestamp_sample;
report.pressure = pressure;
report.timestamp = hrt_absolute_time();
_sensor_baro_pub.update();
}
@@ -53,7 +53,6 @@ public:
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint32_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
void set_range(float range) { _range = range; }
void set_scale(float scale);
void set_temperature(float temperature) { _temperature = temperature; }
@@ -47,7 +47,6 @@ public:
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint32_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
void set_scale(float scale) { _scale = scale; }
void set_temperature(float temperature) { _temperature = temperature; }
+246
View File
@@ -0,0 +1,246 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Barometer.hpp"
#include "Utilities.hpp"
#include <lib/parameters/param.h>
using namespace matrix;
using namespace time_literals;
namespace calibration
{
Barometer::Barometer()
{
Reset();
}
Barometer::Barometer(uint32_t device_id)
{
set_device_id(device_id);
}
void Barometer::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
_device_id = device_id;
_external = external;
Reset();
ParametersUpdate();
SensorCorrectionsUpdate(true);
}
}
void Barometer::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
// valid device id required
if (_device_id == 0) {
return;
}
sensor_correction_s corrections;
if (_sensor_correction_sub.copy(&corrections)) {
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.baro_device_ids[i] == _device_id) {
switch (i) {
case 0:
_thermal_offset = corrections.baro_offset_0;
return;
case 1:
_thermal_offset = corrections.baro_offset_1;
return;
case 2:
_thermal_offset = corrections.baro_offset_2;
return;
case 3:
_thermal_offset = corrections.baro_offset_3;
return;
}
}
}
}
// zero thermal offset if not found
_thermal_offset = 0;
}
}
bool Barometer::set_offset(const float &offset)
{
if (fabsf(_offset - offset) > 0.01f) {
if (PX4_ISFINITE(offset)) {
_offset = offset;
_calibration_count++;
return true;
}
}
return false;
}
bool Barometer::set_calibration_index(int calibration_index)
{
if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
_calibration_index = calibration_index;
return true;
}
return false;
}
void Barometer::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
if (_calibration_index == -1) {
// no saved calibration available
Reset();
} else {
ParametersLoad();
}
}
bool Barometer::ParametersLoad()
{
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// CAL_BAROx_PRIO
_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
if (_priority != CAL_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_calibration_index, _priority);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
}
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
// CAL_BAROx_OFF
set_offset(GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index));
return true;
}
return false;
}
void Barometer::Reset()
{
_offset = 0;
_thermal_offset = 0;
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
}
bool Barometer::ParametersSave(int desired_calibration_index, bool force)
{
if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
_calibration_index = desired_calibration_index;
} else if (!force || (_calibration_index < 0)
|| (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
// ensure we have a valid calibration slot (matching existing or first available slot)
int8_t calibration_index_prev = _calibration_index;
_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
calibration_index_prev, _calibration_index);
}
}
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// save calibration
bool success = true;
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParam(SensorString(), "OFF", _calibration_index, _offset);
return success;
}
return false;
}
void Barometer::PrintStatus()
{
if (external()) {
PX4_INFO_RAW("%s %" PRIu32
" EN: %d, offset: %05.3f, Ext\n",
SensorString(), device_id(), enabled(),
(double)_offset);
} else {
PX4_INFO_RAW("%s %" PRIu32 " EN: %d, offset: %05.3f, Internal\n",
SensorString(), device_id(), enabled(),
(double)_offset);
}
if (fabsf(_thermal_offset) > 0.f) {
PX4_INFO_RAW("%s %" PRIu32 " temperature offset: %.4f\n", SensorString(), _device_id, (double)_thermal_offset);
}
}
} // namespace calibration
+113
View File
@@ -0,0 +1,113 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_correction.h>
namespace calibration
{
class Barometer
{
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "BARO"; }
Barometer();
explicit Barometer(uint32_t device_id);
~Barometer() = default;
void PrintStatus();
bool set_calibration_index(int calibration_index);
void set_device_id(uint32_t device_id);
bool set_offset(const float &offset);
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
uint8_t calibration_count() const { return _calibration_count; }
int8_t calibration_index() const { return _calibration_index; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const float &offset() const { return _offset; }
const int32_t &priority() const { return _priority; }
const float &thermal_offset() const { return _thermal_offset; }
// apply offsets
inline float Correct(const float &data) const
{
return data - _thermal_offset - _offset;
}
inline float Uncorrect(const float &corrected_data) const
{
return corrected_data + _thermal_offset + _offset;
}
// Compute sensor offset from bias (board frame)
float BiasCorrectedSensorOffset(const float &bias) const
{
return bias + _thermal_offset + _offset;
}
bool ParametersLoad();
bool ParametersSave(int desired_calibration_index = -1, bool force = false);
void ParametersUpdate();
void Reset();
void SensorCorrectionsUpdate(bool force = false);
private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
float _offset{0};
float _thermal_offset{0};
int8_t _calibration_index{-1};
uint32_t _device_id{0};
int32_t _priority{-1};
bool _external{false};
uint8_t _calibration_count{0};
};
} // namespace calibration
@@ -34,6 +34,8 @@
px4_add_library(sensor_calibration
Accelerometer.cpp
Accelerometer.hpp
Barometer.cpp
Barometer.hpp
Gyroscope.cpp
Gyroscope.hpp
Magnetometer.cpp
+15
View File
@@ -149,6 +149,21 @@ int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type,
return value;
}
float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance)
{
// eg CAL_BAROn_OFF
char str[20] {};
sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
float value = NAN;
if (param_get(param_find(str), &value) != 0) {
PX4_ERR("failed to get %s", str);
}
return value;
}
Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance)
{
Vector3f values{0.f, 0.f, 0.f};
+1
View File
@@ -71,6 +71,7 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
* @return int32_t The calibration value.
*/
int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance);
float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance);
/**
* @brief Set a single calibration paramter.
+1
View File
@@ -41,6 +41,7 @@ px4_add_module(
SRCS
accelerometer_calibration.cpp
airspeed_calibration.cpp
baro_calibration.cpp
calibration_routines.cpp
Commander.cpp
commander_helper.cpp
+9 -3
View File
@@ -231,6 +231,10 @@ int Commander::custom_command(int argc, char *argv[])
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f);
}
} else if (!strcmp(argv[1], "baro")) {
// baro calibration: param3 = 1
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 1.f, 0.f, 0.0, 0.0, 0.f);
} else if (!strcmp(argv[1], "accel")) {
if (argc > 2 && (strcmp(argv[2], "quick") == 0)) {
// accelerometer quick calibration: param5 = 3
@@ -1392,8 +1396,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
_worker_thread.startTask(WorkerThread::Request::MagCalibration);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
/* baro calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
@@ -4421,7 +4427,7 @@ The commander module contains the state machine for mode switching and failsafe
PRINT_MODULE_USAGE_PARAM_FLAG('h', "Enable HIL mode", true);
#ifndef CONSTRAINED_FLASH
PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration");
PRINT_MODULE_USAGE_ARG("mag|accel|gyro|level|esc|airspeed", "Calibration type", false);
PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false);
PRINT_MODULE_USAGE_ARG("quick", "Quick calibration (accel only, not recommended)", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
PRINT_MODULE_USAGE_COMMAND("arm");
+225
View File
@@ -0,0 +1,225 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file baro_calibration.cpp
*
* Simple barometer calibration routine
*/
#include "baro_calibration.h"
#include "commander_helper.h"
#include "calibration_routines.h"
#include "calibration_messages.h"
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <drivers/drv_hrt.h>
#include <matrix/math.hpp>
#include <lib/geo/geo.h>
#include <lib/sensor_calibration/Barometer.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <lib/systemlib/err.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
using namespace matrix;
using namespace time_literals;
static constexpr char sensor_name[] {"baro"};
static constexpr int MAX_SENSOR_COUNT = 4;
static float PressureToAltitude(float pressure_pa, float temperature)
{
// calculate altitude using the hypsometric equation
static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre
// current pressure at MSL in kPa (QNH in hPa)
const float p1 = 1013.25f * 0.1f;
// measured pressure in kPa
const float p = pressure_pa * 0.001f;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
return altitude;
}
int do_baro_calibration(orb_advert_t *mavlink_log_pub)
{
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
// GPS (used for reference)
uORB::SubscriptionMultiArray<sensor_gps_s, 3> sensor_gps_subs{ORB_ID::sensor_gps};
float gps_altitude_sum = NAN;
int gps_altitude_sum_count = 0;
uORB::SubscriptionMultiArray<sensor_baro_s, MAX_SENSOR_COUNT> sensor_baro_subs{ORB_ID::sensor_baro};
calibration::Barometer calibration[MAX_SENSOR_COUNT] {};
uint64_t timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
float data_sum[MAX_SENSOR_COUNT] {};
float temperature_sum[MAX_SENSOR_COUNT] {};
int data_sum_count[MAX_SENSOR_COUNT] {};
const hrt_abstime time_start_us = hrt_absolute_time();
while (hrt_elapsed_time(&time_start_us) < 3_s) {
for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
sensor_baro_s sensor_baro;
while (sensor_baro_subs[instance].update(&sensor_baro)) {
calibration[instance].set_device_id(sensor_baro.device_id);
// pressure corrected with offset (if available)
const float pressure_corrected = calibration[instance].Correct(sensor_baro.pressure);
timestamp_sample_sum[instance] += sensor_baro.timestamp_sample;
data_sum[instance] += pressure_corrected;
temperature_sum[instance] += sensor_baro.temperature;
data_sum_count[instance]++;
}
}
for (auto &gps_sub : sensor_gps_subs) {
sensor_gps_s sensor_gps;
if (gps_sub.update(&sensor_gps)) {
if ((hrt_elapsed_time(&sensor_gps.timestamp) < 1_s)
&& (sensor_gps.fix_type >= 2) && (sensor_gps.epv < 100)) {
float alt = sensor_gps.alt * 0.001f;
if (PX4_ISFINITE(gps_altitude_sum)) {
gps_altitude_sum += alt;
gps_altitude_sum_count++;
} else {
gps_altitude_sum = alt;
gps_altitude_sum_count = 1;
}
}
}
}
px4_usleep(100_ms);
}
float gps_altitude = NAN;
if (PX4_ISFINITE(gps_altitude_sum) && (gps_altitude_sum_count > 0)) {
gps_altitude = gps_altitude_sum / gps_altitude_sum_count;
}
if (!PX4_ISFINITE(gps_altitude)) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "GPS required for baro cal");
return PX4_ERROR;
}
bool param_save = false;
for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)) {
const float pressure_pa = data_sum[instance] / data_sum_count[instance];
const float temperature = temperature_sum[instance] / data_sum_count[instance];
float pressure_altitude = PressureToAltitude(pressure_pa, temperature);
// Use GPS altitude as a reference to compute the baro bias measurement
const float baro_bias = pressure_altitude - gps_altitude;
float altitude = pressure_altitude - baro_bias;
// find pressure offset that aligns baro altitude with GPS via binary search
float front = -10000.f;
float middle = NAN;
float last = 10000.f;
float bias = NAN;
// perform a binary search
while (front <= last) {
middle = front + (last - front) / 2;
float altitude_calibrated = PressureToAltitude(pressure_pa - middle, temperature);
if (altitude_calibrated > altitude + 0.1f) {
last = middle;
} else if (altitude_calibrated < altitude - 0.1f) {
front = middle;
} else {
bias = middle;
break;
}
}
if (PX4_ISFINITE(bias)) {
float offset = calibration[instance].BiasCorrectedSensorOffset(bias);
calibration[instance].set_offset(offset);
if (calibration[instance].ParametersSave(instance, true)) {
calibration[instance].PrintStatus();
param_save = true;
}
}
}
}
if (param_save) {
param_notify_changes();
}
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
return PX4_OK;
}
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,30 +33,8 @@
#pragma once
#include <drivers/drv_hrt.h>
#include <lib/conversion/rotation.h>
#include <math.h>
#include <stdint.h>
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_baro.h>
class PX4Barometer
{
public:
PX4Barometer(uint32_t device_id);
~PX4Barometer();
const sensor_baro_s &get() { return _sensor_baro_pub.get(); }
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _sensor_baro_pub.get().error_count = error_count; }
void set_temperature(float temperature) { _sensor_baro_pub.get().temperature = temperature; }
void update(const hrt_abstime &timestamp_sample, float pressure);
int get_instance() { return _sensor_baro_pub.get_instance(); };
private:
uORB::PublicationMultiData<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
};
int do_baro_calibration(orb_advert_t *mavlink_log_pub);
+5
View File
@@ -35,6 +35,7 @@
#include "accelerometer_calibration.h"
#include "airspeed_calibration.h"
#include "calibration_routines.h"
#include "baro_calibration.h"
#include "esc_calibration.h"
#include "gyro_calibration.h"
#include "level_calibration.h"
@@ -144,6 +145,10 @@ void WorkerThread::threadEntry()
_ret_value = do_mag_calibration_quick(&_mavlink_log_pub, _heading_radians, _latitude, _longitude);
break;
case Request::BaroCalibration:
_ret_value = do_baro_calibration(&_mavlink_log_pub);
break;
case Request::ParamLoadDefault:
_ret_value = param_load_default();
+1
View File
@@ -58,6 +58,7 @@ public:
AirspeedCalibration,
ESCCalibration,
MagCalibrationQuick,
BaroCalibration,
ParamLoadDefault,
ParamSaveDefault,
+22
View File
@@ -1511,6 +1511,28 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
perf_count(_msg_missed_air_data_perf);
}
bool reset = false;
// check if barometer has changed
if (airdata.baro_device_id != _device_id_baro) {
if (_device_id_baro != 0) {
PX4_WARN("%d - baro sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_baro, airdata.baro_device_id);
}
reset = true;
} else if (airdata.calibration_count > _baro_calibration_count) {
// existing calibration has changed, reset saved baro bias
PX4_DEBUG("%d - baro %" PRIu32 " calibration updated, resetting bias", _instance, _device_id_baro);
reset = true;
}
if (reset) {
// TODO: reset baro ref and bias estimate?
_device_id_baro = airdata.baro_device_id;
_baro_calibration_count = airdata.calibration_count;
}
_ekf.set_air_density(airdata.rho);
_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter});
+1
View File
@@ -219,6 +219,7 @@ private:
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
uint8_t _accel_calibration_count{0};
uint8_t _baro_calibration_count{0};
uint8_t _gyro_calibration_count{0};
uint8_t _mag_calibration_count{0};
-1
View File
@@ -101,7 +101,6 @@ px4_add_module(
airspeed
component_general_json # for checksums.h
drivers_accelerometer
drivers_barometer
drivers_gyroscope
drivers_magnetometer
conversion
+9 -10
View File
@@ -73,7 +73,6 @@ MavlinkReceiver::~MavlinkReceiver()
{
delete _tune_publisher;
delete _px4_accel;
delete _px4_baro;
delete _px4_gyro;
delete _px4_mag;
#if !defined(CONSTRAINED_FLASH)
@@ -2309,15 +2308,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
// baro
if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) {
if (_px4_baro == nullptr) {
// 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
_px4_baro = new PX4Barometer(6620172);
}
if (_px4_baro != nullptr) {
_px4_baro->set_temperature(hil_sensor.temperature);
_px4_baro->update(timestamp, hil_sensor.abs_pressure);
}
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = timestamp;
sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
sensor_baro.pressure = hil_sensor.abs_pressure;
sensor_baro.temperature = hil_sensor.temperature;
sensor_baro.error_count = 0;
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
}
// differential pressure
+5 -5
View File
@@ -51,7 +51,6 @@
#include <geo/geo.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/systemlib/mavlink_log.h>
@@ -92,6 +91,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
@@ -290,7 +290,7 @@ private:
uORB::Publication<actuator_controls_s> _actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
uORB::Publication<camera_status_s> _camera_status_pub{ORB_ID(camera_status)};
uORB::Publication<camera_status_s> _camera_status_pub{ORB_ID(camera_status)};
uORB::Publication<cellular_status_s> _cellular_status_pub{ORB_ID(cellular_status)};
uORB::Publication<collision_report_s> _collision_report_pub{ORB_ID(collision_report)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
@@ -308,7 +308,6 @@ private:
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
@@ -333,9 +332,11 @@ private:
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping)};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
// ORB publications (queue length > 1)
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
@@ -364,7 +365,6 @@ private:
DIFF_PRESS = 0b10000000000
};
PX4Accelerometer *_px4_accel{nullptr};
PX4Barometer *_px4_baro{nullptr};
PX4Gyroscope *_px4_gyro{nullptr};
PX4Magnetometer *_px4_mag{nullptr};
@@ -51,7 +51,6 @@ void DataValidator::put(uint64_t timestamp, float val, uint32_t error_count_in,
void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_t error_count_in, uint8_t priority_in)
{
_event_count++;
if (error_count_in > _error_count) {
@@ -65,31 +64,33 @@ void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_
_priority = priority_in;
for (unsigned i = 0; i < dimensions; i++) {
if (_time_last == 0) {
_mean[i] = 0;
_lp[i] = val[i];
_M2[i] = 0;
} else {
float lp_val = val[i] - _lp[i];
float delta_val = lp_val - _mean[i];
_mean[i] += delta_val / _event_count;
_M2[i] += delta_val * (lp_val - _mean[i]);
_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
if (fabsf(_value[i] - val[i]) < 0.000001f) {
_value_equal_count++;
if (PX4_ISFINITE(val[i])) {
if (_time_last == 0) {
_mean[i] = 0;
_lp[i] = val[i];
_M2[i] = 0;
} else {
_value_equal_count = 0;
float lp_val = val[i] - _lp[i];
float delta_val = lp_val - _mean[i];
_mean[i] += delta_val / _event_count;
_M2[i] += delta_val * (lp_val - _mean[i]);
_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
if (fabsf(_value[i] - val[i]) < 0.000001f) {
_value_equal_count++;
} else {
_value_equal_count = 0;
}
}
// XXX replace with better filter, make it auto-tune to update rate
_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
_value[i] = val[i];
}
// XXX replace with better filter, make it auto-tune to update rate
_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
_value[i] = val[i];
}
_time_last = timestamp;
+39
View File
@@ -159,6 +159,45 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
# Barometer calibration
CAL_BARO${i}_ID:
description:
short: Barometer ${i} calibration device ID
long: Device ID of the barometer this calibration applies to.
category: System
type: int32
default: 0
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_BARO${i}_PRIO:
description:
short: Barometer ${i} priority
category: System
type: enum
values:
-1: Uninitialized
0: Disabled
1: Min
25: Low
50: Medium (Default)
75: High
100: Max
default: -1
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_BARO${i}_OFF:
description:
short: Barometer ${i} offset
category: System
type: float
default: 0.0
#unit: Pa
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
# Gyroscope calibration
CAL_GYRO${i}_ID:
description:
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
# Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,4 +35,8 @@ px4_add_library(vehicle_air_data
VehicleAirData.cpp
VehicleAirData.hpp
)
target_link_libraries(vehicle_air_data PRIVATE px4_work_queue)
target_link_libraries(vehicle_air_data
PRIVATE
px4_work_queue
sensor_calibration
)
@@ -75,44 +75,6 @@ void VehicleAirData::Stop()
}
}
void VehicleAirData::SensorCorrectionsUpdate(bool force)
{
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections;
if (_sensor_correction_sub.copy(&corrections)) {
for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) {
// find sensor (by device id) in sensor_correction
const uint32_t device_id = _last_data[baro_index].device_id;
if (device_id != 0) {
for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) {
if (corrections.baro_device_ids[correction_index] == device_id) {
switch (correction_index) {
case 0:
_thermal_offset[baro_index] = corrections.baro_offset_0;
break;
case 1:
_thermal_offset[baro_index] = corrections.baro_offset_1;
break;
case 2:
_thermal_offset[baro_index] = corrections.baro_offset_2;
break;
case 3:
_thermal_offset[baro_index] = corrections.baro_offset_3;
break;
}
}
}
}
}
}
}
}
void VehicleAirData::AirTemperatureUpdate()
{
differential_pressure_s differential_pressure;
@@ -124,20 +86,43 @@ void VehicleAirData::AirTemperatureUpdate()
// limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight
if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature)
&& fabsf(differential_pressure.temperature) > FLT_EPSILON) {
_air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius,
temperature_max_celsius);
}
}
bool VehicleAirData::ParametersUpdate()
bool VehicleAirData::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
if (_parameter_update_sub.updated() || force) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
// update priority
for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
const int32_t priority_old = _calibration[instance].priority();
_calibration[instance].ParametersUpdate();
const int32_t priority_new = _calibration[instance].priority();
if (priority_old != priority_new) {
if (_priority[instance] == priority_old) {
_priority[instance] = priority_new;
} else {
// change relative priority to incorporate any sensor faults
int priority_change = priority_new - priority_old;
_priority[instance] = math::constrain(_priority[instance] + priority_change, 1, 100);
}
}
}
return true;
}
@@ -152,53 +137,71 @@ void VehicleAirData::Run()
const bool parameter_update = ParametersUpdate();
SensorCorrectionsUpdate();
AirTemperatureUpdate();
bool updated[MAX_SENSOR_COUNT] {};
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
const bool was_advertised = _advertised[uorb_index];
if (!_advertised[uorb_index]) {
// use data's timestamp to throttle advertisement checks
if ((_last_data[uorb_index].timestamp == 0) || (time_now_us > _last_data[uorb_index].timestamp + 1_s)) {
if (_sensor_sub[uorb_index].advertised()) {
if (uorb_index > 0) {
/* the first always exists, but for each further sensor, add a new validator */
if (!_voter.add_new_validator()) {
PX4_ERR("failed to add validator for %s %i", "BARO", uorb_index);
}
}
if ((_last_publication_timestamp[uorb_index] == 0)
|| (time_now_us > _last_publication_timestamp[uorb_index] + 1_s)) {
if (_sensor_sub[uorb_index].advertised()) {
_advertised[uorb_index] = true;
// force temperature correction update
SensorCorrectionsUpdate(true);
if (_selected_sensor_sub_index < 0) {
_sensor_sub[uorb_index].registerCallback();
}
} else {
_last_data[uorb_index].timestamp = time_now_us;
_last_publication_timestamp[uorb_index] = time_now_us;
}
}
}
if (_advertised[uorb_index]) {
updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]);
sensor_baro_s report;
if (updated[uorb_index]) {
// millibar to Pa
const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f;
while (_sensor_sub[uorb_index].update(&report)) {
// pressure corrected with offset (if available)
const float pressure_corrected = (raw_pressure_pascals - _thermal_offset[uorb_index]);
if (_calibration[uorb_index].device_id() != report.device_id) {
_calibration[uorb_index].set_device_id(report.device_id);
_priority[uorb_index] = _calibration[uorb_index].priority();
}
float vect[3] {pressure_corrected, _last_data[uorb_index].temperature, 0.f};
_voter.put(uorb_index, _last_data[uorb_index].timestamp, vect, _last_data[uorb_index].error_count,
_priority[uorb_index]);
if (_calibration[uorb_index].enabled()) {
if (!was_advertised) {
if (uorb_index > 0) {
/* the first always exists, but for each further sensor, add a new validator */
if (!_voter.add_new_validator()) {
PX4_ERR("failed to add validator for %s %i", _calibration[uorb_index].SensorString(), uorb_index);
}
}
if (_selected_sensor_sub_index < 0) {
_sensor_sub[uorb_index].registerCallback();
}
ParametersUpdate(true);
}
// pressure corrected with offset (if available)
_calibration[uorb_index].SensorCorrectionsUpdate();
const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure);
float data_array[3] {pressure_corrected, report.temperature, PressureToAltitude(pressure_corrected)};
_voter.put(uorb_index, report.timestamp, data_array, report.error_count, _priority[uorb_index]);
_timestamp_sample_sum[uorb_index] += report.timestamp_sample;
_data_sum[uorb_index] += pressure_corrected;
_temperature_sum[uorb_index] += report.temperature;
_data_sum_count[uorb_index]++;
_last_data[uorb_index] = pressure_corrected;
updated[uorb_index] = true;
}
}
}
}
@@ -216,7 +219,8 @@ void VehicleAirData::Run()
}
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("%s switch from #%" PRIu8 " -> #%d", "BARO", _selected_sensor_sub_index, best_index);
PX4_INFO("%s switch from #%" PRId8 " -> #%d", _calibration[_selected_sensor_sub_index].SensorString(),
_selected_sensor_sub_index, best_index);
}
_selected_sensor_sub_index = best_index;
@@ -224,79 +228,112 @@ void VehicleAirData::Run()
}
}
if ((_selected_sensor_sub_index >= 0)
&& (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
&& updated[_selected_sensor_sub_index]) {
// Publish
if (_param_sens_baro_rate.get() > 0) {
int interval_us = 1e6f / _param_sens_baro_rate.get();
const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index];
for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if (updated[instance] && (_data_sum_count[instance] > 0)) {
_baro_timestamp_sum += baro.timestamp_sample;
_baro_sum += baro.pressure;
_baro_sum_count++;
const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _data_sum_count[instance];
if ((_param_sens_baro_rate.get() > 0)
&& ((_last_publication_timestamp == 0)
|| (hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())))) {
if (timestamp_sample >= _last_publication_timestamp[instance] + interval_us) {
const float pressure = _baro_sum / _baro_sum_count;
const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count;
bool publish = (time_now_us <= timestamp_sample + 1_s);
// reset
_baro_timestamp_sum = 0;
_baro_sum = 0.f;
_baro_sum_count = 0;
if (publish) {
publish = (_selected_sensor_sub_index >= 0)
&& (instance == _selected_sensor_sub_index)
&& (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR);
}
// populate vehicle_air_data with primary baro and publish
vehicle_air_data_s out{};
out.timestamp_sample = timestamp_sample;
out.baro_device_id = baro.device_id;
out.baro_temp_celcius = baro.temperature;
if (publish) {
const float pressure_pa = _data_sum[instance] / _data_sum_count[instance];
const float temperature = _temperature_sum[instance] / _data_sum_count[instance];
// Convert from millibar to Pa and apply temperature compensation
out.baro_pressure_pa = 100.0f * pressure - _thermal_offset[_selected_sensor_sub_index];
float altitude = PressureToAltitude(pressure_pa, temperature);
// calculate altitude using the hypsometric equation
static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
static constexpr float a = -6.5f / 1000.0f; // temperature gradient in degrees per metre
// calculate air density
float air_density = pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius -
CONSTANTS_ABSOLUTE_NULL_CELSIUS));
// current pressure at MSL in kPa (QNH in hPa)
const float p1 = _param_sens_baro_qnh.get() * 0.1f;
// populate vehicle_air_data with and publish
vehicle_air_data_s out{};
out.timestamp_sample = timestamp_sample;
out.baro_device_id = _calibration[instance].device_id();
out.baro_alt_meter = altitude;
out.baro_temp_celcius = temperature;
out.baro_pressure_pa = pressure_pa;
out.rho = air_density;
out.calibration_count = _calibration[instance].calibration_count();
out.timestamp = hrt_absolute_time();
// measured pressure in kPa
const float p = out.baro_pressure_pa * 0.001f;
_vehicle_air_data_pub.publish(out);
}
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
out.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
_last_publication_timestamp[instance] = timestamp_sample;
// calculate air density
out.rho = out.baro_pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius -
CONSTANTS_ABSOLUTE_NULL_CELSIUS));
out.timestamp = hrt_absolute_time();
_vehicle_air_data_pub.publish(out);
_last_publication_timestamp = out.timestamp;
// reset
_timestamp_sample_sum[instance] = 0;
_data_sum[instance] = 0;
_temperature_sum[instance] = 0;
_data_sum_count[instance] = 0;
}
}
}
}
if (!parameter_update) {
CheckFailover(time_now_us);
}
UpdateStatus();
// reschedule timeout
ScheduleDelayed(50_ms);
perf_end(_cycle_perf);
}
float VehicleAirData::PressureToAltitude(float pressure_pa, float temperature) const
{
// calculate altitude using the hypsometric equation
static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre
// current pressure at MSL in kPa (QNH in hPa)
const float p1 = _param_sens_baro_qnh.get() * 0.1f;
// measured pressure in kPa
const float p = pressure_pa * 0.001f;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
return altitude;
}
void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us)
{
// check failover and report (save failover report for a cycle where parameters didn't update)
if (_last_failover_count != _voter.failover_count() && !parameter_update) {
if (_last_failover_count != _voter.failover_count()) {
uint32_t flags = _voter.failover_state();
int failover_index = _voter.failover_index();
if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
if (failover_index != -1) {
if (failover_index >= 0 && failover_index < MAX_SENSOR_COUNT) {
if (time_now_us > _last_error_message + 3_s) {
mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!\t",
"BARO",
_calibration[failover_index].SensorString(),
failover_index,
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
@@ -334,27 +371,65 @@ void VehicleAirData::Run()
_last_failover_count = _voter.failover_count();
}
}
// reschedule timeout
ScheduleDelayed(50_ms);
void VehicleAirData::UpdateStatus()
{
if (_selected_sensor_sub_index >= 0) {
sensors_status_s sensors_status{};
sensors_status.device_id_primary = _calibration[_selected_sensor_sub_index].device_id();
perf_end(_cycle_perf);
float mean{};
int sensor_count = 0;
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
if ((_calibration[sensor_index].device_id() != 0) && (_calibration[sensor_index].enabled())) {
sensor_count++;
mean += _last_data[sensor_index];
}
}
if (sensor_count > 0) {
mean /= sensor_count;
}
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
if (_calibration[sensor_index].device_id() != 0) {
_sensor_diff[sensor_index] = 0.95f * _sensor_diff[sensor_index] + 0.05f * (_last_data[sensor_index] - mean);
sensors_status.device_ids[sensor_index] = _calibration[sensor_index].device_id();
sensors_status.inconsistency[sensor_index] = _sensor_diff[sensor_index];
sensors_status.healthy[sensor_index] = (_voter.get_sensor_state(sensor_index) == DataValidator::ERROR_FLAG_NO_ERROR);
sensors_status.priority[sensor_index] = _voter.get_sensor_priority(sensor_index);
sensors_status.enabled[sensor_index] = _calibration[sensor_index].enabled();
sensors_status.external[sensor_index] = _calibration[sensor_index].external();
} else {
sensors_status.inconsistency[sensor_index] = NAN;
}
}
sensors_status.timestamp = hrt_absolute_time();
_sensors_status_baro_pub.publish(sensors_status);
}
}
void VehicleAirData::PrintStatus()
{
if (_selected_sensor_sub_index >= 0) {
PX4_INFO_RAW("[vehicle_air_data] selected barometer: %" PRIu32 " (%" PRId8 ")\n",
_last_data[_selected_sensor_sub_index].device_id,
_selected_sensor_sub_index);
if (fabsf(_thermal_offset[_selected_sensor_sub_index]) > 0.f) {
PX4_INFO_RAW("%" PRIu32 " temperature offset: %.4f\n", _last_data[_selected_sensor_sub_index].device_id,
(double)_thermal_offset[_selected_sensor_sub_index]);
}
PX4_INFO_RAW("[vehicle_air_data] selected %s: %" PRIu32 " (%" PRId8 ")\n",
_calibration[_selected_sensor_sub_index].SensorString(),
_calibration[_selected_sensor_sub_index].device_id(), _selected_sensor_sub_index);
}
_voter.print();
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (_advertised[i] && (_priority[i] > 0)) {
_calibration[i].PrintStatus();
}
}
}
}; // namespace sensors
@@ -35,6 +35,7 @@
#include "data_validator/DataValidatorGroup.hpp"
#include <lib/sensor_calibration/Barometer.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
@@ -44,12 +45,14 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensors_status.h>
#include <uORB/topics/vehicle_air_data.h>
using namespace time_literals;
@@ -71,17 +74,21 @@ public:
private:
void Run() override;
bool ParametersUpdate();
void SensorCorrectionsUpdate(bool force = false);
void AirTemperatureUpdate();
void CheckFailover(const hrt_abstime &time_now_us);
bool ParametersUpdate(bool force = false);
void UpdateStatus();
float PressureToAltitude(float pressure_pa, float temperature = 15.f) const;
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<sensors_status_s> _sensors_status_baro_pub{ORB_ID(sensors_status_baro)};
uORB::Publication<vehicle_air_data_s> _vehicle_air_data_pub{ORB_ID(vehicle_air_data)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
@@ -91,23 +98,26 @@ private:
{this, ORB_ID(sensor_baro), 3},
};
calibration::Barometer _calibration[MAX_SENSOR_COUNT];
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
hrt_abstime _last_publication_timestamp{0};
hrt_abstime _last_error_message{0};
orb_advert_t _mavlink_log_pub{nullptr};
DataValidatorGroup _voter{1};
unsigned _last_failover_count{0};
uint64_t _baro_timestamp_sum{0};
float _baro_sum{0.f};
int _baro_sum_count{0};
uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
float _data_sum[MAX_SENSOR_COUNT] {};
float _temperature_sum[MAX_SENSOR_COUNT] {};
int _data_sum_count[MAX_SENSOR_COUNT] {};
hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
sensor_baro_s _last_data[MAX_SENSOR_COUNT] {};
float _last_data[MAX_SENSOR_COUNT] {};
bool _advertised[MAX_SENSOR_COUNT] {};
float _thermal_offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f};
float _sensor_diff[MAX_SENSOR_COUNT] {}; // filtered differences between sensor instances
uint8_t _priority[MAX_SENSOR_COUNT] {};
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -38,9 +38,6 @@
* @max 1500
* @group Sensors
* @unit hPa
*
* @reboot_required true
*
*/
PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
@@ -54,8 +51,5 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
* @max 200
* @group Sensors
* @unit Hz
*
* @reboot_required true
*
*/
PARAM_DEFINE_FLOAT(SENS_BARO_RATE, 20.0f);
-1
View File
@@ -43,7 +43,6 @@ px4_add_module(
DEPENDS
mathlib
drivers_accelerometer
drivers_barometer
drivers_gyroscope
drivers_magnetometer
px4_work_queue
+10 -2
View File
@@ -152,8 +152,16 @@ void Sih::Run()
if (_now - _baro_time >= 50_ms
&& fabs(_baro_offset_m) < 10000) {
_baro_time = _now;
_px4_baro.set_temperature(_baro_temp_c);
_px4_baro.update(_now, _baro_p_mBar);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = _now;
sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
sensor_baro.pressure = _baro_p_mBar * 100.f;
sensor_baro.temperature = _baro_temp_c;
sensor_baro.error_count = 0;
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
}
// gps published at 20Hz
+3 -2
View File
@@ -65,7 +65,6 @@
#include <lib/geo/geo.h> // to get the physical constants
#include <drivers/drv_hrt.h> // to get the real time
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
@@ -75,6 +74,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/vehicle_angular_velocity.h> // to publish groundtruth
#include <uORB/topics/vehicle_attitude.h> // to publish groundtruth
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
@@ -117,7 +117,8 @@ private:
PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
// to publish the gps position
sensor_gps_s _sensor_gps{};
-1
View File
@@ -65,7 +65,6 @@ px4_add_module(
conversion
geo
drivers_accelerometer
drivers_barometer
drivers_gyroscope
drivers_magnetometer
)
@@ -39,7 +39,6 @@ px4_add_module(
SensorBaroSim.cpp
SensorBaroSim.hpp
DEPENDS
drivers_barometer
geo
px4_work_queue
)
@@ -161,8 +161,15 @@ void SensorBaroSim::Run()
// calculate temperature in Celsius
float temperature = temperature_local - 273.0f + _sim_baro_off_t.get();
_px4_baro.set_temperature(temperature);
_px4_baro.update(gpos.timestamp, pressure);
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = gpos.timestamp;
sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
sensor_baro.pressure = pressure;
sensor_baro.temperature = temperature;
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
_last_update_time = gpos.timestamp;
}
@@ -33,16 +33,17 @@
#pragma once
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/vehicle_global_position.h>
using namespace time_literals;
@@ -80,12 +81,12 @@ private:
hrt_abstime _last_update_time{0};
PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_p,
(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_t
(ParamFloat<px4::params::SIM_BARO_OFF_T>) _sim_baro_off_t
)
};
@@ -42,5 +42,6 @@ PARAM_DEFINE_FLOAT(SIM_BARO_OFF_P, 0.0f);
* simulated barometer temperature offset
*
* @group Simulator
* @unit celcius
*/
PARAM_DEFINE_FLOAT(SIM_BARO_OFF_T, 0.0f);
+5 -3
View File
@@ -44,7 +44,6 @@
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/geo/geo.h>
@@ -64,6 +63,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -179,8 +179,7 @@ private:
PX4Magnetometer _px4_mag_0{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag_1{197644, ROTATION_NONE}; // 197644: DRV_MAG_DEVTYPE_MAGSIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro_0{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
PX4Barometer _px4_baro_1{6620428}; // 6620428: DRV_BARO_DEVTYPE_BAROSIM, BUS: 2, ADDR: 4, TYPE: SIMULATION
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pubs[2] {{ORB_ID(sensor_baro)}, {ORB_ID(sensor_baro)}};
float _sensors_temperature{0};
@@ -291,6 +290,9 @@ private:
float _last_magz{0.0f};
bool _use_dynamic_mixing{false};
float _last_baro_pressure{0.0f};
float _last_baro_temperature{0.0f};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
px4::atomic<bool> _has_initialized {false};
#endif
+19 -10
View File
@@ -326,18 +326,27 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
// baro
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) {
if (_baro_stuck) {
_px4_baro_0.update(time, _px4_baro_0.get().pressure);
_px4_baro_0.set_temperature(_px4_baro_0.get().temperature);
_px4_baro_1.update(time, _px4_baro_1.get().pressure);
_px4_baro_1.set_temperature(_px4_baro_1.get().temperature);
} else {
_px4_baro_0.update(time, sensors.abs_pressure);
_px4_baro_0.set_temperature(sensors.temperature);
_px4_baro_1.update(time, sensors.abs_pressure);
_px4_baro_1.set_temperature(sensors.temperature);
if (!_baro_stuck) {
_last_baro_pressure = sensors.abs_pressure;
_last_baro_temperature = sensors.temperature;
}
// publish
sensor_baro_s sensor_baro{};
sensor_baro.timestamp_sample = time;
sensor_baro.pressure = _last_baro_pressure;
sensor_baro.temperature = _last_baro_temperature;
// publish 1st baro
sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pubs[0].publish(sensor_baro);
// publish 2nd baro
sensor_baro.device_id = 6620428; // 6620428: DRV_BARO_DEVTYPE_BAROSIM, BUS: 2, ADDR: 4, TYPE: SIMULATION
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pubs[1].publish(sensor_baro);
}
// differential pressure