From 44531556722947075360f0f36d02737971b58bea Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 17 Oct 2024 17:56:26 +0200 Subject: [PATCH 1/3] 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 cceb34525dc0..90a6596f9d0e 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) { - _check_for_gimbal_device_information(); - - hrt_abstime t = hrt_absolute_time(); + hrt_abstime now = hrt_absolute_time(); + _check_for_gimbal_device_information(); - 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 4b48349a4dfb..532da32c18ff 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 From 6a76c3de5453b63f19e2c3e4adf315b8d4f060cb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 17 Oct 2024 14:57:08 +0200 Subject: [PATCH 2/3] gimbal: introduce timestamp of last setpoint update To enable implementing a timeout when there's no new setpoint coming in. --- src/modules/gimbal/common.h | 1 + src/modules/gimbal/input.cpp | 8 ++++---- src/modules/gimbal/input.h | 3 +-- src/modules/gimbal/input_mavlink.cpp | 30 +++++++++++++++++++--------- src/modules/gimbal/input_mavlink.h | 2 +- src/modules/gimbal/input_rc.cpp | 1 + src/modules/gimbal/input_test.cpp | 1 + 7 files changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/gimbal/common.h b/src/modules/gimbal/common.h index e7f29b726bd4..2436bd68dda8 100644 --- a/src/modules/gimbal/common.h +++ b/src/modules/gimbal/common.h @@ -83,6 +83,7 @@ struct ControlData { // uint8_t sysid_secondary_control = 0; // The MAVLink system ID selected for additional input, not implemented yet. // uint8_t compid_secondary_control = 0; // The MAVLink component ID selected for additional input, not implemented yet. uint8_t device_compid = 0; + uint64_t timestamp_last_update{0}; // Timestamp when there was the last setpoint set by the input used for timeout }; diff --git a/src/modules/gimbal/input.cpp b/src/modules/gimbal/input.cpp index 93ab31b4e6f5..a6c5c137a996 100644 --- a/src/modules/gimbal/input.cpp +++ b/src/modules/gimbal/input.cpp @@ -42,15 +42,15 @@ InputBase::InputBase(Parameters ¶meters) : {} void InputBase::control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, - float roll_angle, - float pitch_fixed_angle) + uint64_t timestamp) { + control_data.timestamp_last_update = timestamp; control_data.type = ControlData::Type::LonLat; control_data.type_data.lonlat.lon = lon; control_data.type_data.lonlat.lat = lat; control_data.type_data.lonlat.altitude = altitude; - control_data.type_data.lonlat.roll_offset = roll_angle; - control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle; + control_data.type_data.lonlat.roll_offset = NAN; + control_data.type_data.lonlat.pitch_fixed_angle = NAN; control_data.type_data.lonlat.pitch_offset = 0.f; control_data.type_data.lonlat.yaw_offset = 0.f; } diff --git a/src/modules/gimbal/input.h b/src/modules/gimbal/input.h index d849d37137e0..49ae53f27443 100644 --- a/src/modules/gimbal/input.h +++ b/src/modules/gimbal/input.h @@ -60,8 +60,7 @@ class InputBase virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) = 0; virtual void print_status() const = 0; protected: - void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, float roll_angle = NAN, - float pitch_fixed_angle = NAN); + void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, uint64_t timestamp); Parameters &_parameters; }; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index ebfde30d2a73..f9f5f4c526f3 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -100,6 +100,7 @@ InputMavlinkROI::update(unsigned int timeout_ms, ControlData &control_data, bool if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { control_data.type = ControlData::Type::Neutral; + control_data.timestamp_last_update = vehicle_roi.timestamp; _cur_roi_mode = vehicle_roi.mode; control_data.sysid_primary_control = _parameters.mav_sysid; control_data.compid_primary_control = _parameters.mav_compid; @@ -120,7 +121,7 @@ InputMavlinkROI::update(unsigned int timeout_ms, ControlData &control_data, bool return UpdateResult::UpdatedActive; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { - control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt, vehicle_roi.timestamp); _cur_roi_mode = vehicle_roi.mode; @@ -155,6 +156,7 @@ void InputMavlinkROI::_read_control_data_from_position_setpoint_sub(ControlData { position_setpoint_triplet_s position_setpoint_triplet; orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + control_data.timestamp_last_update = position_setpoint_triplet.timestamp; control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; @@ -261,6 +263,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ // fallthrough case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: control_data.type = ControlData::Type::Neutral; + control_data.timestamp_last_update = vehicle_command.timestamp; control_data.sysid_primary_control = vehicle_command.source_system; control_data.compid_primary_control = vehicle_command.source_component; update_result = UpdateResult::UpdatedActiveOnce; @@ -268,6 +271,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { control_data.type = ControlData::Type::Angle; + control_data.timestamp_last_update = vehicle_command.timestamp; control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; @@ -302,7 +306,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5, - vehicle_command.param4); + vehicle_command.param4, vehicle_command.timestamp); control_data.sysid_primary_control = vehicle_command.source_system; control_data.compid_primary_control = vehicle_command.source_component; update_result = UpdateResult::UpdatedActive; @@ -348,6 +352,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ } } + control_data.timestamp_last_update = vehicle_command.timestamp; control_data.sysid_primary_control = vehicle_command.source_system; control_data.compid_primary_control = vehicle_command.source_component; @@ -603,7 +608,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_attitude(C set_attitude.angular_velocity_y, set_attitude.angular_velocity_z); - _set_control_data_from_set_attitude(control_data, set_attitude.flags, q, angular_velocity); + _set_control_data_from_set_attitude(control_data, set_attitude.flags, q, angular_velocity, set_attitude.timestamp); return UpdateResult::UpdatedActive; } else { @@ -619,11 +624,13 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_vehicle_roi(Co if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { control_data.type = ControlData::Type::Neutral; + control_data.timestamp_last_update = vehicle_roi.timestamp; _cur_roi_mode = vehicle_roi.mode; return UpdateResult::UpdatedActiveOnce; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { control_data.type = ControlData::Type::LonLat; + control_data.timestamp_last_update = vehicle_roi.timestamp; _read_control_data_from_position_setpoint_sub(control_data); control_data.type_data.lonlat.pitch_fixed_angle = -10.f; @@ -636,7 +643,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_vehicle_roi(Co return UpdateResult::UpdatedActive; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { - control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt, vehicle_roi.timestamp); _cur_roi_mode = vehicle_roi.mode; @@ -656,6 +663,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_position_setpo const position_setpoint_triplet_s &position_setpoint_triplet) { if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { + control_data.timestamp_last_update = position_setpoint_triplet.timestamp; control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; @@ -692,11 +700,13 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: control_data.type = ControlData::Type::Neutral; + control_data.timestamp_last_update = vehicle_command.timestamp; update_result = InputBase::UpdateResult::UpdatedActiveOnce; break; case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { control_data.type = ControlData::Type::Angle; + control_data.timestamp_last_update = vehicle_command.timestamp; control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; @@ -731,7 +741,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5, - vehicle_command.param4); + vehicle_command.param4, vehicle_command.timestamp); update_result = UpdateResult::UpdatedActive; break; } @@ -878,7 +888,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ // TODO: support gimbal device id for multiple gimbals - _set_control_data_from_set_attitude(control_data, flags, q, angular_velocity); + _set_control_data_from_set_attitude(control_data, flags, q, angular_velocity, vehicle_command.timestamp); _ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -922,7 +932,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con matrix::Vector3f(NAN, NAN, NAN); _set_control_data_from_set_attitude(control_data, set_manual_control.flags, q, - angular_velocity); + angular_velocity, set_manual_control.timestamp); return UpdateResult::UpdatedActive; @@ -934,9 +944,10 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con } void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, - const matrix::Quatf &q, - const matrix::Vector3f &angular_velocity) + const matrix::Quatf &q, const matrix::Vector3f &angular_velocity, const uint64_t timestamp) { + control_data.timestamp_last_update = timestamp; + if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_RETRACT) != 0) { // not implemented in ControlData } else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL) != 0) { @@ -984,6 +995,7 @@ void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub(Control { position_setpoint_triplet_s position_setpoint_triplet; orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + control_data.timestamp_last_update = position_setpoint_triplet.timestamp; control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; diff --git a/src/modules/gimbal/input_mavlink.h b/src/modules/gimbal/input_mavlink.h index 04264410d7d1..67bace21846b 100644 --- a/src/modules/gimbal/input_mavlink.h +++ b/src/modules/gimbal/input_mavlink.h @@ -112,7 +112,7 @@ class InputMavlinkGimbalV2 : public InputBase UpdateResult _process_set_manual_control(ControlData &control_data, const gimbal_manager_set_manual_control_s &set_manual_control); void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q, - const matrix::Vector3f &angular_velocity); + const matrix::Vector3f &angular_velocity, const uint64_t timestamp); void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result); void _stream_gimbal_manager_information(const ControlData &control_data); void _stream_gimbal_manager_status(const ControlData &control_data); diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index c15f916ab9a4..07de0b30949c 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -99,6 +99,7 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData manual_control_setpoint_s manual_control_setpoint{}; orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint); control_data.type = ControlData::Type::Angle; + control_data.timestamp_last_update = manual_control_setpoint.timestamp; float new_aux_values[3]; diff --git a/src/modules/gimbal/input_test.cpp b/src/modules/gimbal/input_test.cpp index 146266d80edb..46a8401866d4 100644 --- a/src/modules/gimbal/input_test.cpp +++ b/src/modules/gimbal/input_test.cpp @@ -53,6 +53,7 @@ InputTest::UpdateResult InputTest::update(unsigned int timeout_ms, ControlData & } control_data.type = ControlData::Type::Angle; + control_data.timestamp_last_update = hrt_absolute_time(); control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; From cb7a83ff48b2f92fe7070a169fab28865155ccef Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 17 Oct 2024 17:55:04 +0200 Subject: [PATCH 3/3] gimbal: handle angular_velocity setpoint stream timeout The gimbal would otherwise continue to spin with whatever velocity was commanded before the input connection was lost. --- src/modules/gimbal/gimbal.cpp | 5 +++++ src/modules/gimbal/output.cpp | 23 +++++++++++++++++++++++ src/modules/gimbal/output.h | 8 ++++++++ 3 files changed, 36 insertions(+) diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 804f792fc65e..20f70d296d93 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -264,6 +264,11 @@ static int gimbal_thread_main(int argc, char *argv[]) thread_data.output_obj->set_stabilize(false, false, false); } + if (thread_data.output_obj->check_and_handle_setpoint_timeout(thread_data.control_data, hrt_absolute_time())) { + // Without flagging an update the changes are not processed in the output + update_result = InputBase::UpdateResult::UpdatedActive; + } + // Update output thread_data.output_obj->update( thread_data.control_data, diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index 336f50daa37d..496e15e05d81 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -43,6 +43,8 @@ #include #include +using namespace time_literals; + namespace gimbal { @@ -81,6 +83,27 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude, return atan2f(z, target_distance); } +bool OutputBase::check_and_handle_setpoint_timeout(ControlData &control_data, const hrt_abstime &now) +{ + bool ret = false; + const bool timeout = (control_data.timestamp_last_update + 2_s < now); + const bool type_angle = (control_data.type == ControlData::Type::Angle); + + if (timeout && type_angle) { + // Avoid gimbal keeps on spinning if the last setpoint was angular_velocity but it times out + for (int i = 0; i < 3; ++i) { + float &vel = control_data.type_data.angle.angular_velocity[i]; + + if (PX4_ISFINITE(vel) && (fabsf(vel) > FLT_EPSILON)) { + vel = 0.f; + ret = true; + } + } + } + + return ret; +} + void OutputBase::_set_angle_setpoints(const ControlData &control_data) { switch (control_data.type) { diff --git a/src/modules/gimbal/output.h b/src/modules/gimbal/output.h index f0dd98324056..0da012c35fc7 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -63,6 +63,14 @@ class OutputBase void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize); + /** + * Time out if setpoint that should be streamed stops + * @param control_data setpoint to check timestamp of and amend upon timeout + * @param now Current system timestamp + * @return true iff setpoint was amended because of timeout + */ + bool check_and_handle_setpoint_timeout(ControlData &control_data, const hrt_abstime &now); + protected: float _calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position);