Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

airspeed_selector: switch to events

Browse Source
This commit is contained in:
Beat Küng
2021-09-09 22:08:04 +02:00
committed by Daniel Agar
parent 5fb16e4395
commit 5b70fd4a1d
1 changed files with 32 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+32 -11
View File
@@ -38,6 +38,7 @@
#include <matrix/math.hpp>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -232,12 +233,14 @@ AirspeedModule::init()
_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
if (_number_of_airspeed_sensors == 0) {
mavlink_log_info(&_mavlink_log_pub,
"No airspeed sensor detected. Switch to non-airspeed mode.");
mavlink_log_info(&_mavlink_log_pub, "No airspeed sensor detected. Switch to non-airspeed mode.\t");
events::send(events::ID("airspeed_selector_switch"), events::Log::Info,
"No airspeed sensor detected, switching to non-airspeed mode");
} else {
mavlink_log_info(&_mavlink_log_pub,
"Primary airspeed index bigger than number connected sensors. Take last sensor.");
mavlink_log_info(&_mavlink_log_pub, "Primary airspeed index bigger than number connected sensors. Take last sensor.\t");
events::send(events::ID("airspeed_selector_prim_too_high"), events::Log::Info,
"Primary airspeed index bigger than number connected sensors, taking last sensor");
}
} else {
@@ -423,7 +426,9 @@ void AirspeedModule::update_params()
_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
} else {
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
events::send(events::ID("airspeed_selector_cannot_est_scale"), events::Log::Error,
"Airspeed: cannot estimate scale as there is no valid sensor");
_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
_param_west_scale_estimation_on.commit_no_notification();
}
@@ -435,11 +440,15 @@ void AirspeedModule::update_params()
_param_west_airspeed_scale.commit_no_notification();
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f",
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f\t",
(double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
events::send<float>(events::ID("airspeed_selector_scale"), events::Log::Info,
"Airspeed: estimated scale (ASPD_SCALE): {1:.2}", _airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
} else {
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
events::send(events::ID("airspeed_selector_cannot_est_scale2"), events::Log::Error,
"Airspeed: cannot estimate scale as there is no valid sensor");
}
}
@@ -557,17 +566,29 @@ void AirspeedModule::select_airspeed_and_publish()
(_number_of_airspeed_sensors > 0 || !_vehicle_land_detected.landed) &&
_valid_airspeed_index != _prev_airspeed_index) {
if (_prev_airspeed_index > 0) {
mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected. Return to launch (RTL) is advised.");
mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected. Return to launch (RTL) is advised.\t");
events::send(events::ID("airspeed_selector_sensor_failure"), events::Log::Critical,
"Airspeed sensor failure detected. Return to launch (RTL) is advised");
} else if (_prev_airspeed_index == 0 && _valid_airspeed_index == -1) {
mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation invalid");
mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation invalid\t");
events::send(events::ID("airspeed_selector_estimation_invalid"), events::Log::Error,
"Airspeed estimation invalid");
} else if (_prev_airspeed_index == -1 && _valid_airspeed_index == 0) {
mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation valid");
mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation valid\t");
events::send(events::ID("airspeed_selector_estimation_valid"), events::Log::Info,
"Airspeed estimation valid");
} else {
mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor healthy, start using again (%i, %i)", _prev_airspeed_index,
mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor healthy, start using again (%i, %i)\t", _prev_airspeed_index,
_valid_airspeed_index);
/* EVENT
* @description Previously selected sensor index: {1}, current sensor index: {2}.
*/
events::send<uint8_t, uint8_t>(events::ID("airspeed_selector_estimation_regain"), events::Log::Info,
"Airspeed sensor healthy, start using again", _prev_airspeed_index,
_valid_airspeed_index);
}
}