fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 21:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into acro2

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-20 00:03:00 +02:00
parent b165e6ba20 b250e28abf
commit b12928548c
590 changed files with 32230 additions and 16046 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+91 -87
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @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
@@ -35,9 +32,13 @@
****************************************************************************/
/**
* @file mc_att_control_main.c
* @file mc_att_control_main.cpp
* Multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
@@ -71,7 +72,7 @@
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
/**
@@ -156,13 +157,14 @@ private:
param_t yaw_rate_i;
param_t yaw_rate_d;
param_t yaw_ff;
param_t roll_scale_acro;
param_t pitch_scale_acro;
param_t yaw_scale_acro;
param_t yaw_rate_max;
param_t rc_scale_roll;
param_t rc_scale_pitch;
param_t rc_scale_yaw;
param_t man_roll_max;
param_t man_pitch_max;
param_t man_yaw_max;
param_t acro_roll_max;
param_t acro_pitch_max;
param_t acro_yaw_max;
} _params_handles; /**< handles for interesting parameters */
struct {
@@ -171,9 +173,12 @@ private:
math::Vector<3> rate_i; /**< I gain for angular rate error */
math::Vector<3> rate_d; /**< D gain for angular rate error */
float yaw_ff; /**< yaw control feed-forward */
math::Vector<3> scale_acro; /**< scale for ACRO mode */
float yaw_rate_max; /**< max yaw rate */
math::Vector<3> rc_scale; /**< scale for MANUAL mode */
float man_roll_max;
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
} _params;
/**
@@ -227,9 +232,9 @@ private:
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
* Main attitude control task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace mc_att_control
@@ -278,8 +283,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_d.zero();
_params.scale_acro.zero();
_params.rc_scale.zero();
_params.yaw_ff = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.man_roll_max = 0.0f;
_params.man_pitch_max = 0.0f;
_params.man_yaw_max = 0.0f;
_params.acro_rate_max.zero();
_rates_prev.zero();
_rates_sp.zero();
@@ -289,26 +298,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_I.identity();
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
_params_handles.pitch_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.roll_scale_acro = param_find("MC_ROLL_S_ACRO");
_params_handles.pitch_scale_acro = param_find("MC_PITCH_S_ACRO");
_params_handles.yaw_scale_acro = param_find("MC_YAW_S_ACRO");
_params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
_params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
_params_handles.pitch_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
/* fetch initial parameter values */
parameters_update();
@@ -343,7 +352,7 @@ MulticopterAttitudeControl::parameters_update()
{
float v;
/* roll */
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v);
@@ -352,12 +361,8 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
param_get(_params_handles.roll_scale_acro, &v);
_params.scale_acro(0) = v;
param_get(_params_handles.rc_scale_roll, &v);
_params.rc_scale(0) = v;
/* pitch */
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v;
param_get(_params_handles.pitch_rate_p, &v);
@@ -366,12 +371,8 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
param_get(_params_handles.pitch_scale_acro, &v);
_params.scale_acro(1) = v;
param_get(_params_handles.rc_scale_pitch, &v);
_params.rc_scale(1) = v;
/* yaw */
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
@@ -380,11 +381,26 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.yaw_scale_acro, &v);
_params.scale_acro(2) = v;
param_get(_params_handles.rc_scale_yaw, &v);
_params.rc_scale(2) = v;
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
/* manual control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
/* acro control scale */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
param_get(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v);
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
return OK;
}
@@ -426,7 +442,6 @@ MulticopterAttitudeControl::vehicle_manual_poll()
orb_check(_manual_control_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
}
}
@@ -480,7 +495,6 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (_v_control_mode.flag_control_manual_enabled) {
/* manual input, set or modify attitude setpoint */
vehicle_manual_poll();
if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
@@ -489,7 +503,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (!_v_control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude stabilized mode */
_v_att_sp.thrust = _manual_control_sp.throttle;
_v_att_sp.thrust = _manual_control_sp.z;
publish_att_sp = true;
}
@@ -506,24 +520,19 @@ MulticopterAttitudeControl::control_attitude(float dt)
// reset_yaw_sp = true;
//}
} else {
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale(2);
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
if (yaw_offs < - yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
if (_params.rc_scale(2) > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale(2);
if (_manual_control_sp.yaw > 0.0f) {
yaw_sp_move_rate -= YAW_DEADZONE;
} else {
yaw_sp_move_rate += YAW_DEADZONE;
}
yaw_sp_move_rate *= _params.rc_scale(2);
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
}
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
/* reset yaw setpint to current position if needed */
@@ -536,8 +545,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (!_v_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_v_att_sp.roll_body = _manual_control_sp.roll;
_v_att_sp.pitch_body = _manual_control_sp.pitch;
_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
@@ -650,6 +659,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* limit yaw rate */
_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
/* feed forward yaw setpoint rate */
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}
@@ -765,6 +777,7 @@ MulticopterAttitudeControl::task_main()
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
@@ -784,20 +797,11 @@ MulticopterAttitudeControl::task_main()
}
} else {
/* attitude controller disabled */
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control, ACRO mode */
vehicle_manual_poll();
_rates_sp(0) = _manual_control_sp.roll;
_rates_sp(1) = _manual_control_sp.pitch;
_rates_sp(2) = _manual_control_sp.yaw;
/* rescale controls for ACRO mode */
_rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro);
_thrust_sp = _manual_control_sp.throttle;
_reset_yaw_sp = true;
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp.z;
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
@@ -860,7 +864,7 @@ MulticopterAttitudeControl::start()
_control_task = task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
2000,
(main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
src/modules/mc_att_control/mc_att_control_params.c
+74 -6
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @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
@@ -37,6 +34,10 @@
/**
* @file mc_att_control_params.c
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <systemlib/param/param.h>
@@ -173,6 +174,73 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
PARAM_DEFINE_FLOAT(MC_ROLL_S_ACRO, 5.0f);
PARAM_DEFINE_FLOAT(MC_PITCH_S_ACRO, 5.0f);
PARAM_DEFINE_FLOAT(MC_YAW_S_ACRO, 3.0f);
/**
* Max yaw rate
*
* Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
/**
* Max manual roll
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
/**
* Max manual pitch
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
/**
* Max manual yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
/**
* Max acro roll rate
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
/**
* Max acro pitch rate
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
/**
* Max acro yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);