dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

multirotor_pos_control: tunable velocity feed forward for SEATBELT/EASY modes

This commit is contained in:
Anton Babushkin 2013-12-21 18:52:21 +04:00
parent 05e9a30573
commit 38e5d2b0fb
3 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -612,7 +612,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* run position & altitude controllers, calculate velocity setpoint */ /* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) { 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]; 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;
} else { } else {
reset_man_sp_z = true; reset_man_sp_z = true;
@ -621,8 +621,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (control_mode.flag_control_position_enabled) { if (control_mode.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */ /* 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]; 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]; 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) { if (!control_mode.flag_control_manual_enabled) {
/* limit horizontal speed only in AUTO mode */ /* limit horizontal speed only in AUTO mode */

6
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
View File

@ -49,12 +49,14 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.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_P, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.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); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
int parameters_init(struct multirotor_position_control_param_handles *h) int parameters_init(struct multirotor_position_control_param_handles *h)
@ -69,12 +71,14 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
h->z_vel_i = param_find("MPC_Z_VEL_I"); h->z_vel_i = param_find("MPC_Z_VEL_I");
h->z_vel_d = param_find("MPC_Z_VEL_D"); h->z_vel_d = param_find("MPC_Z_VEL_D");
h->z_vel_max = param_find("MPC_Z_VEL_MAX"); 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_p = param_find("MPC_XY_P");
h->xy_d = param_find("MPC_XY_D"); h->xy_d = param_find("MPC_XY_D");
h->xy_vel_p = param_find("MPC_XY_VEL_P"); h->xy_vel_p = param_find("MPC_XY_VEL_P");
h->xy_vel_i = param_find("MPC_XY_VEL_I"); h->xy_vel_i = param_find("MPC_XY_VEL_I");
h->xy_vel_d = param_find("MPC_XY_VEL_D"); h->xy_vel_d = param_find("MPC_XY_VEL_D");
h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); 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"); h->tilt_max = param_find("MPC_TILT_MAX");
h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
@ -96,12 +100,14 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
param_get(h->z_vel_i, &(p->z_vel_i)); 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_d, &(p->z_vel_d));
param_get(h->z_vel_max, &(p->z_vel_max)); 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_p, &(p->xy_p));
param_get(h->xy_d, &(p->xy_d)); param_get(h->xy_d, &(p->xy_d));
param_get(h->xy_vel_p, &(p->xy_vel_p)); 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_i, &(p->xy_vel_i));
param_get(h->xy_vel_d, &(p->xy_vel_d)); param_get(h->xy_vel_d, &(p->xy_vel_d));
param_get(h->xy_vel_max, &(p->xy_vel_max)); 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)); param_get(h->tilt_max, &(p->tilt_max));
param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));

4
src/modules/multirotor_pos_control/multirotor_pos_control_params.h
View File

@ -51,12 +51,14 @@ struct multirotor_position_control_params {
float z_vel_i; float z_vel_i;
float z_vel_d; float z_vel_d;
float z_vel_max; float z_vel_max;
float z_ff;
float xy_p; float xy_p;
float xy_d; float xy_d;
float xy_vel_p; float xy_vel_p;
float xy_vel_i; float xy_vel_i;
float xy_vel_d; float xy_vel_d;
float xy_vel_max; float xy_vel_max;
float xy_ff;
float tilt_max; float tilt_max;
float rc_scale_pitch; float rc_scale_pitch;
@ -75,12 +77,14 @@ struct multirotor_position_control_param_handles {
param_t z_vel_i; param_t z_vel_i;
param_t z_vel_d; param_t z_vel_d;
param_t z_vel_max; param_t z_vel_max;
param_t z_ff;
param_t xy_p; param_t xy_p;
param_t xy_d; param_t xy_d;
param_t xy_vel_p; param_t xy_vel_p;
param_t xy_vel_i; param_t xy_vel_i;
param_t xy_vel_d; param_t xy_vel_d;
param_t xy_vel_max; param_t xy_vel_max;
param_t xy_ff;
param_t tilt_max; param_t tilt_max;
param_t rc_scale_pitch; param_t rc_scale_pitch;