mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-21 06:03:18 +08:00
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:
@@ -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"'
|
||||
}
|
||||
|
||||
@@ -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
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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.
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -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};
|
||||
|
||||
};
|
||||
|
||||
@@ -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; }
|
||||
|
||||
@@ -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 ×tamp_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; }
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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};
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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");
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
+4
-26
@@ -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 ×tamp_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);
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -58,6 +58,7 @@ public:
|
||||
AirspeedCalibration,
|
||||
ESCCalibration,
|
||||
MagCalibrationQuick,
|
||||
BaroCalibration,
|
||||
|
||||
ParamLoadDefault,
|
||||
ParamSaveDefault,
|
||||
|
||||
@@ -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});
|
||||
|
||||
@@ -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};
|
||||
|
||||
|
||||
@@ -101,7 +101,6 @@ px4_add_module(
|
||||
airspeed
|
||||
component_general_json # for checksums.h
|
||||
drivers_accelerometer
|
||||
drivers_barometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
conversion
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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(¶m_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);
|
||||
|
||||
@@ -43,7 +43,6 @@ px4_add_module(
|
||||
DEPENDS
|
||||
mathlib
|
||||
drivers_accelerometer
|
||||
drivers_barometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
px4_work_queue
|
||||
|
||||
+10
-2
@@ -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
|
||||
|
||||
@@ -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{};
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user