diff options
3 files changed, 126 insertions, 34 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6fe129d84..ad5670edc 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -42,6 +42,7 @@ #include <stdio.h> #include <stdlib.h> #include <string.h> +#include <math.h> #include <stdbool.h> #include <unistd.h> #include <fcntl.h> @@ -59,7 +60,6 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_vicon_position.h> #include <systemlib/systemlib.h> #include <mavlink/mavlink_log.h> @@ -82,6 +82,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +static float scale_control(float ctl, float end, float dz); + +static float limit_value(float v, float limit); + +static float norm(float x, float y); + static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -137,9 +143,32 @@ int multirotor_pos_control_main(int argc, char *argv[]) { exit(1); } +static float scale_control(float ctl, float end, float dz) { + if (ctl > dz) { + return (ctl - dz) / (end - dz); + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + } else { + return 0.0f; + } +} + +static float limit_value(float v, float limit) { + if (v > limit) { + v = limit; + } else if (v < -limit) { + v = -limit; + } + return v; +} + +static float norm(float x, float y) { + return sqrtf(x * x + y * y); +} + static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor_pos_control] started\n"); + warnx("started."); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); @@ -175,7 +204,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { bool reset_sp_pos = true; hrt_abstime t_prev = 0; float alt_integral = 0.0f; - float alt_ctl_dz = 0.2f; + /* integrate in NED frame to estimate wind but not attitude offset */ + float pos_x_integral = 0.0f; + float pos_y_integral = 0.0f; + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; thread_running = true; @@ -235,64 +268,97 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; alt_integral = manual.throttle; - char str[80]; - sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - char str[80]; - sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); - mavlink_log_info(mavlink_fd, str); + pos_x_integral = 0.0f; + pos_y_integral = 0.0f; + mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } - float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + 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; if (status.flag_control_manual_enabled) { /* move altitude setpoint with manual controls */ - float alt_ctl = manual.throttle - 0.5f; - if (fabs(alt_ctl) < alt_ctl_dz) { - alt_ctl = 0.0f; - } else { - if (alt_ctl > 0.0f) - alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz); - else - alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz); - local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt; - if (local_pos_sp.z > local_pos.z + err_linear_limit) - local_pos_sp.z = local_pos.z + err_linear_limit; - else if (local_pos_sp.z < local_pos.z - err_linear_limit) - local_pos_sp.z = local_pos.z - err_linear_limit; + 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; + 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) { + local_pos_sp.z = local_pos.z - alt_err_linear_limit; + } } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - // TODO move position setpoint with manual controls + /* 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); + 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; + /* 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; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit; + if (pos_vec_norm > 1.0f) { + local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; + local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; + } + } } } /* PID for altitude */ - float alt_err = local_pos.z - local_pos_sp.z; /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - if (alt_err > err_linear_limit) { - alt_err = err_linear_limit; - } else if (alt_err < -err_linear_limit) { - alt_err = -err_linear_limit; - } + float alt_err = limit_value(local_pos.z - local_pos_sp.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; + /* integrate */ alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + if (alt_integral < params.thr_min) { + alt_integral = params.thr_min; + } else if (alt_integral > params.thr_max) { + alt_integral = params.thr_max; + } if (thrust_ctl < params.thr_min) { thrust_ctl = params.thr_min; } else if (thrust_ctl > params.thr_max) { thrust_ctl = params.thr_max; } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { - // TODO add position controller + /* PID for position */ + /* don't accelerate more than POS_RATE_MAX, limit error to corresponding value */ + 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; + /* 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); + /* calculate direction and slope in NED frame */ + float dir = atan2f(pos_y_ctl, pos_x_ctl); + /* use approximation: slope ~ sin(slope) = force */ + float slope = limit_value(sqrtf(pos_x_ctl * pos_x_ctl + pos_y_ctl * pos_y_ctl), params.slope_max); + /* convert direction to body frame */ + dir -= att.yaw; + /* calculate roll and pitch */ + att_sp.pitch_body = -cosf(dir) * slope; // reverse pitch + att_sp.roll_body = sinf(dir) * slope; } else { reset_sp_pos = true; } 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 90dd820f4..9610ef9f7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -45,19 +45,29 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f); -PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f); +PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f); PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.3f); 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"); - /* PID parameters */ h->alt_p = param_find("MPC_ALT_P"); h->alt_i = param_find("MPC_ALT_I"); h->alt_d = param_find("MPC_ALT_D"); h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); + h->pos_p = param_find("MPC_POS_P"); + h->pos_i = param_find("MPC_POS_I"); + 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"); + return OK; } @@ -69,5 +79,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->alt_i, &(p->alt_i)); param_get(h->alt_d, &(p->alt_d)); param_get(h->alt_rate_max, &(p->alt_rate_max)); + param_get(h->pos_p, &(p->pos_p)); + param_get(h->pos_i, &(p->pos_i)); + 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)); + 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 849055cd1..589ee92a1 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -48,6 +48,11 @@ struct multirotor_position_control_params { float alt_i; float alt_d; float alt_rate_max; + float pos_p; + float pos_i; + float pos_d; + float pos_rate_max; + float slope_max; }; struct multirotor_position_control_param_handles { @@ -57,6 +62,11 @@ struct multirotor_position_control_param_handles { param_t alt_i; param_t alt_d; param_t alt_rate_max; + param_t pos_p; + param_t pos_i; + param_t pos_d; + param_t pos_rate_max; + param_t slope_max; }; /** |