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

multirotor_pos_control fixes, introduced HARD control mode (disabled by default)

This commit is contained in:
Anton Babushkin 2013-06-15 11:36:26 +04:00
parent e4b25f8570
commit 38ca3bd78a
3 changed files with 49 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -283,11 +283,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
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 pos_sp_speed_x = 0.0f;
float pos_sp_speed_y = 0.0f;
float pos_sp_speed_z = 0.0f;
if (status.flag_control_manual_enabled) {
/* 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) {
local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt;
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) {
@ -297,14 +302,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
/* 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);
float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_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;
float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max;
pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl;
pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl;
local_pos_sp.x += pos_sp_speed_x * dt;
local_pos_sp.y += pos_sp_speed_y * 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;
@ -315,15 +322,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
}
}
}
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.z - local_pos_sp.z, alt_err_linear_limit);
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 + local_pos.vz * params.alt_d;
/* add I component */
float thrust_ctl = thrust_ctl_pd + alt_integral;
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) {
@ -331,6 +342,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
} 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) {
@ -342,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
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;
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 */

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

@ -53,6 +53,7 @@ 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_SLOPE_MAX, 0.5f);
PARAM_DEFINE_INT32(MPC_HARD, 0);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
@ -67,6 +68,11 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
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");
h->slope_max = param_find("MPC_HARD");
h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
h->rc_scale_roll = param_find("RC_SCALE_ROLL");
h->rc_scale_yaw = param_find("RC_SCALE_YAW");
return OK;
}
@ -84,6 +90,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
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));
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));
param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
return OK;
}

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

@ -53,6 +53,11 @@ struct multirotor_position_control_params {
float pos_d;
float pos_rate_max;
float slope_max;
int hard;
float rc_scale_pitch;
float rc_scale_roll;
float rc_scale_yaw;
};
struct multirotor_position_control_param_handles {
@ -67,6 +72,11 @@ struct multirotor_position_control_param_handles {
param_t pos_d;
param_t pos_rate_max;
param_t slope_max;
param_t hard;
param_t rc_scale_pitch;
param_t rc_scale_roll;
param_t rc_scale_yaw;
};
/**