diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index 78613afa27402444f206c81023e24cbf48ff183f..b6c839409a76b2500331f695caf225483377ddd2 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 bbb99468eb23e07d2d809512efaff96d6ce2cde7..8e4c4f2f2d9c06cd8c7e55f99a99c9b16f1646d5 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 5dfa1825a14e01e258107f212c4f9148c0be6af2..fd95b7b3a1af87b8ecf27f650f0ce18e0b04f83c 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 @@ public:
/**
* 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 @@ private:
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 @@ private:
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<px4::params::MPC_THR_MAX>) 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 63101b7bf9edfcb454551a5844f8b56ee6f2c447..ecd402bd9aa31ae2b2b026a221ccbb46e9b266d7 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 @@ private:
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<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
@@ -252,11 +260,11 @@ private:
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 @@ private:
*/
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()
{