fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor(mavlink): extract offboard control mode filling in mavlink receiver

Browse Source 
Extract the repeated `offboard_control_mode_s` population logic into a shared
`fill_offboard_control_mode()` helper in MavlinkReceiver and, similar to
`fill_thrust()`, reuse it in both local and global position target handlers.

Reduces the code duplication without changing any behavior.

Signed-off-by: Onur Özkan <work@onurozkan.dev>
This commit is contained in:
Onur Özkan 2026-04-08 00:04:51 +03:00 committed by GitHub
parent d74db56a06
commit 823f033abe
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1054,6 +1054,18 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
_mocap_odometry_pub.publish(odom);
}
offboard_control_mode_s
MavlinkReceiver::fill_offboard_control_mode(const trajectory_setpoint_s &setpoint)
{
offboard_control_mode_s ocm{};
ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
ocm.attitude = PX4_ISFINITE(setpoint.yaw);
ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed);
return ocm;
}
void
MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
{
@ -1140,12 +1152,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_local_ned.yaw_rate;
offboard_control_mode_s ocm{};
ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
ocm.attitude = PX4_ISFINITE(setpoint.yaw);
ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed);
offboard_control_mode_s ocm = fill_offboard_control_mode(setpoint);
if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported\t");
@ -1264,12 +1271,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_global_int.yaw_rate;
offboard_control_mode_s ocm{};
ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
ocm.attitude = PX4_ISFINITE(setpoint.yaw);
ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed);
offboard_control_mode_s ocm = fill_offboard_control_mode(setpoint);
if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT force not supported\t");

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

@ -248,6 +248,8 @@ private:
void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);
static offboard_control_mode_s fill_offboard_control_mode(const trajectory_setpoint_s &setpoint);
void schedule_tune(const char *tune);
void update_message_statistics(const mavlink_message_t &message);