diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index cc18232ac7c0eb228d9bb68c4baa3f047f946fb7..a824fe9aa617c3bc4b937329ad123fe8ba5dba07 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -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()));
}
diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp
index 8504ccb6dac3b230486bcccf569a9cb32205226a..89d0b21b59609ce05d31ce86a032a78d3ceb4476 100644
--- a/src/modules/mc_pos_control/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl.hpp
@@ -224,23 +224,24 @@ private:
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
)
};
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 b78321d34f019fb8c09f4b3abde7a88dea1fa695..505628afe3ae415f5b2d666b8cc6f03113ca9758 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -150,18 +150,18 @@ private:
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;