From 73044c51f91d55fb9de81407a19a1a9f6759b2b8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 8 Dec 2021 12:40:01 +0100 Subject: [PATCH] vmount: only send CONFIGURE cmd if type changes We don't always have to send MOUNT_CONFIGURE but really only when the control data type actually changes. --- src/modules/vmount/output_mavlink.cpp | 47 ++++++++++++++------------- src/modules/vmount/output_mavlink.h | 2 ++ 2 files changed, 27 insertions(+), 22 deletions(-) diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index 5698c63481..f44278faef 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -66,33 +66,36 @@ int OutputMavlinkV1::update(const ControlData *control_data) //got new command _set_angle_setpoints(control_data); -#if !defined(ALT_MANTIS_GIMBAL_HACKS) - // Don't send this command to ATL Mantis as it just spams the vehicle_command queue and the - // Mantis ignores it anyway. - vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE; - vehicle_command.timestamp = hrt_absolute_time(); + const bool configuration_changed = + (control_data->type != _previous_control_data_type); + _previous_control_data_type = control_data->type; - if (control_data->type == ControlData::Type::Neutral) { - vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; + if (configuration_changed) { - vehicle_command.param5 = 0.0; - vehicle_command.param6 = 0.0; - vehicle_command.param7 = 0.0f; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE; + vehicle_command.timestamp = hrt_absolute_time(); - } else { - vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; + if (control_data->type == ControlData::Type::Neutral) { + vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; - vehicle_command.param5 = static_cast(control_data->type_data.angle.frames[0]); - vehicle_command.param6 = static_cast(control_data->type_data.angle.frames[1]); - vehicle_command.param7 = static_cast(control_data->type_data.angle.frames[2]); + vehicle_command.param5 = 0.0; + vehicle_command.param6 = 0.0; + vehicle_command.param7 = 0.0f; + + } else { + vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; + + vehicle_command.param5 = static_cast(control_data->type_data.angle.frames[0]); + vehicle_command.param6 = static_cast(control_data->type_data.angle.frames[1]); + vehicle_command.param7 = static_cast(control_data->type_data.angle.frames[2]); + } + + vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f; + vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f; + vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f; + + _vehicle_command_pub.publish(vehicle_command); } - - vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f; - vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f; - vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f; - - _vehicle_command_pub.publish(vehicle_command); -#endif } _handle_position_update(); diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index d44f48b3e9..1d86e47a21 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -68,6 +68,8 @@ private: void _stream_device_attitude_status(); uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; uORB::Publication _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; + + ControlData::Type _previous_control_data_type {ControlData::Type::Neutral}; }; class OutputMavlinkV2 : public OutputBase