From 823f033abe98d16d74938b73828614b9f91472ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Wed, 8 Apr 2026 00:04:51 +0300 Subject: [PATCH] refactor(mavlink): extract offboard control mode filling in mavlink receiver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- src/modules/mavlink/mavlink_receiver.cpp | 26 +++++++++++++----------- src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index afab4987fc..ce59b940ad 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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"); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fc399ff223..e2adf224db 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -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);