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.

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

New naming convention of variables linked to a parameter #11686

Merged
merged 18 commits into from
Apr 3, 2019
Merged
Changes from 1 commit
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
1a75635
Parameter update - Add parameter_update.py script used to rename the …
bresch Mar 26, 2019
bbc778a
Parameter update - add -tcpp flag in rg to check C++ files only
bresch Mar 26, 2019
3e1af5c
Parameter update - Rename variables in drivers
bresch Mar 20, 2019
788d7b2
Parameter update - Rename variables in lib
bresch Mar 20, 2019
15a5b5d
Parameter update - Rename variables in templates
bresch Mar 20, 2019
13df179
Parameter update - Rename variables in modules/commander
bresch Mar 20, 2019
519faef
Parameter update - Rename variables in modules/events
bresch Mar 20, 2019
dfa21a4
Parameter update - Rename variables in modules/mc_pos_control
bresch Mar 20, 2019
a31c5aa
Parameter update - Rename variables in modules/fw_pos_control
bresch Mar 25, 2019
c5dbaee
Parameter update - Rename variables in modules/load_mon
bresch Mar 25, 2019
9322931
Parameter update - Rename variables in modules/fw_pos_control
bresch Mar 25, 2019
7023d12
Parameter update - Rename variables in modules/simulator
bresch Mar 25, 2019
613129b
Parameter update - Rename variables in modules/wind_estimator
bresch Mar 25, 2019
b378c50
Parameter update - Rename variables in modules/mc_att_control
bresch Mar 25, 2019
7dea8a1
Parameter update - Rename variables in modules/navigator
bresch Mar 25, 2019
33d2f77
Parameter update - Rename variables in modules/mavlink
bresch Mar 25, 2019
318eb65
Parameter update - Rename variables in modules/ekf2
bresch Mar 26, 2019
8786faa
Add new line at the end of FlightTaskFailsafe.cpp and FlightTaskAutoL…
bresch Mar 26, 2019
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Parameter update - Rename variables in modules/mc_pos_control
using parameter_update.py script
  • Loading branch information
bresch committed Apr 3, 2019
commit dfa21a42221c110c9738b1d93baad749a215b1fc
59 changes: 30 additions & 29 deletions src/modules/mc_pos_control/PositionControl.cpp
Original file line number Diff line number Diff line change
@@ -91,11 +91,11 @@ void PositionControl::generateThrustYawSetpoint(const float dt)
// Limit the thrust vector.
float thr_mag = _thr_sp.length();

if (thr_mag > MPC_THR_MAX.get()) {
_thr_sp = _thr_sp.normalized() * MPC_THR_MAX.get();
if (thr_mag > _param_mpc_thr_max.get()) {
_thr_sp = _thr_sp.normalized() * _param_mpc_thr_max.get();

} else if (thr_mag < MPC_MANTHR_MIN.get() && thr_mag > FLT_EPSILON) {
_thr_sp = _thr_sp.normalized() * MPC_MANTHR_MIN.get();
} else if (thr_mag < _param_mpc_manthr_min.get() && thr_mag > FLT_EPSILON) {
_thr_sp = _thr_sp.normalized() * _param_mpc_manthr_min.get();
}

// Just set the set-points equal to the current vehicle state.
@@ -204,7 +204,7 @@ bool PositionControl::_interfaceMapping()
_thr_sp(0) = _thr_sp(1) = 0.0f;
// 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);
_thr_sp(2) = -(_param_mpc_thr_min.get() + (_param_mpc_thr_hover.get() - _param_mpc_thr_min.get()) * 0.7f);
// position and velocity control-loop is currently unused (flag only for logging purpose)
_setCtrlFlag(false);
}
@@ -215,13 +215,14 @@ bool PositionControl::_interfaceMapping()
void PositionControl::_positionController()
{
// P-position controller
const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(MPC_XY_P.get(), MPC_XY_P.get(), MPC_Z_P.get()));
const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(),
_param_mpc_z_p.get()));
_vel_sp = vel_sp_position + _vel_sp;

// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
Vector2f(_vel_sp - vel_sp_position), MPC_XY_VEL_MAX.get());
Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get());
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
// Constrain velocity in z-direction.
@@ -257,22 +258,22 @@ void PositionControl::_velocityController(const float &dt)
const Vector3f vel_err = _vel_sp - _vel;

// Consider thrust in D-direction.
float thrust_desired_D = MPC_Z_VEL_P.get() * vel_err(2) + MPC_Z_VEL_D.get() * _vel_dot(2) + _thr_int(
2) - MPC_THR_HOVER.get();
float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) + _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
2) - _param_mpc_thr_hover.get();

// The Thrust limits are negated and swapped due to NED-frame.
float uMax = -MPC_THR_MIN.get();
float uMin = -MPC_THR_MAX.get();
float uMax = -_param_mpc_thr_min.get();
float uMin = -_param_mpc_thr_max.get();

// Apply Anti-Windup in D-direction.
bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
(thrust_desired_D <= uMin && vel_err(2) <= 0.0f);

if (!stop_integral_D) {
_thr_int(2) += vel_err(2) * MPC_Z_VEL_I.get() * dt;
_thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;

// limit thrust integral
_thr_int(2) = math::min(fabsf(_thr_int(2)), MPC_THR_MAX.get()) * math::sign(_thr_int(2));
_thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2));
}

// Saturate thrust setpoint in D-direction.
@@ -288,12 +289,12 @@ void PositionControl::_velocityController(const float &dt)
} else {
// PID-velocity controller for NE-direction.
Vector2f thrust_desired_NE;
thrust_desired_NE(0) = MPC_XY_VEL_P.get() * vel_err(0) + MPC_XY_VEL_D.get() * _vel_dot(0) + _thr_int(0);
thrust_desired_NE(1) = MPC_XY_VEL_P.get() * vel_err(1) + MPC_XY_VEL_D.get() * _vel_dot(1) + _thr_int(1);
thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0);
thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1);

// Get maximum allowed thrust in NE based on tilt and excess thrust.
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
float thrust_max_NE = sqrtf(MPC_THR_MAX.get() * MPC_THR_MAX.get() - _thr_sp(2) * _thr_sp(2));
float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);

// Saturate thrust in NE-direction.
@@ -308,15 +309,15 @@ void PositionControl::_velocityController(const float &dt)

// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
float arw_gain = 2.f / MPC_XY_VEL_P.get();
float arw_gain = 2.f / _param_mpc_xy_vel_p.get();

Vector2f vel_err_lim;
vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;

// Update integral
_thr_int(0) += MPC_XY_VEL_I.get() * vel_err_lim(0) * dt;
_thr_int(1) += MPC_XY_VEL_I.get() * vel_err_lim(1) * dt;
_thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
_thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
}
}

@@ -328,20 +329,20 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
// constraints, then just use global constraints for the limits.

if (!PX4_ISFINITE(constraints.tilt)
|| !(constraints.tilt < math::max(MPC_TILTMAX_AIR_rad.get(), MPC_MAN_TILT_MAX_rad.get()))) {
_constraints.tilt = math::max(MPC_TILTMAX_AIR_rad.get(), MPC_MAN_TILT_MAX_rad.get());
|| !(constraints.tilt < math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()))) {
_constraints.tilt = math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get());
}

if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < MPC_Z_VEL_MAX_UP.get())) {
_constraints.speed_up = MPC_Z_VEL_MAX_UP.get();
if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) {
_constraints.speed_up = _param_mpc_z_vel_max_up.get();
}

if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < MPC_Z_VEL_MAX_DN.get())) {
_constraints.speed_down = MPC_Z_VEL_MAX_DN.get();
if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _param_mpc_z_vel_max_dn.get())) {
_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
}

if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < MPC_XY_VEL_MAX.get())) {
_constraints.speed_xy = MPC_XY_VEL_MAX.get();
if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _param_mpc_xy_vel_max.get())) {
_constraints.speed_xy = _param_mpc_xy_vel_max.get();
}
}

