From d5f3e858e8b4ea4fcce69d0d7465484d937d3aa4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 17 Oct 2024 17:56:26 +0200 Subject: [PATCH] gimbal: refactor outputs to take current timestamp at the beginning --- src/modules/gimbal/output_mavlink.cpp | 20 ++++++++++---------- src/modules/gimbal/output_rc.cpp | 7 ++++--- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index cceb34525d..90a6596f9d 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -48,6 +48,8 @@ OutputMavlinkV1::OutputMavlinkV1(const Parameters ¶meters) void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) { + hrt_abstime now = hrt_absolute_time(); + vehicle_command_s vehicle_command{}; vehicle_command.timestamp = hrt_absolute_time(); vehicle_command.target_system = (uint8_t)_parameters.mnt_mav_sysid_v1; @@ -91,10 +93,9 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints _handle_position_update(control_data); - hrt_abstime t = hrt_absolute_time(); - _calculate_angle_output(t); + _calculate_angle_output(now); - vehicle_command.timestamp = t; + vehicle_command.timestamp = now; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL; // gimbal spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively @@ -108,7 +109,7 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints _stream_device_attitude_status(); - _last_update = t; + _last_update = now; } void OutputMavlinkV1::_stream_device_attitude_status() @@ -144,14 +145,13 @@ OutputMavlinkV2::OutputMavlinkV2(const Parameters ¶meters) void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) { + hrt_abstime now = hrt_absolute_time(); + _check_for_gimbal_device_information(); - hrt_abstime t = hrt_absolute_time(); - - - if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) { + if (!_gimbal_device_found && now - _last_gimbal_device_checked > 1000000) { _request_gimbal_device_information(); - _last_gimbal_device_checked = t; + _last_gimbal_device_checked = now; } else { if (new_setpoints) { @@ -159,7 +159,7 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints _set_angle_setpoints(control_data); _handle_position_update(control_data); - _last_update = t; + _last_update = now; } gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0; diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index 4b48349a4d..532da32c18 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -50,14 +50,15 @@ OutputRC::OutputRC(const Parameters ¶meters) void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) { + hrt_abstime now = hrt_absolute_time(); + if (new_setpoints) { _set_angle_setpoints(control_data); } _handle_position_update(control_data); - hrt_abstime t = hrt_absolute_time(); - _calculate_angle_output(t); + _calculate_angle_output(now); _stream_device_attitude_status(); @@ -81,7 +82,7 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8 gimbal_controls.timestamp = hrt_absolute_time(); _gimbal_controls_pub.publish(gimbal_controls); - _last_update = t; + _last_update = now; } void OutputRC::print_status() const