mc_pos_control: check if position-control setpoint update succeeded.

refactor use_obstacle_avoidance method

src/modules/mc_pos_control/PositionControl.cpp

@@ -54,7 +54,7 @@ void PositionControl::updateState(const PositionControlStates &states)
 	_vel_dot = states.acceleration;
 }
 
-void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
+bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
 {
 	// If full manual is required (thrust already generated), don't run position/velocity
 	// controller and just return thrust.
@@ -66,11 +66,13 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
 	_thr_sp = Vector3f(setpoint.thrust);
 	_yaw_sp = setpoint.yaw;
 	_yawspeed_sp = setpoint.yawspeed;
-	_interfaceMapping();
+	bool mapping_succeeded = _interfaceMapping();
 
 	if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) {
 		_skip_controller = true;
 	}
+
+	return mapping_succeeded;
 }
 
 void PositionControl::generateThrustYawSetpoint(const float dt)
@@ -98,7 +100,7 @@ void PositionControl::generateThrustYawSetpoint(const float dt)
 	}
 }
 
-void PositionControl::_interfaceMapping()
+bool PositionControl::_interfaceMapping()
 {
 	// if noting is valid, then apply failsafe landing
 	bool failsafe = false;
@@ -209,6 +211,8 @@ void PositionControl::_interfaceMapping()
 		// 70% of throttle range between min and hover
 		_thr_sp(2) = -(MPC_THR_MIN.get() + (MPC_THR_HOVER.get() - MPC_THR_MIN.get()) * 0.7f);
 	}
+
+	return !(failsafe);
 }
 
 void PositionControl::_positionController()

src/modules/mc_pos_control/PositionControl.hpp

@@ -94,8 +94,9 @@ public:
 	/**
 	 * Update the desired setpoints.
 	 * @param setpoint a vehicle_local_position_setpoint_s structure
+	 * @return true if setpoint has updated correctly
 	 */
-	void updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);
+	bool updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);
 
 	/**
 	 * Set constraints that are stricter than the global limits.
@@ -165,7 +166,12 @@ protected:
 	void updateParams() override;
 
 private:
-	void _interfaceMapping(); /** maps set-points to internal member set-points */
+	/**
+	 * Maps setpoints to internal-setpoints.
+	 * @return true if mapping succeeded.
+	 */
+	bool _interfaceMapping();
+
 	void _positionController(); /** applies the P-position-controller */
 	void _velocityController(const float &dt); /** applies the PID-velocity-controller */
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -267,7 +267,7 @@ private:
 	/**
 	 * Overwrite setpoint with waypoint coming from obstacle avoidance
 	 */
-	void execute_avoidance_waypoint();
+	void execute_avoidance_waypoint(vehicle_local_position_setpoint_s &setpoint);
 
 	/**
 	 * Publish desired vehicle_trajectory_waypoint
@@ -703,11 +703,14 @@ MulticopterPositionControl::run()
 			_control.updateConstraints(constraints);
 			_control.updateState(_states);
 
-			if (!use_obstacle_avoidance()) {
-				_control.updateSetpoint(setpoint);
+			// adjust setpoints based on avoidance
+			if (use_obstacle_avoidance()) {
+				execute_avoidance_waypoint(setpoint);
+			}
 
-			} else {
-				execute_avoidance_waypoint();
+			// update position controller setpoints
+			if (!_control.updateSetpoint(setpoint)) {
+				warn_rate_limited("Position-Control Setpoint-Update failed");
 			}
 
 			// Generate desired thrust and yaw.
@@ -1076,10 +1079,8 @@ MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlSta
 }
 
 void
-MulticopterPositionControl::execute_avoidance_waypoint()
+MulticopterPositionControl::execute_avoidance_waypoint(vehicle_local_position_setpoint_s &setpoint)
 {
-	vehicle_local_position_setpoint_s setpoint;
-
 	setpoint.x = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0];
 	setpoint.y = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1];
 	setpoint.z = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2];
@@ -1093,8 +1094,6 @@ MulticopterPositionControl::execute_avoidance_waypoint()
 	setpoint.yaw = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
 	setpoint.yawspeed = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
 	Vector3f(NAN, NAN, NAN).copyTo(setpoint.thrust);
-
-	_control.updateSetpoint(setpoint);
 }
 
 bool