diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index a12478a2b6..f4bc646ab0 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -94,6 +94,15 @@ FixedwingPositionINDIControl::init() _R_ned_to_enu.renormalize(); _R_enu_to_ned = _R_ned_to_enu; + // initialize wind shear params + _shear_V_max = 0.f; + _shear_alpha = 0.f; + _shear_h_ref = 0.f; + _shear_heading = M_PI_2_F; + + // initialize transform to trajec frame + _compute_trajectory_transform(); + return true; } @@ -354,6 +363,15 @@ FixedwingPositionINDIControl::soaring_controller_status_poll() } } +void +FixedwingPositionINDIControl::_compute_trajectory_transform() +{ + Eulerf e(0.f, 0.f, _shear_heading); + _R_enu_to_trajec = Dcmf(e); + _R_trajec_to_enu = _R_enu_to_trajec.transpose(); + _vec_enu_to_trajec = Vector3f{0.f,0.f,_shear_h_ref}; +} + void FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) { @@ -396,6 +414,28 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) default: strcpy(filename,"trajectory0.csv"); } + + /* + We read a trajectory based on initial energy available at the beginning of the trajectory. + So the trajectory is selected based on two criteria, first the correct wind shear params (alpha and V_max) and the initial energy (potential + kinetic). + + The filename structure of the trajectories is the following: + trajec____ + with + in {nominal, robust} + in [E_min, E_max] + in [8,24] + in [0.1, 1.0] + */ + + /* + Also, we need to place the current trajectory, centered around zero height and assuming wind from the west, into the soaring frame. + Since the necessary computations for position, velocity and acceleration require the basis coefficients, which are not easy to transform, + we choose to transform our position in soaring frame into the "trajectory frame", compute position vector and it's derivatives from the basis coeffs + in the trajectory frame, and then transform these back to soaring frame for control purposes. + + Therefore we define a new transform between the soaring frame and the trajectory frame. + */ _read_trajectory_coeffs_csv(filename); @@ -588,6 +628,9 @@ FixedwingPositionINDIControl::Run() vehicle_angular_acceleration_poll(); soaring_controller_status_poll(); + // update transform from trajectory frame to ENU frame (soaring frame) + _compute_trajectory_transform(); + // =============================== // compute wind pseudo-measurement // =============================== @@ -835,7 +878,7 @@ FixedwingPositionINDIControl::_get_position_ref(float t) float x = _basis_coeffs_x*basis; float y = _basis_coeffs_y*basis; float z = _basis_coeffs_z*basis; - return Vector3f{x, y, z}; + return _R_trajec_to_enu*Vector3f{x, y, z} + _vec_enu_to_trajec; } Vector3f @@ -845,7 +888,7 @@ FixedwingPositionINDIControl::_get_velocity_ref(float t, float T) float x = _basis_coeffs_x*basis; float y = _basis_coeffs_y*basis; float z = _basis_coeffs_z*basis; - return Vector3f{x, y, z}/T; + return _R_trajec_to_enu*(Vector3f{x, y, z}/T); } Vector3f @@ -855,7 +898,7 @@ FixedwingPositionINDIControl::_get_acceleration_ref(float t, float T) float x = _basis_coeffs_x*basis; float y = _basis_coeffs_y*basis; float z = _basis_coeffs_z*basis; - return Vector3f{x, y, z}/powf(T,2); + return _R_trajec_to_enu*(Vector3f{x, y, z}/powf(T,2)); } Quatf @@ -975,7 +1018,6 @@ FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) //PX4_INFO("closest t: %.2f", (double)t); //PX4_INFO("closest distance:%.2f", (double)sqrtf(min_dist)); - const uint n_1 = 20; Vector distances; float t_ref; @@ -1120,9 +1162,22 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 12° aoa) //PX4_INFO("force current, command: \t%.2f\t%.2f", (double)sqrtf(f_current_filtered*f_current_filtered), (double)sqrtf(f_command*f_command)); + // ==================================================================== + // saturate force command to avoid overly agressive maneuvers and stall + // ==================================================================== if (_switch_saturation){ - float f_max = -factor*sqrtf(vel_body*vel_body)*(_C_L0 + _C_L1*0.2f); + float speed = vel_body*vel_body; + // compute amximum achievable force + float f_max; + if (speed>_stall_speed){ + f_max = -factor*sqrtf(vel_body*vel_body)*(_C_L0 + _C_L1*0.25f); // assume stall at 15° AoA + } + else { + f_max = -factor*_stall_speed*(_C_L0 + _C_L1*0.25f); // assume stall at 15° AoA + } + // compute current command float f_now = sqrtf(f_command*f_command); + // saturate current command if (f_now>f_max){ f_command = f_max/f_now * f_command; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 422b5600b4..5e1e5012b1 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -250,6 +250,7 @@ private: const static size_t _num_basis_funs = 16; // number of basis functions used for the trajectory approximation // controller methods + void _compute_trajectory_transform(); // compute the transform between trajectory frame and ENU frame (soaring frame) based on shear params void _select_trajectory(float initial_energy); // select the correct trajectory based on available energy void _read_trajectory_coeffs_csv(char *filename); // read in the correct coefficients of the appropriate trajectory void _set_wind_estimate(Vector3f wind); @@ -332,6 +333,11 @@ private: float _origin_N; float _origin_E; float _origin_D; + // wind shear parameters + float _shear_V_max; + float _shear_alpha; + float _shear_h_ref; + float _shear_heading; // loiter circle int _loiter; // thrust @@ -354,14 +360,17 @@ private: float _slip{0.0f}; // vectors defining the initial velocities, wind speed and shear strength - Vector _initial_velocities_trajectory = {}; - Vector _wind_speed_trajectory = {}; - Vector _shear_param_trajectory = {}; + Vector _initial_energy_arr = {}; + Vector _V_max_arr = {}; + Vector _alpha_arr = {}; // helper variables Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame Dcmf _R_enu_to_ned; // rotation matrix from ENU to NED frame + Dcmf _R_trajec_to_enu; // rotation matrix from trajectory frame to ENU frame + Dcmf _R_enu_to_trajec; // rotation matrix from ENU frame to trajectory frame + Vector3f _vec_enu_to_trajec; // 3D vector from ENU origin to trajectory frame origin (expressed in ENU) Vector3f _zero_crossing_local_pos; // vector denoting the zero crossing of the trajectories in NED frame Vector3f _f_command_filtered {}; Vector3f _m_command_filtered {}; 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 72a1b7db89..12a8922342 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 @@ -533,4 +533,4 @@ PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0); * @increment 1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_INT32(DS_SWITCH_SAT, 0); \ No newline at end of file +PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1); \ No newline at end of file