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 9d1393ed979b..56f8aa6d22ef 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 */ @@ -728,10 +741,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(); @@ -759,37 +770,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; @@ -1088,25 +1105,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 } } @@ -1119,13 +1136,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. @@ -1189,6 +1201,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() {