fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Parameter update - Rename variables in lib

Browse Source 
using paramter_update.py script
This commit is contained in:
bresch
2019-04-03 15:38:50 +02:00
committed by Matthias Grob
parent ec5da55107
commit b14446f0e5
40 changed files with 245 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+4 -2
View File
@@ -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; }
src/lib/CollisionPrevention/CollisionPrevention.hpp
+2 -2
View File
@@ -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();
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+5 -5
View File
@@ -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();
}
}
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+6 -5
View File
@@ -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:
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
+5 -5
View File
@@ -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));
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
+4 -4
View File
@@ -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. */
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+14 -13
View File
@@ -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());
}
}
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+8 -8
View File
@@ -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. */
src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+6 -6
View File
@@ -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()
src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
+7 -5
View File
@@ -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. */
src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
+6 -4
View File
@@ -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()
src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
+7 -5
View File
@@ -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. */
src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp
+2 -2
View File
@@ -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;
}
src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp
+3 -2
View File
@@ -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 */
)
};
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+4 -4
View File
@@ -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;
}
src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+4 -4
View File
@@ -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
)
};
src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp
+5 -5
View File
@@ -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
src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp
+6 -6
View File
@@ -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 */
)
};
src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+12 -12
View File
@@ -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);
}
}
src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+12 -10
View File
@@ -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:
/**
src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
+5 -5
View File
@@ -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);
src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp
+4 -4
View File
@@ -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
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+8 -8
View File
@@ -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]);
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+4 -4
View File
@@ -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:
src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp
+3 -3
View File
@@ -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
src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp
+3 -3
View File
@@ -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
)
};
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
+12 -12
View File
@@ -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;
}
}
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
+11 -11
View File
@@ -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 */
)
};
src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
+14 -14
View File
@@ -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();
}
}
}
src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
+6 -6
View File
@@ -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
)
};
src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
+8 -7
View File
@@ -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) {
src/lib/FlightTasks/tasks/Utility/StraightLine.hpp
+7 -7
View File
@@ -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 */
)
};
src/lib/WeatherVane/WeatherVane.cpp
+3 -3
View File
@@ -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()));
}
src/lib/WeatherVane/WeatherVane.hpp
+5 -5
View File
@@ -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
)
};
src/lib/battery/battery.cpp
+14 -14
View File
@@ -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;
src/lib/battery/battery.h
+12 -12
View File
@@ -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;
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+1 -1
View File
@@ -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
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+1 -1
View File
@@ -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
)
};
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+1 -1
View File
@@ -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
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+1 -1
View File
@@ -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
)
};