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

sensors: move baro aggregation to new sensors/vehicle_air_data

This commit is contained in:
Daniel Agar 2020-01-20 23:28:30 -05:00
parent 093e9ba1ce
commit 0eca583ecf
25 changed files with 479 additions and 276 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -128,8 +128,6 @@ then
param set CAL_MAG0_ID 197388
param set CAL_MAG_PRIME 197388
param set CAL_BARO_PRIME 6620172
param set CBRK_AIRSPD_CHK 0
param set COM_DISARM_LAND 0.5

1
msg/sensor_selection.msg
View File

@ -4,6 +4,5 @@
#
uint64 timestamp # time since system start (microseconds)
uint32 accel_device_id # unique device ID for the selected accelerometers
uint32 baro_device_id # unique device ID for the selected barometer
uint32 gyro_device_id # unique device ID for the selected rate gyros
uint32 mag_device_id # unique device ID for the selected magnetometer

7
msg/vehicle_air_data.msg
View File

@ -1,4 +1,9 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
uint32 baro_device_id # unique device ID for the selected barometer
float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH.
float32 baro_temp_celcius # Temperature in degrees celsius

23
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -221,11 +221,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- BARO ---- */
if (checkSensors) {
//bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_BARO_PRIME"), &prime_id);
int32_t sys_has_baro = 1;
param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
@ -238,29 +233,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t device_id = -1;
if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if ((prime_id > 0) && (device_id == prime_id)) {
//prime_found = true;
}
} else {
if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
baro_fail_reported = true;
}
}
}
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
// if (false) {
// if (reportFailures && !failed) {
// mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
// }
// set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status);
// failed = true;
// }
}
/* ---- IMU CONSISTENCY ---- */

1
src/modules/commander/CMakeLists.txt
View File

@ -41,7 +41,6 @@ px4_add_module(
SRCS
accelerometer_calibration.cpp
airspeed_calibration.cpp
baro_calibration.cpp
calibration_routines.cpp
Commander.cpp
commander_helper.cpp

1
src/modules/commander/Commander.cpp
View File

@ -50,7 +50,6 @@
#include "Arming/HealthFlags/HealthFlags.h"
#include "accelerometer_calibration.h"
#include "airspeed_calibration.h"
#include "baro_calibration.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include "esc_calibration.h"

54
src/modules/commander/baro_calibration.cpp
View File

@ -1,54 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 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
* Barometer calibration routine
*/
#include "baro_calibration.h"
#include <math.h>
#include <fcntl.h>
#include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_baro.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
int do_baro_calibration(orb_advert_t *mavlink_log_pub)
{
// TODO implement this
return PX4_ERROR;
}

2
src/modules/ekf2/ekf2_main.cpp
View File

@ -911,7 +911,7 @@ void Ekf2::Run()
if (_airdata_sub.copy(&airdata)) {
_ekf.set_air_density(airdata.rho);
const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp};
const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp_sample};
_ekf.setBaroData(baro_sample);
ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);

2
src/modules/sensors/CMakeLists.txt
View File

@ -35,6 +35,7 @@ add_subdirectory(sensor_corrections) # used by vehicle_{acceleration, angular_ve
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_subdirectory(vehicle_acceleration)
add_subdirectory(vehicle_angular_velocity)
add_subdirectory(vehicle_air_data)
add_subdirectory(vehicle_imu)
px4_add_module(
@ -54,5 +55,6 @@ px4_add_module(
mathlib
vehicle_acceleration
vehicle_angular_velocity
vehicle_air_data
vehicle_imu
)

6
src/modules/sensors/common.h
View File

@ -46,11 +46,7 @@ namespace sensors
constexpr uint8_t MAG_COUNT_MAX = 4;
constexpr uint8_t GYRO_COUNT_MAX = 3;
constexpr uint8_t ACCEL_COUNT_MAX = 3;
constexpr uint8_t BARO_COUNT_MAX = 3;
constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX,
math::max(GYRO_COUNT_MAX,
math::max(ACCEL_COUNT_MAX,
BARO_COUNT_MAX)));
constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX));
} /* namespace sensors */