@@ -350,6 +351,6 @@ void PositionControl::updateParams()
ModuleParams::updateParams();

// Tilt needs to be in radians
MPC_TILTMAX_AIR_rad.set(math::radians(MPC_TILTMAX_AIR_rad.get()));
MPC_MAN_TILT_MAX_rad.set(math::radians(MPC_MAN_TILT_MAX_rad.get()));
_param_mpc_tiltmax_air.set(math::radians(_param_mpc_tiltmax_air.get()));
_param_mpc_man_tilt_max.set(math::radians(_param_mpc_man_tilt_max.get()));
}
35 changes: 18 additions & 17 deletions src/modules/mc_pos_control/PositionControl.hpp
Original file line number Diff line number Diff line change
@@ -224,23 +224,24 @@ class PositionControl : public ModuleParams
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,
(ParamFloat<px4::params::MPC_THR_HOVER>) MPC_THR_HOVER,
(ParamFloat<px4::params::MPC_THR_MIN>) MPC_THR_MIN,
(ParamFloat<px4::params::MPC_MANTHR_MIN>) MPC_MANTHR_MIN,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) MPC_Z_VEL_MAX_DN,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) MPC_Z_VEL_MAX_UP,
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>)
MPC_TILTMAX_AIR_rad, // maximum tilt for any position controlled mode in radians
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX_rad, // maximum til for stabilized/altitude mode in radians
(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P,
(ParamFloat<px4::params::MPC_Z_VEL_P>) MPC_Z_VEL_P,
(ParamFloat<px4::params::MPC_Z_VEL_I>) MPC_Z_VEL_I,
(ParamFloat<px4::params::MPC_Z_VEL_D>) MPC_Z_VEL_D,
(ParamFloat<px4::params::MPC_XY_P>) MPC_XY_P,
(ParamFloat<px4::params::MPC_XY_VEL_P>) MPC_XY_VEL_P,
(ParamFloat<px4::params::MPC_XY_VEL_I>) MPC_XY_VEL_I,
(ParamFloat<px4::params::MPC_XY_VEL_D>) MPC_XY_VEL_D
_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in radians
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>)
_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in radians
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,
(ParamFloat<px4::params::MPC_Z_VEL_D>) _param_mpc_z_vel_d,
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
(ParamFloat<px4::params::MPC_XY_VEL_P>) _param_mpc_xy_vel_p,
(ParamFloat<px4::params::MPC_XY_VEL_I>) _param_mpc_xy_vel_i,
(ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d
)
};
58 changes: 29 additions & 29 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
Original file line number Diff line number Diff line change
@@ -150,18 +150,18 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};

DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _vel_max_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _vel_max_down,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _land_speed,
(ParamFloat<px4::params::MPC_TKO_SPEED>) _tko_speed,
(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) MPC_POS_MODE,
(ParamInt<px4::params::MPC_AUTO_MODE>) MPC_AUTO_MODE,
(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) MPC_SPOOLUP_TIME, /**< time to let motors spool up after arming */
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID, /**< enable obstacle avoidance */
(ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, /**< enable obstacle avoidance */
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd /**< maximum tilt for landing and smooth takeoff */
);

control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -401,11 +401,11 @@ MulticopterPositionControl::parameters_update(bool force)
_flight_tasks.handleParameterUpdate();

// initialize vectors from params and enforce constraints
_tko_speed.set(math::min(_tko_speed.get(), _vel_max_up.get()));
_land_speed.set(math::min(_land_speed.get(), _vel_max_down.get()));
_param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
_param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));

// set trigger time for takeoff delay
_spoolup_time_hysteresis.set_hysteresis_time_from(false, (int)(MPC_SPOOLUP_TIME.get() * (float)1_s));
_spoolup_time_hysteresis.set_hysteresis_time_from(false, (int)(_param_mpc_spoolup_time.get() * (float)1_s));

if (_wv_controller != nullptr) {
_wv_controller->update_parameters();
@@ -534,7 +534,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
_vel_y_deriv.update(0.0f);
}

