fixed-wing: update rate controller integrator handling

-always reset roll/pitch/yaw integrators at the same time
-reset them while waiting for launch or during FW Takeoff before Climbout
-reset wheel rate integrator only when disarmed

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/vehicle_attitude_setpoint.msg

@@ -13,9 +13,7 @@ float32[4] q_d			# Desired quaternion for quaternion control
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body		# Normalized thrust command in body NED frame [-1,1]
 
-bool roll_reset_integral	# Reset roll integral part (navigation logic change)
-bool pitch_reset_integral	# Reset pitch integral part (navigation logic change)
-bool yaw_reset_integral	# Reset yaw integral part (navigation logic change)
+bool reset_rate_integrals	# Reset roll/pitch/yaw integrals (navigation logic change)
 
 bool fw_control_yaw		# control heading with rudder (used for auto takeoff on runway)
 

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -384,12 +384,6 @@ void FixedwingAttitudeControl::Run()
 			wheel_control = true;
 		}
 
-		// lock integrator if no rate control enabled, or in RW mode (but not transitioning VTOL or tailsitter), or for long intervals (> 20 ms)
-		bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled
-				       || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
-					   !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)
-				       || (dt > 0.02f);
-
 		/* if we are in rotary wing mode, do nothing */
 		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
 			_spoiler_setpoint_with_slewrate.setForcedValue(0.f);
@@ -401,23 +395,13 @@ void FixedwingAttitudeControl::Run()
 		controlFlaps(dt);
 		controlSpoilers(dt);
 
-		/* decide if in stabilized or full manual control */
 		if (_vcontrol_mode.flag_control_rates_enabled) {
 
 			const float airspeed = get_airspeed_and_update_scaling();
 
 			/* reset integrals where needed */
-			if (_att_sp.roll_reset_integral) {
-				_roll_ctrl.reset_integrator();
-			}
-
-			if (_att_sp.pitch_reset_integral) {
-				_pitch_ctrl.reset_integrator();
-			}
-
-			if (_att_sp.yaw_reset_integral) {
-				_yaw_ctrl.reset_integrator();
-				_wheel_ctrl.reset_integrator();
+			if (_att_sp.reset_rate_integrals) {
+				_rate_control.resetIntegral();
 			}
 
 			/* Reset integrators if the aircraft is on ground
@@ -427,9 +411,7 @@ void FixedwingAttitudeControl::Run()
 			    || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 				&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {
 
-				_roll_ctrl.reset_integrator();
-				_pitch_ctrl.reset_integrator();
-				_yaw_ctrl.reset_integrator();
+				_rate_control.resetIntegral();
 				_wheel_ctrl.reset_integrator();
 			}
 
@@ -450,7 +432,6 @@ void FixedwingAttitudeControl::Run()
 			control_input.airspeed_min = _param_fw_airspd_stall.get();
 			control_input.airspeed_max = _param_fw_airspd_max.get();
 			control_input.airspeed = airspeed;
-			control_input.lock_integrator = lock_integrator;
 
 			if (wheel_control) {
 				_local_pos_sub.update(&_local_pos);
@@ -530,7 +511,6 @@ void FixedwingAttitudeControl::Run()
 
 					if (wheel_control) {
 						_wheel_ctrl.control_attitude(dt, control_input);
-						_yaw_ctrl.reset_integrator();
 
 					} else {
 						// runs last, because is depending on output of roll and pitch attitude
@@ -565,19 +545,11 @@ void FixedwingAttitudeControl::Run()
 					_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
 						(PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
 
-					if (!PX4_ISFINITE(roll_u)) {
-						_roll_ctrl.reset_integrator();
-					}
-
 					float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
 					float pitch_u = att_control(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
 					_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
 						(PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
 
-					if (!PX4_ISFINITE(pitch_u)) {
-						_pitch_ctrl.reset_integrator();
-					}
-
 					float yaw_u = 0.0f;
 
 					if (wheel_control) {
@@ -597,9 +569,8 @@ void FixedwingAttitudeControl::Run()
 						_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
 					}
 
-					if (!PX4_ISFINITE(yaw_u)) {
-						// _yaw_ctrl.reset_integrator();
-						_wheel_ctrl.reset_integrator();
+					if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) {
+						_rate_control.resetIntegral();
 					}
 
 					/* throttle passed through if it is finite */
@@ -665,6 +636,11 @@ void FixedwingAttitudeControl::Run()
 			}
 
 			_rate_ctrl_status_pub.publish(rate_ctrl_status);
+
+		} else {
+			// full manual
+			_rate_control.resetIntegral();
+			_wheel_ctrl.reset_integrator();
 		}
 
 		// Add feed-forward from roll control output to yaw control output

