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

fw_pos_control_l1: switch to events

This commit is contained in:
Beat Küng 2021-09-09 22:07:14 +02:00 committed by Daniel Agar
parent 0a1ae37c1a
commit 5fb16e4395
2 changed files with 66 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -34,6 +34,7 @@
#include "FixedwingPositionControl.hpp"
#include <vtol_att_control/vtol_type.h>
#include <px4_platform_common/events.h>
using math::constrain;
using math::max;
@ -146,23 +147,57 @@ FixedwingPositionControl::parameters_update()
// sanity check parameters
if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) {
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min");
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min\t");
/* EVENT
* @description
* - <param>FW_AIRSPD_MAX</param>: {1:.1}
* - <param>FW_AIRSPD_MIN</param>: {2:.1}
*/
events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_airspeed"), events::Log::Error,
"Invalid configuration: Airspeed max smaller than min",
_param_fw_airspd_max.get(), _param_fw_airspd_min.get());
check_ret = PX4_ERROR;
}
if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) {
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s");
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s\t");
/* EVENT
* @description
* - <param>FW_AIRSPD_MAX</param>: {1:.1}
* - <param>FW_AIRSPD_MIN</param>: {2:.1}
*/
events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_airspeed_bounds"), events::Log::Error,
"Invalid configuration: Airspeed max \\< 5 m/s or min \\> 100 m/s",
_param_fw_airspd_max.get(), _param_fw_airspd_min.get());
check_ret = PX4_ERROR;
}
if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() ||
_param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) {
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds");
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds\t");
/* EVENT
* @description
* - <param>FW_AIRSPD_MAX</param>: {1:.1}
* - <param>FW_AIRSPD_MIN</param>: {2:.1}
* - <param>FW_AIRSPD_TRIM</param>: {3:.1}
*/
events::send<float, float, float>(events::ID("fixedwing_position_control_conf_invalid_cruise_bounds"),
events::Log::Error,
"Invalid configuration: Airspeed cruise out of min or max bounds",
_param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get());
check_ret = PX4_ERROR;
}
if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get() * 0.9f) {
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min");
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min\t");
/* EVENT
* @description
* - <param>FW_AIRSPD_MIN</param>: {1:.1}
* - <param>FW_AIRSPD_STALL</param>: {2:.1}
*/
events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error,
"Invalid configuration: Stall airspeed higher than 90% of minimum airspeed",
_param_fw_airspd_min.get(), _param_fw_airspd_stall.get());
check_ret = PX4_ERROR;
}
@ -475,7 +510,9 @@ FixedwingPositionControl::abort_landing(bool abort)
{
// only announce changes
if (abort && !_land_abort) {
mavlink_log_critical(&_mavlink_log_pub, "Landing aborted");
mavlink_log_critical(&_mavlink_log_pub, "Landing aborted\t");
// TODO: add reason
events::send(events::ID("fixedwing_position_control_land_aborted"), events::Log::Critical, "Landing aborted");
}
_land_abort = abort;
@ -1238,7 +1275,8 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
* doesn't matter if it gets reset when takeoff is detected eventually */
_takeoff_ground_alt = _current_altitude;
mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway");
mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway\t");
events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway");
}
float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt);
@ -1287,7 +1325,8 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
/* Inform user that launchdetection is running every 4s */
if ((now - _launch_detection_notify) > 4_s) {
mavlink_log_critical(&_mavlink_log_pub, "Launch detection running");
mavlink_log_critical(&_mavlink_log_pub, "Launch detection running\t");
events::send(events::ID("fixedwing_position_control_launch_detection"), events::Log::Info, "Launch detection running");
_launch_detection_notify = now;
}
@ -1452,7 +1491,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
}
_land_noreturn_horizontal = true;
mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold");
mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold\t");
events::send(events::ID("fixedwing_position_control_landing"), events::Log::Info, "Landing, heading hold");
}
if (_land_noreturn_horizontal) {
@ -1541,7 +1581,9 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
if (!_land_motor_lim) {
_land_motor_lim = true;
mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle");
mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle\t");
events::send(events::ID("fixedwing_position_control_landing_limit_throttle"), events::Log::Info,
"Landing, limiting throttle");
}
}
@ -1572,7 +1614,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
// just started with the flaring phase
_flare_pitch_sp = radians(_param_fw_psp_off.get());
_flare_height = _current_altitude - terrain_alt;
mavlink_log_info(&_mavlink_log_pub, "Landing, flaring");
mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t");
events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring");
_land_noreturn_vertical = true;
} else {
@ -1609,7 +1652,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
altitude_desired = terrain_alt + landing_slope_alt_rel_desired;
if (!_land_onslope) {
mavlink_log_info(&_mavlink_log_pub, "Landing, on slope");
mavlink_log_info(&_mavlink_log_pub, "Landing, on slope\t");
events::send(events::ID("fixedwing_position_control_landing_on_slope"), events::Log::Info, "Landing, on slope");
_land_onslope = true;
}
@ -1747,7 +1791,9 @@ FixedwingPositionControl::Run()
}
} else {
mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint");
mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint\t");
events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error,
"Invalid offboard setpoint");
}
}

11
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
View File

@ -45,6 +45,7 @@
#include "RunwayTakeoff.h"
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/events.h>
using namespace time_literals;
@ -88,7 +89,9 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl
case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
if (airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get()) {
_state = RunwayTakeoffState::TAKEOFF;
mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached");
mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached\t");
events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info,
"Takeoff airspeed reached");
}
break;
@ -106,7 +109,8 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl
_start_wp(1) = current_lon;
}
mavlink_log_info(mavlink_log_pub, "#Climbout");
mavlink_log_info(mavlink_log_pub, "#Climbout\t");
events::send(events::ID("runway_takeoff_climbout"), events::Log::Info, "Climbout");
}
break;
@ -115,7 +119,8 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl
if (alt_agl > _param_fw_clmbout_diff.get()) {
_climbout = false;
_state = RunwayTakeoffState::FLY;
mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint");
mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint\t");
events::send(events::ID("runway_takeoff_nav_to_wp"), events::Log::Info, "Navigating to waypoint");
}
break;