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

multirotor_pos_control: use separate PID controllers for position and velocity

This commit is contained in:
Anton Babushkin 2013-07-09 14:09:48 +04:00
parent 320a5b7579
commit 04fbed493a
7 changed files with 423 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/multirotor_pos_control/module.mk
View File

@ -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

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

@ -61,9 +61,11 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <systemlib/systemlib.h>
#include <systemlib/pid/pid.h>
#include <mavlink/mavlink_log.h>
#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(&params_h);
parameters_update(&params_h, &params);
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, &param_updated);
if (param_updated) {
parameters_update(&params_h, &params);
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;

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

@ -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));

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

@ -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;

179
src/modules/multirotor_pos_control/thrust_pid.c Normal file
View File

@ -0,0 +1,179 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Julian Oes <joes@student.ethz.ch>
*
* 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 <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include "thrust_pid.h"
#include <math.h>
__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;
}

75
src/modules/multirotor_pos_control/thrust_pid.h Normal file
View File

@ -0,0 +1,75 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* 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 <anton.babushkin@me.com>
*/
#ifndef THRUST_PID_H_
#define THRUST_PID_H_
#include <stdint.h>
__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_ */

2
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -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);