dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FlightTask: add subscription to VELOCITY_LIMITS msg

This commit is contained in:
Marcin 2023-07-26 02:09:22 +02:00 committed by Matthias Grob
parent 54ce9813c8
commit 4cf43a68a3
3 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/velocity_limits.msg Normal file
View File

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

19
src/modules/mavlink/mavlink_receiver.cpp
View File

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

3
src/modules/mavlink/mavlink_receiver.h
View File

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