Skip to content

Commit

Permalink
PositionControl: skip controller only if adjusted thrust setpoint are…
Browse files Browse the repository at this point in the history 
… 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.
  • Loading branch information
@MaEtUgR
Dennis Mannhart authored and MaEtUgR committed Dec 19, 2018
1 parent fce35ba commit a1a2c2f
Show file tree
Hide file tree
Showing 4 changed files with 142 additions and 60 deletions.
9 changes: 7 additions & 2 deletions src/modules/land_detector/MulticopterLandDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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()) {

Expand Down
27 changes: 24 additions & 3 deletions src/modules/mc_pos_control/PositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -67,15 +84,16 @@ 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;
}

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();
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand All @@ -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)
Expand Down
42 changes: 38 additions & 4 deletions src/modules/mc_pos_control/PositionControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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:

Expand All @@ -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 */
Expand All @@ -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<px4::params::MPC_THR_MAX>) MPC_THR_MAX,
Expand Down
124 changes: 73 additions & 51 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,16 +130,24 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
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 */
Expand Down Expand Up @@ -252,11 +260,11 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
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.
Expand Down Expand Up @@ -287,6 +295,11 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
*/
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
*/
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
}
}

Expand All @@ -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.
Expand Down Expand Up @@ -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()
{
Expand Down

0 comments on commit a1a2c2f

Please sign in to comment.