From 5887a2393cefad7268d165e07352384be61e1404 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 13 Dec 2018 12:14:44 +0100 Subject: [PATCH] PositionControl: skip controller only if adjusted thrust setpoint are not finite PositionControl: update comment about the order of thrust and position mc_pos_control: reset setpoints to NAN if required MulticopterLandDetector: consider to be landed if vehicle is not armed mc_pos_control: initialize landing struct with landed mc-pos-ctrl: adjust thrust-setpoint and yaw during ground-contact/maybe-landed without capturing that information within vehicle-local-position-setpoint topic because that information does not belong to user intention PositionControl: set local position/velocity setpoint to NAN if not used in the control pipeline mc-pos-ctrl: Fill vehicle_local_position_setpoint_s structure as follow: The message contains setpoints where each type of setpoint is either the input to the PositionController or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID). Example: If the desired setpoint is position-setpoint, _local_pos_sp will contain position-, velocity- and thrust-setpoint where the velocity- and thrust-setpoint were generated by the PositionControlller. If the desired setpoint has a velocity-setpoint only, then _local_pos_sp will contain valid velocity- and thrust-setpoint, but the position-setpoint will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the PositionController. mc_pos_control: switch to designated initializer for landed It's less error prone because it produces an error on every discrepancy. --- .../land_detector/MulticopterLandDetector.cpp | 9 +- .../mc_pos_control/PositionControl.cpp | 27 +++- .../mc_pos_control/PositionControl.hpp | 42 +++++- .../mc_pos_control/mc_pos_control_main.cpp | 124 +++++++++++------- 4 files changed, 142 insertions(+), 60 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 78613afa2740..b6c839409a76 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -151,7 +151,7 @@ bool MulticopterLandDetector::_get_freefall_state() bool MulticopterLandDetector::_get_ground_contact_state() { - // only trigger flight conditions if we are armed + // When not armed, consider to have ground-contact if (!_arming.armed) { return true; } @@ -202,7 +202,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() // Time base for this function const hrt_abstime now = hrt_absolute_time(); - // only trigger flight conditions if we are armed + // When not armed, consider to be maybe-landed if (!_arming.armed) { return true; } @@ -249,6 +249,11 @@ bool MulticopterLandDetector::_get_maybe_landed_state() bool MulticopterLandDetector::_get_landed_state() { + // When not armed, consider to be landed + if (!_arming.armed) { + return true; + } + // reset the landed_time if (!_maybe_landed_hysteresis.get_state()) { diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index bbb99468eb23..8e4c4f2f2d9c 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -55,8 +55,25 @@ void PositionControl::updateState(const PositionControlStates &states) _vel_dot = states.acceleration; } +void PositionControl::_setCtrlFlagTrue() +{ + for (int i = 0; i <= 2; i++) { + _ctrl_pos[i] = _ctrl_vel[i] = true; + } +} + +void PositionControl::_setCtrlFlagFalse() +{ + for (int i = 0; i <= 2; i++) { + _ctrl_pos[i] = _ctrl_vel[i] = false; + } +} + bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint) { + // Only for logging purpose: by default we use the entire position-velocity control-loop pipeline + _setCtrlFlagTrue(); + _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); _acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z); @@ -67,8 +84,8 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se // If full manual is required (thrust already generated), don't run position/velocity // controller and just return thrust. - _skip_controller = PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) - && PX4_ISFINITE(setpoint.thrust[2]); + _skip_controller = PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) + && PX4_ISFINITE(_thr_sp(2)); return mapping_succeeded; } @@ -76,6 +93,7 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se void PositionControl::generateThrustYawSetpoint(const float dt) { if (_skip_controller) { + // Already received a valid thrust set-point. // Limit the thrust vector. float thr_mag = _thr_sp.length(); @@ -132,6 +150,7 @@ bool PositionControl::_interfaceMapping() // Velocity controller is active without position control. // Set integral states and setpoints to 0 _pos_sp(i) = _pos(i) = 0.0f; + _ctrl_pos[i] = false; // position control-loop is not used // thrust setpoint is not supported in velocity control _thr_sp(i) = NAN; @@ -147,6 +166,7 @@ bool PositionControl::_interfaceMapping() // Set all integral states and setpoints to 0 _pos_sp(i) = _pos(i) = 0.0f; _vel_sp(i) = _vel(i) = 0.0f; + _ctrl_pos[i] = _ctrl_vel[i] = false; // position/velocity control loop is not used // Reset the Integral term. _thr_int(i) = 0.0f; @@ -192,6 +212,8 @@ bool PositionControl::_interfaceMapping() // throttle down such that vehicle goes down with // 70% of throttle range between min and hover _thr_sp(2) = -(MPC_THR_MIN.get() + (MPC_THR_HOVER.get() - MPC_THR_MIN.get()) * 0.7f); + // position and velocity control-loop is not used (note: only for logging purpose) + _setCtrlFlagFalse(); // position/velocity control-loop is not used } return !(failsafe); @@ -215,7 +237,6 @@ void PositionControl::_positionController() void PositionControl::_velocityController(const float &dt) { - // Generate desired thrust setpoint. // PID // u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral) diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index 5dfa1825a14e..fd95b7b3a1af 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -69,6 +69,8 @@ struct PositionControlStates { * priority over the feed-forward component. * * A setpoint that is NAN is considered as not set. + * If there is a position/velocity- and thrust-setpoint present, then + * the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop. */ class PositionControl : public ModuleParams { @@ -150,16 +152,44 @@ class PositionControl : public ModuleParams /** * Get the * @see _vel_sp - * @return The velocity set-point member. + * @return The velocity set-point that was executed in the control-loop. Nan if velocity control-loop was skipped. */ - const matrix::Vector3f &getVelSp() { return _vel_sp; } + const matrix::Vector3f getVelSp() + { + matrix::Vector3f vel_sp{}; + + for (int i = 0; i <= 2; i++) { + if (_ctrl_vel[i]) { + vel_sp(i) = _vel_sp(i); + + } else { + vel_sp(i) = NAN; + } + } + + return vel_sp; + } /** * Get the * @see _pos_sp - * @return The position set-point member. + * @return The position set-point that was executed in the control-loop. Nan if the position control-loop was skipped. */ - const matrix::Vector3f &getPosSp() { return _pos_sp; } + const matrix::Vector3f getPosSp() + { + matrix::Vector3f pos_sp{}; + + for (int i = 0; i <= 2; i++) { + if (_ctrl_pos[i]) { + pos_sp(i) = _pos_sp(i); + + } else { + pos_sp(i) = NAN; + } + } + + return pos_sp; + } protected: @@ -174,6 +204,8 @@ class PositionControl : public ModuleParams void _positionController(); /** applies the P-position-controller */ void _velocityController(const float &dt); /** applies the PID-velocity-controller */ + void _setCtrlFlagTrue(); /**< set control-loop flags to true (only required for logging) */ + void _setCtrlFlagFalse(); /**< set control-loop flags to false (only required for logging) */ matrix::Vector3f _pos{}; /**< MC position */ matrix::Vector3f _vel{}; /**< MC velocity */ @@ -189,6 +221,8 @@ class PositionControl : public ModuleParams matrix::Vector3f _thr_int{}; /**< thrust integral term */ vehicle_constraints_s _constraints{}; /**< variable constraints */ bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */ + bool _ctrl_pos[3] = {true, true, true}; /**< True if the control-loop for position was used */ + bool _ctrl_vel[3] = {true, true, true}; /**< True if the control-loop for velocity was used */ DEFINE_PARAMETERS( (ParamFloat) MPC_THR_MAX, 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 63101b7bf9ed..ecd402bd9aa3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -130,16 +130,24 @@ class MulticopterPositionControl : public ModuleBase false; /**< Smooth velocity takeoff can be initiated either through position or velocity setpoint */ vehicle_status_s _vehicle_status{}; /**< vehicle status */ - vehicle_land_detected_s _vehicle_land_detected{}; /**< vehicle land detected */ - vehicle_attitude_setpoint_s _att_sp{}; /**< vehicle attitude setpoint */ - vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */ - vehicle_local_position_s _local_pos{}; /**< vehicle local position */ - home_position_s _home_pos{}; /**< home position */ - vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */ - vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */ + /**< vehicle-land-detection: initialze to landed */ + vehicle_land_detected_s _vehicle_land_detected = { + .timestamp = 0, + .alt_max = -1.0f, + .landed = true, + .freefall = false, + .ground_contact = false, + .maybe_landed = false, + }; + + vehicle_attitude_setpoint_s _att_sp{}; /**< vehicle attitude setpoint */ + vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */ + vehicle_local_position_s _local_pos{}; /**< vehicle local position */ + home_position_s _home_pos{}; /**< home position */ + vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */ + vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */ landing_gear_s _landing_gear{}; - - int8_t _old_landing_gear_position; + int8_t _old_landing_gear_position; DEFINE_PARAMETERS( (ParamFloat) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ @@ -252,11 +260,11 @@ class MulticopterPositionControl : public ModuleBase void update_smooth_takeoff(const float &position_setpoint_z, const float &velocity_setpoint_z); /** - * Adjust the thrust setpoint during landing. + * Adjust the setpoint during landing. * Thrust is adjusted to support the land-detector during detection. - * @param thrust_setpoint gets adjusted based on land-detector state + * @param setpoint gets adjusted based on land-detector state */ - void limit_thrust_during_landing(matrix::Vector3f &thrust_sepoint); + void limit_thrust_during_landing(vehicle_local_position_setpoint_s &setpoint); /** * Start flightasks based on navigation state. @@ -287,6 +295,11 @@ class MulticopterPositionControl : public ModuleBase */ bool use_obstacle_avoidance(); + /** + * Reset setpoints to NAN + */ + void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); + /** * Overwrite setpoint with waypoint coming from obstacle avoidance */ @@ -725,10 +738,8 @@ MulticopterPositionControl::run() if (_vehicle_land_detected.landed && !_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { // Keep throttle low when landed and NOT in smooth takeoff + reset_setpoint_to_nan(setpoint); setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; - setpoint.x = setpoint.y = setpoint.z = NAN; - setpoint.vx = setpoint.vy = setpoint.vz = NAN; - setpoint.yawspeed = NAN; setpoint.yaw = _states.yaw; // reactivate the task which will reset the setpoint to current state _flight_tasks.reActivate(); @@ -756,37 +767,43 @@ MulticopterPositionControl::run() // Generate desired thrust and yaw. _control.generateThrustYawSetpoint(_dt); - matrix::Vector3f thr_sp = _control.getThrustSetpoint(); - - // Adjust thrust setpoint based on landdetector only if the - // vehicle is NOT in pure Manual mode and NOT in smooth takeoff - if (!_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { - limit_thrust_during_landing(thr_sp); - } - publish_trajectory_sp(setpoint); // Fill local position, velocity and thrust setpoint. + // This message contains setpoints where each type of setpoint is either the input to the PositionController + // or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID). + // Example: + // If the desired setpoint is position-setpoint, _local_pos_sp will contain + // position-, velocity- and thrust-setpoint where the velocity- and thrust-setpoint were generated by the PositionControlller. + // If the desired setpoint has a velocity-setpoint only, then _local_pos_sp will contain valid velocity- and thrust-setpoint, but the position-setpoint + // will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the + // PositionController. vehicle_local_position_setpoint_s local_pos_sp{}; local_pos_sp.timestamp = hrt_absolute_time(); - local_pos_sp.x = _control.getPosSp()(0); - local_pos_sp.y = _control.getPosSp()(1); - local_pos_sp.z = _control.getPosSp()(2); + local_pos_sp.x = setpoint.x; + local_pos_sp.y = setpoint.y; + local_pos_sp.z = setpoint.z; local_pos_sp.yaw = _control.getYawSetpoint(); local_pos_sp.yawspeed = _control.getYawspeedSetpoint(); - - local_pos_sp.vx = _control.getVelSp()(0); - local_pos_sp.vy = _control.getVelSp()(1); - local_pos_sp.vz = _control.getVelSp()(2); - thr_sp.copyTo(local_pos_sp.thrust); - - // Publish local position setpoint (for logging only) + local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx; + local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy; + local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz; + _control.getThrustSetpoint().copyTo(local_pos_sp.thrust); + + // Publish local position setpoint + // This message will be used by other modules (such as Landdetector) to determine + // vehicle intention. publish_local_pos_sp(local_pos_sp); - _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), thr_sp); // Inform FlightTask about the input and output of the velocity controller + // Part of landing logic: if ground-contact/maybe landed was detected, turn off + // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. + // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction. + if (!_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { + limit_thrust_during_landing(local_pos_sp); + } // Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint. - _att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint()); + _att_sp = ControlMath::thrustToAttitude(matrix::Vector3f(local_pos_sp.thrust), local_pos_sp.yaw); _att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint(); _att_sp.fw_control_yaw = false; _att_sp.apply_flaps = false; @@ -1085,25 +1102,25 @@ MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float } void -MulticopterPositionControl::limit_thrust_during_landing(matrix::Vector3f &thr_sp) +MulticopterPositionControl::limit_thrust_during_landing(vehicle_local_position_setpoint_s &setpoint) { if (_vehicle_land_detected.ground_contact) { // Set thrust in xy to zero - thr_sp(0) = 0.0f; - thr_sp(1) = 0.0f; - // Reset integral in xy is required because PID-controller does - // know about the overwrite and would therefore increase the intragral term + setpoint.thrust[0] = 0.0f; + setpoint.thrust[1] = 0.0f; _control.resetIntegralXY(); + // set yaw-sp to current yaw + setpoint.yaw = _states.yaw; } if (_vehicle_land_detected.maybe_landed) { + // set yaw-sp to current yaw + setpoint.yaw = _states.yaw; // we set thrust to zero // this will help to decide if we are actually landed or not - thr_sp.zero(); - // We need to reset all integral terms otherwise the PID-controller - // will continue to integrate - _control.resetIntegralXY(); - _control.resetIntegralZ(); + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; + _control.resetIntegralXY(); //reuqired to prevent integrator from increasing + _control.resetIntegralZ(); //reuqired to prevent integrator from increasing } } @@ -1116,13 +1133,8 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint if (!_failsafe_land_hysteresis.get_state() && !force) { // just keep current setpoint and don't do anything. - - } else { - setpoint.x = setpoint.y = setpoint.z = NAN; - setpoint.vx = setpoint.vy = setpoint.vz = NAN; - setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; - setpoint.yaw = setpoint.yawspeed = NAN; + reset_setpoint_to_nan(setpoint); if (PX4_ISFINITE(_states.velocity(2))) { // We have a valid velocity in D-direction. @@ -1186,6 +1198,16 @@ MulticopterPositionControl::execute_avoidance_waypoint(vehicle_local_position_se Vector3f(NAN, NAN, NAN).copyTo(setpoint.thrust); } +void +MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint) +{ + setpoint.x = setpoint.y = setpoint.z = NAN; + setpoint.vx = setpoint.vy = setpoint.vz = NAN; + setpoint.yaw = setpoint.yawspeed = NAN; + setpoint.acc_x = setpoint.acc_y = setpoint.acc_z = NAN; + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; +} + bool MulticopterPositionControl::use_obstacle_avoidance() {