src/modules/fw_att_control/ecl_controller.h

@@ -69,7 +69,6 @@ struct ECL_ControlData {
 	float airspeed;
 	float groundspeed;
 	float groundspeed_scaler;
-	bool lock_integrator;
 };
 
 class ECL_Controller

src/modules/fw_att_control/ecl_wheel_controller.cpp

@@ -62,7 +62,7 @@ float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlDat
 	/* Calculate body angular rate error */
 	const float rate_error = _body_rate_setpoint - ctl_data.body_z_rate; //body angular rate error
 
-	if (!ctl_data.lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
+	if (_k_i > 0.0f && ctl_data.groundspeed > min_speed) {
 
 		float id = rate_error * dt * ctl_data.groundspeed_scaler;
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -908,8 +908,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		publishOrbitStatus(current_sp);
 	}
 
-	const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
-
 	switch (position_sp_type) {
 	case position_setpoint_s::SETPOINT_TYPE_IDLE:
 		_att_sp.thrust_body[0] = 0.0f;
@@ -933,11 +931,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		break;
 	}
 
-	if (was_circle_mode && !_l1_control.circle_mode()) {
-		/* just kicked out of loiter, reset roll integrals */
-		_att_sp.roll_reset_integral = true;
-	}
-
 	/* Copy thrust output for publication, handle special cases */
 	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 
@@ -1482,8 +1475,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 		if (_runway_takeoff.resetIntegrators()) {
 			// reset integrals except yaw (which also counts for the wheel controller)
-			_att_sp.roll_reset_integral = true;
-			_att_sp.pitch_reset_integral = true;
+			_att_sp.reset_rate_integrals = true;
 
 			// throttle is open loop anyway during ground roll, no need to wind up the integrator
 			_tecs.resetIntegrals();
@@ -1621,11 +1613,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_att_sp.pitch_body = get_tecs_pitch();
 
 		} else {
-			/* Tell the attitude controller to stop integrating while we are waiting
-			 * for the launch */
-			_att_sp.roll_reset_integral = true;
-			_att_sp.pitch_reset_integral = true;
-			_att_sp.yaw_reset_integral = true;
+			/* Tell the attitude controller to stop integrating while we are waiting for the launch */
+			_att_sp.reset_rate_integrals = true;
 
 			/* Set default roll and pitch setpoints during detection phase */
 			_att_sp.roll_body = 0.0f;
@@ -2234,9 +2223,7 @@ FixedwingPositionControl::Run()
 		_npfg.setPeriod(_param_npfg_period.get());
 		_l1_control.set_l1_period(_param_fw_l1_period.get());
 
-		_att_sp.roll_reset_integral = false;
-		_att_sp.pitch_reset_integral = false;
-		_att_sp.yaw_reset_integral = false;
+		_att_sp.reset_rate_integrals = false;
 
 		// by default we don't want yaw to be contoller directly with rudder
 		_att_sp.fw_control_yaw = false;

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -62,9 +62,7 @@ TEST(PositionControlTest, EmptySetpoint)
 	EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f);
 	EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
 	EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
-	EXPECT_EQ(attitude.roll_reset_integral, false);
-	EXPECT_EQ(attitude.pitch_reset_integral, false);
-	EXPECT_EQ(attitude.yaw_reset_integral, false);
+	EXPECT_EQ(attitude.reset_rate_integrals, false);
 	EXPECT_EQ(attitude.fw_control_yaw, false);
 	EXPECT_FLOAT_EQ(attitude.apply_flaps, 0.f);//vehicle_attitude_setpoint_s::FLAPS_OFF); // TODO why no reference?
 }