diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 804f792fc6..20f70d296d 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 336f50daa3..496e15e05d 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 f0dd983240..0da012c35f 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -63,6 +63,14 @@ public: 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);