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

Minor fixes and cleanups, ready for first bench tests

Browse Source
This commit is contained in:
Lorenz Meier
2013-07-12 09:09:11 +02:00
parent 9b7ee09d43
commit f6b3692696
3 changed files with 35 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp
+10 -11
View File
@@ -48,20 +48,20 @@
#include <mathlib/mathlib.h>
#include "ecl_mc_att_control_vector.h"
ECL_MCAttControlVector::ECL_MCAttControlVector() :
_integral_error(0.0f, 0.0f),
_integral_max(1000.0f),
_integral_lock(false)
{
}
void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des_in,
float Kp, float Kd, float Ki, const math::Vector &angular_rates,
math::Vector &rates_des, float &thrust)
{
//function [rates_des,Thrust,ie_out] = attitudeController(R1,yaw,F_des,Kp,Kd,Ki,e_Rd_v,ie)
//% dt
//dt=1/50;
//% shoult the controll be in the body frame or in the inertial frame: 0 is
//% body frame, 1 is inertial frame.
// XXX
bool earth = true;
bool integral_freeze = false;
bool integral_reset = false;
math::Matrix R_bn = R_nb.transpose();
@@ -70,7 +70,6 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw,
float sy = sinf(yaw);
math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f);
//%F_des=RYaw*F_des;
math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2));
math::Vector3 F_des = F_des_in;
@@ -97,7 +96,7 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw,
// desired direction in world coordinates (yaw angle)
math::Vector3 x_C(cy, sy, 0.0f);
//desired body y axis
// desired body y axis
y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm();
// desired body x axis
x_B_des = y_B_des.cross(z_B_des);
@@ -122,7 +121,7 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw,
// include an integral part
math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f);
if (!integral_freeze) {
if (!_integral_lock) {
if (thrust > 0.3f && !integral_reset) {
if (fabsf(_integral_error(0)) < _integral_max(0)) {
_integral_error(0) = _integral_error(0) + e_R_v(0) * dt;
src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h
+16
View File
@@ -50,11 +50,27 @@
class ECL_MCAttControlVector {
public:
ECL_MCAttControlVector(float integral_max);
void control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des,
float Kp, float Kd, float Ki, const math::Vector &angular_rates,
math::Vector &rates_des, float &thrust);
void set_imax(float integral_max) {
_integral_max(0) = integral_max;
_integral_max(1) = integral_max;
}
void reset_integral() {
_integral_error(0) = 0.0f;
_integral_error(1) = 0.0f;
}
void lock_integral(bool lock) {
_integral_lock = lock;
}
protected:
math::Vector2 _integral_error;
math::Vector2 _integral_max;
bool _integral_lock;
};
src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+9 -8
View File
@@ -309,11 +309,12 @@ MulticopterAttitudeControl::parameters_update()
param_get(_parameter_handles.att_rate_p, &(_parameters.att_rate_p));
/* class control parameters */
// _att_ctrl.set_tau(_parameters.p_tconst);
// _att_ctrl.set_k_p(math::radians(_parameters.p_p));
// _att_ctrl.set_k_i(math::radians(_parameters.p_i));
// _att_ctrl.set_k_d(math::radians(_parameters.p_d));
/* att control parameters */
_att_control.set_imax(_parameters.att_imax);
//_att_control.set_tau(_parameters.p_tconst);
// _att_control.set_k_p(math::radians(_parameters.p_p));
// _att_control.set_k_i(math::radians(_parameters.p_i));
// _att_control.set_k_d(math::radians(_parameters.p_d));
return OK;
}
@@ -488,11 +489,11 @@ MulticopterAttitudeControl::task_main()
vehicle_manual_poll();
/* lock integrator until armed */
bool lock_integrator;
if (_arming.armed) {
lock_integrator = false;
_att_control.lock_integrator(false);
} else {
lock_integrator = true;
_att_control.reset_integral();
_att_control.lock_integrator(true);
}
/* decide if in auto or full manual control */