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

gimbal: refactor outputs to take current timestamp at the beginning

Browse Source
This commit is contained in:
Matthias Grob 2024-10-17 17:56:26 +02:00 committed by Julian Oes
parent a3a83c718a
commit d5f3e858e8
2 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/gimbal/output_mavlink.cpp
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)
{
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 &parameters)
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;

7
src/modules/gimbal/output_rc.cpp
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)
{
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