diff --git a/msg/velocity_limits.msg b/msg/velocity_limits.msg new file mode 100644 index 0000000000..9ab5115abc --- /dev/null +++ b/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] diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e0697f787a..1079eedc3b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7939e813ae..f617ccbaef 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,6 +110,7 @@ #include #include #include +#include #if !defined(CONSTRAINED_FLASH) # include @@ -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_pub{ORB_ID(obstacle_distance)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; + uORB::Publication _velocity_limits_pub{ORB_ID(velocity_limits)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};