fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 12:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

gimbal: refactor outputs to take current timestamp at the beginning

Browse Source
This commit is contained in:
Matthias Grob
2024-10-30 15:12:08 +13:00
committed by Julian Oes
parent a3a83c718a
commit d5f3e858e8
2 changed files with 14 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gimbal/output_mavlink.cpp
+10 -10
View File
@@ -48,6 +48,8 @@ OutputMavlinkV1::OutputMavlinkV1(const Parameters &parameters)
void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) 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_s vehicle_command{};
vehicle_command.timestamp = hrt_absolute_time(); vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.target_system = (uint8_t)_parameters.mnt_mav_sysid_v1; 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); _handle_position_update(control_data);
hrt_abstime t = hrt_absolute_time(); _calculate_angle_output(now);
_calculate_angle_output(t);
vehicle_command.timestamp = t; vehicle_command.timestamp = now;
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL; 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 // 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(); _stream_device_attitude_status();
_last_update = t; _last_update = now;
} }
void OutputMavlinkV1::_stream_device_attitude_status() void OutputMavlinkV1::_stream_device_attitude_status()
@@ -144,14 +145,13 @@ OutputMavlinkV2::OutputMavlinkV2(const Parameters &parameters)
void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) 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(); _check_for_gimbal_device_information();
hrt_abstime t = hrt_absolute_time(); if (!_gimbal_device_found && now - _last_gimbal_device_checked > 1000000) {
if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) {
_request_gimbal_device_information(); _request_gimbal_device_information();
_last_gimbal_device_checked = t; _last_gimbal_device_checked = now;
} else { } else {
if (new_setpoints) { if (new_setpoints) {
@@ -159,7 +159,7 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints
_set_angle_setpoints(control_data); _set_angle_setpoints(control_data);
_handle_position_update(control_data); _handle_position_update(control_data);
_last_update = t; _last_update = now;
} }
gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0; gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0;
src/modules/gimbal/output_rc.cpp
+4 -3
View File
@@ -50,14 +50,15 @@ OutputRC::OutputRC(const Parameters &parameters)
void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id)
{ {
hrt_abstime now = hrt_absolute_time();
if (new_setpoints) { if (new_setpoints) {
_set_angle_setpoints(control_data); _set_angle_setpoints(control_data);
} }
_handle_position_update(control_data); _handle_position_update(control_data);
hrt_abstime t = hrt_absolute_time(); _calculate_angle_output(now);
_calculate_angle_output(t);
_stream_device_attitude_status(); _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.timestamp = hrt_absolute_time();
_gimbal_controls_pub.publish(gimbal_controls); _gimbal_controls_pub.publish(gimbal_controls);
_last_update = t; _last_update = now;
} }
void OutputRC::print_status() const void OutputRC::print_status() const