From 38ca3bd78a9b415df4a260a8cba43e2c13703972 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 15 Jun 2013 11:36:26 +0400 Subject: multirotor_pos_control fixes, introduced HARD control mode (disabled by default) --- .../multirotor_pos_control.c | 43 ++++++++++++++-------- .../multirotor_pos_control_params.c | 11 ++++++ .../multirotor_pos_control_params.h | 10 +++++ 3 files changed, 49 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index ad5670edc..c94972e7d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -283,11 +283,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; + float pos_sp_speed_x = 0.0f; + float pos_sp_speed_y = 0.0f; + float pos_sp_speed_z = 0.0f; + if (status.flag_control_manual_enabled) { /* move altitude setpoint with manual controls */ float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (alt_sp_ctl != 0.0f) { - local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt; + pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max; + local_pos_sp.z += pos_sp_speed_z * dt; if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { local_pos_sp.z = local_pos.z + alt_err_linear_limit; } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { @@ -297,14 +302,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { /* move position setpoint with manual controls */ - float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz); - float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz); + float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); - float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt; - local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl; - local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl; + float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max; + pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl; + pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl; + local_pos_sp.x += pos_sp_speed_x * dt; + local_pos_sp.y += pos_sp_speed_y * dt; /* limit maximum setpoint from position offset and preserve direction */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; @@ -315,15 +322,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } } } + + if (params.hard == 0) { + pos_sp_speed_x = 0.0f; + pos_sp_speed_y = 0.0f; + pos_sp_speed_z = 0.0f; + } } /* PID for altitude */ /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit); + float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit); /* P and D components */ - float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; - /* add I component */ - float thrust_ctl = thrust_ctl_pd + alt_integral; + float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d); // altitude = -z /* integrate */ alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; if (alt_integral < params.thr_min) { @@ -331,6 +342,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } else if (alt_integral > params.thr_max) { alt_integral = params.thr_max; } + /* add I component */ + float thrust_ctl = thrust_ctl_pd + alt_integral; if (thrust_ctl < params.thr_min) { thrust_ctl = params.thr_min; } else if (thrust_ctl > params.thr_max) { @@ -342,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); /* P and D components */ - float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d; - float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d; - /* add I component */ - float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; - float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; + float pos_x_ctl_pd = - pos_x_err * params.pos_p + (pos_sp_speed_x - local_pos.vx) * params.pos_d; + float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d; /* integrate */ pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + /* add I component */ + float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; + float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; /* calculate direction and slope in NED frame */ float dir = atan2f(pos_y_ctl, pos_x_ctl); /* use approximation: slope ~ sin(slope) = force */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 7f2ba3db8..7c3296cae 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -53,6 +53,7 @@ PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); +PARAM_DEFINE_INT32(MPC_HARD, 0); int parameters_init(struct multirotor_position_control_param_handles *h) { @@ -67,6 +68,11 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->pos_d = param_find("MPC_POS_D"); h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); + h->slope_max = param_find("MPC_HARD"); + + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); return OK; } @@ -84,6 +90,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->pos_d, &(p->pos_d)); param_get(h->pos_rate_max, &(p->pos_rate_max)); param_get(h->slope_max, &(p->slope_max)); + param_get(h->hard, &(p->hard)); + + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 589ee92a1..13b667ad3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -53,6 +53,11 @@ struct multirotor_position_control_params { float pos_d; float pos_rate_max; float slope_max; + int hard; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; }; struct multirotor_position_control_param_handles { @@ -67,6 +72,11 @@ struct multirotor_position_control_param_handles { param_t pos_d; param_t pos_rate_max; param_t slope_max; + param_t hard; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; }; /** -- cgit v1.2.3