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

FW PositionController: remove mavlink_log_pub, and only use events 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-11-24 09:52:01 +01:00
parent bd176241f8
commit 79c55614d8
6 changed files with 6 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -149,7 +149,6 @@ 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\t");
/* EVENT
* @description
* - <param>FW_AIRSPD_MAX</param>: {1:.1}
@ -162,7 +161,6 @@ FixedwingPositionControl::parameters_update()
}
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\t");
/* EVENT
* @description
* - <param>FW_AIRSPD_MAX</param>: {1:.1}
@ -176,7 +174,6 @@ FixedwingPositionControl::parameters_update()
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 trim out of min or max bounds\t");
/* EVENT
* @description
* - <param>FW_AIRSPD_MAX</param>: {1:.1}
@ -191,7 +188,6 @@ FixedwingPositionControl::parameters_update()
}
if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get()) {
mavlink_log_critical(&_mavlink_log_pub, "Config invalid: FW_AIRSPD_STALL airspeed higher than FW_AIRSPD_MIN\t");
/* EVENT
* @description
* - <param>FW_AIRSPD_MIN</param>: {1:.1}
@ -626,28 +622,24 @@ FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_statu
switch (new_abort_status) {
case (position_controller_landing_status_s::ABORTED_BY_OPERATOR): {
mavlink_log_critical(&_mavlink_log_pub, "Landing aborted by operator\t");
events::send(events::ID("fixedwing_position_control_landing_abort_status_operator_abort"), events::Log::Critical,
"Landing aborted by operator");
break;
}
case (position_controller_landing_status_s::TERRAIN_NOT_FOUND): {
mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: terrain estimate not found\t");
events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_not_found"), events::Log::Critical,
"Landing aborted: terrain measurement not found");
break;
}
case (position_controller_landing_status_s::TERRAIN_TIMEOUT): {
mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: terrain estimate timed out\t");
events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_timeout"), events::Log::Critical,
"Landing aborted: terrain estimate timed out");
break;
}
default: {
mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: unknown criterion\t");
events::send(events::ID("fixedwing_position_control_landing_abort_status_unknown_criterion"), events::Log::Critical,
"Landing aborted: unknown criterion");
}
@ -813,7 +805,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
&& !_vehicle_status.in_transition_mode) {
if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
// Need to init because last loop iteration was in a different mode
mavlink_log_critical(&_mavlink_log_pub, "Start loiter with fixed bank angle.\t");
events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
"Start loiter with fixed bank angle");
}
@ -822,7 +813,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
} else {
if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
mavlink_log_critical(&_mavlink_log_pub, "Start descending.\t");
events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending");
}
@ -1430,7 +1420,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_launch_current_yaw = _yaw;
mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway\t");
events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway");
}
@ -1439,8 +1428,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
}
_runway_takeoff.update(now, takeoff_airspeed, _airspeed, _current_altitude - _takeoff_ground_alt,
clearance_altitude_amsl - _takeoff_ground_alt,
&_mavlink_log_pub);
clearance_altitude_amsl - _takeoff_ground_alt);
// yaw control is disabled once in "taking off" state
_att_sp.fw_control_yaw_wheel = _runway_takeoff.controlYaw();
@ -1544,7 +1532,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
/* Perform launch detection */
/* Detect launch using body X (forward) acceleration */
_launchDetector.update(control_interval, _body_acceleration(0), &_mavlink_log_pub);
_launchDetector.update(control_interval, _body_acceleration(0));
}
} else {
@ -1714,7 +1702,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
_flare_states.start_time = now;
_flare_states.initial_height_rate_setpoint = _tecs.hgt_rate_setpoint();
_flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0];
mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t");
events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info,
"Landing, flaring");
}
@ -2222,7 +2209,6 @@ FixedwingPositionControl::Run()
}
if (!valid_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");

2
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -191,8 +191,6 @@ public:
private:
void Run() override;
orb_advert_t _mavlink_log_pub{nullptr};
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

8
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp
View File

@ -40,13 +40,12 @@
#include "LaunchDetector.h"
#include <px4_platform_common/log.h>
#include <systemlib/mavlink_log.h>
#include <px4_platform_common/events.h>
namespace launchdetection
{
void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t *mavlink_log_pub)
void LaunchDetector::update(const float dt, const float accel_x)
{
switch (state_) {
case launch_detection_status_s::STATE_WAITING_FOR_LAUNCH:
@ -55,7 +54,6 @@ void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t *
/* Inform user that launchdetection is running every kInfoDelay seconds */
if (info_delay_counter_s_ >= kInfoDelay) {
mavlink_log_info(mavlink_log_pub, "Launch detection running\t");
events::send(events::ID("launch_detection_running_info"), events::Log::Info, "Launch detection running");
info_delay_counter_s_ = 0.f; // reset counter
}
@ -67,15 +65,12 @@ void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t *
if (acceleration_detected_counter_ > param_fw_laun_ac_t_.get()) {
if (param_fw_laun_mot_del_.get() > 0.f) {
state_ = launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR;
mavlink_log_info(mavlink_log_pub, "Launch detected: enable control, waiting %8.1fs until throttling up\t",
(double)param_fw_laun_mot_del_.get());
events::send<float>(events::ID("launch_detection_wait_for_throttle"), {events::Log::Info, events::LogInternal::Info},
"Launch detected: enablecontrol, waiting {1:.1}s until full throttle", (double)param_fw_laun_mot_del_.get());
} else {
/* No motor delay set: go directly to enablemotors state */
state_ = launch_detection_status_s::STATE_FLYING;
mavlink_log_info(mavlink_log_pub, "Launch detected: enable motors (no motor delay)\t");
events::send(events::ID("launch_detection_no_motor_delay"), {events::Log::Info, events::LogInternal::Info},
"Launch detected: enable motors (no motor delay)");
}
@ -93,7 +88,6 @@ void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t *
motor_delay_counter_ += dt;
if (motor_delay_counter_ > param_fw_laun_mot_del_.get()) {
mavlink_log_info(mavlink_log_pub, "Launch detected: enable motors\t");
events::send(events::ID("launch_detection_enable_motors"), {events::Log::Info, events::LogInternal::Info},
"Launch detected: enable motors");
state_ = launch_detection_status_s::STATE_FLYING;

3
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h
View File

@ -69,9 +69,8 @@ public:
*
* @param dt Time step [us]
* @param accel_x Measured acceleration in body x [m/s/s]
* @param mavlink_log_pub
*/
void update(const float dt, const float accel_x, orb_advert_t *mavlink_log_pub);
void update(const float dt, const float accel_x);
/**
* @brief Get the Launch Detected state

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

@ -43,7 +43,6 @@
#include <math.h>
#include "RunwayTakeoff.h"
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/events.h>
@ -63,7 +62,7 @@ void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, c
}
void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed,
const float vehicle_altitude, const float clearance_altitude, orb_advert_t *mavlink_log_pub)
const float vehicle_altitude, const float clearance_altitude)
{
switch (takeoff_state_) {
case RunwayTakeoffState::THROTTLE_RAMP:
@ -80,7 +79,6 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs
if (calibrated_airspeed > rotation_airspeed) {
takeoff_time_ = time_now;
takeoff_state_ = RunwayTakeoffState::CLIMBOUT;
mavlink_log_info(mavlink_log_pub, "Takeoff airspeed reached, climbout\t");
events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info,
"Takeoff airspeed reached, climbout");
}
@ -91,7 +89,6 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs
case RunwayTakeoffState::CLIMBOUT:
if (vehicle_altitude > clearance_altitude) {
takeoff_state_ = RunwayTakeoffState::FLY;
mavlink_log_info(mavlink_log_pub, "Reached clearance altitude\t");
events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude");
}

4
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
View File

@ -47,7 +47,6 @@
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
@ -84,10 +83,9 @@ public:
* @param calibrated_airspeed Vehicle calibrated airspeed [m/s]
* @param vehicle_altitude Vehicle altitude (AGL) [m]
* @param clearance_altitude Altitude (AGL) above which we have cleared all occlusions in the runway path [m]
* @param mavlink_log_pub
*/
void update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed,
const float vehicle_altitude, const float clearance_altitude, orb_advert_t *mavlink_log_pub);
const float vehicle_altitude, const float clearance_altitude);
/**
* @return Current takeoff state