From 5a2127d026e3819ff1091fb54a8147a087dec240 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 12 Aug 2022 14:54:21 +0200 Subject: [PATCH] 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 --- msg/vehicle_attitude_setpoint.msg | 4 +- .../FixedwingAttitudeControl.cpp | 44 +++++-------------- src/modules/fw_att_control/ecl_controller.h | 1 - .../fw_att_control/ecl_wheel_controller.cpp | 2 +- .../FixedwingPositionControl.cpp | 21 ++------- .../PositionControl/PositionControlTest.cpp | 4 +- 6 files changed, 17 insertions(+), 59 deletions(-) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index aeb0f2aedd..05888d52c5 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/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) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index caef8cfac7..33417a77eb 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/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 diff --git a/src/modules/fw_att_control/ecl_controller.h b/src/modules/fw_att_control/ecl_controller.h index 27cea46a74..5e4112e013 100644 --- a/src/modules/fw_att_control/ecl_controller.h +++ b/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 diff --git a/src/modules/fw_att_control/ecl_wheel_controller.cpp b/src/modules/fw_att_control/ecl_wheel_controller.cpp index dd8a3ceae9..869aaa22ca 100644 --- a/src/modules/fw_att_control/ecl_wheel_controller.cpp +++ b/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; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 1de2414f72..161fdca5e0 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 89d22fe39b..8be0f44fca 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/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? }