diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index f64c1e9d03..e5e39cd9a0 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -63,7 +63,7 @@ void PositionControl::setThrustLimits(const float min, const float max) _lim_thr_max = max; } -void PositionControl::updateState(const PositionControlStates &states) +void PositionControl::setState(const PositionControlStates &states) { _pos = states.position; _vel = states.velocity; @@ -78,7 +78,7 @@ void PositionControl::_setCtrlFlag(bool value) } } -bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint) +bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint) { // by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose) _setCtrlFlag(true); @@ -99,7 +99,32 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se return mapping_succeeded; } -void PositionControl::generateThrustYawSetpoint(const float dt) +void PositionControl::setConstraints(const vehicle_constraints_s &constraints) +{ + _constraints = constraints; + + // For safety check if adjustable constraints are below global constraints. If they are not stricter than global + // constraints, then just use global constraints for the limits. + + if (!PX4_ISFINITE(constraints.tilt) + || !(constraints.tilt < _lim_tilt)) { + _constraints.tilt = _lim_tilt; + } + + if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _lim_vel_up)) { + _constraints.speed_up = _lim_vel_up; + } + + if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _lim_vel_down)) { + _constraints.speed_down = _lim_vel_down; + } + + if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _lim_vel_horizontal)) { + _constraints.speed_xy = _lim_vel_horizontal; + } +} + +void PositionControl::update(const float dt) { if (_skip_controller) { @@ -120,8 +145,8 @@ void PositionControl::generateThrustYawSetpoint(const float dt) _acc_sp = _vel_dot; } else { - _positionController(); - _velocityController(dt); + _positionControl(); + _velocityControl(dt); } } @@ -178,7 +203,7 @@ bool PositionControl::_interfaceMapping() _ctrl_pos[i] = _ctrl_vel[i] = false; // position/velocity control loop is not used // Reset the Integral term. - _thr_int(i) = 0.0f; + _vel_int(i) = 0.0f; // Don't require velocity derivative. _vel_dot(i) = 0.0f; @@ -228,7 +253,7 @@ bool PositionControl::_interfaceMapping() return !(failsafe); } -void PositionControl::_positionController() +void PositionControl::_positionControl() { // P-position controller const Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); @@ -244,7 +269,7 @@ void PositionControl::_positionController() _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down); } -void PositionControl::_velocityController(const float &dt) +void PositionControl::_velocityControl(const float dt) { // Generate desired thrust setpoint. // PID @@ -273,7 +298,7 @@ void PositionControl::_velocityController(const float &dt) const Vector3f vel_err = _vel_sp - _vel; // Consider thrust in D-direction. - float thrust_desired_D = _gain_vel_p(2) * vel_err(2) + _gain_vel_d(2) * _vel_dot(2) + _thr_int( + float thrust_desired_D = _gain_vel_p(2) * vel_err(2) + _gain_vel_d(2) * _vel_dot(2) + _vel_int( 2) - _hover_thrust; // The Thrust limits are negated and swapped due to NED-frame. @@ -288,10 +313,10 @@ void PositionControl::_velocityController(const float &dt) (thrust_desired_D <= uMin && vel_err(2) <= 0.0f); if (!stop_integral_D) { - _thr_int(2) += vel_err(2) * _gain_vel_i(2) * dt; + _vel_int(2) += vel_err(2) * _gain_vel_i(2) * dt; // limit thrust integral - _thr_int(2) = math::min(fabsf(_thr_int(2)), _lim_thr_max) * math::sign(_thr_int(2)); + _vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * math::sign(_vel_int(2)); } // Saturate thrust setpoint in D-direction. @@ -307,8 +332,8 @@ void PositionControl::_velocityController(const float &dt) } else { // PID-velocity controller for NE-direction. Vector2f thrust_desired_NE; - thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _thr_int(0); - thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _thr_int(1); + thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _vel_int(0); + thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _vel_int(1); // Get maximum allowed thrust in NE based on tilt and excess thrust. float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt); @@ -334,35 +359,11 @@ void PositionControl::_velocityController(const float &dt) vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain; // Update integral - _thr_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt; - _thr_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt; + _vel_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt; + _vel_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt; } } -void PositionControl::updateConstraints(const vehicle_constraints_s &constraints) -{ - _constraints = constraints; - - // For safety check if adjustable constraints are below global constraints. If they are not stricter than global - // constraints, then just use global constraints for the limits. - - if (!PX4_ISFINITE(constraints.tilt) - || !(constraints.tilt < _lim_tilt)) { - _constraints.tilt = _lim_tilt; - } - - if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _lim_vel_up)) { - _constraints.speed_up = _lim_vel_up; - } - - if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _lim_vel_down)) { - _constraints.speed_down = _lim_vel_down; - } - - if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _lim_vel_horizontal)) { - _constraints.speed_xy = _lim_vel_horizontal; - } -} void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const { diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index eae9c9586e..1aebe52aa3 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -121,23 +121,25 @@ public: void setHoverThrust(const float thrust) { _hover_thrust = thrust; } /** - * Update the current vehicle state. + * Pass the current vehicle state to the controller * @param PositionControlStates structure */ - void updateState(const PositionControlStates &states); + void setState(const PositionControlStates &states); /** - * Update the desired setpoints. + * Pass the desired setpoints + * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. * @param setpoint a vehicle_local_position_setpoint_s structure - * @return true if setpoint has updated correctly + * @return true if a valid setpoint was set */ - bool updateSetpoint(const vehicle_local_position_setpoint_s &setpoint); + bool setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint); /** - * Set constraints that are stricter than the global limits. + * Pass constraints that are stricter than the global limits + * Note: NAN value means no constraint, take maximum limit of controller. * @param constraints a PositionControl structure with supported constraints */ - void updateConstraints(const vehicle_constraints_s &constraints); + void setConstraints(const vehicle_constraints_s &constraints); /** * Apply P-position and PID-velocity controller that updates the member @@ -145,21 +147,16 @@ public: * @see _thr_sp * @see _yaw_sp * @see _yawspeed_sp - * @param dt the delta-time + * @param dt time in seconds since last iteration + * @return true if output setpoint is executable, false if not */ - void generateThrustYawSetpoint(const float dt); + void update(const float dt); /** - * Set the integral term in xy to 0. - * @see _thr_int + * Set the integral term in xy to 0. + * @see _vel_int */ - void resetIntegralXY() { _thr_int(0) = _thr_int(1) = 0.f; } - - /** - * Set the integral term in z to 0. - * @see _thr_int - */ - void resetIntegralZ() { _thr_int(2) = 0.f; } + void resetIntegral() { _vel_int.setZero(); } /** * Get the @@ -205,8 +202,8 @@ private: */ bool _interfaceMapping(); - void _positionController(); /** applies the P-position-controller */ - void _velocityController(const float &dt); /** applies the PID-velocity-controller */ + void _positionControl(); ///< Position proportional control + void _velocityControl(const float dt); ///< Velocity PID control void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */ // Gains @@ -229,7 +226,7 @@ private: matrix::Vector3f _pos; /**< current position */ matrix::Vector3f _vel; /**< current velocity */ matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */ - matrix::Vector3f _thr_int; /**< integral term of the velocity controller */ + matrix::Vector3f _vel_int; /**< integral term of the velocity controller */ float _yaw{}; /**< current heading */ vehicle_constraints_s _constraints{}; /**< variable constraints */ diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 90563f028a..43ff73f4c9 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -102,9 +102,9 @@ public: void runController() { - _position_control.updateConstraints(_contraints); - _position_control.updateSetpoint(_input_setpoint); - _position_control.generateThrustYawSetpoint(.1f); + _position_control.setConstraints(_contraints); + _position_control.setInputSetpoint(_input_setpoint); + _position_control.update(.1f); _position_control.getLocalPositionSetpoint(_output_setpoint); _position_control.getAttitudeSetpoint(_attitude); } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5e6fa851f8..e733ab300f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -609,8 +609,7 @@ MulticopterPositionControl::Run() setpoint.yaw = _states.yaw; setpoint.yawspeed = 0.f; // prevent any integrator windup - _control.resetIntegralXY(); - _control.resetIntegralZ(); + _control.resetIntegral(); // reactivate the task which will reset the setpoint to current state _flight_tasks.reActivate(); } @@ -624,20 +623,18 @@ MulticopterPositionControl::Run() limit_altitude(setpoint); } - // Update states, setpoints and constraints. - _control.updateConstraints(constraints); - _control.updateState(_states); + // Run position control + _control.setState(_states); + _control.setConstraints(constraints); - // update position controller setpoints - if (!_control.updateSetpoint(setpoint)) { - warn_rate_limited("Position-Control Setpoint-Update failed"); + if (!_control.setInputSetpoint(setpoint)) { + warn_rate_limited("PositionControl: invalid setpoints"); failsafe(setpoint, _states, true, !was_in_failsafe); - _control.updateSetpoint(setpoint); + _control.setInputSetpoint(setpoint); constraints = FlightTask::empty_constraints; } - // Generate desired thrust and yaw. - _control.generateThrustYawSetpoint(_dt); + _control.update(_dt); // Fill local position, velocity and thrust setpoint. // This message contains setpoints where each type of setpoint is either the input to the PositionController @@ -660,8 +657,7 @@ MulticopterPositionControl::Run() local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz; // Publish local position setpoint - // This message will be used by other modules (such as Landdetector) to determine - // vehicle intention. + // This message will be used by other modules (such as Landdetector) to determine vehicle intention. _local_pos_sp_pub.publish(local_pos_sp); // Inform FlightTask about the input and output of the velocity controller @@ -691,10 +687,8 @@ MulticopterPositionControl::Run() // if there's any change in landing gear setpoint publish it if (gear.landing_gear != _old_landing_gear_position && gear.landing_gear != landing_gear_s::GEAR_KEEP) { - - _landing_gear.landing_gear = gear.landing_gear; _landing_gear.timestamp = time_stamp_now; - + _landing_gear.landing_gear = gear.landing_gear; _landing_gear_pub.publish(_landing_gear); } @@ -940,8 +934,7 @@ MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoin Quatf(AxisAngle(0, 0, _states.yaw)).copyTo(setpoint.q_d); setpoint.yaw_sp_move_rate = 0.f; // prevent any position control integrator windup - _control.resetIntegralXY(); - _control.resetIntegralZ(); + _control.resetIntegral(); } }