diff --git a/msg/rate_ctrl_status.msg b/msg/rate_ctrl_status.msg index 145bf25a5c..41488b2b6e 100644 --- a/msg/rate_ctrl_status.msg +++ b/msg/rate_ctrl_status.msg @@ -1,4 +1,5 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) # rate controller integrator status float32 rollspeed_integ diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg index f30ae2fc8f..f80619be60 100644 --- a/msg/vehicle_rates_setpoint.msg +++ b/msg/vehicle_rates_setpoint.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) # body angular rates in NED frame float32 roll # [rad/s] roll rate setpoint diff --git a/src/modules/mc_rate_control/CMakeLists.txt b/src/modules/mc_rate_control/CMakeLists.txt index 758d56b7c8..959eb70c20 100644 --- a/src/modules/mc_rate_control/CMakeLists.txt +++ b/src/modules/mc_rate_control/CMakeLists.txt @@ -42,7 +42,6 @@ px4_add_module( MulticopterRateControl.cpp MulticopterRateControl.hpp DEPENDS - circuit_breaker mathlib RateControl px4_work_queue diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 02a54c9155..c291258f6d 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,12 +46,10 @@ using math::radians; MulticopterRateControl::MulticopterRateControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) + _actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)) { - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - parameters_updated(); + _controller_status_pub.advertise(); } @@ -60,8 +58,7 @@ MulticopterRateControl::~MulticopterRateControl() perf_free(_loop_perf); } -bool -MulticopterRateControl::init() +bool MulticopterRateControl::init() { if (!_vehicle_angular_velocity_sub.registerCallback()) { PX4_ERR("callback registration failed"); @@ -71,8 +68,7 @@ MulticopterRateControl::init() return true; } -void -MulticopterRateControl::parameters_updated() +void MulticopterRateControl::parameters_updated() { // rate control parameters // The controller gain K is used to convert the parallel (P + I/s + sD) form @@ -89,15 +85,9 @@ MulticopterRateControl::parameters_updated() _rate_control.setFeedForwardGain( Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); - - - // manual rate control acro mode rate limits - _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), - radians(_param_mc_acro_y_max.get())); } -void -MulticopterRateControl::Run() +void MulticopterRateControl::Run() { if (should_exit()) { _vehicle_angular_velocity_sub.unregisterCallback(); @@ -105,8 +95,6 @@ MulticopterRateControl::Run() return; } - perf_begin(_loop_perf); - // Check if parameters have changed if (_parameter_update_sub.updated()) { // clear update @@ -117,60 +105,76 @@ MulticopterRateControl::Run() parameters_updated(); } - /* run controller on gyro changes */ - vehicle_angular_velocity_s angular_velocity; + perf_begin(_loop_perf); - if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _rate_control_enabled = vehicle_control_mode.flag_control_rates_enabled; + _attitude_control_enabled = vehicle_control_mode.flag_control_attitude_enabled; + _manual_enabled = vehicle_control_mode.flag_control_manual_enabled; + } + } + + // run controller on gyro changes + vehicle_angular_velocity_s angular_velocity; + vehicle_angular_acceleration_s angular_acceleration; + + if (_rate_control_enabled && _vehicle_angular_velocity_sub.update(&angular_velocity)) { // grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy - vehicle_angular_acceleration_s v_angular_acceleration{}; - _vehicle_angular_acceleration_sub.copy(&v_angular_acceleration); + if (!_vehicle_angular_acceleration_sub.copy(&angular_acceleration)) { + Vector3f().copyTo(angular_acceleration.xyz); + } - const hrt_abstime now = angular_velocity.timestamp_sample; - - // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); - _last_run = now; - - const Vector3f angular_accel{v_angular_acceleration.xyz}; - const Vector3f rates{angular_velocity.xyz}; - - /* check for updates in other topics */ - _vehicle_control_mode_sub.update(&_vehicle_control_mode); - - if (_vehicle_land_detected_sub.updated()) { + { vehicle_land_detected_s vehicle_land_detected; - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { - _landed = vehicle_land_detected.landed; - _maybe_landed = vehicle_land_detected.maybe_landed; + if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed || vehicle_land_detected.maybe_landed; } } - _vehicle_status_sub.update(&_vehicle_status); + { + vehicle_status_s vehicle_status; - if (_landing_gear_sub.updated()) { - landing_gear_s landing_gear; + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + _vtol = vehicle_status.is_vtol; + } + } - if (_landing_gear_sub.copy(&landing_gear)) { - if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { - if (landing_gear.landing_gear == landing_gear_s::GEAR_UP && (_landed || _maybe_landed)) { - mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear\t"); - events::send(events::ID("mc_rate_control_not_retract_landing_gear_landed"), - {events::Log::Error, events::LogInternal::Info}, - "Landed, unable to retract landing gear"); + { + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; - } else { - _landing_gear = landing_gear.landing_gear; + if (_control_allocator_status_sub.update(&control_allocator_status)) { + if (!control_allocator_status.torque_setpoint_achieved) { + + Vector saturation_positive{}; + Vector saturation_negative{}; + + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + + } else { + _rate_control.clearSaturationStatus(); } } } - // use rates setpoint topic - vehicle_rates_setpoint_s vehicle_rates_setpoint{}; - - if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { + if (_manual_enabled && !_attitude_control_enabled) { // generate the rate setpoint from sticks manual_control_setpoint_s manual_control_setpoint; @@ -181,138 +185,147 @@ MulticopterRateControl::Run() math::superexpo(-manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; - _rates_setpoint = man_rate_sp.emult(_acro_rate_max); - _thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f); - _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + // manual rate control acro mode rate limits + const Vector3f acro_rate_max(radians(_param_mc_acro_r_max.get()), + radians(_param_mc_acro_p_max.get()), + radians(_param_mc_acro_y_max.get())); + + _rates_setpoint = man_rate_sp.emult(acro_rate_max); + + _thrust_setpoint(0) = 0.f; + _thrust_setpoint(1) = 0.f; + _thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.f, 1.f); // publish rate setpoint - vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint_s vehicle_rates_setpoint; + vehicle_rates_setpoint.timestamp_sample = manual_control_setpoint.timestamp_sample; + vehicle_rates_setpoint.roll = _rates_setpoint(0); vehicle_rates_setpoint.pitch = _rates_setpoint(1); - vehicle_rates_setpoint.yaw = _rates_setpoint(2); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); vehicle_rates_setpoint.timestamp = hrt_absolute_time(); - _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + + + publishThrustSetpoint(manual_control_setpoint.timestamp); } - } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { - if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { - _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); - _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); - _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + } else { + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint; + + if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = vehicle_rates_setpoint.roll; + _rates_setpoint(1) = vehicle_rates_setpoint.pitch; + _rates_setpoint(2) = vehicle_rates_setpoint.yaw; _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + + publishThrustSetpoint(vehicle_rates_setpoint.timestamp); } } - // run the rate controller - if (_vehicle_control_mode.flag_control_rates_enabled) { + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((angular_velocity.timestamp_sample - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = angular_velocity.timestamp_sample; - // reset integral if disarmed - if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rate_control.resetIntegral(); - } - - // update saturation status from control allocation feedback - control_allocator_status_s control_allocator_status; - - if (_control_allocator_status_sub.update(&control_allocator_status)) { - Vector saturation_positive; - Vector saturation_negative; - - if (!control_allocator_status.torque_setpoint_achieved) { - for (size_t i = 0; i < 3; i++) { - if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { - saturation_positive(i) = true; - - } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { - saturation_negative(i) = true; - } - } - } - - // TODO: send the unallocated value directly for better anti-windup - _rate_control.setSaturationStatus(saturation_positive, saturation_negative); - } - - // run rate controller - const Vector3f att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); - - // publish rate controller status - rate_ctrl_status_s rate_ctrl_status{}; - _rate_control.getRateControlStatus(rate_ctrl_status); - rate_ctrl_status.timestamp = hrt_absolute_time(); - _controller_status_pub.publish(rate_ctrl_status); - - // publish actuator controls - actuator_controls_s actuators{}; - actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f; - actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f; - actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f; - actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint( - 2) : 0.0f; - actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear; - actuators.timestamp_sample = angular_velocity.timestamp_sample; - - if (!_vehicle_status.is_vtol) { - publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample); - publishThrustSetpoint(angular_velocity.timestamp_sample); - } - - // scale effort by battery status if enabled - if (_param_mc_bat_scale_en.get()) { - if (_battery_status_sub.updated()) { - battery_status_s battery_status; - - if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { - _battery_status_scale = battery_status.scale; - } - } - - if (_battery_status_scale > 0.0f) { - for (int i = 0; i < 4; i++) { - actuators.control[i] *= _battery_status_scale; - } - } - } - - actuators.timestamp = hrt_absolute_time(); - _actuator_controls_0_pub.publish(actuators); - - updateActuatorControlsStatus(actuators, dt); - - } else if (_vehicle_control_mode.flag_control_termination_enabled) { - if (!_vehicle_status.is_vtol) { - // publish actuator controls - actuator_controls_s actuators{}; - actuators.timestamp = hrt_absolute_time(); - _actuator_controls_0_pub.publish(actuators); - } + // reset integral if disarmed + if (!_armed || !_rotary_wing) { + _rate_control.resetIntegral(); } + + // run rate controller + const Vector3f torque_sp{_rate_control.update(Vector3f(angular_velocity.xyz), _rates_setpoint, Vector3f(angular_acceleration.xyz), dt, _landed)}; + + publishTorqueSetpoint(angular_velocity.timestamp_sample, torque_sp); + publishRateControllerStatus(angular_velocity.timestamp_sample); + + publishActuatorControls(angular_velocity.timestamp_sample, torque_sp, dt); } perf_end(_loop_perf); } -void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime ×tamp_sample) -{ - vehicle_torque_setpoint_s vehicle_torque_setpoint{}; - vehicle_torque_setpoint.timestamp = hrt_absolute_time(); - vehicle_torque_setpoint.timestamp_sample = timestamp_sample; - vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; - vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; - vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; - - _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); -} - void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) { - vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); - vehicle_thrust_setpoint.timestamp_sample = timestamp_sample; - _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + if (!_vtol) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + vehicle_thrust_setpoint.timestamp_sample = timestamp_sample; + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); - _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + } +} + +void MulticopterRateControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample, const Vector3f &torque_sp) +{ + if (!_vtol) { + vehicle_torque_setpoint_s vehicle_torque_setpoint; + vehicle_torque_setpoint.timestamp_sample = timestamp_sample; + torque_sp.copyTo(vehicle_torque_setpoint.xyz); + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + } +} + +void MulticopterRateControl::publishRateControllerStatus(const hrt_abstime ×tamp_sample) +{ + rate_ctrl_status_s rate_ctrl_status; + rate_ctrl_status.timestamp_sample = timestamp_sample; + rate_ctrl_status.rollspeed_integ = _rate_control.getIntegral()(0); + rate_ctrl_status.pitchspeed_integ = _rate_control.getIntegral()(1); + rate_ctrl_status.yawspeed_integ = _rate_control.getIntegral()(2); + rate_ctrl_status.additional_integ1 = 0; + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); +} + +void MulticopterRateControl::publishActuatorControls(const hrt_abstime ×tamp_sample, const Vector3f &torque_sp, + float dt) +{ + // scale effort by battery status if enabled + if (_param_mc_bat_scale_en.get() && _battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status)) { + if (battery_status.connected && (battery_status.scale > 0.f)) { + _battery_status_scale = battery_status.scale; + } + } + } + + { + // landing gear passed through actuator controls + landing_gear_s landing_gear; + + if (_landing_gear_sub.update(&landing_gear)) { + if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { + if (landing_gear.landing_gear == landing_gear_s::GEAR_UP && _landed) { + mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear\t"); + events::send(events::ID("mc_rate_control_not_retract_landing_gear_landed"), + {events::Log::Error, events::LogInternal::Info}, + "Landed, unable to retract landing gear"); + + } else { + _landing_gear = landing_gear.landing_gear; + } + } + } + } + + // publish actuator controls + actuator_controls_s actuators{}; + actuators.timestamp_sample = timestamp_sample; + actuators.control[actuator_controls_s::INDEX_ROLL] = torque_sp(0) * _battery_status_scale; + actuators.control[actuator_controls_s::INDEX_PITCH] = torque_sp(1) * _battery_status_scale; + actuators.control[actuator_controls_s::INDEX_YAW] = torque_sp(2) * _battery_status_scale; + actuators.control[actuator_controls_s::INDEX_THROTTLE] = -_thrust_setpoint(2) * _battery_status_scale; + actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear; + actuators.timestamp = hrt_absolute_time(); + _actuator_controls_0_pub.publish(actuators); + + updateActuatorControlsStatus(actuators, dt); } void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt) diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index d3a0be95f1..167526aa67 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -91,12 +91,14 @@ private: */ void parameters_updated(); + void publishActuatorControls(const hrt_abstime ×tamp_sample, const matrix::Vector3f &torque_sp, float dt); + void publishRateControllerStatus(const hrt_abstime ×tamp_sample); + void publishThrustSetpoint(const hrt_abstime ×tamp_sample); + void publishTorqueSetpoint(const hrt_abstime ×tamp_sample, const matrix::Vector3f &torque_sp); + void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt); - void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime ×tamp_sample); - void publishThrustSetpoint(const hrt_abstime ×tamp_sample); - - RateControl _rate_control; ///< class for rate control calculations + RateControl _rate_control{}; ///< class for rate control calculations uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; @@ -121,28 +123,31 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - vehicle_control_mode_s _vehicle_control_mode{}; - vehicle_status_s _vehicle_status{}; - + bool _armed{false}; bool _landed{true}; - bool _maybe_landed{true}; + + bool _rotary_wing{true}; + bool _vtol{false}; + + bool _rate_control_enabled{false}; + bool _attitude_control_enabled{false}; + bool _manual_enabled{false}; hrt_abstime _last_run{0}; - perf_counter_t _loop_perf; /**< loop duration performance counter */ - // keep setpoint values between updates - matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ matrix::Vector3f _rates_setpoint{}; - - float _battery_status_scale{0.0f}; matrix::Vector3f _thrust_setpoint{}; + float _battery_status_scale{1.f}; + float _energy_integration_time{0.0f}; float _control_energy[4] {}; int8_t _landing_gear{landing_gear_s::GEAR_DOWN}; + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; /**< loop duration performance counter */ + DEFINE_PARAMETERS( (ParamFloat) _param_mc_rollrate_p, (ParamFloat) _param_mc_rollrate_i, diff --git a/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/modules/mc_rate_control/RateControl/RateControl.cpp index bf659315f8..ac24ccf61c 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -68,6 +68,11 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons updateIntegral(rate_error, dt); } + if (!math::isFinite(torque)) { + _rate_int.zero(); + return {}; + } + return torque; } @@ -96,16 +101,6 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt) // Perform the integration using a first order method float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt; - // do not propagate the result if out of range or invalid - if (PX4_ISFINITE(rate_i)) { - _rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i)); - } + _rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i)); } } - -void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status) -{ - rate_ctrl_status.rollspeed_integ = _rate_int(0); - rate_ctrl_status.pitchspeed_integ = _rate_int(1); - rate_ctrl_status.yawspeed_integ = _rate_int(2); -} diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index 96698bc565..116643f818 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -42,7 +42,6 @@ #include #include -#include class RateControl { @@ -78,6 +77,12 @@ public: void setSaturationStatus(const matrix::Vector &saturation_positive, const matrix::Vector &saturation_negative); + void clearSaturationStatus() + { + _control_allocator_saturation_negative.zero(); + _control_allocator_saturation_positive.zero(); + } + /** * Run one control loop cycle calculation * @param rate estimation of the current vehicle angular rate @@ -95,10 +100,9 @@ public: void resetIntegral() { _rate_int.zero(); } /** - * Get status message of controller for logging/debugging - * @param rate_ctrl_status status message to fill with internal states + * Get integral for logging/debugging */ - void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); + const matrix::Vector3f &getIntegral() const { return _rate_int; } private: void updateIntegral(matrix::Vector3f &rate_error, const float dt); @@ -111,7 +115,7 @@ private: matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters // States - matrix::Vector3f _rate_int; ///< integral term of the rate controller + matrix::Vector3f _rate_int{}; ///< integral term of the rate controller // Feedback from control allocation matrix::Vector _control_allocator_saturation_negative;