diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/VehicleAttitudeSetpoint.msg index 05888d52c5..e1f444aae0 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/VehicleAttitudeSetpoint.msg @@ -13,7 +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 reset_rate_integrals # Reset roll/pitch/yaw integrals (navigation logic change) +bool reset_integral # 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/msg/VehicleRatesSetpoint.msg b/msg/VehicleRatesSetpoint.msg index f30ae2fc8f..88adcf3bea 100644 --- a/msg/VehicleRatesSetpoint.msg +++ b/msg/VehicleRatesSetpoint.msg @@ -8,3 +8,5 @@ float32 yaw # [rad/s] yaw rate setpoint # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. # 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 reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 172f54f8d9..80c7457596 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -157,7 +157,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); - _att_sp.reset_rate_integrals = false; + _att_sp.reset_integral = false; _att_sp.timestamp = hrt_absolute_time(); @@ -392,8 +392,8 @@ void FixedwingAttitudeControl::Run() const float airspeed = get_airspeed_and_update_scaling(); /* reset integrals where needed */ - if (_att_sp.reset_rate_integrals) { - _rate_control.resetIntegral(); + if (_att_sp.reset_integral) { + _rates_sp.reset_integral = true; } /* Reset integrators if the aircraft is on ground @@ -403,7 +403,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)) { - _rate_control.resetIntegral(); + _rates_sp.reset_integral = true; _wheel_ctrl.reset_integrator(); } @@ -605,7 +605,7 @@ void FixedwingAttitudeControl::Run() _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; - if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) { + if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u) || _rates_sp.reset_integral) { _rate_control.resetIntegral(); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 3391f01e8e..c525801035 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1487,7 +1487,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.reset_rate_integrals = true; + _att_sp.reset_integral = true; // throttle is open loop anyway during ground roll, no need to wind up the integrator _tecs.resetIntegrals(); @@ -1626,7 +1626,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } else { /* Tell the attitude controller to stop integrating while we are waiting for the launch */ - _att_sp.reset_rate_integrals = true; + _att_sp.reset_integral = true; /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; @@ -2249,7 +2249,7 @@ FixedwingPositionControl::Run() _npfg.setPeriod(_param_npfg_period.get()); _l1_control.set_l1_period(_param_fw_l1_period.get()); - _att_sp.reset_rate_integrals = false; + _att_sp.reset_integral = 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 384646afd4..ebcdb5b782 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -62,7 +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.reset_rate_integrals, false); + EXPECT_EQ(attitude.reset_integral, 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? }