From efb60ab7794579d4cf1b2684c30fb4a13108c0fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 May 2015 15:19:16 +0200 Subject: multiplatform pos controller: Rename params to prevent mixup with MPC params --- .../mc_pos_control.cpp | 40 +++++++++++----------- .../mc_pos_control_params.c | 40 +++++++++++----------- .../mc_pos_control_params.h | 40 +++++++++++----------- 3 files changed, 60 insertions(+), 60 deletions(-) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index d135eecfb..90281d2bc 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -69,26 +69,26 @@ MulticopterPositionControl::MulticopterPositionControl() : /* parameters */ _params_handles({ - .thr_min = px4::ParameterFloat("MPC_THR_MIN", PARAM_MPC_THR_MIN_DEFAULT), - .thr_max = px4::ParameterFloat("MPC_THR_MAX", PARAM_MPC_THR_MAX_DEFAULT), - .z_p = px4::ParameterFloat("MPC_Z_P", PARAM_MPC_Z_P_DEFAULT), - .z_vel_p = px4::ParameterFloat("MPC_Z_VEL_P", PARAM_MPC_Z_VEL_P_DEFAULT), - .z_vel_i = px4::ParameterFloat("MPC_Z_VEL_I", PARAM_MPC_Z_VEL_I_DEFAULT), - .z_vel_d = px4::ParameterFloat("MPC_Z_VEL_D", PARAM_MPC_Z_VEL_D_DEFAULT), - .z_vel_max = px4::ParameterFloat("MPC_Z_VEL_MAX", PARAM_MPC_Z_VEL_MAX_DEFAULT), - .z_ff = px4::ParameterFloat("MPC_Z_FF", PARAM_MPC_Z_FF_DEFAULT), - .xy_p = px4::ParameterFloat("MPC_XY_P", PARAM_MPC_XY_P_DEFAULT), - .xy_vel_p = px4::ParameterFloat("MPC_XY_VEL_P", PARAM_MPC_XY_VEL_P_DEFAULT), - .xy_vel_i = px4::ParameterFloat("MPC_XY_VEL_I", PARAM_MPC_XY_VEL_I_DEFAULT), - .xy_vel_d = px4::ParameterFloat("MPC_XY_VEL_D", PARAM_MPC_XY_VEL_D_DEFAULT), - .xy_vel_max = px4::ParameterFloat("MPC_XY_VEL_MAX", PARAM_MPC_XY_VEL_MAX_DEFAULT), - .xy_ff = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT), - .tilt_max_air = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT), - .land_speed = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT), - .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT), - .man_roll_max = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), - .man_pitch_max = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), - .man_yaw_max = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT), + .thr_min = px4::ParameterFloat("MPP_THR_MIN", PARAM_MPP_THR_MIN_DEFAULT), + .thr_max = px4::ParameterFloat("MPP_THR_MAX", PARAM_MPP_THR_MAX_DEFAULT), + .z_p = px4::ParameterFloat("MPP_Z_P", PARAM_MPP_Z_P_DEFAULT), + .z_vel_p = px4::ParameterFloat("MPP_Z_VEL_P", PARAM_MPP_Z_VEL_P_DEFAULT), + .z_vel_i = px4::ParameterFloat("MPP_Z_VEL_I", PARAM_MPP_Z_VEL_I_DEFAULT), + .z_vel_d = px4::ParameterFloat("MPP_Z_VEL_D", PARAM_MPP_Z_VEL_D_DEFAULT), + .z_vel_max = px4::ParameterFloat("MPP_Z_VEL_MAX", PARAM_MPP_Z_VEL_MAX_DEFAULT), + .z_ff = px4::ParameterFloat("MPP_Z_FF", PARAM_MPP_Z_FF_DEFAULT), + .xy_p = px4::ParameterFloat("MPP_XY_P", PARAM_MPP_XY_P_DEFAULT), + .xy_vel_p = px4::ParameterFloat("MPP_XY_VEL_P", PARAM_MPP_XY_VEL_P_DEFAULT), + .xy_vel_i = px4::ParameterFloat("MPP_XY_VEL_I", PARAM_MPP_XY_VEL_I_DEFAULT), + .xy_vel_d = px4::ParameterFloat("MPP_XY_VEL_D", PARAM_MPP_XY_VEL_D_DEFAULT), + .xy_vel_max = px4::ParameterFloat("MPP_XY_VEL_MAX", PARAM_MPP_XY_VEL_MAX_DEFAULT), + .xy_ff = px4::ParameterFloat("MPP_XY_FF", PARAM_MPP_XY_FF_DEFAULT), + .tilt_max_air = px4::ParameterFloat("MPP_TILTMAX_AIR", PARAM_MPP_TILTMAX_AIR_DEFAULT), + .land_speed = px4::ParameterFloat("MPP_LAND_SPEED", PARAM_MPP_LAND_SPEED_DEFAULT), + .tilt_max_land = px4::ParameterFloat("MPP_TILTMAX_LND", PARAM_MPP_TILTMAX_LND_DEFAULT), + .man_roll_max = px4::ParameterFloat("MPP_MAN_R_MAX", PARAM_MPP_MAN_R_MAX_DEFAULT), + .man_pitch_max = px4::ParameterFloat("MPP_MAN_P_MAX", PARAM_MPP_MAN_P_MAX_DEFAULT), + .man_yaw_max = px4::ParameterFloat("MPP_MAN_Y_MAX", PARAM_MPP_MAN_Y_MAX_DEFAULT), .mc_att_yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT) }), _ref_alt(0.0f), diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c index 709085662..1b3c5b7e9 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -52,7 +52,7 @@ * @max 1.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); +PX4_PARAM_DEFINE_FLOAT(MPP_THR_MIN); /** * Maximum thrust @@ -63,7 +63,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); * @max 1.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_THR_MAX); /** * Proportional gain for vertical position error @@ -71,7 +71,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_P); /** * Proportional gain for vertical velocity error @@ -79,7 +79,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_P); /** * Integral gain for vertical velocity error @@ -89,7 +89,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_I); /** * Differential gain for vertical velocity error @@ -97,7 +97,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_D); /** * Maximum vertical velocity @@ -108,7 +108,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_MAX); /** * Vertical velocity feed forward @@ -119,7 +119,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); * @max 1.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_FF); /** * Proportional gain for horizontal position error @@ -127,7 +127,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_P); /** * Proportional gain for horizontal velocity error @@ -135,7 +135,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_P); /** * Integral gain for horizontal velocity error @@ -145,7 +145,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_I); /** * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. @@ -153,7 +153,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_D); /** * Maximum horizontal velocity @@ -164,7 +164,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_MAX); /** * Horizontal velocity feed forward @@ -175,7 +175,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); * @max 1.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_FF); /** * Maximum tilt angle in air @@ -187,7 +187,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); * @max 90.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); +PX4_PARAM_DEFINE_FLOAT(MPP_TILTMAX_AIR); /** * Maximum tilt during landing @@ -199,7 +199,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); * @max 90.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); +PX4_PARAM_DEFINE_FLOAT(MPP_TILTMAX_LND); /** * Landing descend rate @@ -208,7 +208,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); +PX4_PARAM_DEFINE_FLOAT(MPP_LAND_SPEED); /** * Max manual roll @@ -218,7 +218,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); * @max 90.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_MAN_R_MAX); /** * Max manual pitch @@ -228,7 +228,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX); * @max 90.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_MAN_P_MAX); /** * Max manual yaw rate @@ -237,5 +237,5 @@ PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_MAN_Y_MAX); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h index 8c8b707ae..d9c9fb595 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -41,24 +41,24 @@ #pragma once -#define PARAM_MPC_THR_MIN_DEFAULT 0.1f -#define PARAM_MPC_THR_MAX_DEFAULT 1.0f -#define PARAM_MPC_Z_P_DEFAULT 1.0f -#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f -#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f -#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f -#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f -#define PARAM_MPC_Z_FF_DEFAULT 0.5f -#define PARAM_MPC_XY_P_DEFAULT 1.0f -#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f -#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f -#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f -#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f -#define PARAM_MPC_XY_FF_DEFAULT 0.5f -#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f -#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f -#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f -#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f -#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f -#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f +#define PARAM_MPP_THR_MIN_DEFAULT 0.1f +#define PARAM_MPP_THR_MAX_DEFAULT 1.0f +#define PARAM_MPP_Z_P_DEFAULT 1.0f +#define PARAM_MPP_Z_VEL_P_DEFAULT 0.1f +#define PARAM_MPP_Z_VEL_I_DEFAULT 0.02f +#define PARAM_MPP_Z_VEL_D_DEFAULT 0.0f +#define PARAM_MPP_Z_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPP_Z_FF_DEFAULT 0.5f +#define PARAM_MPP_XY_P_DEFAULT 1.0f +#define PARAM_MPP_XY_VEL_P_DEFAULT 0.1f +#define PARAM_MPP_XY_VEL_I_DEFAULT 0.02f +#define PARAM_MPP_XY_VEL_D_DEFAULT 0.01f +#define PARAM_MPP_XY_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPP_XY_FF_DEFAULT 0.5f +#define PARAM_MPP_TILTMAX_AIR_DEFAULT 45.0f +#define PARAM_MPP_TILTMAX_LND_DEFAULT 15.0f +#define PARAM_MPP_LAND_SPEED_DEFAULT 1.0f +#define PARAM_MPP_MAN_R_MAX_DEFAULT 35.0f +#define PARAM_MPP_MAN_P_MAX_DEFAULT 35.0f +#define PARAM_MPP_MAN_Y_MAX_DEFAULT 120.0f -- cgit v1.2.3