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

multirotor_pos_control: some refactoring and cleanup, attitude-thrust correction moved to thrust_pid

This commit is contained in:
Anton Babushkin 2013-07-21 22:42:45 +04:00
parent 1dac58571e
commit 98a4345410
5 changed files with 54 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
for (int i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 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(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
@ -259,7 +259,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
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(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
@ -416,12 +416,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
}
/* run position & altitude controllers, calculate velocity setpoint */
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt);
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt);
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt);
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
/* limit horizontal speed */
float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
@ -439,9 +439,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
/* run velocity controllers, calculate thrust vector */
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt);
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
/* calculate velocity set point in NED frame */
@ -452,33 +452,24 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* 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]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (thrust_xy_norm > params.slope_max) {
thrust_xy_norm = params.slope_max;
if (tilt > params.tilt_max) {
tilt = params.tilt_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
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
}
/* 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.thrust = -thrust_sp[2];
att_sp.timestamp = hrt_absolute_time();
if (status.flag_control_manual_enabled) {

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

@ -56,7 +56,7 @@ 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_FLOAT(MPC_TILT_MAX, 0.5f);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
@ -74,7 +74,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
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->tilt_max = param_find("MPC_TILT_MAX");
h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
h->rc_scale_roll = param_find("RC_SCALE_ROLL");
@ -99,7 +99,7 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
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->tilt_max, &(p->tilt_max));
param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
param_get(h->rc_scale_roll, &(p->rc_scale_roll));

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

@ -56,7 +56,7 @@ struct multirotor_position_control_params {
float xy_vel_i;
float xy_vel_d;
float xy_vel_max;
float slope_max;
float tilt_max;
float rc_scale_pitch;
float rc_scale_roll;
@ -78,7 +78,7 @@ struct multirotor_position_control_param_handles {
param_t xy_vel_i;
param_t xy_vel_d;
param_t xy_vel_max;
param_t slope_max;
param_t tilt_max;
param_t rc_scale_pitch;
param_t rc_scale_roll;

61
src/modules/multirotor_pos_control/thrust_pid.c
View File

@ -1,11 +1,7 @@
/****************************************************************************
*
* 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>
* 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
@ -39,13 +35,9 @@
/**
* @file thrust_pid.c
*
* Implementation of generic PID control interface.
* Implementation of thrust control PID.
*
* @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"
@ -108,16 +100,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl
return ret;
}
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt)
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
{
/* 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
*
* start:
* error = setpoint - current_value
* integral = integral + (Ki * error * dt)
* derivative = (error - previous_error) / dt
* previous_error = error
* output = (Kp * error) + integral + (Kd * derivative)
* wait(dt)
* goto start
*/
if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
@ -147,21 +141,32 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa
d = 0.0f;
}
// Calculate the error integral and check for saturation
/* calculate the error integral */
i = pid->integral + (pid->ki * error * dt);
float output = (error * pid->kp) + i + (d * pid->kd);
/* attitude-thrust compensation
* r22 is (2, 2) componet of rotation matrix for current attitude */
float att_comp;
if (r22 > 0.8f)
att_comp = 1.0f / r22;
else if (r22 > 0.0f)
att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
else
att_comp = 1.0f;
/* calculate output */
float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
/* check for saturation */
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);
/* saturated, recalculate output with old integral */
output = (error * pid->kp) + pid->integral + (d * pid->kd);
} else {
if (!isfinite(i)) {
i = 0.0f;
if (isfinite(i)) {
pid->integral = i;
}
pid->integral = i;
}
if (isfinite(output)) {

2
src/modules/multirotor_pos_control/thrust_pid.h
View File

@ -68,7 +68,7 @@ typedef struct {
__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);
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
__END_DECLS