From 672051c34aab11b78422afa87db37941faa88804 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 4 Aug 2022 17:11:26 -0400 Subject: [PATCH] gimbal: add paranoid quaternion checks and initialized values --- src/modules/gimbal/common.h | 5 ++- src/modules/gimbal/input_rc.cpp | 4 +- src/modules/gimbal/input_rc.h | 2 +- src/modules/gimbal/output.cpp | 59 ++++++++++++++++++--------- src/modules/gimbal/output.h | 2 +- src/modules/gimbal/output_mavlink.cpp | 15 ++++--- 6 files changed, 53 insertions(+), 34 deletions(-) diff --git a/src/modules/gimbal/common.h b/src/modules/gimbal/common.h index d7fe3f096a..000c9c0a0d 100644 --- a/src/modules/gimbal/common.h +++ b/src/modules/gimbal/common.h @@ -34,6 +34,7 @@ #pragma once +#include #include namespace gimbal @@ -55,8 +56,8 @@ struct ControlData { union TypeData { struct TypeAngle { - float q[4]; - float angular_velocity[3]; + float q[4] {NAN, NAN, NAN, NAN}; + float angular_velocity[3] {NAN, NAN, NAN}; enum class Frame : uint8_t { AngleBodyFrame = 0, // Also called follow mode, angle relative to vehicle forward (usually default for yaw axis). diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index 182bab9b05..0446483277 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -100,7 +100,7 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint); control_data.type = ControlData::Type::Angle; - float new_aux_values[3]; + float new_aux_values[3] {}; for (int i = 0; i < 3; ++i) { new_aux_values[i] = _get_aux_value(manual_control_setpoint, i); @@ -112,7 +112,7 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData // Detect a big stick movement const bool major_movement = [&]() { for (int i = 0; i < 3; ++i) { - if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) { + if (!PX4_ISFINITE(_last_set_aux_values[i]) || (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f)) { return true; } } diff --git a/src/modules/gimbal/input_rc.h b/src/modules/gimbal/input_rc.h index 2bf0c2f51f..82dcdaeeab 100644 --- a/src/modules/gimbal/input_rc.h +++ b/src/modules/gimbal/input_rc.h @@ -61,7 +61,7 @@ private: int _manual_control_setpoint_sub{-1}; - float _last_set_aux_values[3] {}; + float _last_set_aux_values[3] {NAN, NAN, NAN}; }; } /* namespace gimbal */ diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index d72ffd6a07..a89f0ca461 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 { @@ -84,9 +86,7 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude, void OutputBase::_set_angle_setpoints(const ControlData &control_data) { switch (control_data.type) { - case ControlData::Type::Angle: - - { + case ControlData::Type::Angle: { for (int i = 0; i < 3; ++i) { switch (control_data.type_data.angle.frames[i]) { case ControlData::TypeData::TypeAngle::Frame::AngularRate: @@ -104,9 +104,7 @@ void OutputBase::_set_angle_setpoints(const ControlData &control_data) _angle_velocity[i] = control_data.type_data.angle.angular_velocity[i]; } - for (int i = 0; i < 4; ++i) { - _q_setpoint[i] = control_data.type_data.angle.q[i]; - } + _q_setpoint = matrix::Quatf{control_data.type_data.angle.q}; } break; @@ -116,10 +114,10 @@ void OutputBase::_set_angle_setpoints(const ControlData &control_data) break; case ControlData::Type::Neutral: - _q_setpoint[0] = 1.f; - _q_setpoint[1] = 0.f; - _q_setpoint[2] = 0.f; - _q_setpoint[3] = 0.f; + _q_setpoint(0) = 1.f; + _q_setpoint(1) = 0.f; + _q_setpoint(2) = 0.f; + _q_setpoint(3) = 0.f; _angle_velocity[0] = NAN; _angle_velocity[1] = NAN; _angle_velocity[2] = NAN; @@ -173,7 +171,7 @@ void OutputBase::_handle_position_update(const ControlData &control_data, bool f yaw += control_data.type_data.lonlat.yaw_offset; } - matrix::Quatf(matrix::Eulerf(roll, pitch, yaw)).copyTo(_q_setpoint); + _q_setpoint = matrix::Eulerf(roll, pitch, yaw); _angle_velocity[0] = NAN; _angle_velocity[1] = NAN; @@ -201,25 +199,41 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) } // get the output angles and stabilize if necessary + bool attitude_valid = false; matrix::Eulerf euler_vehicle{}; if (compensate[0] || compensate[1] || compensate[2]) { vehicle_attitude_s vehicle_attitude; if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { - euler_vehicle = matrix::Quatf(vehicle_attitude.q); + const matrix::Quatf q{vehicle_attitude.q}; + + const bool no_element_larger_than_one = (fabsf(q(0)) <= 1.f) + && (fabsf(q(1)) <= 1.f) + && (fabsf(q(2)) <= 1.f) + && (fabsf(q(3)) <= 1.f); + const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f); + + attitude_valid = (vehicle_attitude.timestamp != 0) && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 1_s) + && norm_in_tolerance && no_element_larger_than_one; + + if (attitude_valid) { + euler_vehicle = q; + } } } float dt = math::constrain((t - _last_update) * 1.e-6f, 0.001f, 1.f); - const bool q_setpoint_valid = PX4_ISFINITE(_q_setpoint[0]) && PX4_ISFINITE(_q_setpoint[1]) - && PX4_ISFINITE(_q_setpoint[2]) && PX4_ISFINITE(_q_setpoint[3]); - matrix::Eulerf euler_gimbal{}; + const bool q_setpoint_no_element_larger_than_one = (fabsf(_q_setpoint(0)) <= 1.f) + && (fabsf(_q_setpoint(1)) <= 1.f) + && (fabsf(_q_setpoint(2)) <= 1.f) + && (fabsf(_q_setpoint(3)) <= 1.f); + const bool q_setpoint_norm_in_tolerance = (fabsf(1.f - _q_setpoint.norm()) <= 1e-6f); - if (q_setpoint_valid) { - euler_gimbal = matrix::Quatf{_q_setpoint}; - } + const bool q_setpoint_valid = q_setpoint_no_element_larger_than_one && q_setpoint_norm_in_tolerance; + + const matrix::Eulerf euler_gimbal{_q_setpoint}; for (int i = 0; i < 3; ++i) { @@ -231,17 +245,22 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) _angle_outputs[i] += dt * _angle_velocity[i]; } - if (compensate[i] && PX4_ISFINITE(euler_vehicle(i))) { + if (compensate[i] && PX4_ISFINITE(_angle_outputs[i]) && + attitude_valid && PX4_ISFINITE(euler_vehicle(i))) { + _angle_outputs[i] -= euler_vehicle(i); } if (PX4_ISFINITE(_angle_outputs[i])) { // bring angles into proper range [-pi, pi] _angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]); + + } else { + // reset + _angle_outputs[i] = 0.f; } } - // constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed if (_landed) { if (PX4_ISFINITE(_angle_outputs[1])) { diff --git a/src/modules/gimbal/output.h b/src/modules/gimbal/output.h index 9b2e3693ea..025d71941d 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -76,7 +76,7 @@ protected: void _handle_position_update(const ControlData &control_data, bool force_update = false); - float _q_setpoint[4] = { NAN, NAN, NAN, NAN }; ///< can be NAN if not specifically set + matrix::Quatf _q_setpoint{NAN, NAN, NAN, NAN}; ///< can be NAN if not specifically set float _angle_velocity[3] = { NAN, NAN, NAN }; //< [rad/s], can be NAN if not specifically set bool _stabilize[3] = { false, false, false }; diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index 8f4f55b7aa..a446fe169a 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -198,10 +198,11 @@ void OutputMavlinkV2::print_status() const PX4_INFO("Output: MAVLink gimbal protocol v2"); PX4_INFO_RAW(" quaternion: [%.1f %.1f %.1f %.1f]\n", - (double)_q_setpoint[0], - (double)_q_setpoint[1], - (double)_q_setpoint[2], - (double)_q_setpoint[3]); + (double)_q_setpoint(0), + (double)_q_setpoint(1), + (double)_q_setpoint(2), + (double)_q_setpoint(3)); + PX4_INFO_RAW(" angular velocity: [%.1f %.1f %.1f]\n", (double)_angle_velocity[0], (double)_angle_velocity[1], @@ -218,10 +219,8 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude() set_attitude.angular_velocity_x = _angle_velocity[0]; set_attitude.angular_velocity_y = _angle_velocity[1]; set_attitude.angular_velocity_z = _angle_velocity[2]; - set_attitude.q[0] = _q_setpoint[0]; - set_attitude.q[1] = _q_setpoint[1]; - set_attitude.q[2] = _q_setpoint[2]; - set_attitude.q[3] = _q_setpoint[3]; + + _q_setpoint.copyTo(set_attitude.q); if (_absolute_angle[0]) { set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK;