diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 75344c2d6b..a50cee905d 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -72,6 +72,7 @@ void FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) { _wind_estimate = wind; + return; } Vector @@ -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; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 4823c55b73..71a51f60fd 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -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 _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) _param_fw_mass, (ParamFloat) _param_fw_wing_area, - (ParamFloat) _param_rho + (ParamFloat) _param_rho, // aerodynamic params - /* - (ParamFloat) _param_fw_c_l0, - (ParamFloat) _param_fw_c_l1, - (ParamFloat) _param_fw_c_d0, - (ParamFloat) _param_fw_c_d1, - (ParamFloat) _param_fw_c_d2, + + (ParamFloat) _param_fw_c_l0, + (ParamFloat) _param_fw_c_l1, + (ParamFloat) _param_fw_c_d0, + (ParamFloat) _param_fw_c_d1, + (ParamFloat) _param_fw_c_d2, // filter params (ParamFloat) _param_filter_a1, (ParamFloat) _param_filter_a2, (ParamFloat) _param_filter_b1, (ParamFloat) _param_filter_b2, - (ParamFloat) _param_filter_b3 - */ + (ParamFloat) _param_filter_b3, + // controller params + (ParamFloat) _param_k_x_roll, + (ParamFloat) _param_k_x_pitch, + (ParamFloat) _param_k_x_yaw, + (ParamFloat) _param_k_v_roll, + (ParamFloat) _param_k_v_pitch, + (ParamFloat) _param_k_v_yaw, + (ParamFloat) _param_k_a_roll, + (ParamFloat) _param_k_a_pitch, + (ParamFloat) _param_k_a_yaw, + (ParamFloat) _param_k_q_roll, + (ParamFloat) _param_k_q_pitch, + (ParamFloat) _param_k_q_yaw, + (ParamFloat) _param_k_w_roll, + (ParamFloat) _param_k_w_pitch, + (ParamFloat) _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 _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1] Vector _get_d_dt_basis_funs(float t=0); // compute the vector of basis function gradients at normalized time t in [0,1] Vector _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 _basis_coeffs_x; // coefficients of the current path Vector _basis_coeffs_y; // coefficients of the current path Vector _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 _f_list; diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 6ebf6fefa5..f10042c037 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -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);