mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 13:53:34 +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:
Daniel Agar
@@ -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;
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <math.h>
|
||||
#include <stdint.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
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,
|
||||
|
||||
Reference in New Issue
Block a user