5
src/modules/sensors/parameters.cpp
View File

@ -58,9 +58,6 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* Barometer QNH */
parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
@ -107,8 +104,6 @@ void update_parameters(const ParameterHandles &parameter_handles, Parameters &pa
param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1]));
param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2]));
param_get(parameter_handles.baro_qnh, &(parameters.baro_qnh));
param_get(parameter_handles.air_cmodel, &parameters.air_cmodel);
param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);

4
src/modules/sensors/parameters.h
View File

@ -56,8 +56,6 @@ struct Parameters {
float board_offset[3];
float baro_qnh;
int32_t air_cmodel;
float air_tube_length;
float air_tube_diameter_mm;
@ -73,8 +71,6 @@ struct ParameterHandles {
param_t board_offset[3];
param_t baro_qnh;
param_t air_cmodel;
param_t air_tube_length;
param_t air_tube_diameter_mm;

21
src/modules/sensors/sensor_params.c
View File

@ -31,14 +31,6 @@
*
****************************************************************************/
/**
* Primary baro ID
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0);
/**
* Airspeed sensor compensation model for the SDP3x
*
@ -108,19 +100,6 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
/**
* QNH for barometer
*
* @min 500
* @max 1500
* @group Sensors
* @unit hPa
*
* @reboot_required true
*
*/
PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
/**
* Board rotation
*

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

@ -74,6 +74,7 @@
#include "voted_sensors_update.h"
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_air_data/VehicleAirData.hpp"
#include "vehicle_imu/VehicleIMU.hpp"
using namespace sensors;
@ -112,7 +113,6 @@ private:
bool _armed{false}; /**< arming status of the vehicle */
hrt_abstime _last_config_update{0};
hrt_abstime _airdata_prev_timestamp{0};
hrt_abstime _sensor_combined_prev_timestamp{0};
hrt_abstime _magnetometer_prev_timestamp{0};
@ -123,11 +123,11 @@ private:
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */
uORB::Publication<sensor_preflight_s> _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */
uORB::Publication<vehicle_air_data_s> _airdata_pub{ORB_ID(vehicle_air_data)}; /**< combined sensor data topic */
uORB::Publication<vehicle_magnetometer_s> _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */
uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub[GYRO_COUNT_MAX] {
@ -159,6 +159,7 @@ private:
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAirData _vehicle_air_data;
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@ -175,7 +176,7 @@ private:
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void diff_pres_poll(const vehicle_air_data_s &airdata);
void diff_pres_poll();
/**
* Check for changes in parameters.
@ -213,6 +214,7 @@ Sensors::Sensors(bool hil_enabled) :
_vehicle_acceleration.Start();
_vehicle_angular_velocity.Start();
_vehicle_air_data.Start();
InitializeVehicleIMU();
}
@ -221,6 +223,7 @@ Sensors::~Sensors()
{
_vehicle_acceleration.Stop();
_vehicle_angular_velocity.Stop();
_vehicle_air_data.Stop();
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
@ -268,15 +271,17 @@ int Sensors::adc_init()
return OK;
}
void
Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
void Sensors::diff_pres_poll()
{
differential_pressure_s diff_pres{};
if (_diff_pres_sub.update(&diff_pres)) {
vehicle_air_data_s air_data{};
_vehicle_air_data_sub.copy(&air_data);
float air_temperature_celsius = (diff_pres.temperature > -300.0f) ? diff_pres.temperature :
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
(air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
airspeed_s airspeed{};
airspeed.timestamp = diff_pres.timestamp;
@ -312,10 +317,10 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa,
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
air_temperature_celsius);
airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa,
airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
air_temperature_celsius); // assume that EAS = IAS as we don't have an EAS-scale here
airspeed.air_temperature_celsius = air_temperature_celsius;
@ -482,14 +487,13 @@ void Sensors::Run()
}
}
vehicle_air_data_s airdata{};
vehicle_magnetometer_s magnetometer{};
_voted_sensors_update.sensorsPoll(_sensor_combined, airdata, magnetometer);
_voted_sensors_update.sensorsPoll(_sensor_combined, magnetometer);
// check analog airspeed
adc_poll();
diff_pres_poll(airdata);
diff_pres_poll();
// check failover and update subscribed sensor (if necessary)
@ -517,11 +521,6 @@ void Sensors::Run()
}
}
if ((airdata.timestamp != 0) && (airdata.timestamp != _airdata_prev_timestamp)) {
_airdata_pub.publish(airdata);
_airdata_prev_timestamp = airdata.timestamp;
}
if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) {
_magnetometer_pub.publish(magnetometer);
_magnetometer_prev_timestamp = magnetometer.timestamp;
@ -618,6 +617,8 @@ int Sensors::print_status()
{
_voted_sensors_update.printStatus();
PX4_INFO_RAW("\n");
PX4_INFO("Airspeed status:");
_airspeed_validator.print();
@ -627,6 +628,11 @@ int Sensors::print_status()
PX4_INFO_RAW("\n");
_vehicle_angular_velocity.PrintStatus();
PX4_INFO_RAW("\n");
_vehicle_air_data.PrintStatus();
PX4_INFO_RAW("\n");
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
PX4_INFO_RAW("\n");

38
src/modules/sensors/vehicle_air_data/CMakeLists.txt Normal file
View File

@ -0,0 +1,38 @@
############################################################################
#
# Copyright (c) 2020 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(vehicle_air_data
VehicleAirData.cpp
VehicleAirData.hpp
)
target_link_libraries(vehicle_air_data PRIVATE px4_work_queue)

283
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp Normal file
View File

@ -0,0 +1,283 @@
/****************************************************************************
*
* Copyright (c) 2020 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 "VehicleAirData.hpp"
#include <px4_platform_common/log.h>
#include <lib/ecl/geo/geo.h>
using namespace matrix;
using namespace time_literals;
static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
VehicleAirData::VehicleAirData() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
{
_voter.set_timeout(SENSOR_TIMEOUT);
}
VehicleAirData::~VehicleAirData()
{
Stop();
perf_free(_cycle_perf);
}
bool VehicleAirData::Start()
{
ScheduleNow();
return true;
}
void VehicleAirData::Stop()
{
Deinit();
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
}
void VehicleAirData::SensorCorrectionsUpdate()
{
// check if the selected sensor has updated
sensor_correction_s corrections;
if (_sensor_correction_sub.update(&corrections)) {
for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) {
const sensor_baro_s &baro = _last_data[baro_index];
// find sensor (by device id) in sensor_correction
if (_sensor_correction_index[baro_index] < 0) {
for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) {
if ((baro.device_id > 0) && (corrections.baro_device_ids[correction_index] == baro.device_id)) {
_sensor_correction_index[baro_index] = correction_index;
}
}
}
switch (_sensor_correction_index[baro_index]) {
case 0:
_offset[baro_index] = corrections.baro_offset_0;
_scale[baro_index] = corrections.baro_scale_0;
break;
case 1:
_offset[baro_index] = corrections.baro_offset_1;
_scale[baro_index] = corrections.baro_scale_1;
break;
case 2:
_offset[baro_index] = corrections.baro_offset_2;
_scale[baro_index] = corrections.baro_scale_2;
break;
default:
_offset[baro_index] = 0.f;
_scale[baro_index] = 1.f;
}
}
}
}
void VehicleAirData::ParametersUpdate()
{
// Check if parameters have changed
if (_params_sub.updated()) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
updateParams();
}
}
void VehicleAirData::Run()
{
perf_begin(_cycle_perf);
bool updated[MAX_SENSOR_COUNT] {};
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
if (!_advertised[uorb_index]) {
// use data's timestamp to throttle advertisement checks
if (hrt_elapsed_time(&_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 sensor_baro:%i", uorb_index);
}
}
_advertised[uorb_index] = true;
} else {
_last_data[uorb_index].timestamp = hrt_absolute_time();
}
}
} else {
updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]);
if (updated[uorb_index]) {
if (_priority[uorb_index] == 0) {
// set initial priority
_priority[uorb_index] = _sensor_sub[uorb_index].get_priority();
}
// millibar to Pa
const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f;
// pressure corrected with offset and scale (if available)
const float pressure_corrected = (raw_pressure_pascals - _offset[uorb_index]) * _scale[uorb_index];
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]);
}
}
}
// check for the current best sensor
int best_index = 0;
_voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
if (_selected_sensor_sub_index != best_index) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
_selected_sensor_sub_index = best_index;
_sensor_sub[_selected_sensor_sub_index].registerCallback();
}
}
if ((_selected_sensor_sub_index >= 0) && updated[_selected_sensor_sub_index]) {
ParametersUpdate();
SensorCorrectionsUpdate();
const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index];
// populate vehicle_air_data with primary baro and publish
vehicle_air_data_s out{};
out.timestamp_sample = baro.timestamp; // TODO: baro.timestamp_sample;
out.baro_device_id = baro.device_id;
out.baro_temp_celcius = baro.temperature;
// Convert from millibar to Pa and apply temperature compensation
out.baro_pressure_pa = (100.0f * baro.pressure - _offset[_selected_sensor_sub_index]) *
_scale[_selected_sensor_sub_index];
// 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
// 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 = out.baro_pressure_pa * 0.001f;
/*
* 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;
// calculate air density
// estimate air density assuming typical 20degC ambient temperature
// TODO: use air temperature if available (differential pressure sensors)
static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f -
CONSTANTS_ABSOLUTE_NULL_CELSIUS));
out.rho = pressure_to_density * out.baro_pressure_pa;
out.timestamp = hrt_absolute_time();
_vehicle_air_data_pub.publish(out);
}
// check failover and report
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) {
// we switched due to a non-critical reason. No need to panic.
PX4_INFO("sensor_baro switch from #%i", failover_index);
}
} else {
if (failover_index != -1) {
mavlink_log_emergency(&_mavlink_log_pub, "sensor_baro:#%i failed: %s%s%s%s%s!, reconfiguring priorities",
failover_index,
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
// reduce priority of failed sensor to the minimum
_priority[failover_index] = ORB_PRIO_MIN;
}
}
}
// reschedule timeout
ScheduleDelayed(100_ms);
perf_end(_cycle_perf);
}
void VehicleAirData::PrintStatus()
{
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("selected barometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id, _selected_sensor_sub_index);
}
perf_print_counter(_cycle_perf);
_voter.print();
}

105
src/modules/sensors/vehicle_air_data/VehicleAirData.hpp Normal file
View File

@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (c) 2020 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 <lib/ecl/validation/data_validator_group.h>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/vehicle_air_data.h>
class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem
{
public:
VehicleAirData();
~VehicleAirData() override;
bool Start();
void Stop();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate();
void SensorCorrectionsUpdate();
static constexpr int MAX_SENSOR_COUNT = 3;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh
)
uORB::Publication<vehicle_air_data_s> _vehicle_air_data_pub{ORB_ID(vehicle_air_data)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(sensor_baro), 0},
{this, ORB_ID(sensor_baro), 1},
{this, ORB_ID(sensor_baro), 2}
};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
orb_advert_t _mavlink_log_pub{nullptr};
DataValidatorGroup _voter{1};
unsigned _last_failover_count{0};
sensor_baro_s _last_data[MAX_SENSOR_COUNT] {};
bool _advertised[MAX_SENSOR_COUNT] {};
float _offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f};
float _scale[MAX_SENSOR_COUNT] {1.f, 1.f, 1.f};
int8_t _sensor_correction_index[MAX_SENSOR_COUNT] {-1, -1, -1};
uint8_t _priority[MAX_SENSOR_COUNT] {};
int8_t _selected_sensor_sub_index{-1};
};

24
src/modules/commander/baro_calibration.h → src/modules/sensors/vehicle_air_data/params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 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
@ -32,16 +32,14 @@
****************************************************************************/
/**
* @file mag_calibration.h
* Barometer calibration routine
* QNH for barometer
*
* @min 500
* @max 1500
* @group Sensors
* @unit hPa
*
* @reboot_required true
*
*/
#ifndef BARO_CALIBRATION_H_
#define BARO_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_baro_calibration(orb_advert_t *mavlink_log_pub);
#endif /* BARO_CALIBRATION_H_ */
PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);

112
src/modules/sensors/voted_sensors_update.cpp
View File

@ -63,11 +63,6 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_en
_corrections.accel_scale_2[i] = 1.0f;
}
_corrections.baro_scale_0 = 1.0f;
_corrections.baro_scale_1 = 1.0f;
_corrections.baro_scale_2 = 1.0f;
_baro.voter.set_timeout(300000);
_mag.voter.set_timeout(300000);
_mag.voter.set_equal_value_threshold(1000);
@ -94,7 +89,6 @@ void VotedSensorsUpdate::initializeSensors()
initSensorClass(ORB_ID(sensor_gyro_integrated), _gyro, GYRO_COUNT_MAX);
initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
initSensorClass(ORB_ID(sensor_accel_integrated), _accel, ACCEL_COUNT_MAX);
initSensorClass(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX);
}
void VotedSensorsUpdate::deinit()
@ -110,10 +104,6 @@ void VotedSensorsUpdate::deinit()
for (int i = 0; i < _mag.subscription_count; i++) {
orb_unsubscribe(_mag.subscription[i]);
}
for (int i = 0; i < _baro.subscription_count; i++) {
orb_unsubscribe(_baro.subscription[i]);
}
}
void VotedSensorsUpdate::parametersUpdate()
@ -715,101 +705,6 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
}
}
void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
{
bool got_update = false;
float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 };
float *scales[] = {&_corrections.baro_scale_0, &_corrections.baro_scale_1, &_corrections.baro_scale_2 };
for (int uorb_index = 0; uorb_index < _baro.subscription_count; uorb_index++) {
bool baro_updated;
orb_check(_baro.subscription[uorb_index], &baro_updated);
if (baro_updated) {
sensor_baro_s baro_report{};
int ret = orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report);
if (ret != PX4_OK || baro_report.timestamp == 0) {
continue; //ignore invalid data
}
// Convert from millibar to Pa
float corrected_pressure = 100.0f * baro_report.pressure;
// apply temperature compensation
corrected_pressure = (corrected_pressure - *offsets[uorb_index]) * *scales[uorb_index];
// First publication with data
if (_baro.priority[uorb_index] == 0) {
int32_t priority = 0;
orb_priority(_baro.subscription[uorb_index], &priority);
_baro.priority[uorb_index] = (uint8_t)priority;
}
_baro_device_id[uorb_index] = baro_report.device_id;
got_update = true;
float vect[3] = {baro_report.pressure, baro_report.temperature, 0.f};
_last_airdata[uorb_index].timestamp = baro_report.timestamp;
_last_airdata[uorb_index].baro_temp_celcius = baro_report.temperature;
_last_airdata[uorb_index].baro_pressure_pa = corrected_pressure;
_baro.voter.put(uorb_index, baro_report.timestamp, vect, baro_report.error_count, _baro.priority[uorb_index]);
}
}
if (got_update) {
int best_index;
_baro.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
airdata = _last_airdata[best_index];
if (_baro.last_best_vote != best_index) {
_baro.last_best_vote = (uint8_t)best_index;
}
if (_selection.baro_device_id != _baro_device_id[best_index]) {
_selection_changed = true;
_selection.baro_device_id = _baro_device_id[best_index];
}
// 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 */
/* current pressure at MSL in kPa (QNH in hPa)*/
const float p1 = _parameters.baro_qnh * 0.1f;
/* measured pressure in kPa */
const float p = airdata.baro_pressure_pa * 0.001f;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
airdata.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
// calculate air density
// estimate air density assuming typical 20degC ambient temperature
// TODO: use air temperature if available (differential pressure sensors)
static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f -
CONSTANTS_ABSOLUTE_NULL_CELSIUS));
airdata.rho = pressure_to_density * airdata.baro_pressure_pa;
}
}
}
bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type)
{
if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {
@ -917,19 +812,15 @@ void VotedSensorsUpdate::printStatus()
_accel.voter.print();
PX4_INFO("mag status:");
_mag.voter.print();
PX4_INFO("baro status:");
_baro.voter.print();
}
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata,
vehicle_magnetometer_s &magnetometer)
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer)
{
_corrections_sub.update(&_corrections);
accelPoll(raw);
gyroPoll(raw);
magPoll(magnetometer);
baroPoll(airdata);
// publish sensor selection if changed
if (_selection_changed) {
@ -946,7 +837,6 @@ void VotedSensorsUpdate::checkFailover()
checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC);
checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
checkFailover(_baro, "Baro", subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE);
}
void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)

