From faa3826de6c47c5516df7b4c01b70c96f9e9d9d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 15 Dec 2013 11:47:26 +0400 Subject: multirotor_pos_control: fixes and improvements --- .../multirotor_pos_control.c | 184 +++++++++++++++------ 1 file changed, 132 insertions(+), 52 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a0541d41a..cf2d5f931 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -171,42 +171,67 @@ static float norm(float x, float y) return sqrtf(x * x + y * y); } -static void cross3(float a[3], float b[3], float res[3]) { +static void cross3(float a[3], float b[3], float res[3]) +{ res[0] = a[1] * b[2] - a[2] * b[1]; res[1] = a[2] * b[0] - a[0] * b[2]; res[2] = a[0] * b[1] - a[1] * b[0]; } +static float normalize3(float x[3]) +{ + float n = sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); + + if (n > 0.0f) { + x[0] /= n; + x[1] /= n; + x[2] /= n; + } + + return n; +} + static float rt_atan2f_snf(float u0, float u1) { float y; int32_t b_u0; int32_t b_u1; + if (isnanf(u0) || isnanf(u1)) { y = NAN; + } else if (isinff(u0) && isinff(u1)) { if (u0 > 0.0f) { b_u0 = 1; + } else { b_u0 = -1; } + if (u1 > 0.0f) { b_u1 = 1; + } else { b_u1 = -1; } + y = atan2f((float)b_u0, (float)b_u1); + } else if (u1 == 0.0f) { if (u0 > 0.0f) { y = M_PI_F / 2.0f; + } else if (u0 < 0.0f) { y = -(M_PI_F / 2.0f); + } else { y = 0.0F; } + } else { y = atan2f(u0, u1); } + return y; } @@ -272,9 +297,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; - PID_t xy_vel_pids[2]; PID_t z_pos_pid; - thrust_pid_t z_vel_pid; float thrust_int[3] = { 0.0f, 0.0f, 0.0f }; thread_running = true; @@ -286,11 +309,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, 0.02f); bool param_updated = true; @@ -307,23 +328,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* update params */ parameters_update(¶ms_h, ¶ms); - /* integral_limit * ki = tilt_max / 2 */ - float i_limit; - - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - - } else { - i_limit = 0.0f; // not used - } - 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(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } bool updated; @@ -367,7 +376,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) t_prev = t; - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { + 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); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); @@ -420,8 +429,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } @@ -638,48 +645,67 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* calculate desired thrust vector in NED frame */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - if (control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = params.thr_min; - if (reset_int_z_manual) { - i = manual.throttle; + if (reset_int_z) { + reset_int_z = false; + float i = params.thr_min; - if (i < params.thr_min) { - i = params.thr_min; + if (reset_int_z_manual) { + i = manual.throttle; - } else if (i > params.thr_max) { - i = params.thr_max; - } - } + if (i < params.thr_min) { + i = params.thr_min; - thrust_int[2] = -i; - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } else if (i > params.thr_max) { + i = params.thr_max; + } } - thrust_int[2] += (global_vel_sp.vz - local_pos.vz) * params.z_vel_i * dt; - thrust_sp[2] = (global_vel_sp.vz - local_pos.vz) * params.z_vel_p + thrust_int[2]; - if (-thrust_sp[2] < params.thr_min) - thrust_sp[2] = -params.thr_min; - } else { - reset_int_z = true; + + thrust_int[2] = -i; + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } + + thrust_sp[2] = (global_vel_sp.vz - local_pos.vz) * params.z_vel_p + thrust_int[2]; + if (control_mode.flag_control_velocity_enabled) { if (reset_int_xy) { reset_int_xy = false; thrust_int[0] = 0.0f; thrust_int[1] = 0.0f; - mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); + mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); } - thrust_int[0] += (global_vel_sp.vx - local_pos.vx) * params.xy_vel_i * dt; - thrust_int[1] += (global_vel_sp.vy - local_pos.vy) * params.xy_vel_i * dt; + thrust_sp[0] = (global_vel_sp.vx - local_pos.vx) * params.xy_vel_p + thrust_int[0]; thrust_sp[1] = (global_vel_sp.vy - local_pos.vy) * params.xy_vel_p + thrust_int[1]; + } else { reset_int_xy = true; } + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + if (-thrust_sp[2] < params.thr_min) { + thrust_sp[2] = -params.thr_min; + saturation_z = true; + } + + /* limit max tilt */ + float tilt = atan2f(norm(thrust_sp[0], thrust_sp[1]), -thrust_sp[2]); + + if (tilt > params.tilt_max && params.thr_min > 0.0f) { + /* crop horizontal component */ + float k = tanf(params.tilt_max) / tanf(tilt); + thrust_sp[0] *= k; + thrust_sp[1] *= k; + saturation_xy = true; + } + + /* limit max thrust */ float thrust_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1] + thrust_sp[2] * thrust_sp[2]); + if (thrust_abs > params.thr_max) { if (thrust_sp[2] < 0.0f) { if (-thrust_sp[2] > params.thr_max) { @@ -687,13 +713,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) thrust_sp[0] = 0.0f; thrust_sp[1] = 0.0f; thrust_sp[2] = -params.thr_max; + saturation_xy = true; + saturation_z = true; + } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ float thrust_xy_max = sqrtf(params.thr_max * params.thr_max - thrust_sp[2] * thrust_sp[2]); - float thrust_xy_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1]); + float thrust_xy_abs = norm(thrust_sp[0], thrust_sp[1]); float k = thrust_xy_max / thrust_xy_abs; thrust_sp[0] *= k; thrust_sp[1] *= k; + saturation_xy = true; } } else { @@ -702,59 +732,106 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) thrust_sp[0] *= k; thrust_sp[1] *= k; thrust_sp[2] *= k; + saturation_xy = true; + saturation_z = true; } + thrust_abs = params.thr_max; } + /* update integrals */ + if (control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int[0] += (global_vel_sp.vx - local_pos.vx) * params.xy_vel_i * dt; + thrust_int[1] += (global_vel_sp.vy - local_pos.vy) * params.xy_vel_i * dt; + } + + if (control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int[2] += (global_vel_sp.vz - local_pos.vz) * params.z_vel_i * dt; + } + /* calculate attitude and thrust from thrust vector */ if (control_mode.flag_control_velocity_enabled) { - /* vector of desired yaw direction in XY plane, rotated by PI/2 */ - float y_C[3]; - y_C[0] = -sinf(att_sp.yaw_body); - y_C[1] = cosf(att_sp.yaw_body); - y_C[2] = 0; - /* desired body_z axis = -normalize(thrust_vector) */ float body_x[3]; float body_y[3]; float body_z[3]; + if (thrust_abs > 0.0f) { body_z[0] = -thrust_sp[0] / thrust_abs; body_z[1] = -thrust_sp[1] / thrust_abs; body_z[2] = -thrust_sp[2] / thrust_abs; + } else { body_z[0] = 0.0f; body_z[1] = 0.0f; body_z[2] = -1.0f; } - /* desired body_x axis = cross(x_C, body_z) */ + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + float y_C[3]; + y_C[0] = -sinf(att_sp.yaw_body); + y_C[1] = cosf(att_sp.yaw_body); + y_C[2] = 0; + + /* desired body_x axis = cross(y_C, body_z) */ cross3(y_C, body_z, body_x); + float body_x_norm = normalize3(body_x); + static const float body_x_norm_max = 0.5f; /* desired body_y axis = cross(body_z, body_x) */ cross3(body_z, body_x, body_y); + if (body_x_norm < body_x_norm_max) { + /* roll is close to +/- PI/2, don't try to hold yaw exactly */ + float x_C[3]; + x_C[0] = cos(att_sp.yaw_body); + x_C[1] = sinf(att_sp.yaw_body); + x_C[2] = 0; + + float body_y_1[3]; + /* desired body_y axis for approximate yaw = cross(body_z, x_C) */ + cross3(body_z, x_C, body_y_1); + + float w = body_x_norm / body_x_norm_max; + float w1 = 1.0f - w; + + /* mix two body_y axes */ + body_y[0] = body_y[0] * w + body_y_1[0] * w1; + body_y[1] = body_y[1] * w + body_y_1[1] * w1; + body_y[2] = body_y[2] * w + body_y_1[2] * w1; + + normalize3(body_y); + + /* desired body_x axis = cross(body_y, body_z) */ + cross3(body_y, body_z, body_x); + } + /* fill rotation matrix */ for (int i = 0; i < 3; i++) { att_sp.R_body[i][0] = body_x[i]; att_sp.R_body[i][1] = body_y[i]; att_sp.R_body[i][2] = body_z[i]; } + att_sp.R_valid = true; - /* calculate roll, pitch from rotation matrix, yaw already used to construct rot matrix */ + /* calculate roll, pitch from rotation matrix */ att_sp.roll_body = rt_atan2f_snf(att_sp.R_body[2][1], att_sp.R_body[2][2]); att_sp.pitch_body = -asinf(att_sp.R_body[2][0]); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ //att_sp.yaw_body = rt_atan2f_snf(att_sp.R_body[1][0], att_sp.R_body[0][0]); } else { /* thrust compensation for altitude only control mode */ float att_comp; + if (att.R[2][2] > 0.8f) att_comp = 1.0f / att.R[2][2]; else if (att.R[2][2] > 0.0f) att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; else att_comp = 1.0f; + thrust_abs *= att_comp; } @@ -764,6 +841,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + } else { + reset_int_z = true; } } else { -- cgit v1.2.3