gimbal: add paranoid quaternion checks and initialized values

src/modules/gimbal/common.h

@@ -34,6 +34,7 @@
 
 #pragma once
 
+#include <math.h>
 #include <stdint.h>
 
 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).

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;
 			}
 		}

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 */

src/modules/gimbal/output.cpp

@@ -43,6 +43,8 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 
+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])) {

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 };

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;