15
src/modules/sensors/voted_sensors_update.h
View File

@ -44,7 +44,6 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
@ -62,7 +61,6 @@
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/subsystem_info.h>
@ -112,7 +110,7 @@ public:
/**
* read new sensor data
*/
void sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer);
void sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer);
/**
* set the relative timestamps of each sensor timestamp, based on the last sensorsPoll,
@ -194,14 +192,6 @@ private:
*/
void magPoll(vehicle_magnetometer_s &magnetometer);
/**
* Poll the barometer for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void baroPoll(vehicle_air_data_s &airdata);
/**
* Check & handle failover of a sensor
* @return true if a switch occured (could be for a non-critical reason)
@ -211,7 +201,6 @@ private:
SensorData _accel {};
SensorData _gyro {};
SensorData _mag {};
SensorData _baro {};
orb_advert_t _mavlink_log_pub{nullptr};
@ -222,7 +211,6 @@ private:
uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)};
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
matrix::Dcmf _board_rotation {}; /**< rotation matrix for the orientation that the board is mounted */
@ -238,7 +226,6 @@ private:
float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */
uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */
uint32_t _baro_device_id[SENSOR_COUNT_MAX] {}; /**< baro driver device id for each uorb instance */
uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */
uint32_t _mag_device_id[SENSOR_COUNT_MAX] {}; /**< mag driver device id for each uorb instance */

