diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp
index 76a32e1c6f0a2d01cea6912aeb2d38349486b239..39c044a2591a4eb4d0f0007cf39cc3e0bc77cd07 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.cpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp
@@ -122,8 +122,10 @@ void CollisionPrevention::update_range_constraints()
float angle = math::radians((float)i * obstacle_distance.increment);
//calculate normalized velocity reductions
- float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * cos(angle);
- float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * sin(angle);
+ float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - _param_mpc_col_prev_d.get()) * cos(
+ angle);
+ float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - _param_mpc_col_prev_d.get()) * sin(
+ angle);
if (vel_lim_x > 0 && vel_lim_x > _move_constraints_x_normalized(1)) { _move_constraints_x_normalized(1) = vel_lim_x; }
diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp
index 36b41ef7f096a6c1220937e69007daa443acf471..7689fdd845b4dd72632bc8e18fc5e601668a3539 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.hpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp
@@ -66,7 +66,7 @@ public:
*/
bool initializeSubscriptions(SubscriptionArray &subscription_array);
- bool is_active() { return MPC_COL_PREV_D.get() > 0; }
+ bool is_active() { return _param_mpc_col_prev_d.get() > 0; }
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed);
@@ -90,7 +90,7 @@ private:
matrix::Vector2f _move_constraints_y;
DEFINE_PARAMETERS(
- (ParamFloat<px4::params::MPC_COL_PREV_D>) MPC_COL_PREV_D /**< collision prevention keep minimum distance */
+ (ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d /**< collision prevention keep minimum distance */
)
void update();
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index 5989bd4d377012955a45c1bdd23e017668487903..8275b70c2a9052ca71400dfc7b596cc1c2d10329 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -231,7 +231,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_mission_gear = _sub_triplet_setpoint->get().current.landing_gear;
}
- if (COM_OBS_AVOID.get() && _sub_vehicle_status->get().is_rotary_wing) {
+ if (_param_com_obs_avoid.get() && _sub_vehicle_status->get().is_rotary_wing) {
_checkAvoidanceProgress();
}
@@ -243,7 +243,7 @@ void FlightTaskAuto::_set_heading_from_mode()
Vector2f v; // Vector that points towards desired location
- switch (MPC_YAW_MODE.get()) {
+ switch (_param_mpc_yaw_mode.get()) {
case 0: // Heading points towards the current waypoint.
v = Vector2f(_target) - Vector2f(_position);
@@ -337,7 +337,7 @@ void FlightTaskAuto::_checkAvoidanceProgress()
const float pos_to_target_z = fabsf(_triplet_target(2) - _position(2));
- if (pos_to_target.length() < _target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
+ if (pos_to_target.length() < _target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()) {
// vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
}
@@ -410,8 +410,8 @@ void FlightTaskAuto::_setDefaultConstraints()
FlightTask::_setDefaultConstraints();
// only adjust limits if the new limit is lower
- if (_constraints.speed_xy >= MPC_XY_CRUISE.get()) {
- _constraints.speed_xy = MPC_XY_CRUISE.get();
+ if (_constraints.speed_xy >= _param_mpc_xy_cruise.get()) {
+ _constraints.speed_xy = _param_mpc_xy_cruise.get();
}
}
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index cb1eeb3b242c1e6fe7d6f46b6c3ccee4b6ea360f..d2101037c14049433b267316a459c8132456be6e 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -105,11 +105,12 @@ protected:
int _mission_gear = landing_gear_s::GEAR_KEEP;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
- (ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE,
- (ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line
- (ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD, //vertical acceptance radius at which waypoints are updated
- (ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE, // defines how heading is executed,
- (ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID // obstacle avoidance active
+ (ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
+ (ParamFloat<px4::params::MPC_CRUISE_90>) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line
+ (ParamFloat<px4::params::NAV_MC_ALT_RAD>)
+ _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
+ (ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
+ (ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid // obstacle avoidance active
);
private:
diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
index 7ff14c8e02b8f5c445a441eb89e594de2b97521c..31c60161f284f989efd43495af21804d9902e761 100644
--- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
@@ -66,7 +66,7 @@ void FlightTaskAutoLine::_setSpeedAtTarget()
Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+ 1.0f;
- _speed_at_target = math::expontialFromLimits(angle, 0.0f, MPC_CRUISE_90.get(), _mc_cruise_speed);
+ _speed_at_target = math::expontialFromLimits(angle, 0.0f, _param_mpc_cruise_90.get(), _mc_cruise_speed);
}
}
@@ -171,7 +171,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
}
// If yaw offset is large, only accelerate with 0.5 m/s^2.
- float acc_max = (fabsf(yaw_diff) > math::radians(MIS_YAW_ERR.get())) ? 0.5f : MPC_ACC_HOR.get();
+ float acc_max = (fabsf(yaw_diff) > math::radians(_param_mis_yaw_err.get())) ? 0.5f : _param_mpc_acc_hor.get();
if (acc_track > acc_max) {
// accelerate towards target
@@ -210,8 +210,8 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
// limit vertical downwards speed (positive z) close to ground
// for now we use the altitude above home and assume that we want to land at same height as we took off
float vel_limit = math::gradual(_alt_above_ground,
- MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
- MPC_LAND_SPEED.get(), _constraints.speed_down);
+ _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+ _param_mpc_land_speed.get(), _constraints.speed_down);
// Speed at threshold is by default maximum speed. Threshold defines
// the point in z at which vehicle slows down to reach target altitude.
@@ -244,7 +244,7 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
// we want to accelerate
const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
- const float acc_max = (flying_upward) ? (MPC_ACC_UP_MAX.get() * 0.5f) : (MPC_ACC_DOWN_MAX.get() * 0.5f);
+ const float acc_max = (flying_upward) ? (_param_mpc_acc_up_max.get() * 0.5f) : (_param_mpc_acc_down_max.get() * 0.5f);
if (acc > acc_max) {
speed_sp = acc_max * _deltatime + fabsf(_velocity_setpoint(2));
diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
index b6406a6ab4ab61812d54eca755a8d4a7004af387..e73225e6d87c30969826a69907185e503cc76873 100644
--- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
+++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
@@ -51,10 +51,10 @@ public:
protected:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
- (ParamFloat<px4::params::MIS_YAW_ERR>) MIS_YAW_ERR, // yaw-error threshold
- (ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR, // acceleration in flight
- (ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
- (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX
+ (ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
+ (ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
+ (ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+ (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
);
void _generateSetpoints() override; /**< Generate setpoints along line. */
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index 4a4b496d3d3dcfe5b1e934d4faa91e5ebd5422a1..f46dd955b0ba5fc96cd5a2aa3dab81ee6e287ebb 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -67,7 +67,7 @@ void FlightTaskAutoLineSmoothVel::_setDefaultConstraints()
{
FlightTaskAuto::_setDefaultConstraints();
- _constraints.speed_xy = MPC_XY_VEL_MAX.get(); // TODO : Should be computed using heading
+ _constraints.speed_xy = _param_mpc_xy_vel_max.get(); // TODO : Should be computed using heading
}
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
@@ -166,8 +166,9 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
- float speed_sp_track = Vector2f(pos_traj_to_dest).length() * MPC_XY_TRAJ_P.get();
+ float speed_sp_track = Vector2f(pos_traj_to_dest).length() * _param_mpc_xy_traj_p.get();
speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
+
Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
for (int i = 0; i < 2; i++) {
@@ -180,14 +181,14 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
}
_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
- MPC_XY_TRAJ_P.get(); // Along-track setpoint + cross-track P controller
+ _param_mpc_xy_traj_p.get(); // Along-track setpoint + cross-track P controller
}
}
if (PX4_ISFINITE(_position_setpoint(2))) {
const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
- MPC_Z_TRAJ_P.get(); // Generate a velocity target for the trajectory using a simple P loop
+ _param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop
// If available, constrain the velocity using _velocity_setpoint(.)
if (PX4_ISFINITE(_velocity_setpoint(2))) {
@@ -203,21 +204,21 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
{
// Update the constraints of the trajectories
- _trajectory[0].setMaxAccel(MPC_ACC_HOR_MAX.get()); // TODO : Should be computed using heading
- _trajectory[1].setMaxAccel(MPC_ACC_HOR_MAX.get());
+ _trajectory[0].setMaxAccel(_param_mpc_acc_hor_max.get()); // TODO : Should be computed using heading
+ _trajectory[1].setMaxAccel(_param_mpc_acc_hor_max.get());
_trajectory[0].setMaxVel(_constraints.speed_xy);
_trajectory[1].setMaxVel(_constraints.speed_xy);
- _trajectory[0].setMaxJerk(MPC_JERK_MIN.get()); // TODO : Should be computed using heading
- _trajectory[1].setMaxJerk(MPC_JERK_MIN.get());
- _trajectory[2].setMaxJerk(MPC_JERK_MIN.get());
+ _trajectory[0].setMaxJerk(_param_mpc_jerk_min.get()); // TODO : Should be computed using heading
+ _trajectory[1].setMaxJerk(_param_mpc_jerk_min.get());
+ _trajectory[2].setMaxJerk(_param_mpc_jerk_min.get());
if (_velocity_setpoint(2) < 0.f) { // up
- _trajectory[2].setMaxAccel(MPC_ACC_UP_MAX.get());
- _trajectory[2].setMaxVel(MPC_Z_VEL_MAX_UP.get());
+ _trajectory[2].setMaxAccel(_param_mpc_acc_up_max.get());
+ _trajectory[2].setMaxVel(_param_mpc_z_vel_max_up.get());
} else { // down
- _trajectory[2].setMaxAccel(MPC_ACC_DOWN_MAX.get());
- _trajectory[2].setMaxVel(MPC_Z_VEL_MAX_DN.get());
+ _trajectory[2].setMaxAccel(_param_mpc_acc_down_max.get());
+ _trajectory[2].setMaxVel(_param_mpc_z_vel_max_dn.get());
}
}
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
index 17c2b6e617645576255c462e07b2dd0cfff20c21..f10ffab7a7446c1786409aa67fee6b3ff74b731f 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
@@ -55,14 +55,14 @@ public:
protected:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper2,
- (ParamFloat<px4::params::MIS_YAW_ERR>) MIS_YAW_ERR, // yaw-error threshold
- (ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR, // acceleration in flight
- (ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
- (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX,
- (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX,
- (ParamFloat<px4::params::MPC_JERK_MIN>) MPC_JERK_MIN,
- (ParamFloat<px4::params::MPC_XY_TRAJ_P>) MPC_XY_TRAJ_P,
- (ParamFloat<px4::params::MPC_Z_TRAJ_P>) MPC_Z_TRAJ_P
+ (ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
+ (ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
+ (ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+ (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
+ (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
+ (ParamFloat<px4::params::MPC_JERK_MIN>) _param_mpc_jerk_min,
+ (ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
+ (ParamFloat<px4::params::MPC_Z_TRAJ_P>) _param_mpc_z_traj_p
);
void _generateSetpoints() override; /**< Generate setpoints along line. */
diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
index e447ff0ba714f724af425c8225c17c0d1a30c51d..4dcbca40c03dd2dca3ed3709aedbf4879981cf39 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
@@ -117,10 +117,10 @@ void FlightTaskAutoMapper::_generateLandSetpoints()
{
// Keep xy-position and go down with landspeed
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
- _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, MPC_LAND_SPEED.get()));
+ _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _param_mpc_land_speed.get()));
// set constraints
- _constraints.tilt = MPC_TILTMAX_LND.get();
- _constraints.speed_down = MPC_LAND_SPEED.get();
+ _constraints.tilt = _param_mpc_tiltmax_lnd.get();
+ _constraints.speed_down = _param_mpc_land_speed.get();
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
@@ -131,8 +131,8 @@ void FlightTaskAutoMapper::_generateTakeoffSetpoints()
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
// limit vertical speed during takeoff
- _constraints.speed_up = math::gradual(_alt_above_ground, MPC_LAND_ALT2.get(),
- MPC_LAND_ALT1.get(), MPC_TKO_SPEED.get(), _constraints.speed_up);
+ _constraints.speed_up = math::gradual(_alt_above_ground, _param_mpc_land_alt2.get(),
+ _param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up);
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
@@ -166,7 +166,7 @@ void FlightTaskAutoMapper::updateParams()
FlightTaskAuto::updateParams();
// make sure that alt1 is above alt2
- MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get()));
+ _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
}
bool FlightTaskAutoMapper::_highEnoughForLandingGear()
diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
index efff0f6db0b650022515cb140dd9f3d624fffeea..3811b3e7dc68a1c734ea48ed1f8a729d1c0e7202 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
@@ -55,11 +55,13 @@ protected:
float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
- (ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
- (ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND,
- (ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed
- (ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed
- (ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED
+ (ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+ (ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
+ (ParamFloat<px4::params::MPC_LAND_ALT1>)
+ _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
+ (ParamFloat<px4::params::MPC_LAND_ALT2>)
+ _param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
+ (ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed
);
virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
index 83aff05f51cfaf4eedbbac4fe19a454bf14599c2..b3a1c404a05de66e651f2cfc7cbb8e279702755d 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
@@ -124,11 +124,12 @@ void FlightTaskAutoMapper2::_prepareLandSetpoints()
{
// Keep xy-position and go down with landspeed
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
- const float speed_lnd = (_alt_above_ground > MPC_LAND_ALT1.get()) ? _constraints.speed_down : MPC_LAND_SPEED.get();
+ const float speed_lnd = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_down :
+ _param_mpc_land_speed.get();
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd));
// set constraints
- _constraints.tilt = MPC_TILTMAX_LND.get();
+ _constraints.tilt = _param_mpc_tiltmax_lnd.get();
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
@@ -136,7 +137,8 @@ void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
{
// Takeoff is completely defined by target position
_position_setpoint = _target;
- const float speed_tko = (_alt_above_ground > MPC_LAND_ALT1.get()) ? _constraints.speed_up : MPC_TKO_SPEED.get();
+ const float speed_tko = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
+ _param_mpc_tko_speed.get();
_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
@@ -178,7 +180,7 @@ void FlightTaskAutoMapper2::updateParams()
FlightTaskAuto::updateParams();
// make sure that alt1 is above alt2
- MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get()));
+ _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
}
bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
index 8bd236ba6f8599a431d2a08be34355d49f1b5a77..226a54427f80f89346c1bb27c6abdad89752133f 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
@@ -55,11 +55,13 @@ protected:
float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
- (ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
- (ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND,
- (ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed
- (ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed
- (ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED
+ (ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+ (ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
+ (ParamFloat<px4::params::MPC_LAND_ALT1>)
+ _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
+ (ParamFloat<px4::params::MPC_LAND_ALT2>)
+ _param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
+ (ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed
);
virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
diff --git a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp
index 69d2e083915e737715f941728b5d3d796da65bf7..db3604e2dca71e6f6af41af5f7d560e04bfaec5b 100644
--- a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp
+++ b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp
@@ -41,7 +41,7 @@ bool FlightTaskFailsafe::activate()
bool ret = FlightTask::activate();
_position_setpoint = _position;
_velocity_setpoint.zero();
- _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -MPC_THR_HOVER.get() * 0.6f);
+ _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f);
_yaw_setpoint = _yaw;
_yawspeed_setpoint = 0.0f;
return ret;
@@ -67,7 +67,7 @@ bool FlightTaskFailsafe::update()
} else if (PX4_ISFINITE(_velocity(2))) {
// land with landspeed
- _velocity_setpoint(2) = MPC_LAND_SPEED.get();
+ _velocity_setpoint(2) = _param_mpc_land_speed.get();
_position_setpoint(2) = NAN;
_thrust_setpoint(2) = NAN;
}
diff --git a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp
index acd6eb598c5837654301c4207c4cefbab26ce796..4264b9b1205301698cb7453f8e395437d8ff87bc 100644
--- a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp
+++ b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp
@@ -51,7 +51,8 @@ public:
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
- (ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
- (ParamFloat<px4::params::MPC_THR_HOVER>) MPC_THR_HOVER /**< throttle value at which vehicle is at hover equilibrium */
+ (ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+ (ParamFloat<px4::params::MPC_THR_HOVER>)
+ _param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */
)
};
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
index 2a1c81a87bc9e0be307b7749ac05fddd8f352b31..f94ed879cae658e80151aa6fbd023dea3a0d55a2 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
@@ -146,10 +146,10 @@ void FlightTask::_evaluateVehicleLocalPosition()
void FlightTask::_setDefaultConstraints()
{
- _constraints.speed_xy = MPC_XY_VEL_MAX.get();
- _constraints.speed_up = MPC_Z_VEL_MAX_UP.get();
- _constraints.speed_down = MPC_Z_VEL_MAX_DN.get();
- _constraints.tilt = math::radians(MPC_TILTMAX_AIR.get());
+ _constraints.speed_xy = _param_mpc_xy_vel_max.get();
+ _constraints.speed_up = _param_mpc_z_vel_max_up.get();
+ _constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+ _constraints.tilt = math::radians(_param_mpc_tiltmax_air.get());
_constraints.min_distance_to_ground = NAN;
_constraints.max_distance_to_ground = NAN;
}
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
index 04eec386d5c8fb599ea7774593a94914ea9b59a8..7e15cd9d32750cded83b9bd9f9c500b956853cf9 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -240,9 +240,9 @@ protected:
vehicle_trajectory_waypoint_s _desired_waypoint{};
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
- (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_TILTMAX_AIR>) MPC_TILTMAX_AIR
+ (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>) _param_mpc_tiltmax_air
)
};
diff --git a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp
index ed26d4ded9fd06436ff788f4710c1c5029d4fc08..f6d5c2f994bb88b2e4e43f6fbf333af5011239cd 100644
--- a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp
+++ b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp
@@ -69,7 +69,7 @@ bool FlightTaskManual::updateInitialize()
bool FlightTaskManual::_evaluateSticks()
{
- hrt_abstime rc_timeout = (COM_RC_LOSS_T.get() * 1.5f) * 1_s;
+ hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
/* Sticks are rescaled linearly and exponentially to [-1,1] */
if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < rc_timeout) {
@@ -81,10 +81,10 @@ bool FlightTaskManual::_evaluateSticks()
_sticks(3) = _sub_manual_control_setpoint->get().r; /* "yaw" [-1,1] */
/* Exponential scale */
- _sticks_expo(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _stick_dz.get());
- _sticks_expo(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _stick_dz.get());
- _sticks_expo(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _stick_dz.get());
- _sticks_expo(3) = math::expo_deadzone(_sticks(3), _yaw_expo.get(), _stick_dz.get());
+ _sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
+ _sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
+ _sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
+ _sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
// Only switch the landing gear up if the user switched from gear down to gear up.
// If the user had the switch in the gear up position and took off ignore it
diff --git a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp
index a85b91efa55069660e3783e47dbce5ffbaf49c4b..7fb49a75bbf0b9f6485cc3400ca496c379b42b4c 100644
--- a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp
+++ b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp
@@ -61,7 +61,7 @@ protected:
matrix::Vector<float, 4> _sticks_expo; /**< modified manual sticks using expo function*/
int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; /**< old switch state*/
- float stickDeadzone() const { return _stick_dz.get(); }
+ float stickDeadzone() const { return _param_mpc_hold_dz.get(); }
private:
@@ -71,13 +71,13 @@ private:
uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
- (ParamFloat<px4::params::MPC_HOLD_DZ>) _stick_dz, /**< 0-deadzone around the center for the sticks */
+ (ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */
(ParamFloat<px4::params::MPC_XY_MAN_EXPO>)
- _xy_vel_man_expo, /**< ratio of exponential curve for stick input in xy direction */
+ _param_mpc_xy_man_expo, /**< ratio of exponential curve for stick input in xy direction */
(ParamFloat<px4::params::MPC_Z_MAN_EXPO>)
- _z_vel_man_expo, /**< ratio of exponential curve for stick input in z direction */
+ _param_mpc_z_man_expo, /**< ratio of exponential curve for stick input in z direction */
(ParamFloat<px4::params::MPC_YAW_EXPO>)
- _yaw_expo, /**< ratio of exponential curve for stick input in yaw for modes except acro */
- (ParamFloat<px4::params::COM_RC_LOSS_T>) COM_RC_LOSS_T /**< time at which commander considers RC lost */
+ _param_mpc_yaw_expo, /**< ratio of exponential curve for stick input in yaw for modes except acro */
+ (ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t /**< time at which commander considers RC lost */
)
};
diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index e8eb46b5e7ef20809e55b177aa98ddb7fafd8e6c..deae1751d5b57705b699830a3d761deffd622742 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -71,7 +71,7 @@ bool FlightTaskManualAltitude::activate()
_velocity_setpoint(2) = 0.0f;
_setDefaultConstraints();
- _constraints.tilt = math::radians(MPC_MAN_TILT_MAX.get());
+ _constraints.tilt = math::radians(_param_mpc_man_tilt_max.get());
if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) {
_constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
@@ -96,7 +96,7 @@ bool FlightTaskManualAltitude::activate()
void FlightTaskManualAltitude::_scaleSticks()
{
// Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed
- _yawspeed_setpoint = _sticks_expo(3) * math::radians(MPC_MAN_Y_MAX.get());
+ _yawspeed_setpoint = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get());
const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
@@ -111,11 +111,11 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
const bool apply_brake = fabsf(_sticks_expo(2)) <= FLT_EPSILON;
// Check if vehicle has stopped
- const bool stopped = (MPC_HOLD_MAX_Z.get() < FLT_EPSILON || fabsf(_velocity(2)) < MPC_HOLD_MAX_Z.get());
+ const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());
// Manage transition between use of distance to ground and distance to local origin
// when terrain hold behaviour has been selected.
- if (MPC_ALT_MODE.get() == 2) {
+ if (_param_mpc_alt_mode.get() == 2) {
// Use horizontal speed as a transition criteria
float spd_xy = Vector2f(_velocity).length();
@@ -124,7 +124,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
bool stick_input = stick_xy > 0.001f;
if (_terrain_hold) {
- bool too_fast = spd_xy > MPC_HOLD_MAX_XY.get();
+ bool too_fast = spd_xy > _param_mpc_hold_max_xy.get();
if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
// Stop using distance to ground
@@ -141,7 +141,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
}
} else {
- bool not_moving = spd_xy < 0.5f * MPC_HOLD_MAX_XY.get();
+ bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get();
if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
// Start using distance to ground
@@ -157,7 +157,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
}
- if ((MPC_ALT_MODE.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) {
+ if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) {
// terrain following
_terrainFollowing(apply_brake, stopped);
// respect maximum altitude
@@ -242,7 +242,7 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
// if there is a valid maximum distance to ground, linearly increase speed limit with distance
// below the maximum, preserving control loop vertical position error gain.
if (PX4_ISFINITE(_constraints.max_distance_to_ground)) {
- _constraints.speed_up = math::constrain(MPC_Z_P.get() * (_constraints.max_distance_to_ground - _dist_to_bottom),
+ _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_constraints.max_distance_to_ground - _dist_to_bottom),
-_min_speed_down, _max_speed_up);
} else {
@@ -280,11 +280,11 @@ void FlightTaskManualAltitude::_respectGroundSlowdown()
// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
if (PX4_ISFINITE(dist_to_ground)) {
const float limit_down = math::gradual(dist_to_ground,
- MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
- MPC_LAND_SPEED.get(), _constraints.speed_down);
+ _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+ _param_mpc_land_speed.get(), _constraints.speed_down);
const float limit_up = math::gradual(dist_to_ground,
- MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
- MPC_TKO_SPEED.get(), _constraints.speed_up);
+ _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+ _param_mpc_tko_speed.get(), _constraints.speed_up);
_velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down);
}
}
diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
index 8cd75f493df1bb32995d383cfc2f84d28021b851..2993f4d38f73aada311917ea52afbabd0a3629ac 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -70,16 +70,18 @@ protected:
void _updateAltitudeLock();
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
- (ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
- (ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
- (ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
- (ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P, /**< position controller altitude propotional gain */
- (ParamFloat<px4::params::MPC_MAN_Y_MAX>) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */
- (ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX, /**< maximum tilt allowed for manual flight */
- (ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, /**< altitude at which to start downwards slowdown */
- (ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, /**< altitude below wich to land with land speed */
- (ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED, /**< desired downwards speed when approaching the ground */
- (ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED /**< desired upwards speed when still close to the ground */
+ (ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
+ (ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
+ (ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
+ (ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
+ (ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
+ (ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
+ (ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
+ (ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below wich to land with land speed */
+ (ParamFloat<px4::params::MPC_LAND_SPEED>)
+ _param_mpc_land_speed, /**< desired downwards speed when approaching the ground */
+ (ParamFloat<px4::params::MPC_TKO_SPEED>)
+ _param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
)
private:
/**
diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
index 6b2ec40431d8793869e38ca17c67d7081fbec925..e1b7e759e268c55255543ba429b3ad9efbe67aed 100644
--- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
@@ -74,11 +74,11 @@ bool FlightTaskManualPosition::activate()
// all requirements from altitude-mode still have to hold
bool ret = FlightTaskManualAltitude::activate();
- _constraints.tilt = math::radians(MPC_TILTMAX_AIR.get());
+ _constraints.tilt = math::radians(_param_mpc_tiltmax_air.get());
// set task specific constraint
- if (_constraints.speed_xy >= MPC_VEL_MANUAL.get()) {
- _constraints.speed_xy = MPC_VEL_MANUAL.get();
+ if (_constraints.speed_xy >= _param_mpc_vel_manual.get()) {
+ _constraints.speed_xy = _param_mpc_vel_manual.get();
}
_position_setpoint(0) = _position(0);
@@ -118,7 +118,7 @@ void FlightTaskManualPosition::_scaleSticks()
// raise the limit at a constant rate up to the user specified value
if (_velocity_scale < _constraints.speed_xy) {
- _velocity_scale += _deltatime * MPC_ACC_HOR_ESTM.get();
+ _velocity_scale += _deltatime * _param_mpc_acc_hor_estm.get();
} else {
_velocity_scale = _constraints.speed_xy;
@@ -146,7 +146,7 @@ void FlightTaskManualPosition::_updateXYlock()
/* If position lock is not active, position setpoint is set to NAN.*/
const float vel_xy_norm = Vector2f(_velocity).length();
const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON;
- const bool stopped = (MPC_HOLD_MAX_XY.get() < FLT_EPSILON || vel_xy_norm < MPC_HOLD_MAX_XY.get());
+ const bool stopped = (_param_mpc_hold_max_xy.get() < FLT_EPSILON || vel_xy_norm < _param_mpc_hold_max_xy.get());
if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint(0) = _position(0);
diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp
index 031c5cb1a423bbe13c46b5c2dfe6442753fc55f6..2a5feefa4b0a80719cad03e4d0f4301cce9f135b 100644
--- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp
+++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp
@@ -65,10 +65,10 @@ protected:
void _scaleSticks() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
- (ParamFloat<px4::params::MPC_VEL_MANUAL>) MPC_VEL_MANUAL,
- (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX,
- (ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
- (ParamFloat<px4::params::MPC_ACC_HOR_ESTM>) MPC_ACC_HOR_ESTM
+ (ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+ (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
+ (ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
+ (ParamFloat<px4::params::MPC_ACC_HOR_ESTM>) _param_mpc_acc_hor_estm
)
private:
float _velocity_scale{0.0f}; //scales the stick input to velocity
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 709d489773d219d88c98366d32b10ef162a7dc35..7fd9b10ce224e6b6fb1eb1fd2b7221ce8de0b5a7 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -121,21 +121,21 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
FlightTaskManualPosition::_updateSetpoints();
/* Update constraints */
- _smoothing[0].setMaxAccel(MPC_ACC_HOR_MAX.get());
- _smoothing[1].setMaxAccel(MPC_ACC_HOR_MAX.get());
+ _smoothing[0].setMaxAccel(_param_mpc_acc_hor_max.get());
+ _smoothing[1].setMaxAccel(_param_mpc_acc_hor_max.get());
_smoothing[0].setMaxVel(_constraints.speed_xy);
_smoothing[1].setMaxVel(_constraints.speed_xy);
if (_velocity_setpoint(2) < 0.f) { // up
- _smoothing[2].setMaxAccel(MPC_ACC_UP_MAX.get());
+ _smoothing[2].setMaxAccel(_param_mpc_acc_up_max.get());
_smoothing[2].setMaxVel(_constraints.speed_up);
} else { // down
- _smoothing[2].setMaxAccel(MPC_ACC_DOWN_MAX.get());
+ _smoothing[2].setMaxAccel(_param_mpc_acc_down_max.get());
_smoothing[2].setMaxVel(_constraints.speed_down);
}
- float jerk[3] = {_jerk_max.get(), _jerk_max.get(), _jerk_max.get()};
+ float jerk[3] = {_param_mpc_jerk_max.get(), _param_mpc_jerk_max.get(), _param_mpc_jerk_max.get()};
_checkEkfResetCounters();
@@ -176,11 +176,11 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
jerk[1] = 1.f;
} else {
- jerk[0] = _jerk_max.get();
- jerk[1] = _jerk_max.get();
+ jerk[0] = _param_mpc_jerk_max.get();
+ jerk[1] = _param_mpc_jerk_max.get();
}
- jerk[2] = _position_lock_z_active ? 1.f : _jerk_max.get();
+ jerk[2] = _position_lock_z_active ? 1.f : _param_mpc_jerk_max.get();
for (int i = 0; i < 3; ++i) {
_smoothing[i].setMaxJerk(jerk[i]);
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
index ac6a77508b0aceb682739db00fefabe82bb361f4..54d3701fdd54bf50bd94bf66d4aec388b7202ce7 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -57,10 +57,10 @@ protected:
virtual void _updateSetpoints() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition,
- (ParamFloat<px4::params::MPC_JERK_MIN>) _jerk_min, /**< Minimum jerk (velocity-based if > 0) */
- (ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max,
- (ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
- (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX
+ (ParamFloat<px4::params::MPC_JERK_MIN>) _param_mpc_jerk_min, /**< Minimum jerk (velocity-based if > 0) */
+ (ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
+ (ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+ (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
)
private:
diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp
index 95732e255b56b70e1d4f021a3c995ac8bdc72d9e..866f49f4e159a222d39f1c050a0ef47af38f0822 100644
--- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp
+++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp
@@ -125,7 +125,7 @@ bool FlightTaskOffboard::update()
// just do takeoff to default altitude
if (!PX4_ISFINITE(_position_lock(0))) {
_position_setpoint = _position_lock = _position;
- _position_setpoint(2) = _position_lock(2) = _position(2) - MIS_TAKEOFF_ALT.get();
+ _position_setpoint(2) = _position_lock(2) = _position(2) - _param_mis_takeoff_alt.get();
} else {
_position_setpoint = _position_lock;
@@ -144,11 +144,11 @@ bool FlightTaskOffboard::update()
if (!PX4_ISFINITE(_position_lock(0))) {
_position_setpoint = _position_lock = _position;
_position_setpoint(2) = _position_lock(2) = NAN;
- _velocity_setpoint(2) = MPC_LAND_SPEED.get();
+ _velocity_setpoint(2) = _param_mpc_land_speed.get();
} else {
_position_setpoint = _position_lock;
- _velocity_setpoint(2) = MPC_LAND_SPEED.get();
+ _velocity_setpoint(2) = _param_mpc_land_speed.get();
}
// don't have to continue
diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp
index 01fd8ff157b2b3a6668b9902b563d0f969fada55..c1a1e817916079e99d2d7b653dca534e44657c05 100644
--- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp
+++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp
@@ -58,8 +58,8 @@ private:
matrix::Vector3f _position_lock{};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
- (ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
- (ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED,
- (ParamFloat<px4::params::MIS_TAKEOFF_ALT>) MIS_TAKEOFF_ALT
+ (ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+ (ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
+ (ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt
)
};
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
index 755ba5cbcf779ae58720283bd4060b9a4150df9e..f32b8932489b20f2165bf6b749e98409f1fecdf0 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
@@ -46,8 +46,8 @@ ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const Vector2f &vel)
ModuleParams(parent), _vel_sp_prev(vel)
{
- _acc_state_dependent = _acc_xy_max.get();
- _jerk_state_dependent = _jerk_max.get();
+ _acc_state_dependent = _param_mpc_acc_hor.get();
+ _jerk_state_dependent = _param_mpc_jerk_max.get();
}
void
@@ -134,17 +134,17 @@ ManualSmoothingXY::_setStateAcceleration(const Vector2f &vel_sp, const Vector2f
} else if (intention != _intention) {
// start the brake with lowest acceleration which
// makes stopping smoother
- _acc_state_dependent = _dec_xy_min.get();
+ _acc_state_dependent = _param_mpc_dec_hor_slow.get();
// Adjust jerk based on current velocity. This ensures
// that the vehicle will stop much quicker at large speed but
// very slow at low speed.
_jerk_state_dependent = 1e6f; // default
- if (_jerk_max.get() > _jerk_min.get() && _jerk_min.get() > FLT_EPSILON) {
+ if (_param_mpc_jerk_max.get() > _param_mpc_jerk_min.get() && _param_mpc_jerk_min.get() > FLT_EPSILON) {
- _jerk_state_dependent = math::min((_jerk_max.get() - _jerk_min.get())
- / _vel_max * vel.length() + _jerk_min.get(), _jerk_max.get());
+ _jerk_state_dependent = math::min((_param_mpc_jerk_max.get() - _param_mpc_jerk_min.get())
+ / _vel_max * vel.length() + _param_mpc_jerk_min.get(), _param_mpc_jerk_max.get());
}
// User wants to brake smoothly but does NOT want the vehicle to
@@ -154,14 +154,14 @@ ManualSmoothingXY::_setStateAcceleration(const Vector2f &vel_sp, const Vector2f
}
/* limit jerk when braking to zero */
- float jerk = (_acc_hover.get() - _acc_state_dependent) / dt;
+ float jerk = (_param_mpc_acc_hor_max.get() - _acc_state_dependent) / dt;
if (jerk > _jerk_state_dependent) {
_acc_state_dependent = _jerk_state_dependent * dt
+ _acc_state_dependent;
} else {
- _acc_state_dependent = _acc_hover.get();
+ _acc_state_dependent = _param_mpc_acc_hor_max.get();
}
break;
@@ -174,19 +174,19 @@ ManualSmoothingXY::_setStateAcceleration(const Vector2f &vel_sp, const Vector2f
// Because previous setpoint is equal to current setpoint,
// slewrate will have no effect. Nonetheless, just set
// acceleration to maximum.
- _acc_state_dependent = _acc_hover.get();
+ _acc_state_dependent = _param_mpc_acc_hor_max.get();
break;
}
case Intention::acceleration: {
// Limit acceleration linearly based on velocity setpoint.
- _acc_state_dependent = (_acc_xy_max.get() - _dec_xy_min.get())
- / _vel_max * vel_sp.length() + _dec_xy_min.get();
+ _acc_state_dependent = (_param_mpc_acc_hor.get() - _param_mpc_dec_hor_slow.get())
+ / _vel_max * vel_sp.length() + _param_mpc_dec_hor_slow.get();
break;
}
case Intention::deceleration: {
- _acc_state_dependent = _dec_xy_min.get();
+ _acc_state_dependent = _param_mpc_dec_hor_slow.get();
break;
}
}
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
index c2b5bfc299cdcaf74d82030d0b4c705cc7136d1b..b62656da3428b92a10374cf27038dc111002516a 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
@@ -97,11 +97,11 @@ public:
* Overwrite methods:
* Needed if different parameter values than default required.
*/
- void overwriteHoverAcceleration(float acc) { _acc_hover.set(acc); }
- void overwriteMaxAcceleration(float acc) { _acc_xy_max.set(acc); }
- void overwriteDecelerationMin(float dec) { _dec_xy_min.set(dec); }
- void overwriteJerkMax(float jerk) { _jerk_max.set(jerk); }
- void overwriteJerkMin(float jerk) { _jerk_min.set(jerk); }
+ void overwriteHoverAcceleration(float acc) { _param_mpc_acc_hor_max.set(acc); }
+ void overwriteMaxAcceleration(float acc) { _param_mpc_acc_hor.set(acc); }
+ void overwriteDecelerationMin(float dec) { _param_mpc_dec_hor_slow.set(dec); }
+ void overwriteJerkMax(float jerk) { _param_mpc_jerk_max.set(jerk); }
+ void overwriteJerkMin(float jerk) { _param_mpc_jerk_min.set(jerk); }
private:
/**
@@ -165,11 +165,11 @@ private:
matrix::Vector2f _vel_sp_prev; /**< previous velocity setpoint */
DEFINE_PARAMETERS(
- (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _acc_hover, /**< acceleration in hover */
- (ParamFloat<px4::params::MPC_ACC_HOR>) _acc_xy_max, /**< acceleration in flight */
- (ParamFloat<px4::params::MPC_DEC_HOR_SLOW>) _dec_xy_min, /**< deceleration in flight */
- (ParamFloat<px4::params::MPC_JERK_MIN>) _jerk_min, /**< jerk min during brake */
- (ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max, /**< jerk max during brake */
- (ParamFloat<px4::params::MPC_VEL_MANUAL>) _vel_manual /**< maximum velocity in manual controlled mode */
+ (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max, /**< acceleration in hover */
+ (ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, /**< acceleration in flight */
+ (ParamFloat<px4::params::MPC_DEC_HOR_SLOW>) _param_mpc_dec_hor_slow, /**< deceleration in flight */
+ (ParamFloat<px4::params::MPC_JERK_MIN>) _param_mpc_jerk_min, /**< jerk min during brake */
+ (ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< jerk max during brake */
+ (ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual /**< maximum velocity in manual controlled mode */
)
};
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
index abf7b00776dc7df2df828511bef03983c1aac228..714ae0e498c7c153fb233981dd82a5efb538e029 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
@@ -43,8 +43,8 @@ ManualSmoothingZ::ManualSmoothingZ(ModuleParams *parent, const float &vel, const
ModuleParams(parent),
_vel(vel), _stick(stick), _vel_sp_prev(vel)
{
- _acc_state_dependent = _acc_max_up.get();
- _max_acceleration = _acc_max_up.get();
+ _acc_state_dependent = _param_mpc_acc_up_max.get();
+ _max_acceleration = _param_mpc_acc_up_max.get();
}
void
@@ -77,7 +77,7 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
// we start with lowest acceleration
- _acc_state_dependent = _acc_max_down.get();
+ _acc_state_dependent = _param_mpc_acc_down_max.get();
// reset slew-rate: this ensures that there
// is no delay present when user demands to brake
@@ -89,13 +89,13 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
case ManualIntentionZ::brake: {
// limit jerk when braking to zero
- float jerk = (_acc_max_up.get() - _acc_state_dependent) / dt;
+ float jerk = (_param_mpc_acc_up_max.get() - _acc_state_dependent) / dt;
- if (jerk > _jerk_max.get()) {
- _acc_state_dependent = _jerk_max.get() * dt + _acc_state_dependent;
+ if (jerk > _param_mpc_jerk_max.get()) {
+ _acc_state_dependent = _param_mpc_jerk_max.get() * dt + _acc_state_dependent;
} else {
- _acc_state_dependent = _acc_max_up.get();
+ _acc_state_dependent = _param_mpc_acc_up_max.get();
}
break;
@@ -103,8 +103,8 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
case ManualIntentionZ::acceleration: {
- _acc_state_dependent = (getMaxAcceleration() - _acc_max_down.get())
- * fabsf(_stick) + _acc_max_down.get();
+ _acc_state_dependent = (getMaxAcceleration() - _param_mpc_acc_down_max.get())
+ * fabsf(_stick) + _param_mpc_acc_down_max.get();
break;
}
}
@@ -117,26 +117,26 @@ ManualSmoothingZ::setMaxAcceleration()
{
if (_stick < -FLT_EPSILON) {
// accelerating upward
- _max_acceleration = _acc_max_up.get();
+ _max_acceleration = _param_mpc_acc_up_max.get();
} else if (_stick > FLT_EPSILON) {
// accelerating downward
- _max_acceleration = _acc_max_down.get();
+ _max_acceleration = _param_mpc_acc_down_max.get();
} else {
// want to brake
if (fabsf(_vel_sp_prev) < FLT_EPSILON) {
// at rest
- _max_acceleration = _acc_max_up.get();
+ _max_acceleration = _param_mpc_acc_up_max.get();
} else if (_vel_sp_prev < 0.0f) {
// braking downward
- _max_acceleration = _acc_max_down.get();
+ _max_acceleration = _param_mpc_acc_down_max.get();
} else {
// braking upward
- _max_acceleration = _acc_max_up.get();
+ _max_acceleration = _param_mpc_acc_up_max.get();
}
}
}
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
index b5f30630228d1053c9a430d20567cc382af2124f..5425f8c5c83be189f1d2e141cbc12bcfc661188c 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
@@ -82,9 +82,9 @@ public:
* Overwrite methods:
* Needed if different parameter values than default required.
*/
- void overwriteAccelerationUp(float acc_max_up) { _acc_max_up.set(acc_max_up); }
- void overwriteAccelerationDown(float acc_max_down) {_acc_max_down.set(acc_max_down); }
- void overwriteJerkMax(float jerk_max) {_jerk_max.set(jerk_max); }
+ void overwriteAccelerationUp(float acc_max_up) { _param_mpc_acc_up_max.set(acc_max_up); }
+ void overwriteAccelerationDown(float acc_max_down) {_param_mpc_acc_down_max.set(acc_max_down); }
+ void overwriteJerkMax(float jerk_max) {_param_mpc_jerk_max.set(jerk_max); }
private:
/**
@@ -126,8 +126,8 @@ private:
float _vel_sp_prev; /**< previous velocity setpoint */
DEFINE_PARAMETERS(
- (ParamFloat<px4::params::MPC_ACC_UP_MAX>) _acc_max_up,
- (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _acc_max_down,
- (ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max
+ (ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+ (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
+ (ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max
)
};
diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
index 8fc5df22d25fc824cc34915c2f4d2dcbd5109243..98936eb577026749d966b43b7bbe17d219b809a0 100644
--- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
@@ -56,7 +56,7 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix
{
// Check if target position has been reached
if (_desired_speed_at_target < VEL_ZERO_THRESHOLD &&
- (_pos - _target).length() < NAV_ACC_RAD.get()) {
+ (_pos - _target).length() < _param_nav_acc_rad.get()) {
// Vehicle has reached target. Lock position
position_setpoint = _target;
velocity_setpoint = Vector3f(0.0f, 0.0f, 0.0f);
@@ -107,7 +107,7 @@ float StraightLine::getMaxAcc()
{
// check if origin and target are different points
if ((_target - _origin).length() < FLT_EPSILON) {
- return MPC_ACC_HOR_MAX.get();
+ return _param_mpc_acc_hor_max.get();
}
// unit vector in the direction of the straight line
@@ -115,7 +115,7 @@ float StraightLine::getMaxAcc()
// calculate the maximal horizontal acceleration
float divider = Vector2f(u_orig_to_target).length();
- float max_acc_hor = MPC_ACC_HOR_MAX.get();
+ float max_acc_hor = _param_mpc_acc_hor_max.get();
if (divider > FLT_EPSILON) {
max_acc_hor /= divider;
@@ -125,7 +125,7 @@ float StraightLine::getMaxAcc()
}
// calculate the maximal vertical acceleration
- float max_acc_vert_original = u_orig_to_target(2) < 0 ? MPC_ACC_UP_MAX.get() : MPC_ACC_DOWN_MAX.get();
+ float max_acc_vert_original = u_orig_to_target(2) < 0 ? _param_mpc_acc_up_max.get() : _param_mpc_acc_down_max.get();
float max_acc_vert = max_acc_vert_original;
if (fabs(u_orig_to_target(2)) > FLT_EPSILON) {
@@ -142,7 +142,7 @@ float StraightLine::getMaxVel()
{
// check if origin and target are different points
if ((_target - _origin).length() < FLT_EPSILON) {
- return MPC_XY_VEL_MAX.get();
+ return _param_mpc_xy_vel_max.get();
}
// unit vector in the direction of the straight line
@@ -150,7 +150,7 @@ float StraightLine::getMaxVel()
// calculate the maximal horizontal velocity
float divider = Vector2f(u_orig_to_target).length();
- float max_vel_hor = MPC_XY_VEL_MAX.get();
+ float max_vel_hor = _param_mpc_xy_vel_max.get();
if (divider > FLT_EPSILON) {
max_vel_hor /= divider;
@@ -160,7 +160,8 @@ float StraightLine::getMaxVel()
}
// calculate the maximal vertical velocity
- float max_vel_vert_directional = u_orig_to_target(2) < 0 ? MPC_Z_VEL_MAX_UP.get() : MPC_Z_VEL_MAX_DN.get();
+ float max_vel_vert_directional = u_orig_to_target(2) < 0 ? _param_mpc_z_vel_max_up.get() :
+ _param_mpc_z_vel_max_dn.get();
float max_vel_vert = max_vel_vert_directional;
if (fabs(u_orig_to_target(2)) > FLT_EPSILON) {
diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp
index 1347b2d456c984a4a645e815e25967ffebcdc3ab..eaefdb4b42d853bf82ff56361740af819866c16f 100644
--- a/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp
@@ -96,13 +96,13 @@ private:
// parameters for default values
DEFINE_PARAMETERS(
- (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX, /**< maximum horizontal acceleration */
- (ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX, /**< maximum vertical acceleration upwards */
- (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX, /**< maximum vertical acceleration downwards*/
- (ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX, /**< maximum horizontal velocity */
- (ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) MPC_Z_VEL_MAX_UP, /**< maximum vertical velocity upwards */
- (ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) MPC_Z_VEL_MAX_DN, /**< maximum vertical velocity downwards */
- (ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD /**< acceptance radius if a waypoint is reached */
+ (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max, /**< maximum horizontal acceleration */
+ (ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max, /**< maximum vertical acceleration upwards */
+ (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max, /**< maximum vertical acceleration downwards*/
+ (ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max, /**< maximum horizontal velocity */
+ (ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up, /**< maximum vertical velocity upwards */
+ (ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn, /**< maximum vertical velocity downwards */
+ (ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad /**< acceptance radius if a waypoint is reached */
)
};
diff --git a/src/lib/WeatherVane/WeatherVane.cpp b/src/lib/WeatherVane/WeatherVane.cpp
index a3be265f435ff54232f46d3637a281222360fb71..cf4bad6e421316cd16c94038527e9917dc0aba97 100644
--- a/src/lib/WeatherVane/WeatherVane.cpp
+++ b/src/lib/WeatherVane/WeatherVane.cpp
@@ -67,7 +67,7 @@ float WeatherVane::get_weathervane_yawrate()
float roll_sp = -asinf(body_z_sp(1));
float roll_exceeding_treshold = 0.0f;
- float min_roll_rad = math::radians(_wv_min_roll.get());
+ float min_roll_rad = math::radians(_param_wv_roll_min.get());
if (roll_sp > min_roll_rad) {
roll_exceeding_treshold = roll_sp - min_roll_rad;
@@ -77,6 +77,6 @@ float WeatherVane::get_weathervane_yawrate()
}
- return math::constrain(roll_exceeding_treshold * _wv_gain.get(), -math::radians(_wv_max_yaw_rate.get()),
- math::radians(_wv_max_yaw_rate.get()));
+ return math::constrain(roll_exceeding_treshold * _param_wv_gain.get(), -math::radians(_param_wv_yrate_max.get()),
+ math::radians(_param_wv_yrate_max.get()));
}
diff --git a/src/lib/WeatherVane/WeatherVane.hpp b/src/lib/WeatherVane/WeatherVane.hpp
index c96bbb6392266339cf534d279aac7391e40a7e30..136fe15b6ee808b3f638604fa67ae3141b5f8a67 100644
--- a/src/lib/WeatherVane/WeatherVane.hpp
+++ b/src/lib/WeatherVane/WeatherVane.hpp
@@ -58,7 +58,7 @@ public:
bool is_active() {return _is_active;}
- bool weathervane_enabled() { return _wv_enabled.get(); }
+ bool weathervane_enabled() { return _param_wv_en.get(); }
void update(const matrix::Quatf &q_sp_prev, float yaw);
@@ -73,10 +73,10 @@ private:
bool _is_active = true;
DEFINE_PARAMETERS(
- (ParamBool<px4::params::WV_EN>) _wv_enabled,
- (ParamFloat<px4::params::WV_ROLL_MIN>) _wv_min_roll,
- (ParamFloat<px4::params::WV_GAIN>) _wv_gain,
- (ParamFloat<px4::params::WV_YRATE_MAX>) _wv_max_yaw_rate
+ (ParamBool<px4::params::WV_EN>) _param_wv_en,
+ (ParamFloat<px4::params::WV_ROLL_MIN>) _param_wv_roll_min,
+ (ParamFloat<px4::params::WV_GAIN>) _param_wv_gain,
+ (ParamFloat<px4::params::WV_YRATE_MAX>) _param_wv_yrate_max
)
};
diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp
index 2b2075e244c0c592e33009d4ce090c1945c9adb3..e550c56c5b39e54102a8d48ddd1a6ab62aee5cbe 100644
--- a/src/lib/battery/battery.cpp
+++ b/src/lib/battery/battery.cpp
@@ -58,7 +58,7 @@ Battery::reset(battery_status_s *battery_status)
battery_status->current_a = -1.f;
battery_status->remaining = 1.f;
battery_status->scale = 1.f;
- battery_status->cell_count = _n_cells.get();
+ battery_status->cell_count = _param_bat_n_cells.get();
// TODO: check if it is sane to reset warning to NONE
battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
battery_status->connected = false;
@@ -168,21 +168,21 @@ void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed)
{
// remaining battery capacity based on voltage
- float cell_voltage = voltage_v / _n_cells.get();
+ float cell_voltage = voltage_v / _param_bat_n_cells.get();
// correct battery voltage locally for load drop to avoid estimation fluctuations
- if (_r_internal.get() >= 0.f) {
- cell_voltage += _r_internal.get() * current_a;
+ if (_param_bat_r_internal.get() >= 0.f) {
+ cell_voltage += _param_bat_r_internal.get() * current_a;
} else {
// assume linear relation between throttle and voltage drop
- cell_voltage += throttle * _v_load_drop.get();
+ cell_voltage += throttle * _param_bat_v_load_drop.get();
}
- _remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f);
+ _remaining_voltage = math::gradual(cell_voltage, _param_bat_v_empty.get(), _param_bat_v_charged.get(), 0.f, 1.f);
// choose which quantity we're using for final reporting
- if (_capacity.get() > 0.f) {
+ if (_param_bat_capacity.get() > 0.f) {
// if battery capacity is known, fuse voltage measurement with used capacity
if (!_battery_initialized) {
// initialization of the estimation state
@@ -193,7 +193,7 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle, boo
const float weight_v = 3e-4f * (1 - _remaining_voltage);
_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
// directly apply current capacity slope calculated using current
- _remaining -= _discharged_mah_loop / _capacity.get();
+ _remaining -= _discharged_mah_loop / _param_bat_capacity.get();
_remaining = math::max(_remaining, 0.f);
}
@@ -208,13 +208,13 @@ Battery::determineWarning(bool connected)
{
if (connected) {
// propagate warning state only if the state is higher, otherwise remain in current warning state
- if (_remaining < _emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
+ if (_remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
- } else if (_remaining < _crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
+ } else if (_remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
- } else if (_remaining < _low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
+ } else if (_remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
_warning = battery_status_s::BATTERY_WARNING_LOW;
}
}
@@ -223,12 +223,12 @@ Battery::determineWarning(bool connected)
void
Battery::computeScale()
{
- const float voltage_range = (_v_charged.get() - _v_empty.get());
+ const float voltage_range = (_param_bat_v_charged.get() - _param_bat_v_empty.get());
// reusing capacity calculation to get single cell voltage before drop
- const float bat_v = _v_empty.get() + (voltage_range * _remaining_voltage);
+ const float bat_v = _param_bat_v_empty.get() + (voltage_range * _remaining_voltage);
- _scale = _v_charged.get() / bat_v;
+ _scale = _param_bat_v_charged.get() / bat_v;
if (_scale > 1.3f) { // Allow at most 30% compensation
_scale = 1.3f;
diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h
index b007c43863d46ebff4980714461db7f4c30d9194..c4aae7a25254bd18ec9263b7b0e64664850501d0 100644
--- a/src/lib/battery/battery.h
+++ b/src/lib/battery/battery.h
@@ -59,17 +59,17 @@ public:
/**
* Get the battery cell count
*/
- int cell_count() { return _n_cells.get(); }
+ int cell_count() { return _param_bat_n_cells.get(); }
/**
* Get the empty voltage per cell
*/
- float empty_cell_voltage() { return _v_empty.get(); }
+ float empty_cell_voltage() { return _param_bat_v_empty.get(); }
/**
* Get the full voltage per cell
*/
- float full_cell_voltage() { return _v_charged.get(); }
+ float full_cell_voltage() { return _param_bat_v_charged.get(); }
/**
* Update current battery status message.
@@ -96,15 +96,15 @@ private:
void computeScale();
DEFINE_PARAMETERS(
- (ParamFloat<px4::params::BAT_V_EMPTY>) _v_empty,
- (ParamFloat<px4::params::BAT_V_CHARGED>) _v_charged,
- (ParamInt<px4::params::BAT_N_CELLS>) _n_cells,
- (ParamFloat<px4::params::BAT_CAPACITY>) _capacity,
- (ParamFloat<px4::params::BAT_V_LOAD_DROP>) _v_load_drop,
- (ParamFloat<px4::params::BAT_R_INTERNAL>) _r_internal,
- (ParamFloat<px4::params::BAT_LOW_THR>) _low_thr,
- (ParamFloat<px4::params::BAT_CRIT_THR>) _crit_thr,
- (ParamFloat<px4::params::BAT_EMERGEN_THR>) _emergency_thr
+ (ParamFloat<px4::params::BAT_V_EMPTY>) _param_bat_v_empty,
+ (ParamFloat<px4::params::BAT_V_CHARGED>) _param_bat_v_charged,
+ (ParamInt<px4::params::BAT_N_CELLS>) _param_bat_n_cells,
+ (ParamFloat<px4::params::BAT_CAPACITY>) _param_bat_capacity,
+ (ParamFloat<px4::params::BAT_V_LOAD_DROP>) _param_bat_v_load_drop,
+ (ParamFloat<px4::params::BAT_R_INTERNAL>) _param_bat_r_internal,
+ (ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
+ (ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
+ (ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr
)
bool _battery_initialized = false;
diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
index 8f4aabda9e226c876b34d6193dee4a31e6b6402f..ba6b11e1763cb58e225019f6e4a3a4e9b61fca56 100644
--- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
@@ -49,7 +49,7 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
// set software low pass filter for controllers
updateParams();
- configure_filter(_filter_cutoff.get());
+ configure_filter(_param_imu_accel_cutoff.get());
// force initial publish to allocate uORB buffer
// TODO: can be removed once all drivers are in threads
diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
index 72f9a09329478de8ebf89731774891a3cb410597..4ec57ca6840fb93ebf9263a9dbf673ba650bccc3 100644
--- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
@@ -82,7 +82,7 @@ private:
unsigned _sample_rate{1000};
DEFINE_PARAMETERS(
- (ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _filter_cutoff
+ (ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
)
};
diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
index 491cfe4ca32428ec8d9f935fb7d75e5caa1108a8..aaa8188b9d7470d645b50c8276f79509c14cc475 100644
--- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
@@ -49,7 +49,7 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
// set software low pass filter for controllers
updateParams();
- configure_filter(_filter_cutoff.get());
+ configure_filter(_param_imu_gyro_cutoff.get());
// force initial publish to allocate uORB buffer
// TODO: can be removed once all drivers are in threads
diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
index 6c878251ae3f22fb82213b8e370ea586d9bace6e..6eb7950cdc03031cc1eb23d554734f58b53b3302 100644
--- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
@@ -82,7 +82,7 @@ private:
unsigned _sample_rate{1000};
DEFINE_PARAMETERS(
- (ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _filter_cutoff
+ (ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff
)
};