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

drivers/differential_pressure: remove lib/drivers/airspeed dependency and cleanup

Browse Source 
- split ms4525_airspeed into separate ms4515 and ms4525 drivers
This commit is contained in:
Daniel Agar
2022-04-26 08:39:19 -04:00
parent d1d15a6f6d
commit f390f52058
50 changed files with 1350 additions and 879 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/airspeed_calibration.cpp
+20 -28
View File
@@ -49,12 +49,14 @@
#include <fcntl.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>
using namespace time_literals;
static const char *sensor_name = "airspeed";
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
@@ -69,7 +71,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
int result = PX4_OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 2400;
const unsigned maxcount = 500;
/* give directions */
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
@@ -78,15 +80,10 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
float diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
return PX4_ERROR;
}
calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
px4_usleep(500 * 1000);
uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
uORB::SubscriptionData<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
while (calibration_counter < calibration_count) {
@@ -94,31 +91,24 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
return PX4_ERROR;
}
differential_pressure_s diff_pres;
if (diff_pres_sub.update()) {
if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
const differential_pressure_s &diff_pres = diff_pres_sub.get();
diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
/* any differential pressure failure a reason to abort */
if (diff_pres.error_count != 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu32 ")",
diff_pres.error_count);
calibration_log_critical(mavlink_log_pub, "[cal] Check wiring, reboot vehicle, and try again");
feedback_calibration_failed(mavlink_log_pub);
return PX4_ERROR;
}
if (calibration_counter % (calibration_count / 20) == 0) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
}
} else {
/* any poll failure for 1s is a reason to abort */
if (hrt_elapsed_time(&calibration_started) > 30_s) {
feedback_calibration_failed(mavlink_log_pub);
return PX4_ERROR;
}
px4_usleep(10000);
}
diff_pres_offset = diff_pres_offset / calibration_count;
@@ -132,7 +122,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
diff_pres_offset = 0.00000001f;
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset)) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
return PX4_ERROR;
}
@@ -161,9 +151,9 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
return PX4_ERROR;
}
differential_pressure_s diff_pres;
if (diff_pres_sub.update()) {
const differential_pressure_s &diff_pres = diff_pres_sub.get();
if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
differential_pressure_sum += diff_pres.differential_pressure_pa;
differential_pressure_sum_count++;
@@ -197,7 +187,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
}
}
if (calibration_counter % 500 == 0) {
if (calibration_counter % 300 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)differential_pressure_pa);
tune_neutral(true);
@@ -208,12 +198,14 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
}
calibration_counter++;
}
} else {
/* any poll failure for 1s is a reason to abort */
if (hrt_elapsed_time(&calibration_started) > 90_s) {
feedback_calibration_failed(mavlink_log_pub);
return PX4_ERROR;
}
px4_usleep(10000);
}
if (calibration_counter == maxcount) {
@@ -227,7 +219,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
tune_neutral(true);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
px4_sleep(1);
px4_usleep(200000);
return result;
}
src/modules/mavlink/mavlink_receiver.cpp
+4 -3
View File
@@ -2312,7 +2312,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
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.pressure = hil_sensor.abs_pressure * 100.0f; // hPa to Pa
sensor_baro.temperature = hil_sensor.temperature;
sensor_baro.error_count = 0;
sensor_baro.timestamp = hrt_absolute_time();
@@ -2323,9 +2323,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
differential_pressure_s report{};
report.timestamp_sample = timestamp;
report.device_id = 0; // TODO
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
report.temperature = hil_sensor.temperature;
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
report.timestamp = hrt_absolute_time();
_differential_pressure_pub.publish(report);
}
@@ -2653,6 +2653,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* airspeed */
{
airspeed_s airspeed{};
airspeed.timestamp_sample = timestamp_sample;
airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
airspeed.air_temperature_celsius = 15.f;
src/modules/mavlink/streams/SCALED_PRESSURE.hpp
+5 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-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
@@ -72,8 +72,8 @@ private:
if (_sensor_baro_sub.copy(&sensor_baro)) {
msg.time_boot_ms = sensor_baro.timestamp / 1000;
msg.press_abs = sensor_baro.pressure; // millibar to hPa
msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees
msg.press_abs = sensor_baro.pressure * 0.01f; // Pa to hPa
msg.temperature = roundf(sensor_baro.temperature * 100.f); // cdegC (centidegrees)
}
differential_pressure_s differential_pressure;
@@ -83,8 +83,8 @@ private:
msg.time_boot_ms = differential_pressure.timestamp / 1000;
}
msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
msg.press_diff = differential_pressure.differential_pressure_pa * 0.01f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // cdegC (centidegrees)
}
mavlink_msg_scaled_pressure_send_struct(_mavlink->get_channel(), &msg);
src/modules/mavlink/streams/SCALED_PRESSURE2.hpp
+5 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-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
@@ -72,8 +72,8 @@ private:
if (_sensor_baro_sub.copy(&sensor_baro)) {
msg.time_boot_ms = sensor_baro.timestamp / 1000;
msg.press_abs = sensor_baro.pressure; // millibar to hPa
msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees
msg.press_abs = sensor_baro.pressure * 0.01f; // Pa to hPa
msg.temperature = roundf(sensor_baro.temperature * 100.f); // cdegC (centidegrees)
}
differential_pressure_s differential_pressure;
@@ -83,8 +83,8 @@ private:
msg.time_boot_ms = differential_pressure.timestamp / 1000;
}
msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
msg.press_diff = differential_pressure.differential_pressure_pa * 0.01f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // cdegC (centidegrees)
}
mavlink_msg_scaled_pressure2_send_struct(_mavlink->get_channel(), &msg);
src/modules/mavlink/streams/SCALED_PRESSURE3.hpp
+5 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-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
@@ -72,8 +72,8 @@ private:
if (_sensor_baro_sub.copy(&sensor_baro)) {
msg.time_boot_ms = sensor_baro.timestamp / 1000;
msg.press_abs = sensor_baro.pressure; // millibar to hPa
msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees
msg.press_abs = sensor_baro.pressure * 0.01f; // Pa to hPa
msg.temperature = roundf(sensor_baro.temperature * 100.f); // cdegC (centidegrees)
}
differential_pressure_s differential_pressure;
@@ -83,8 +83,8 @@ private:
msg.time_boot_ms = differential_pressure.timestamp / 1000;
}
msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
msg.press_diff = differential_pressure.differential_pressure_pa * 0.01f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // cdegC (centidegrees)
}
mavlink_msg_scaled_pressure3_send_struct(_mavlink->get_channel(), &msg);
src/modules/sensors/sensors.cpp
+1 -1
View File
@@ -439,7 +439,7 @@ void Sensors::diff_pres_poll()
_baro_pressure_sum += air_data.baro_pressure_pa;
_diff_pres_count++;
if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 100_ms) {
if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 50_ms) {
// average data and apply calibration offset (SENS_DPRES_OFF)
const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count;
src/modules/sih/sih.cpp
+3 -2
View File
@@ -522,12 +522,13 @@ void Sih::send_gps()
void Sih::send_airspeed()
{
airspeed_s airspeed{};
airspeed.timestamp = _now;
airspeed_s airspeed{};
airspeed.timestamp_sample = _now;
airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f);
airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
airspeed.air_temperature_celsius = _baro_temp_c;
airspeed.confidence = 0.7f;
airspeed.timestamp = hrt_absolute_time();
_airspeed_pub.publish(airspeed);
}
src/modules/simulator/simulator.h
-2
View File
@@ -161,8 +161,6 @@ private:
static Simulator *_instance;
static constexpr float hPa2Pa = 100.0f; // hectopascal to pascal multiplier
// simulated sensor instances
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] {
src/modules/simulator/simulator_mavlink.cpp
+3 -3
View File
@@ -328,7 +328,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) {
if (!_baro_stuck) {
_last_baro_pressure = sensors.abs_pressure * hPa2Pa; // convert hPa to Pa
_last_baro_pressure = sensors.abs_pressure * 100.f; // hPa to Pa
_last_baro_temperature = sensors.temperature;
}
@@ -353,8 +353,8 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) {
differential_pressure_s report{};
report.timestamp_sample = time;
report.device_id = 0; // TODO
report.differential_pressure_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
report.differential_pressure_pa = sensors.diff_pressure * 100.f; // hPa to Pa;
report.temperature = _sensors_temperature;
report.timestamp = hrt_absolute_time();
_differential_pressure_pub.publish(report);