FlightTask: add subscription to VELOCITY_LIMITS msg

msg/velocity_limits.msg

@@ -0,0 +1,8 @@
+# Velocity and yaw rate limits for a multicopter position slow mode only
+
+uint64 timestamp # time since system start (microseconds)
+
+# absolute speeds, NAN means use default limit
+float32 horizontal_velocity # [m/s]
+float32 vertical_velocity # [m/s]
+float32 yaw_rate # [rad/s]

src/modules/mavlink/mavlink_receiver.cpp

@@ -323,6 +323,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_gimbal_device_attitude_status(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SET_VELOCITY_LIMITS:
+		handle_message_set_velocity_limits(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -1211,6 +1215,21 @@ MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg)
 	handle_request_message_command(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN);
 }
 
+void MavlinkReceiver::handle_message_set_velocity_limits(mavlink_message_t *msg)
+{
+	mavlink_set_velocity_limits_t mavlink_set_velocity_limits;
+	mavlink_msg_set_velocity_limits_decode(msg, &mavlink_set_velocity_limits);
+
+	if (true) {
+		velocity_limits_s velocity_limits{};
+		velocity_limits.horizontal_velocity = mavlink_set_velocity_limits.horizontal_speed_limit;
+		velocity_limits.vertical_velocity = mavlink_set_velocity_limits.vertical_speed_limit;
+		velocity_limits.yaw_rate = mavlink_set_velocity_limits.yaw_rate_limit;
+		velocity_limits.timestamp = hrt_absolute_time();
+		_velocity_limits_pub.publish(velocity_limits);
+	}
+}
+
 void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -110,6 +110,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_bezier.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/velocity_limits.h>
 
 #if !defined(CONSTRAINED_FLASH)
 # include <uORB/topics/debug_array.h>
@@ -196,6 +197,7 @@ private:
 	void handle_message_trajectory_representation_bezier(mavlink_message_t *msg);
 	void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
 	void handle_message_utm_global_position(mavlink_message_t *msg);
+	void handle_message_set_velocity_limits(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
 	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
@@ -305,6 +307,7 @@ private:
 	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};
 	uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
+	uORB::Publication<velocity_limits_s>			_velocity_limits_pub{ORB_ID(velocity_limits)};
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};