FW PositionController: remove mavlink_log_pub, and only use events

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -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,
+				       clearance_altitude_amsl - _takeoff_ground_alt);
-				       &_mavlink_log_pub);
 
 		// 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");
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -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};

src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp

@@ -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;

src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h

@@ -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

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp

@@ -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");
 		}
 

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@@ -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