Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix: Stop gimbal from spinning when an angular velocity setpoint stream times out #23825

Merged
merged 3 commits into from
Oct 30, 2024
Merged

Fix: Stop gimbal from spinning when an angular velocity setpoint stream times out #23825

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4453155
gimbal: refactor outputs to take current timestamp at the beginning
MaEtUgR Oct 17, 2024
6a76c3d
gimbal: introduce timestamp of last setpoint update
MaEtUgR Oct 17, 2024
cb7a83f
gimbal: handle angular_velocity setpoint stream timeout
MaEtUgR Oct 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/modules/gimbal/common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
};


Expand Down
5 changes: 5 additions & 0 deletions src/modules/gimbal/gimbal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
8 changes: 4 additions & 4 deletions src/modules/gimbal/input.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,15 +42,15 @@ InputBase::InputBase(Parameters &parameters) :
{}

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;
}
Expand Down
3 changes: 1 addition & 2 deletions src/modules/gimbal/input.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down
30 changes: 21 additions & 9 deletions src/modules/gimbal/input_mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -261,13 +263,15 @@ 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;
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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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 {
Expand All @@ -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;

Expand All @@ -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;

Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;

Expand All @@ -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) {
Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion src/modules/gimbal/input_mavlink.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
1 change: 1 addition & 0 deletions src/modules/gimbal/input_rc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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];

Expand Down
1 change: 1 addition & 0 deletions src/modules/gimbal/input_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
23 changes: 23 additions & 0 deletions src/modules/gimbal/output.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,8 @@
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>

using namespace time_literals;

namespace gimbal
{

Expand Down Expand Up @@ -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);
julianoes marked this conversation as resolved.
Show resolved Hide resolved

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) {
Expand Down
8 changes: 8 additions & 0 deletions src/modules/gimbal/output.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
20 changes: 10 additions & 10 deletions src/modules/gimbal/output_mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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()
Expand Down Expand Up @@ -144,22 +145,21 @@ OutputMavlinkV2::OutputMavlinkV2(const Parameters &parameters)

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) {
//got new command
_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;
Expand Down
7 changes: 4 additions & 3 deletions src/modules/gimbal/output_rc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand All @@ -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
Expand Down
Loading