mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 01:33:35 +08:00
added NDI control function
This commit is contained in:
Marvin Harms
@@ -72,6 +72,7 @@ void
|
||||
FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
|
||||
{
|
||||
_wind_estimate = wind;
|
||||
return;
|
||||
}
|
||||
|
||||
Vector<float, FixedwingPositionINDIControl::_num_basis_funs>
|
||||
@@ -148,7 +149,7 @@ FixedwingPositionINDIControl::_get_acceleration_ref(float t, float T)
|
||||
return Vector3f{x, y, z}/powf(T,2);
|
||||
}
|
||||
|
||||
Dcmf
|
||||
Quatf
|
||||
FixedwingPositionINDIControl::_get_attitude_ref(float t, float T)
|
||||
{
|
||||
Vector3f vel = _get_velocity_ref(t,T);
|
||||
@@ -176,7 +177,7 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T)
|
||||
R_bi(2,2) = lift_normalized(2);
|
||||
// compute required AoA
|
||||
Vector3f f_phi = R_bi*f_lift;
|
||||
float AoA = ((2.f*f_phi(2))/(1.223f*0.4f*(vel_air*vel_air)) - 0.356f)/2.354f;
|
||||
float AoA = ((2.f*f_phi(2))/(_param_rho.get()*_param_fw_wing_area.get()*(vel_air*vel_air)) - _param_fw_c_l0.get())/_param_fw_c_l1.get();
|
||||
// compute final rotation matrix
|
||||
Eulerf e(0.f, AoA, 0.f);
|
||||
Dcmf R_pitch(e);
|
||||
@@ -188,11 +189,8 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T)
|
||||
Rotation(2,0) *= -1;
|
||||
Rotation(2,1) *= -1;
|
||||
Rotation(2,2) *= -1;
|
||||
return Rotation.transpose();
|
||||
// compute quaternion
|
||||
//Quatf q;
|
||||
//q(transpose(Rotation));
|
||||
//return q;
|
||||
Quatf q(Rotation.transpose());
|
||||
return q;
|
||||
}
|
||||
|
||||
Vector3f
|
||||
@@ -201,8 +199,8 @@ FixedwingPositionINDIControl::_get_angular_velocity_ref(float t, float T)
|
||||
float dt = 0.001;
|
||||
float t_lower = fmaxf(0.f,t-dt);
|
||||
float t_upper = fminf(t+dt,1.f);
|
||||
Dcmf R_i0 = _get_attitude_ref(t_lower, T);
|
||||
Dcmf R_i1 = _get_attitude_ref(t_upper, T);
|
||||
Dcmf R_i0(_get_attitude_ref(t_lower, T));
|
||||
Dcmf R_i1(_get_attitude_ref(t_upper, T));
|
||||
Dcmf R_10 = R_i1.transpose()*R_i0;
|
||||
AxisAnglef w_01(R_10);
|
||||
return -w_01.axis()*w_01.angle()/(T*(t_upper-t_lower));
|
||||
@@ -215,10 +213,10 @@ FixedwingPositionINDIControl::_get_angular_acceleration_ref(float t, float T)
|
||||
float t_lower = fmaxf(0.f,t-dt);
|
||||
float t_upper = fminf(t+dt,1.f);
|
||||
// compute roational velocity in inertial frame
|
||||
Dcmf R_i0 = _get_attitude_ref(t_lower, T);
|
||||
Dcmf R_i0(_get_attitude_ref(t_lower, T));
|
||||
AxisAnglef w_0(R_i0*_get_angular_velocity_ref(t_lower, T));
|
||||
// compute roational velocity in inertial frame
|
||||
Dcmf R_i1 = _get_attitude_ref(t_upper, T);
|
||||
Dcmf R_i1(_get_attitude_ref(t_upper, T));
|
||||
AxisAnglef w_1(R_i1*_get_angular_velocity_ref(t_upper, T));
|
||||
// compute gradient via finite differences
|
||||
Vector3f dw_dt = (w_1.axis()*w_1.angle() - w_0.axis()*w_0.angle()) / (T*(t_upper-t_lower));
|
||||
@@ -249,7 +247,7 @@ FixedwingPositionINDIControl::_get_closest_t(Vector3f pos)
|
||||
return t;
|
||||
}
|
||||
|
||||
Dcmf
|
||||
Quatf
|
||||
FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f)
|
||||
{
|
||||
Vector3f vel_air = vel - _wind_estimate;
|
||||
@@ -271,7 +269,7 @@ FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f)
|
||||
R_bi(2,2) = lift_normalized(2);
|
||||
// compute required AoA
|
||||
Vector3f f_phi = R_bi*f_lift;
|
||||
float AoA = ((2.f*f_phi(2))/(1.223f*0.4f*(vel_air*vel_air)) - 0.356f)/2.354f;
|
||||
float AoA = ((2.f*f_phi(2))/(_param_rho.get()*_param_fw_wing_area.get()*(vel_air*vel_air)) - _param_fw_c_l0.get())/_param_fw_c_l1.get();
|
||||
// compute final rotation matrix
|
||||
Eulerf e(0.f, AoA, 0.f);
|
||||
Dcmf R_pitch(e);
|
||||
@@ -283,7 +281,52 @@ FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f)
|
||||
Rotation(2,0) *= -1;
|
||||
Rotation(2,1) *= -1;
|
||||
Rotation(2,2) *= -1;
|
||||
return Rotation.transpose();
|
||||
Quatf q(Rotation.transpose());
|
||||
return q;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::_compute_NDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha)
|
||||
{
|
||||
Dcmf R_ib(att);
|
||||
Dcmf R_bi(R_ib.transpose());
|
||||
// get reference values
|
||||
float t_ref = _get_closest_t(pos);
|
||||
// downscale velocity to match current one,
|
||||
// terminal time is determined such that current velocity is met
|
||||
Vector3f v_ref_ = _get_velocity_ref(t_ref, 1.f);
|
||||
float T = sqrt((v_ref_*v_ref_)/(vel*vel+0.01f));
|
||||
// get time-scaled version of reference trajectory
|
||||
Vector3f x_ref = _get_position_ref(t_ref);
|
||||
Vector3f v_ref = _get_velocity_ref(t_ref,T);
|
||||
Vector3f a_ref = _get_acceleration_ref(t_ref,T);
|
||||
Vector3f omega_ref = _get_angular_velocity_ref(t_ref,T);
|
||||
Vector3f alpha_ref = _get_angular_acceleration_ref(t_ref,T);
|
||||
// get acceleration command in world frame (without gravity)
|
||||
Vector3f acc_command = R_ib*(_K_x*R_bi*(x_ref-pos) + _K_v*R_bi*(v_ref-vel) + _K_a*R_bi*(a_ref-acc)) + a_ref;
|
||||
// add gravity
|
||||
acc_command(2) += 9.81f;
|
||||
// get force comand in world frame
|
||||
Vector3f f_command = _param_fw_mass.get()*acc_command;
|
||||
// get required attitude (assuming we can fly the target velocity)
|
||||
Dcmf R_ref(_get_attitude(v_ref,f_command));
|
||||
// get attitude error
|
||||
Dcmf R_ref_true(R_ref.transpose()*R_ib);
|
||||
// get required rotation vector (in body frame)
|
||||
AxisAnglef q_err(R_ref_true);
|
||||
Vector3f w_err = -q_err.angle()*q_err.axis();
|
||||
//Vector3f v_norm = (vel-_wind_estimate).unit();
|
||||
//Vector3f v_ref_norm = (v_ref-_wind_estimate).unit();
|
||||
// compute angular acceleration command (in body frame)
|
||||
Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-omega) + alpha_ref;
|
||||
// publish control input
|
||||
_angular_accel_sp = {};
|
||||
_angular_accel_sp.timestamp = hrt_absolute_time();
|
||||
_angular_accel_sp.xyz[0] = rot_acc_command(0);
|
||||
_angular_accel_sp.xyz[1] = rot_acc_command(1);
|
||||
_angular_accel_sp.xyz[2] = rot_acc_command(2);
|
||||
_alpha_sp_pub.publish(_angular_accel_sp);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -62,8 +62,7 @@ using matrix::Dcmf;
|
||||
using matrix::Eulerf;
|
||||
using matrix::Quatf;
|
||||
using matrix::Vector;
|
||||
using matrix::Vector2f;
|
||||
using matrix::Vector2d;
|
||||
using matrix::Matrix3f;
|
||||
using matrix::Vector3f;
|
||||
|
||||
|
||||
@@ -111,6 +110,7 @@ private:
|
||||
uORB::Publication<vehicle_angular_acceleration_setpoint_s> _alpha_sp_pub;
|
||||
|
||||
// Message structs
|
||||
vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
|
||||
manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data
|
||||
vehicle_control_mode_s _control_mode {}; ///< control mode
|
||||
vehicle_local_position_s _local_pos {}; ///< vehicle local position
|
||||
@@ -121,21 +121,37 @@ private:
|
||||
// aircraft params
|
||||
(ParamFloat<px4::params::FW_MASS>) _param_fw_mass,
|
||||
(ParamFloat<px4::params::FW_WING_AREA>) _param_fw_wing_area,
|
||||
(ParamFloat<px4::params::RHO>) _param_rho
|
||||
(ParamFloat<px4::params::RHO>) _param_rho,
|
||||
// aerodynamic params
|
||||
/*
|
||||
(ParamFloat<px4::params::C_l0>) _param_fw_c_l0,
|
||||
(ParamFloat<px4::params::C_l1>) _param_fw_c_l1,
|
||||
(ParamFloat<px4::params::C_d0>) _param_fw_c_d0,
|
||||
(ParamFloat<px4::params::C_d1>) _param_fw_c_d1,
|
||||
(ParamFloat<px4::params::C_d2>) _param_fw_c_d2,
|
||||
|
||||
(ParamFloat<px4::params::C_L0>) _param_fw_c_l0,
|
||||
(ParamFloat<px4::params::C_L1>) _param_fw_c_l1,
|
||||
(ParamFloat<px4::params::C_D0>) _param_fw_c_d0,
|
||||
(ParamFloat<px4::params::C_D1>) _param_fw_c_d1,
|
||||
(ParamFloat<px4::params::C_D2>) _param_fw_c_d2,
|
||||
// filter params
|
||||
(ParamFloat<px4::params::FILTER_A1>) _param_filter_a1,
|
||||
(ParamFloat<px4::params::FILTER_A2>) _param_filter_a2,
|
||||
(ParamFloat<px4::params::FILTER_B1>) _param_filter_b1,
|
||||
(ParamFloat<px4::params::FILTER_B2>) _param_filter_b2,
|
||||
(ParamFloat<px4::params::FILTER_B3>) _param_filter_b3
|
||||
*/
|
||||
(ParamFloat<px4::params::FILTER_B3>) _param_filter_b3,
|
||||
// controller params
|
||||
(ParamFloat<px4::params::K_X_ROLL>) _param_k_x_roll,
|
||||
(ParamFloat<px4::params::K_X_PITCH>) _param_k_x_pitch,
|
||||
(ParamFloat<px4::params::K_X_YAW>) _param_k_x_yaw,
|
||||
(ParamFloat<px4::params::K_V_ROLL>) _param_k_v_roll,
|
||||
(ParamFloat<px4::params::K_V_PITCH>) _param_k_v_pitch,
|
||||
(ParamFloat<px4::params::K_V_YAW>) _param_k_v_yaw,
|
||||
(ParamFloat<px4::params::K_A_ROLL>) _param_k_a_roll,
|
||||
(ParamFloat<px4::params::K_A_PITCH>) _param_k_a_pitch,
|
||||
(ParamFloat<px4::params::K_A_YAW>) _param_k_a_yaw,
|
||||
(ParamFloat<px4::params::K_Q_ROLL>) _param_k_q_roll,
|
||||
(ParamFloat<px4::params::K_Q_PITCH>) _param_k_q_pitch,
|
||||
(ParamFloat<px4::params::K_Q_YAW>) _param_k_q_yaw,
|
||||
(ParamFloat<px4::params::K_W_ROLL>) _param_k_w_roll,
|
||||
(ParamFloat<px4::params::K_W_PITCH>) _param_k_w_pitch,
|
||||
(ParamFloat<px4::params::K_W_YAW>) _param_k_w_yaw
|
||||
|
||||
)
|
||||
|
||||
|
||||
@@ -167,6 +183,7 @@ private:
|
||||
|
||||
// controller methods
|
||||
void _set_wind_estimate(Vector3f wind);
|
||||
float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position
|
||||
Vector<float, _num_basis_funs> _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1]
|
||||
Vector<float, _num_basis_funs> _get_d_dt_basis_funs(float t=0); // compute the vector of basis function gradients at normalized time t in [0,1]
|
||||
Vector<float, _num_basis_funs> _get_d2_dt2_basis_funs(float t=0); // compute the vector of basis function curvatures at normalized time t in [0,1]
|
||||
@@ -174,11 +191,10 @@ private:
|
||||
Vector3f _get_position_ref(float t=0); // get the reference position on the current path, at normalized time t in [0,1]
|
||||
Vector3f _get_velocity_ref(float t=0, float T=1); // get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
|
||||
Vector3f _get_acceleration_ref(float t=0, float T=1); // get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
|
||||
Dcmf _get_attitude_ref(float t=0, float T=1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T
|
||||
Quatf _get_attitude_ref(float t=0, float T=1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T
|
||||
Vector3f _get_angular_velocity_ref(float t=0, float T=1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
|
||||
Vector3f _get_angular_acceleration_ref(float t=0, float T=1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
|
||||
float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position
|
||||
Dcmf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel
|
||||
Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel
|
||||
void _compute_NDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha);
|
||||
void _compute_INDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha);
|
||||
|
||||
@@ -186,6 +202,7 @@ private:
|
||||
Vector<float, _num_basis_funs> _basis_coeffs_x; // coefficients of the current path
|
||||
Vector<float, _num_basis_funs> _basis_coeffs_y; // coefficients of the current path
|
||||
Vector<float, _num_basis_funs> _basis_coeffs_z; // coefficients of the current path
|
||||
/*
|
||||
Vector3f _pos;
|
||||
Vector3f _pos_sp;
|
||||
Vector3f _vel;
|
||||
@@ -197,8 +214,14 @@ private:
|
||||
Vector3f _omega;
|
||||
Vector3f _omega_sp;
|
||||
Vector3f _alpha;
|
||||
*/
|
||||
Vector3f _alpha_sp;
|
||||
Vector3f _wind_estimate;
|
||||
Matrix3f _K_x;
|
||||
Matrix3f _K_v;
|
||||
Matrix3f _K_a;
|
||||
Matrix3f _K_q;
|
||||
Matrix3f _K_w;
|
||||
|
||||
// filter variables
|
||||
std::array<Vector3f, 3> _f_list;
|
||||
|
||||
@@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(RHO, 1.223f);
|
||||
* @increment 0.001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_l0, 0.356f);
|
||||
PARAM_DEFINE_FLOAT(C_L0, 0.356f);
|
||||
|
||||
/**
|
||||
* estimated lift coefficients used for lift and drag computation
|
||||
@@ -77,7 +77,7 @@ PARAM_DEFINE_FLOAT(C_l0, 0.356f);
|
||||
* @increment 0.001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_l1, 2.354f);
|
||||
PARAM_DEFINE_FLOAT(C_L1, 2.354f);
|
||||
|
||||
|
||||
/**
|
||||
@@ -93,7 +93,7 @@ PARAM_DEFINE_FLOAT(C_l1, 2.354f);
|
||||
* @increment 0.0001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_d0, 0.0288f);
|
||||
PARAM_DEFINE_FLOAT(C_D0, 0.0288f);
|
||||
|
||||
/**
|
||||
* estimated drag coefficients used for lift and drag computation
|
||||
@@ -108,7 +108,7 @@ PARAM_DEFINE_FLOAT(C_d0, 0.0288f);
|
||||
* @increment 0.0001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_d1, 0.3783f);
|
||||
PARAM_DEFINE_FLOAT(C_D1, 0.3783f);
|
||||
|
||||
/**
|
||||
* estimated drag coefficients used for lift and drag computation
|
||||
@@ -123,7 +123,7 @@ PARAM_DEFINE_FLOAT(C_d1, 0.3783f);
|
||||
* @increment 0.0001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_d2, 1.984f);
|
||||
PARAM_DEFINE_FLOAT(C_D2, 1.984f);
|
||||
|
||||
|
||||
/**
|
||||
@@ -186,4 +186,185 @@ PARAM_DEFINE_FLOAT(FILTER_B2, 0.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FILTER_B3, 0.0f);
|
||||
|
||||
// ========================================================
|
||||
// =================== CONTROL GAINS ======================
|
||||
// ========================================================
|
||||
/**
|
||||
* roll gain of K_x (position error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_X_ROLL, 2.0f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_x (position error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_X_PITCH, 2.0f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_x (position error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_X_YAW, 2.0f);
|
||||
|
||||
/**
|
||||
* roll gain of K_v (velocity error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_V_ROLL, 2.0f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_v (velocity error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_V_PITCH, 2.0f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_v (velocity error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_V_YAW, 2.0f);
|
||||
|
||||
/**
|
||||
* roll gain of K_A (acceleration error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_A_ROLL, 1.0f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_A (acceleration error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_A_PITCH, 1.0f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_A (acceleration error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min -10
|
||||
* @max 10
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_A_YAW, 1.0f);
|
||||
|
||||
/**
|
||||
* roll gain of K_Q (attitude error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min 0
|
||||
* @max 20
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_Q_ROLL, 10.0f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_Q (attitude error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min 0
|
||||
* @max 20
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_Q_PITCH, 10.0f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_Q (attitude error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min 0
|
||||
* @max 20
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_Q_YAW, 5.0f);
|
||||
|
||||
/**
|
||||
* roll gain of K_W (angular velocity error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min 0
|
||||
* @max 20
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_W_ROLL, 5.0f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_W (angular velocity error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min 0
|
||||
* @max 20
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_W (angular velocity error gain)
|
||||
*
|
||||
* @unit
|
||||
* @min 0
|
||||
* @max 20
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_W_YAW, 5.0f);
|
||||
|
||||
Reference in New Issue
Block a user