Skip to content

Commit

Permalink
mc_pos_control: remove trailing zeroes
Browse files Browse the repository at this point in the history
  • Loading branch information
MaEtUgR committed Jun 22, 2023
1 parent d01092c commit d25938a
Show file tree
Hide file tree
Showing 8 changed files with 74 additions and 74 deletions.
10 changes: 5 additions & 5 deletions src/modules/mc_pos_control/PositionControl/ControlMath.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -83,15 +83,15 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
Vector3f body_x = y_C % body_z;

// keep nose to front while inverted upside down
if (body_z(2) < 0.0f) {
if (body_z(2) < 0.f) {
body_x = -body_x;
}

if (fabsf(body_z(2)) < 0.000001f) {
// desired thrust is in XY plane, set X downside to construct correct matrix,
// but yaw component will not be used actually
body_x.zero();
body_x(2) = 1.0f;
body_x(2) = 1.f;
}

body_x.normalize();
Expand Down Expand Up @@ -201,7 +201,7 @@ bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
// we have triangle CDX with known CD and CX = R, find DX
float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);

if ((sphere_c - line_b) * ab_norm > 0.0f) {
if ((sphere_c - line_b) * ab_norm > 0.f) {
// target waypoint is already behind us
res = line_b;

Expand All @@ -218,12 +218,12 @@ bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
res = d; // go directly to line

// previous waypoint is still in front of us
if ((sphere_c - line_a) * ab_norm < 0.0f) {
if ((sphere_c - line_a) * ab_norm < 0.f) {
res = line_a;
}

// target waypoint is already behind us
if ((sphere_c - line_b) * ab_norm > 0.0f) {
if ((sphere_c - line_b) * ab_norm > 0.f) {
res = line_b;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -152,8 +152,8 @@ void PositionControl::_velocityControl(const float dt)
_accelerationControl();

// Integrator anti-windup in vertical direction
if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
(_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) {
if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) ||
(_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) {
vel_error(2) = 0.f;
}

Expand Down
2 changes: 1 addition & 1 deletion src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ TEST(TakeoffTest, RegularTakeoffRamp)
{
TakeoffHandling takeoff;
takeoff.setSpoolupTime(1.f);
takeoff.setTakeoffRampTime(2.0);
takeoff.setTakeoffRampTime(2.f);
takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f);

// disarmed, landed, don't want takeoff
Expand Down
34 changes: 17 additions & 17 deletions src/modules/mc_pos_control/multicopter_autonomous_params.c
Original file line number Diff line number Diff line change
Expand Up @@ -37,13 +37,13 @@
* e.g. in Missions, RTL, Goto if the waypoint does not specify differently
*
* @unit m/s
* @min 3.0
* @max 20.0
* @min 3
* @max 20
* @decimal 0
* @increment 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.f);

/**
* Ascent velocity in autonomous modes
Expand All @@ -52,7 +52,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
*
* @unit m/s
* @min 0.5
* @max 8.0
* @max 8
* @decimal 1
* @increment 0.5
* @group Multicopter Position Control
Expand All @@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f);
*
* @unit m/s
* @min 0.5
* @max 4.0
* @max 4
* @decimal 1
* @increment 0.5
* @group Multicopter Position Control
Expand All @@ -79,13 +79,13 @@ PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.5f);
* When piloting manually, this parameter is only used in MPC_POS_MODE 4.
*
* @unit m/s^2
* @min 2.0
* @max 15.0
* @min 2
* @max 15
* @decimal 1
* @increment 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.0f);
PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.f);

/**
* Jerk limit in autonomous modes
Expand All @@ -94,19 +94,19 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.0f);
* A lower value leads to smoother vehicle motions but also limited agility.
*
* @unit m/s^3
* @min 1.0
* @max 80.0
* @min 1
* @max 80
* @decimal 1
* @increment 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_JERK_AUTO, 4.0f);
PARAM_DEFINE_FLOAT(MPC_JERK_AUTO, 4.f);

/**
* Proportional gain for horizontal trajectory position error
*
* @min 0.1
* @max 1.0
* @max 1
* @decimal 1
* @increment 0.1
* @group Multicopter Position Control
Expand All @@ -125,12 +125,12 @@ PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
* capabilities of the vehicle.
*
* @min 0.1
* @max 10.0
* @max 10
* @decimal 1
* @increment 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.f);

/**
* Max yaw rate in autonomous modes
Expand All @@ -139,13 +139,13 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
* control output and mixer saturation.
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @min 0
* @max 360
* @decimal 0
* @increment 5
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f);
PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f);

/**
* Heading behavior in autonomous modes
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,15 +42,15 @@
* @increment 0.1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.f);

/**
* Proportional gain for horizontal position error
*
* Defined as corrective velocity in m/s per m position error
*
* @min 0.0
* @max 2.0
* @min 0
* @max 2
* @decimal 1
* @increment 0.1
* @group Multicopter Position Control
Expand All @@ -62,21 +62,21 @@ PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f);
*
* Defined as corrective acceleration in m/s^2 per m/s velocity error
*
* @min 2.0
* @max 15.0
* @min 2
* @max 15
* @decimal 1
* @increment 0.1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.f);

/**
* Proportional gain for horizontal velocity error
*
* Defined as corrective acceleration in m/s^2 per m/s velocity error
*
* @min 1.2
* @max 5.0
* @max 5
* @decimal 1
* @increment 0.1
* @group Multicopter Position Control
Expand All @@ -89,21 +89,21 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f);
* Defined as corrective acceleration in m/s^2 per m velocity integral
*
* @min 0.2
* @max 3.0
* @max 3
* @decimal 1
* @increment 0.1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.f);

/**
* Integral gain for horizontal velocity error
*
* Defined as correction acceleration in m/s^2 per m velocity integral
* Allows to eliminate steady state errors in disturbances like wind.
*
* @min 0.0
* @max 60.0
* @min 0
* @max 60
* @decimal 2
* @increment 0.02
* @group Multicopter Position Control
Expand All @@ -115,21 +115,21 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f);
*
* Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative
*
* @min 0.0
* @max 2.0
* @min 0
* @max 2
* @decimal 2
* @increment 0.02
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.f);

/**
* Differential gain for horizontal velocity error
*
* Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative
*
* @min 0.1
* @max 2.0
* @max 2
* @decimal 2
* @increment 0.02
* @group Multicopter Position Control
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,13 @@
* Any higher value is truncated.
*
* @unit m/s
* @min 0.0
* @max 20.0
* @min 0
* @max 20
* @decimal 0
* @increment 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.f);

/**
* Maximum tilt angle in air
Expand All @@ -53,27 +53,27 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.0f);
* Any higher value is truncated.
*
* @unit deg
* @min 20.0
* @max 89.0
* @min 20
* @max 89
* @decimal 0
* @increment 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.f);

/**
* Maximum tilt during inital takeoff ramp
*
* Tighter tilt limit during takeoff to avoid tip over.
*
* @unit deg
* @min 5.0
* @max 89.0
* @min 5
* @max 89
* @decimal 0
* @increment 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.f);

/**
* Minimum collective thrust in climb rate controlled modes
Expand All @@ -96,10 +96,10 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
* Limit allowed thrust e.g. for indoor test of overpowered vehicle.
*
* @unit norm
* @min 0.0
* @max 1.0
* @min 0
* @max 1
* @decimal 2
* @increment 0.05
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f);
Loading

0 comments on commit d25938a

Please sign in to comment.