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

multirotor_pos_control: position controller implemented

This commit is contained in:
Anton Babushkin 2013-06-13 06:48:24 +04:00
parent 4256e43de7
commit 4860c73008
3 changed files with 126 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

@ -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;
};
/**