1
src/modules/uORB/Subscription.hpp
View File

@ -137,6 +137,7 @@ public:
uint8_t get_instance() const { return _instance; }
orb_id_t get_topic() const { return get_orb_meta(_orb_id); }
ORB_PRIO get_priority() { return advertised() ? _node->get_priority() : ORB_PRIO_UNINITIALIZED; }
protected:

1
src/modules/uORB/SubscriptionInterval.hpp
View File

@ -132,6 +132,7 @@ public:
uint8_t get_instance() const { return _subscription.get_instance(); }
orb_id_t get_topic() const { return _subscription.get_topic(); }
ORB_PRIO get_priority() { return _subscription.get_priority(); }
/**
* Set the interval in microseconds

3
src/modules/uORB/uORB.h
View File

@ -67,7 +67,8 @@ typedef const struct orb_metadata *orb_id_t;
* Relevant for multi-topics / topic groups
*/
enum ORB_PRIO {
ORB_PRIO_MIN = 1, // leave 0 free for other purposes, eg. marking an uninitialized value
ORB_PRIO_UNINITIALIZED = 0,
ORB_PRIO_MIN = 1,
ORB_PRIO_VERY_LOW = 25,
ORB_PRIO_LOW = 50,
ORB_PRIO_DEFAULT = 75,

2
src/modules/uORB/uORBDeviceMaster.cpp
View File

@ -142,7 +142,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertise
if (existing_node != nullptr &&
(!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) {
if (is_advertiser) {
existing_node->set_priority(priority);
existing_node->set_priority((ORB_PRIO)priority);
/* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers
* could get the same instance).
*/

4
src/modules/uORB/uORBDeviceNode.hpp
View File

@ -198,8 +198,8 @@ public:
uint8_t get_instance() const { return _instance; }
int get_priority() const { return _priority; }
void set_priority(uint8_t priority) { _priority = priority; }
ORB_PRIO get_priority() const { return (ORB_PRIO)_priority; }
void set_priority(ORB_PRIO priority) { _priority = priority; }
/**
* Copies data and the corresponding generation