diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk index d048477455..bc4b48fb40 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/multirotor_pos_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = multirotor_pos_control SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c + multirotor_pos_control_params.c \ + thrust_pid.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index acae03fae0..d56c3d58f8 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,9 +61,11 @@ #include #include #include +#include #include #include "multirotor_pos_control_params.h" +#include "thrust_pid.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -84,8 +86,6 @@ 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) { @@ -110,11 +110,12 @@ int multirotor_pos_control_main(int argc, char *argv[]) { if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor_pos_control already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } + warnx("start"); thread_should_exit = false; deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, @@ -126,15 +127,16 @@ int multirotor_pos_control_main(int argc, char *argv[]) { } if (!strcmp(argv[1], "stop")) { + warnx("stop"); thread_should_exit = true; exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor_pos_control app is running\n"); + warnx("app is running"); } else { - printf("\tmultirotor_pos_control app not started\n"); + warnx("app not started"); } exit(0); } @@ -153,22 +155,13 @@ static float scale_control(float ctl, float end, float dz) { } } -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 */ - warnx("started."); + warnx("started"); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); @@ -203,15 +196,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { bool reset_sp_alt = true; bool reset_sp_pos = true; hrt_abstime t_prev = 0; - float alt_integral = 0.0f; /* 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; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; + static PID_t xy_pos_pids[2]; + static PID_t xy_vel_pids[2]; + static PID_t z_pos_pid; + static thrust_pid_t z_vel_pid; + thread_running = true; struct multirotor_position_control_params params; @@ -219,6 +214,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + } + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + int paramcheck_counter = 0; while (!thread_should_exit) { @@ -231,6 +233,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_check(param_sub, ¶m_updated); if (param_updated) { parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_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); } paramcheck_counter = 0; } @@ -269,7 +277,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; - alt_integral = manual.throttle; + z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } @@ -277,18 +285,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - pos_x_integral = 0.0f; - pos_y_integral = 0.0f; + xy_vel_pids[0].integral = 0.0f; + xy_vel_pids[1].integral = 0.0f; mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } - 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 z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; + float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float pos_sp_speed_x = 0.0f; - float pos_sp_speed_y = 0.0f; - float pos_sp_speed_z = 0.0f; + float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; + /* manual control */ if (status.flag_control_manual_enabled) { if (local_pos.home_timestamp != home_alt_t) { if (home_alt_t != 0) { @@ -299,14 +306,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { home_alt = local_pos.home_alt; } /* 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) { - 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) { - local_pos_sp.z = local_pos.z - alt_err_linear_limit; + float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (z_sp_ctl != 0.0f) { + sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; + local_pos_sp.z += sp_move_rate[2] * dt; + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { + local_pos_sp.z = local_pos.z + z_sp_offs_max; + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { + local_pos_sp.z = local_pos.z - z_sp_offs_max; } } @@ -316,76 +323,84 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max; - pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed; - pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed; - local_pos_sp.x += pos_sp_speed_x * dt; - local_pos_sp.y += pos_sp_speed_y * dt; + float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; + sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + local_pos_sp.x += sp_move_rate[0] * dt; + local_pos_sp.y += sp_move_rate[1] * 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; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; 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; } } } - - 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_sp.z - local_pos.z, alt_err_linear_limit); - /* P and D components */ - 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) { - alt_integral = params.thr_min; - } 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) { - thrust_ctl = params.thr_max; - } + /* run position & altitude controllers, calculate velocity setpoint */ + float vel_sp[3] = { 0.0f, 0.0f, 0.0f }; + vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { - /* 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 + (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 */ - 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; + /* calculate velocity set point in NED frame */ + vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); + vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); } else { reset_sp_pos = true; } - att_sp.thrust = thrust_ctl; + /* calculate direction and norm of thrust in NED frame + * limit 3D speed by ellipsoid: + * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */ + float v; + float vel_sp_norm = 0.0f; + v = vel_sp[0] / params.xy_vel_max; + vel_sp_norm += v * v; + v = vel_sp[1] / params.xy_vel_max; + vel_sp_norm += v * v; + v = vel_sp[2] / params.z_vel_max; + vel_sp_norm += v * v; + vel_sp_norm = sqrtf(vel_sp_norm); + if (vel_sp_norm > 1.0f) { + vel_sp[0] /= vel_sp_norm; + vel_sp[1] /= vel_sp_norm; + vel_sp[2] /= vel_sp_norm; + } + + /* run velocity controllers, calculate thrust vector */ + float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + /* calculate velocity set point in NED frame */ + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt); + } + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ + /* limit horizontal part of thrust */ + float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); + float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + if (thrust_xy_norm > params.slope_max) { + thrust_xy_norm = params.slope_max; + } + /* use approximation: slope ~ sin(slope) = force */ + /* convert direction to body frame */ + thrust_xy_dir -= att.yaw; + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + /* calculate roll and pitch */ + att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; + att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch + } + /* attitude-thrust compensation */ + 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; + att_sp.thrust = -thrust_sp[2] * att_comp; att_sp.timestamp = hrt_absolute_time(); if (status.flag_control_manual_enabled) { /* publish local position setpoint in manual mode */ @@ -403,8 +418,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } - printf("[multirotor_pos_control] exiting\n"); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped"); thread_running = false; 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 d284de433c..f8a982c6c0 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -44,31 +44,37 @@ /* controller parameters */ 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.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, 10.0f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_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) { h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); - 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->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->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->slope_max = param_find("MPC_SLOPE_MAX"); - h->hard = param_find("MPC_HARD"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); @@ -81,16 +87,19 @@ 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->alt_p, &(p->alt_p)); - 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->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->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->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)); 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 13b667ad30..e9b1f5c390 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -44,16 +44,19 @@ struct multirotor_position_control_params { float thr_min; float thr_max; - float alt_p; - float alt_i; - float alt_d; - float alt_rate_max; - float pos_p; - float pos_i; - float pos_d; - float pos_rate_max; + float z_p; + float z_d; + float z_vel_p; + float z_vel_i; + float z_vel_d; + float z_vel_max; + float xy_p; + float xy_d; + float xy_vel_p; + float xy_vel_i; + float xy_vel_d; + float xy_vel_max; float slope_max; - int hard; float rc_scale_pitch; float rc_scale_roll; @@ -63,16 +66,19 @@ struct multirotor_position_control_params { struct multirotor_position_control_param_handles { param_t thr_min; param_t thr_max; - param_t alt_p; - 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 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 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 slope_max; - param_t hard; param_t rc_scale_pitch; param_t rc_scale_roll; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c new file mode 100644 index 0000000000..89efe1334e --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Anton Babushkin + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file thrust_pid.c + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Anton Babushkin + * @author Julian Oes + */ + +#include "thrust_pid.h" +#include + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->limit_min = limit_min; + pid->limit_max = limit_max; + pid->mode = mode; + pid->dt_min = dt_min; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; +} + +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(limit_min)) { + pid->limit_min = limit_min; + + } else { + ret = 1; + } + + if (isfinite(limit_max)) { + pid->limit_max = limit_max; + + } else { + ret = 1; + } + + return ret; +} + +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt) +{ + /* Alternative integral component calculation + error = setpoint - actual_position + integral = integral + (Ki*error*dt) + derivative = (error - previous_error)/dt + output = (Kp*error) + integral + (Kd*derivative) + previous_error = error + wait(dt) + goto start + */ + + if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + // Calculate or measured current error derivative + if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; + + } else { + d = 0.0f; + } + + if (!isfinite(d)) { + d = 0.0f; + } + + // Calculate the error integral and check for saturation + i = pid->integral + (pid->ki * error * dt); + + float output = (error * pid->kp) + i + (d * pid->kd); + if (output < pid->limit_min || output > pid->limit_max) { + i = pid->integral; // If saturated then do not update integral value + // recalculate output with old integral + output = (error * pid->kp) + i + (d * pid->kd); + + } else { + if (!isfinite(i)) { + i = 0.0f; + } + + pid->integral = i; + } + + if (isfinite(output)) { + if (output > pid->limit_max) { + output = pid->limit_max; + + } else if (output < pid->limit_min) { + output = pid->limit_min; + } + + pid->last_output = output; + } + + return pid->last_output; +} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h new file mode 100644 index 0000000000..65ee33c51c --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file thrust_pid.h + * + * Definition of thrust control PID interface. + * + * @author Anton Babushkin + */ + +#ifndef THRUST_PID_H_ +#define THRUST_PID_H_ + +#include + +__BEGIN_DECLS + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ +#define THRUST_PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ +#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 + +typedef struct { + float kp; + float ki; + float kd; + float sp; + float integral; + float error_previous; + float last_output; + float limit_min; + float limit_max; + float dt_min; + uint8_t mode; +} thrust_pid_t; + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt); + +__END_DECLS + +#endif /* THRUST_PID_H_ */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index fb5a779bc0..9db2633c68 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -120,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[]) verbose_mode = true; thread_should_exit = false; - position_estimator_inav_task = task_spawn("position_estimator_inav", + position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL);