Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

mc_pos_control: handle landed and takeoff setpoint limiting the same way #14326

Merged
merged 4 commits into from Mar 18, 2020
Merged

mc_pos_control: handle landed and takeoff setpoint limiting the same way #14326

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
6d6f5eb
mc_pos_control: switch order of setpoint amendment
MaEtUgR Mar 9, 2020
8d11942
mc_pos_control: prevent takeoff with thrust setpoint
MaEtUgR Mar 9, 2020
a003624
mc_pos_control: handle takeoff and landing thrust limit the same way
MaEtUgR Mar 9, 2020
ca9929d
mc_pos_control: allow zero thrust during takeoff and landing
MaEtUgR Mar 9, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
58 changes: 26 additions & 32 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
View file
Open in desktop
Expand Up @@ -593,12 +593,34 @@ MulticopterPositionControl::Run()
constraints.speed_up = _param_mpc_z_vel_max_up.get();
}

// limit tilt during takeoff ramupup
if (_takeoff.getTakeoffState() < TakeoffState::flight) {
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
}

// limit altitude only if local position is valid
if (PX4_ISFINITE(_states.position(2))) {
limit_altitude(setpoint);
}

// handle smooth takeoff
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff,
constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up);

if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup;
const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight;
const bool flying_but_ground_contact = flying && _vehicle_land_detected.ground_contact;

if (flying) {
_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());

} else {
// allow zero thrust when taking off and landing
_control.setThrustLimits(0.f, _param_mpc_thr_max.get());
}

if (not_taken_off || flying_but_ground_contact) {
// we are not flying yet and need to avoid any corrections
reset_setpoint_to_nan(setpoint);
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
Expand All @@ -608,17 +630,11 @@ MulticopterPositionControl::Run()
setpoint.yawspeed = 0.f;
// prevent any integrator windup
_control.resetIntegral();
// reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate();
}

if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
}

// limit altitude only if local position is valid
if (PX4_ISFINITE(_states.position(2))) {
limit_altitude(setpoint);
if (not_taken_off) {
// reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate();
}

// Run position control
Expand Down Expand Up @@ -660,13 +676,6 @@ MulticopterPositionControl::Run()
attitude_setpoint.timestamp = time_stamp_now;
_control.getAttitudeSetpoint(attitude_setpoint);

// 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 (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
limit_thrust_during_landing(attitude_setpoint);
}

// publish attitude setpoint
// It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized.
// Not publishing when not running a flight task
Expand Down Expand Up @@ -907,21 +916,6 @@ MulticopterPositionControl::start_flight_task()
}
}

void
MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint)
{
if (_vehicle_land_detected.ground_contact
|| _vehicle_land_detected.maybe_landed) {
// we set the collective thrust to zero, this will help to decide if we are actually landed or not
setpoint.thrust_body[2] = 0.f;
// go level to avoid corrections but keep the heading we have
Quatf(AxisAngle<float>(0, 0, _states.yaw)).copyTo(setpoint.q_d);
setpoint.yaw_sp_move_rate = 0.f;
// prevent any position control integrator windup
_control.resetIntegral();
}
}

void
MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states,
const bool force, bool warn)
Expand Down