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

mc_att_control_vector: fixes, parameters added

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-26 23:02:09 +04:00
parent 26daae0b0a
commit e103729de3
3 changed files with 25 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+3 -2
View File
@@ -616,12 +616,13 @@ MulticopterAttitudeControl::task_main()
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
if (e_R_z_cos < 0.0) {
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
q.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag();
float angle_d = 2.0f * atan2f(e_R_d.length(), q(0));
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
src/modules/mc_att_control_vector/mc_att_control_vector_params.c
+20
View File
@@ -0,0 +1,20 @@
/*
* mc_att_control_vector_params.c
*
* Created on: 26.12.2013
* Author: ton
*/
#include <systemlib/param/param.h>
/* multicopter attitude controller parameters */
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f);
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
src/modules/mc_att_control_vector/module.mk
+2 -1
View File
@@ -37,4 +37,5 @@
MODULE_COMMAND = mc_att_control_vector
SRCS = mc_att_control_vector_main.cpp
SRCS = mc_att_control_vector_main.cpp \
mc_att_control_vector_params.c