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

multirotor_pos_control: remove unused parameters, use ellipsoid for velocity setpoint limiting

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-22 22:41:46 +04:00
parent fae6c69423
commit 4c6f6ed12c
3 changed files with 20 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/multirotor_pos_control/multirotor_pos_control.c
+20 -29
View File
@@ -173,6 +173,11 @@ static float norm(float x, float y)
return sqrtf(x * x + y * y);
}
static float norm3(float x, float y, float z)
{
return sqrtf(x * x + y * y + z * z);
}
static void cross3(float a[3], float b[3], float res[3])
{
res[0] = a[1] * b[2] - a[2] * b[1];
@@ -298,8 +303,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
hrt_abstime ref_alt_t = 0;
uint64_t local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t z_pos_pid;
float thrust_int[3] = { 0.0f, 0.0f, 0.0f };
thread_running = true;
@@ -309,12 +312,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
parameters_init(&params_h);
parameters_update(&params_h, &params);
for (int i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f);
}
pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f);
bool param_updated = true;
while (!thread_should_exit) {
@@ -329,12 +326,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* update params */
parameters_update(&params_h, &params);
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
}
bool updated;
@@ -362,6 +353,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
dt = 0.0f;
}
t_prev = t;
if (control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
@@ -376,8 +368,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
was_armed = control_mode.flag_armed;
t_prev = t;
if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_position_enabled || control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
@@ -612,7 +602,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2] * params.z_ff;
global_vel_sp.vz = (local_pos_sp.z - local_pos.z) * params.z_p + sp_move_rate[2] * params.z_ff;
} else {
reset_man_sp_z = true;
@@ -621,18 +611,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (control_mode.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0] * params.xy_ff;
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1] * params.xy_ff;
if (!control_mode.flag_control_manual_enabled) {
/* limit horizontal speed only in AUTO mode */
float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
if (xy_vel_sp_norm > 1.0f) {
global_vel_sp.vx /= xy_vel_sp_norm;
global_vel_sp.vy /= xy_vel_sp_norm;
}
}
global_vel_sp.vx = (local_pos_sp.x - local_pos.x) * params.xy_p + sp_move_rate[0] * params.xy_ff;
global_vel_sp.vy = (local_pos_sp.y - local_pos.y) * params.xy_p + sp_move_rate[1] * params.xy_ff;
} else {
reset_man_sp_xy = true;
@@ -640,6 +620,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
global_vel_sp.vy = 0.0f;
}
if (!control_mode.flag_control_manual_enabled) {
/* limit 3D speed only in AUTO mode */
float vel_sp_norm = norm3(global_vel_sp.vx / params.xy_vel_max, global_vel_sp.vy / params.xy_vel_max, global_vel_sp.vz / params.z_vel_max);
if (vel_sp_norm > 1.0f) {
global_vel_sp.vx /= vel_sp_norm;
global_vel_sp.vy /= vel_sp_norm;
global_vel_sp.vz /= vel_sp_norm;
}
}
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
// TODO subscribe to velocity setpoint if altitude/position control disabled
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
-12
View File
@@ -44,17 +44,13 @@
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
@@ -66,17 +62,13 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
h->thr_min = param_find("MPC_THR_MIN");
h->thr_max = param_find("MPC_THR_MAX");
h->z_p = param_find("MPC_Z_P");
h->z_d = param_find("MPC_Z_D");
h->z_vel_p = param_find("MPC_Z_VEL_P");
h->z_vel_i = param_find("MPC_Z_VEL_I");
h->z_vel_d = param_find("MPC_Z_VEL_D");
h->z_vel_max = param_find("MPC_Z_VEL_MAX");
h->z_ff = param_find("MPC_Z_FF");
h->xy_p = param_find("MPC_XY_P");
h->xy_d = param_find("MPC_XY_D");
h->xy_vel_p = param_find("MPC_XY_VEL_P");
h->xy_vel_i = param_find("MPC_XY_VEL_I");
h->xy_vel_d = param_find("MPC_XY_VEL_D");
h->xy_vel_max = param_find("MPC_XY_VEL_MAX");
h->xy_ff = param_find("MPC_XY_FF");
h->tilt_max = param_find("MPC_TILT_MAX");
@@ -95,17 +87,13 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
param_get(h->thr_min, &(p->thr_min));
param_get(h->thr_max, &(p->thr_max));
param_get(h->z_p, &(p->z_p));
param_get(h->z_d, &(p->z_d));
param_get(h->z_vel_p, &(p->z_vel_p));
param_get(h->z_vel_i, &(p->z_vel_i));
param_get(h->z_vel_d, &(p->z_vel_d));
param_get(h->z_vel_max, &(p->z_vel_max));
param_get(h->z_ff, &(p->z_ff));
param_get(h->xy_p, &(p->xy_p));
param_get(h->xy_d, &(p->xy_d));
param_get(h->xy_vel_p, &(p->xy_vel_p));
param_get(h->xy_vel_i, &(p->xy_vel_i));
param_get(h->xy_vel_d, &(p->xy_vel_d));
param_get(h->xy_vel_max, &(p->xy_vel_max));
param_get(h->xy_ff, &(p->xy_ff));
param_get(h->tilt_max, &(p->tilt_max));
src/modules/multirotor_pos_control/multirotor_pos_control_params.h
-8
View File
@@ -46,17 +46,13 @@ struct multirotor_position_control_params {
float thr_min;
float thr_max;
float z_p;
float z_d;
float z_vel_p;
float z_vel_i;
float z_vel_d;
float z_vel_max;
float z_ff;
float xy_p;
float xy_d;
float xy_vel_p;
float xy_vel_i;
float xy_vel_d;
float xy_vel_max;
float xy_ff;
float tilt_max;
@@ -72,17 +68,13 @@ struct multirotor_position_control_param_handles {
param_t thr_min;
param_t thr_max;
param_t z_p;
param_t z_d;
param_t z_vel_p;
param_t z_vel_i;
param_t z_vel_d;
param_t z_vel_max;
param_t z_ff;
param_t xy_p;
param_t xy_d;
param_t xy_vel_p;
param_t xy_vel_i;
param_t xy_vel_d;
param_t xy_vel_max;
param_t xy_ff;
param_t tilt_max;