FlightModeManager: handle MAV_CMD_DO_CHANGE_SPEED

- support setting the cruise speed of the auto flight task via command

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -441,6 +441,12 @@ void FlightModeManager::handleCommand()
 			command_ack.target_component = command.source_component;
 			command_ack.timestamp = hrt_absolute_time();
 			_vehicle_command_ack_pub.publish(command_ack);
+		} else if (_current_task.task) {
+			// check for other commands not related to task switching
+			if (command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED && command.param2 >= 0
+			    && (uint8_t)command.param1 == vehicle_command_s::SPEED_TYPE_GROUNDSPEED) {
+				_current_task.task->setCruisingSpeed(command.param2);
+			}
 		}
 	}
 }

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -81,6 +81,7 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp
 	_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
 	_updateTrajConstraints();
 	_is_emergency_braking_active = false;
+	_commanded_speed_ts = 0;
 
 	return ret;
 }
@@ -344,8 +345,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	_type = (WaypointType)_sub_triplet_setpoint.get().current.type;
 
-	// Always update cruise speed since that can change without waypoint changes.
-	_mc_cruise_speed = _sub_triplet_setpoint.get().current.cruising_speed;
+	// Override a commanded cruise speed if the cruise speed from the triplet is valid and more recent
+	const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed;
+
+	if (PX4_ISFINITE(cruise_speed_from_triplet) && cruise_speed_from_triplet >= 0
+	    && _sub_triplet_setpoint.get().current.timestamp > _commanded_speed_ts) {
+		_mc_cruise_speed = cruise_speed_from_triplet;
+	}
 
 	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) {
 		// If no speed is planned use the default cruise speed as limit

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -92,6 +92,13 @@ public:
 	bool updateInitialize() override;
 	bool update() override;
 
+	bool setCruisingSpeed(const float cruising_speed_m_s) override
+	{
+		_mc_cruise_speed = cruising_speed_m_s;
+		_commanded_speed_ts = hrt_absolute_time();
+		return true;
+	}
+
 	/**
 	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
 	 */
@@ -128,7 +135,7 @@ protected:
 	matrix::Vector3f _target{}; /**< Target waypoint  (local frame).*/
 	matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
 	bool _next_was_valid{false};
-	float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
+	float _mc_cruise_speed{-1.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
 
 	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
@@ -201,6 +208,8 @@ private:
 	_triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
 	matrix::Vector3f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
 
+	hrt_abstime _commanded_speed_ts{0};
+
 	MapProjection _reference_position{}; /**< Class used to project lat/lon setpoint into local frame. */
 	float _reference_altitude{NAN}; /**< Altitude relative to ground. */
 	hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -175,6 +175,8 @@ public:
 		_acceleration_setpoint_feedback = acc_sp;
 	}
 
+	virtual bool setCruisingSpeed(const float cruising_speed_m_s) { return false; }
+
 protected:
 	uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
 	uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};