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

mc_pos_control: remove trailing zeroes

Browse Source
This commit is contained in:
Matthias Grob
2023-06-22 13:31:16 +02:00
parent 54a351639c
commit a6a913bf25
8 changed files with 74 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+5 -5
View File
@@ -83,7 +83,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
Vector3f body_x = y_C % body_z; Vector3f body_x = y_C % body_z;
// keep nose to front while inverted upside down // keep nose to front while inverted upside down
if (body_z(2) < 0.0f) { if (body_z(2) < 0.f) {
body_x = -body_x; body_x = -body_x;
} }
@@ -91,7 +91,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
// desired thrust is in XY plane, set X downside to construct correct matrix, // desired thrust is in XY plane, set X downside to construct correct matrix,
// but yaw component will not be used actually // but yaw component will not be used actually
body_x.zero(); body_x.zero();
body_x(2) = 1.0f; body_x(2) = 1.f;
} }
body_x.normalize(); body_x.normalize();
@@ -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 // we have triangle CDX with known CD and CX = R, find DX
float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); 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 // target waypoint is already behind us
res = line_b; res = line_b;
@@ -218,12 +218,12 @@ bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
res = d; // go directly to line res = d; // go directly to line
// previous waypoint is still in front of us // 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; res = line_a;
} }
// target waypoint is already behind us // 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; res = line_b;
} }
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+2 -2
View File
@@ -152,8 +152,8 @@ void PositionControl::_velocityControl(const float dt)
_accelerationControl(); _accelerationControl();
// Integrator anti-windup in vertical direction // Integrator anti-windup in vertical direction
if ((_thr_sp(2) >= -_lim_thr_min && 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.0f)) { (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) {
vel_error(2) = 0.f; vel_error(2) = 0.f;
} }
src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp
+1 -1
View File
@@ -46,7 +46,7 @@ TEST(TakeoffTest, RegularTakeoffRamp)
{ {
TakeoffHandling takeoff; TakeoffHandling takeoff;
takeoff.setSpoolupTime(1.f); takeoff.setSpoolupTime(1.f);
takeoff.setTakeoffRampTime(2.0); takeoff.setTakeoffRampTime(2.f);
takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f); takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f);
// disarmed, landed, don't want takeoff // disarmed, landed, don't want takeoff
src/modules/mc_pos_control/multicopter_autonomous_params.c
+17 -17
View File
@@ -37,13 +37,13 @@
* e.g. in Missions, RTL, Goto if the waypoint does not specify differently * e.g. in Missions, RTL, Goto if the waypoint does not specify differently
* *
* @unit m/s * @unit m/s
* @min 3.0 * @min 3
* @max 20.0 * @max 20
* @decimal 0 * @decimal 0
* @increment 1 * @increment 1
* @group Multicopter Position Control * @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 * Ascent velocity in autonomous modes
@@ -52,7 +52,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
* *
* @unit m/s * @unit m/s
* @min 0.5 * @min 0.5
* @max 8.0 * @max 8
* @decimal 1 * @decimal 1
* @increment 0.5 * @increment 0.5
* @group Multicopter Position Control * @group Multicopter Position Control
@@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f);
* *
* @unit m/s * @unit m/s
* @min 0.5 * @min 0.5
* @max 4.0 * @max 4
* @decimal 1 * @decimal 1
* @increment 0.5 * @increment 0.5
* @group Multicopter Position Control * @group Multicopter Position Control
@@ -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. * When piloting manually, this parameter is only used in MPC_POS_MODE 4.
* *
* @unit m/s^2 * @unit m/s^2
* @min 2.0 * @min 2
* @max 15.0 * @max 15
* @decimal 1 * @decimal 1
* @increment 1 * @increment 1
* @group Multicopter Position Control * @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 * Jerk limit in autonomous modes
@@ -94,19 +94,19 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.0f);
* A lower value leads to smoother vehicle motions but also limited agility. * A lower value leads to smoother vehicle motions but also limited agility.
* *
* @unit m/s^3 * @unit m/s^3
* @min 1.0 * @min 1
* @max 80.0 * @max 80
* @decimal 1 * @decimal 1
* @increment 1 * @increment 1
* @group Multicopter Position Control * @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 * Proportional gain for horizontal trajectory position error
* *
* @min 0.1 * @min 0.1
* @max 1.0 * @max 1
* @decimal 1 * @decimal 1
* @increment 0.1 * @increment 0.1
* @group Multicopter Position Control * @group Multicopter Position Control
@@ -125,12 +125,12 @@ PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
* capabilities of the vehicle. * capabilities of the vehicle.
* *
* @min 0.1 * @min 0.1
* @max 10.0 * @max 10
* @decimal 1 * @decimal 1
* @increment 1 * @increment 1
* @group Multicopter Position Control * @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 * Max yaw rate in autonomous modes
@@ -139,13 +139,13 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
* control output and mixer saturation. * control output and mixer saturation.
* *
* @unit deg/s * @unit deg/s
* @min 0.0 * @min 0
* @max 360.0 * @max 360
* @decimal 0 * @decimal 0
* @increment 5 * @increment 5
* @group Multicopter Attitude Control * @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 * Heading behavior in autonomous modes
src/modules/mc_pos_control/multicopter_position_control_gain_params.c
+15 -15
View File
@@ -42,15 +42,15 @@
* @increment 0.1 * @increment 0.1
* @group Multicopter Position Control * @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 * Proportional gain for horizontal position error
* *
* Defined as corrective velocity in m/s per m position error * Defined as corrective velocity in m/s per m position error
* *
* @min 0.0 * @min 0
* @max 2.0 * @max 2
* @decimal 1 * @decimal 1
* @increment 0.1 * @increment 0.1
* @group Multicopter Position Control * @group Multicopter Position Control
@@ -62,13 +62,13 @@ PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f);
* *
* Defined as corrective acceleration in m/s^2 per m/s velocity error * Defined as corrective acceleration in m/s^2 per m/s velocity error
* *
* @min 2.0 * @min 2
* @max 15.0 * @max 15
* @decimal 1 * @decimal 1
* @increment 0.1 * @increment 0.1
* @group Multicopter Position Control * @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 * Proportional gain for horizontal velocity error
@@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.0f);
* Defined as corrective acceleration in m/s^2 per m/s velocity error * Defined as corrective acceleration in m/s^2 per m/s velocity error
* *
* @min 1.2 * @min 1.2
* @max 5.0 * @max 5
* @decimal 1 * @decimal 1
* @increment 0.1 * @increment 0.1
* @group Multicopter Position Control * @group Multicopter Position Control
@@ -89,12 +89,12 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f);
* Defined as corrective acceleration in m/s^2 per m velocity integral * Defined as corrective acceleration in m/s^2 per m velocity integral
* *
* @min 0.2 * @min 0.2
* @max 3.0 * @max 3
* @decimal 1 * @decimal 1
* @increment 0.1 * @increment 0.1
* @group Multicopter Position Control * @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 * Integral gain for horizontal velocity error
@@ -102,8 +102,8 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f);
* Defined as correction acceleration in m/s^2 per m velocity integral * Defined as correction acceleration in m/s^2 per m velocity integral
* Allows to eliminate steady state errors in disturbances like wind. * Allows to eliminate steady state errors in disturbances like wind.
* *
* @min 0.0 * @min 0
* @max 60.0 * @max 60
* @decimal 2 * @decimal 2
* @increment 0.02 * @increment 0.02
* @group Multicopter Position Control * @group Multicopter Position Control
@@ -115,13 +115,13 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f);
* *
* Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative * Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative
* *
* @min 0.0 * @min 0
* @max 2.0 * @max 2
* @decimal 2 * @decimal 2
* @increment 0.02 * @increment 0.02
* @group Multicopter Position Control * @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 * Differential gain for horizontal velocity error
@@ -129,7 +129,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f);
* Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative * Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative
* *
* @min 0.1 * @min 0.1
* @max 2.0 * @max 2
* @decimal 2 * @decimal 2
* @increment 0.02 * @increment 0.02
* @group Multicopter Position Control * @group Multicopter Position Control
src/modules/mc_pos_control/multicopter_position_control_limits_params.c
+12 -12
View File
@@ -38,13 +38,13 @@
* Any higher value is truncated. * Any higher value is truncated.
* *
* @unit m/s * @unit m/s
* @min 0.0 * @min 0
* @max 20.0 * @max 20
* @decimal 0 * @decimal 0
* @increment 1 * @increment 1
* @group Multicopter Position Control * @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 * Maximum tilt angle in air
@@ -53,13 +53,13 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.0f);
* Any higher value is truncated. * Any higher value is truncated.
* *
* @unit deg * @unit deg
* @min 20.0 * @min 20
* @max 89.0 * @max 89
* @decimal 0 * @decimal 0
* @increment 1 * @increment 1
* @group Multicopter Position Control * @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 * Maximum tilt during inital takeoff ramp
@@ -67,13 +67,13 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
* Tighter tilt limit during takeoff to avoid tip over. * Tighter tilt limit during takeoff to avoid tip over.
* *
* @unit deg * @unit deg
* @min 5.0 * @min 5
* @max 89.0 * @max 89
* @decimal 0 * @decimal 0
* @increment 1 * @increment 1
* @group Multicopter Position Control * @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 * Minimum collective thrust in climb rate controlled modes
@@ -96,10 +96,10 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
* Limit allowed thrust e.g. for indoor test of overpowered vehicle. * Limit allowed thrust e.g. for indoor test of overpowered vehicle.
* *
* @unit norm * @unit norm
* @min 0.0 * @min 0
* @max 1.0 * @max 1
* @decimal 2 * @decimal 2
* @increment 0.05 * @increment 0.05
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f);
src/modules/mc_pos_control/multicopter_position_mode_params.c
+18 -18
View File
@@ -58,13 +58,13 @@ PARAM_DEFINE_INT32(MPC_POS_MODE, 4);
* using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively. * using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively.
* *
* @unit m/s * @unit m/s
* @min 3.0 * @min 3
* @max 20.0 * @max 20
* @increment 1 * @increment 1
* @decimal 0 * @decimal 0
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.0f); PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.f);
/** /**
* Maximum sideways velocity in Position mode * Maximum sideways velocity in Position mode
@@ -73,13 +73,13 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.0f);
* MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used.
* *
* @unit m/s * @unit m/s
* @min -1.0 * @min -1
* @max 20.0 * @max 20
* @increment 1 * @increment 1
* @decimal 0 * @decimal 0
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.0f); PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.f);
/** /**
* Maximum backward velocity in Position mode * Maximum backward velocity in Position mode
@@ -88,13 +88,13 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.0f);
* MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used.
* *
* @unit m/s * @unit m/s
* @min -1.0 * @min -1
* @max 20.0 * @max 20
* @increment 1 * @increment 1
* @decimal 0 * @decimal 0
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.0f); PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f);
/** /**
* Maximum ascent velocity in manually piloted climb rate controlled modes and offboard * Maximum ascent velocity in manually piloted climb rate controlled modes and offboard
@@ -103,7 +103,7 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.0f);
* *
* @unit m/s * @unit m/s
* @min 0.5 * @min 0.5
* @max 8.0 * @max 8
* @increment 0.1 * @increment 0.1
* @decimal 1 * @decimal 1
* @group Multicopter Position Control * @group Multicopter Position Control
@@ -117,7 +117,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f);
* *
* @unit m/s * @unit m/s
* @min 0.5 * @min 0.5
* @max 4.0 * @max 4
* @increment 0.1 * @increment 0.1
* @decimal 1 * @decimal 1
* @group Multicopter Position Control * @group Multicopter Position Control
@@ -133,13 +133,13 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.5f);
* 4 use MPC_ACC_HOR instead * 4 use MPC_ACC_HOR instead
* *
* @unit m/s^2 * @unit m/s^2
* @min 2.0 * @min 2
* @max 15.0 * @max 15
* @increment 1 * @increment 1
* @decimal 2 * @decimal 2
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f);
/** /**
* Maximum horizontal and vertical jerk in Position/Altitude mode * Maximum horizontal and vertical jerk in Position/Altitude mode
@@ -154,20 +154,20 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.0f);
* *
* @unit m/s^3 * @unit m/s^3
* @min 0.5 * @min 0.5
* @max 500.0 * @max 500
* @decimal 0 * @decimal 0
* @increment 1 * @increment 1
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 8.0f); PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 8.f);
/** /**
* Deadzone for sticks in manual piloted modes * Deadzone for sticks in manual piloted modes
* *
* Does not apply to manual throttle and direct attitude piloting by stick. * Does not apply to manual throttle and direct attitude piloting by stick.
* *
* @min 0.0 * @min 0
* @max 1.0 * @max 1
* @decimal 2 * @decimal 2
* @increment 0.01 * @increment 0.01
* @group Multicopter Position Control * @group Multicopter Position Control
src/modules/mc_pos_control/multicopter_takeoff_land_params.c
+4 -4
View File
@@ -43,7 +43,7 @@
* @max 5 * @max 5
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f); PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.f);
/** /**
* Takeoff climb rate * Takeoff climb rate
@@ -69,7 +69,7 @@ PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f);
* @decimal 1 * @decimal 1
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f); PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.f);
/** /**
* Altitude for 2. step of slow landing (landing) * Altitude for 2. step of slow landing (landing)
@@ -84,7 +84,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f);
* @decimal 1 * @decimal 1
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.f);
/** /**
* Altitude for 3. step of slow landing * Altitude for 3. step of slow landing
@@ -99,7 +99,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);
* @decimal 1 * @decimal 1
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.0f); PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.f);
/** /**
* Landing descend rate * Landing descend rate