if (MPC_ALT_MODE.get() && _local_pos.dist_bottom_valid && PX4_ISFINITE(_local_pos.dist_bottom_rate)) {
if (_param_mpc_alt_mode.get() && _local_pos.dist_bottom_valid && PX4_ISFINITE(_local_pos.dist_bottom_rate)) {
// terrain following
_states.velocity(2) = -_local_pos.dist_bottom_rate;
_states.acceleration(2) = _vel_z_deriv.update(-_states.velocity(2));
@@ -546,7 +546,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {
// A change in velocity is demanded. Set velocity to the derivative of position
// because it has less bias but blend it in across the landing speed range
float weighting = fminf(fabsf(vel_sp_z) / _land_speed.get(), 1.0f);
float weighting = fminf(fabsf(vel_sp_z) / _param_mpc_land_speed.get(), 1.0f);
_states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting);
}

@@ -718,7 +718,7 @@ MulticopterPositionControl::run()
}

// limit tilt during smooth takeoff when still close to ground
constraints.tilt = math::radians(MPC_TILTMAX_LND.get());
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
}
}

@@ -921,7 +921,7 @@ MulticopterPositionControl::start_flight_task()
// Auto related maneuvers
should_disable_task = false;
int error = 0;
switch (MPC_AUTO_MODE.get()) {
switch (_param_mpc_auto_mode.get()) {
case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
break;
@@ -949,7 +949,7 @@ MulticopterPositionControl::start_flight_task()
should_disable_task = false;
int error = 0;

switch (MPC_POS_MODE.get()) {
switch (_param_mpc_pos_mode.get()) {
case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth);
break;
@@ -985,7 +985,7 @@ MulticopterPositionControl::start_flight_task()
should_disable_task = false;
int error = 0;

switch (MPC_POS_MODE.get()) {
switch (_param_mpc_pos_mode.get()) {
case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmooth);
break;
@@ -1072,23 +1072,23 @@ MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float

// If there is a valid position setpoint, then set the desired speed to the takeoff speed.
if (!_smooth_velocity_takeoff) {
desired_tko_speed = _tko_speed.get();
desired_tko_speed = _param_mpc_tko_speed.get();
}

// Ramp up takeoff speed.
if (_takeoff_ramp_time.get() > _dt) {
_takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get();
if (_param_mpc_tko_ramp_t.get() > _dt) {
_takeoff_speed += desired_tko_speed * _dt / _param_mpc_tko_ramp_t.get();

} else {
// No ramp, directly go to desired takeoff speed
_takeoff_speed = desired_tko_speed;
}

_takeoff_speed = math::min(_takeoff_speed, _tko_speed.get());
_takeoff_speed = math::min(_takeoff_speed, _param_mpc_tko_speed.get());

// Smooth takeoff is achieved once desired altitude/velocity setpoint is reached.
if (!_smooth_velocity_takeoff) {
_in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - MPC_LAND_ALT2.get());
_in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get());

} else {
// Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
@@ -1127,7 +1127,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
if (PX4_ISFINITE(_states.velocity(2))) {
// We have a valid velocity in D-direction.
// descend downwards with landspeed.
setpoint.vz = _land_speed.get();
setpoint.vz = _param_mpc_land_speed.get();
setpoint.thrust[0] = setpoint.thrust[1] = 0.0f;
if (warn) {
PX4_WARN("Failsafe: Descend with land-speed.");
@@ -1147,7 +1147,7 @@ void
MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states,
vehicle_local_position_setpoint_s &setpoint)
{
if (COM_OBS_AVOID.get()) {
if (_param_com_obs_avoid.get()) {
_traj_wp_avoidance_desired = _flight_tasks.getAvoidanceWaypoint();
_traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
_traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
@@ -1199,7 +1199,7 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
bool
MulticopterPositionControl::use_obstacle_avoidance()
{
if (COM_OBS_AVOID.get()) {
if (_param_com_obs_avoid.get()) {
const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
const bool avoidance_point_valid = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
const bool in_mission = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;