gimbal-bug-fix: store angle setpoints for next iteration

msg/GimbalControls.msg

@@ -4,4 +4,4 @@ uint8 INDEX_PITCH = 1
 uint8 INDEX_YAW = 2
 
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[3] control
+float32[3] control	# Normalized output. 1 means maximum positive position. -1 maximum negative position. 0 means no deflection. NaN maps to disarmed.

src/modules/gimbal/output.cpp

@@ -146,6 +146,9 @@ void OutputBase::_set_angle_setpoints(const ControlData &control_data)
 		_angle_velocity[0] = NAN;
 		_angle_velocity[1] = NAN;
 		_angle_velocity[2] = NAN;
+		_absolute_angle[0] = false;
+		_absolute_angle[1] = false;
+		_absolute_angle[2] = false;
 		break;
 	}
 }
@@ -237,16 +240,19 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 	float dt = math::constrain((t - _last_update) * 1.e-6f, 0.001f, 1.f);
 
 	const matrix::Quatf q_setpoint(_q_setpoint);
-	const bool q_setpoint_valid = q_setpoint.isAllFinite();
-	matrix::Eulerf euler_gimbal{};
 
-	if (q_setpoint_valid) {
-		euler_gimbal = q_setpoint;
+	if (q_setpoint.isAllFinite()) {
+		_last_valid_setpoint = q_setpoint;
 	}
 
+	matrix::Eulerf euler_gimbal(_last_valid_setpoint);
+
 	for (int i = 0; i < 3; ++i) {
 
-		if (q_setpoint_valid && PX4_ISFINITE(euler_gimbal(i))) {
+		if (PX4_ISFINITE(euler_gimbal(i)) && compensate[i] && PX4_ISFINITE(euler_vehicle(i))) {
+			_angle_outputs[i] = euler_gimbal(i) - euler_vehicle(i);
+
+		} else if (PX4_ISFINITE(euler_gimbal(i)) && !_absolute_angle[i]) {
 			_angle_outputs[i] = euler_gimbal(i);
 		}
 
@@ -254,10 +260,6 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 			_angle_outputs[i] += dt * _angle_velocity[i];
 		}
 
-		if (compensate[i] && PX4_ISFINITE(euler_vehicle(i))) {
-			_angle_outputs[i] -= euler_vehicle(i);
-		}
-
 		if (PX4_ISFINITE(_angle_outputs[i]) && _parameters.mnt_rc_in_mode == 0) {
 			// if we are in angle input mode, we bring angles into proper range [-pi, pi]
 			_angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]);
@@ -293,4 +295,54 @@ void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool y
 	_stabilize[2] = yaw_stabilize;
 }
 
+void OutputBase::set_last_valid_setpoint(const bool compensate[3], const matrix::Eulerf euler_vehicle)
+{
+	// No updates from angular velocity, hence no modification of last valid setpoint
+	if (!PX4_ISFINITE(_angle_velocity[0]) && !PX4_ISFINITE(_angle_velocity[1]) && !PX4_ISFINITE(_angle_velocity[2])) {
+		return;
+	}
+
+	// Angle outputs, from vehicle body frame to end effector (last actuator in the kinematic chain)
+	_last_valid_setpoint.phi() = _angle_outputs[0];
+	_last_valid_setpoint.theta() = _angle_outputs[1];
+	_last_valid_setpoint.psi() = _angle_outputs[2];
+
+	// Take into account vehicle attitude if it should
+	switch (_parameters.mnt_do_stab) {
+
+	case MntDoStabilize::ALL_AXES: {
+
+			for (int i = 0; i < 3; i++) {
+				if (compensate[i]) {
+					_last_valid_setpoint(i) += euler_vehicle(i);
+				}
+			}
+
+			break;
+		}
+
+	case MntDoStabilize::YAW_LOCK: {
+			if (compensate[2]) {
+				_last_valid_setpoint.psi() += euler_vehicle(2);
+			}
+
+			break;
+		}
+
+	case MntDoStabilize::PITCH_LOCK: {
+
+			if (compensate[1]) {
+				_last_valid_setpoint.theta() += euler_vehicle(1);
+			}
+
+			break;
+		}
+
+	default: {
+			// Dont add anything
+			break;
+		}
+	}
+}
+
 } /* namespace gimbal */

src/modules/gimbal/output.h

@@ -96,7 +96,22 @@ protected:
 	/** calculate the _angle_outputs (with speed) and stabilize if needed */
 	void _calculate_angle_output(const hrt_abstime &t);
 
+	/**
+	 * The angular velocity setpoint modifies the angle setpoint.
+	 * To correctly track and lock onto the desired orientation, the resulting change in angle
+	 * must be incorporated into the world-frame attitude setpoint. Depending on MNT_DO_STAB and
+	 * the received MAVLink command, the last valid setpoint is updated to account for the vehicle attitude.
+	 *
+	 * @param compensate Boolean per axis (roll, pitch, yaw). If true, the vehicle attitude is taken into account.
+	 * @param euler_vehicle
+	 */
+	void set_last_valid_setpoint(const bool compensate[3], const matrix::Eulerf euler_vehicle);
+
 	float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad]
+
+	// Quaternion float last valid setpoint.
+	// Depending on MNT_DO_STAB and mavlink command the setpoint can be relative to vehicle or in the world frame
+	matrix::Eulerf _last_valid_setpoint;
 	hrt_abstime _last_update;
 
 private:

src/modules/gimbal/output_rc.cpp

@@ -157,12 +157,12 @@ void OutputRC::_stream_device_attitude_status()
 			if (_absolute_angle[2]) {
 				attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK;
 				// absolute frame
-				q = matrix::Quaternionf(_q_setpoint);
+				q = matrix::Quaternionf(_last_valid_setpoint);
 
 			} else {
 				attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME;
 				// yaw vehicle frame
-				q = matrix::Quaternionf(_q_setpoint);
+				q = matrix::Quaternionf(_last_valid_setpoint);
 			}