diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index 09e5eac986..dae425b7a7 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -80,7 +80,8 @@ protected: private: uORB::Subscription *_sub_triplet_setpoint{nullptr}; - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _mc_cruise_default); /**< Default mc cruise speed.*/ + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _mc_cruise_default); /**< Default mc cruise speed.*/ map_projection_reference_s _reference; /**< Reference frame from global to local. */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index ac0a209128..575188d3b0 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -65,17 +65,17 @@ protected: State _current_state{State::none}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, - (ParamFloat) _land_speed, - (ParamFloat) _vel_max_up, - (ParamFloat) _vel_max_down, - (ParamFloat) _acc_max_xy, - (ParamFloat) _acc_xy, - (ParamFloat) _acc_max_up, - (ParamFloat) _acc_max_down, - (ParamFloat) _cruise_speed_90, - (ParamFloat) _nav_rad, - (ParamFloat) _mis_yaw_error - ) + (ParamFloat) _land_speed, + (ParamFloat) _vel_max_up, + (ParamFloat) _vel_max_down, + (ParamFloat) _acc_max_xy, + (ParamFloat) _acc_xy, + (ParamFloat) _acc_max_up, + (ParamFloat) _acc_max_down, + (ParamFloat) _cruise_speed_90, + (ParamFloat) _nav_rad, + (ParamFloat) _mis_yaw_error + ) void _generateIdleSetpoints(); void _generateLandSetpoints(); diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index 5017eb5960..ee8c380917 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -43,7 +43,7 @@ using namespace matrix; PositionControl::PositionControl(ModuleParams *parent) : -ModuleParams(parent) + ModuleParams(parent) {} void PositionControl::updateState(const vehicle_local_position_s &state, const Vector3f &vel_dot) @@ -200,7 +200,8 @@ void PositionControl::_velocityController(const float &dt) Vector3f vel_err = _vel_sp - _vel; // Consider thrust in D-direction. - float thrust_desired_D = MPC_Z_VEL_P.get() * vel_err(2) + MPC_Z_VEL_D.get() * _vel_dot(2) + _thr_int(2) - MPC_THR_HOVER.get(); + float thrust_desired_D = MPC_Z_VEL_P.get() * vel_err(2) + MPC_Z_VEL_D.get() * _vel_dot(2) + _thr_int( + 2) - MPC_THR_HOVER.get(); // The Thrust limits are negated and swapped due to NED-frame. float uMax = -MPC_THR_MIN.get(); @@ -221,7 +222,7 @@ void PositionControl::_velocityController(const float &dt) if (fabsf(_thr_sp(0)) + fabsf(_thr_sp(1)) > FLT_EPSILON) { // Thrust set-point in NE-direction is already provided. Only // scaling by the maximum tilt is required. - float thr_xy_max = fabsf(_thr_sp(2)) * tanf(MPC_MAN_TILT_MAX.get()); + float thr_xy_max = fabsf(_thr_sp(2)) * tanf(MPC_MAN_TILT_MAX_rad.get()); _thr_sp(0) *= thr_xy_max; _thr_sp(1) *= thr_xy_max; @@ -268,8 +269,8 @@ void PositionControl::updateConstraints(const Controller::Constraints &constrain // Check if adjustable constraints are below global constraints. If they are not stricter than global // constraints, then just use global constraints for the limits. - if (!PX4_ISFINITE(constraints.tilt_max) || !(constraints.tilt_max < MPC_TILTMAX_AIR.get())) { - _constraints.tilt_max = MPC_TILTMAX_AIR.get(); + if (!PX4_ISFINITE(constraints.tilt_max) || !(constraints.tilt_max < MPC_TILTMAX_AIR_rad.get())) { + _constraints.tilt_max = MPC_TILTMAX_AIR_rad.get(); } if (!PX4_ISFINITE(constraints.vel_max_z_up) || !(constraints.vel_max_z_up < MPC_Z_VEL_MAX_UP.get())) { diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index 3927be40e2..c62b22c810 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -192,22 +192,23 @@ private: bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */ DEFINE_PARAMETERS( - (ParamFloat) MPC_THR_MAX, - (ParamFloat) MPC_THR_HOVER, - (ParamFloat) MPC_THR_MIN, - (ParamFloat) MPC_MANTHR_MIN, - (ParamFloat) MPC_XY_VEL_MAX, - (ParamFloat) MPC_Z_VEL_MAX_DN, - (ParamFloat) MPC_Z_VEL_MAX_UP, - (ParamFloat) MPC_TILTMAX_AIR, - (ParamFloat) MPC_MAN_TILT_MAX, - (ParamFloat) MPC_Z_P, - (ParamFloat) MPC_Z_VEL_P, - (ParamFloat) MPC_Z_VEL_I, - (ParamFloat) MPC_Z_VEL_D, - (ParamFloat) MPC_XY_P, - (ParamFloat) MPC_XY_VEL_P, - (ParamFloat) MPC_XY_VEL_I, - (ParamFloat) MPC_XY_VEL_D + (ParamFloat) MPC_THR_MAX, + (ParamFloat) MPC_THR_HOVER, + (ParamFloat) MPC_THR_MIN, + (ParamFloat) MPC_MANTHR_MIN, + (ParamFloat) MPC_XY_VEL_MAX, + (ParamFloat) MPC_Z_VEL_MAX_DN, + (ParamFloat) MPC_Z_VEL_MAX_UP, + (ParamFloat) + MPC_TILTMAX_AIR_rad, // maximum tilt for any position controlled mode in radians + (ParamFloat) MPC_MAN_TILT_MAX_rad, // maximum til for stabilized/altitude mode in radians + (ParamFloat) MPC_Z_P, + (ParamFloat) MPC_Z_VEL_P, + (ParamFloat) MPC_Z_VEL_I, + (ParamFloat) MPC_Z_VEL_D, + (ParamFloat) MPC_XY_P, + (ParamFloat) MPC_XY_VEL_P, + (ParamFloat) MPC_XY_VEL_I, + (ParamFloat) MPC_XY_VEL_D ) };