From bb1acb0ce36cb409d9cbe3014384075ac935167b Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Tue, 26 Jul 2022 12:24:29 +0200 Subject: [PATCH] added trajectroy loader --- .../FixedwingPositionINDIControl.cpp | 142 +++++++++++++++--- .../FixedwingPositionINDIControl.hpp | 18 ++- .../imu_gyro_parameters.c | 2 +- 3 files changed, 137 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index c274fe5e03..8f2bf40c82 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -95,7 +95,7 @@ FixedwingPositionINDIControl::init() _R_enu_to_ned = _R_ned_to_enu; // initialize wind shear params - _shear_V_max = 0.f; + _shear_v_max = 0.f; _shear_alpha = 0.f; _shear_h_ref = 0.f; _shear_heading = M_PI_2_F; @@ -396,9 +396,92 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) return; } +float +FixedwingPositionINDIControl::_getClosest(float val1, float val2, float target) +{ + if (target - val1 >= val2 - target) + return val2; + else + return val1; +} + +float +FixedwingPositionINDIControl::_findClosest(float arr[], int n, float target) +{ + // Corner cases + if (target <= arr[0]) + return arr[0]; + if (target >= arr[n - 1]) + return arr[n - 1]; + + // Doing binary search + int i = 0, j = n, mid = 0; + while (i < j) { + mid = (i + j) / 2; + + if (fabs(arr[mid]-target) < 0.000001f) + return arr[mid]; + + /* If target is less than array element, + then search in left */ + if (target < arr[mid]) { + + // If target is greater than previous + // to mid, return closest of two + if (mid > 0 && target > arr[mid - 1]) + return _getClosest(arr[mid - 1], + arr[mid], target); + + /* Repeat for left half */ + j = mid; + } + + // If target is greater than mid + else { + if (mid < n - 1 && target < arr[mid + 1]) + return _getClosest(arr[mid], + arr[mid + 1], target); + // update i + i = mid + 1; + } + } + + // Only single element left after search + return arr[mid]; +} + void FixedwingPositionINDIControl::_select_trajectory(float initial_energy) { + /* + 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 [08,24] + in [0.10, 1.00] + in [E_min, E_max] + */ + float v = _findClosest(_v_max_arr, _gridsize, _shear_v_max); // wind velocity + float a = _findClosest(_alpha_arr, _gridsize, _shear_alpha); // shear strength + float e = _findClosest(_energy_arr, _gridsize, _shear_energy); // initial energy + PX4_INFO("V, A, E: \t%.2f\t%.2f\t%.2f", double(v), double(a), double(e)); + char file[30] = "nominal"; + char v_str[10]; + char a_str[10]; + char e_str[10]; + strcat(file,"_"); + strcat(file,gcvt(v, 3, v_str)); + strcat(file,"_"); + strcat(file,gcvt(a, 3, a_str)); + strcat(file,"_"); + strcat(file,gcvt(e, 3, e_str)); + strcat(file,".csv"); + PX4_INFO("filename: \t%.30s", file); + // select loiter trajectory for loiter test char filename[16]; switch (_loiter) @@ -432,25 +515,11 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) 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. */ @@ -659,8 +728,8 @@ FixedwingPositionINDIControl::Run() // approximate lift force, since implicit equation cannot be solved analytically: // since alpha<<1, we approximate the lift force L = sin(alpha)*Fx - cos(alpha)*Fz // as L = alpha*Fx - Fz - float Fx = body_force(0); - float Fz = -body_force(2); + float Fx = cosf(_aoa_offset)*body_force(0) - sinf(_aoa_offset)*body_force(2); + float Fz = -cosf(_aoa_offset)*body_force(2) - sinf(_aoa_offset)*body_force(0); float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) / (1 - ((2.f*Fx)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f)/_C_L1)); AoA_approx = constrain(AoA_approx,-0.2f,0.2f); @@ -1217,6 +1286,42 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v } } + // ======================================================================================================================= + // get an alternative way of computing the rot acc command: + // since te want the system to behave linearly (as implied by the PD controller) in it's position states, it would be nice + // to have a high-level controller which actually respects this property, e.g. the setpoints of the high-level controller + // should result in linear behaviour of the position states, if tracked perfectly. + // to achieve this, we use a control law, which tries to follow a linear force increment + // from the current body force to the target body force. + // ======================================================================================================================= + /* + // compute force increment + Vector3f f_delta = _mass*(acc_command - acc_filtered); + // take a fraction of the full length + Vector3f f_dot = 0.01f*f_delta; + // compute target pose which produces f_target = f_current_filtered + f_dot + Vector3f f_target = f_current_filtered + f_dot; + // compute a rotation vector (should always be <<1 to produce a linear force increment over time) + Dcmf R_ref_(_get_attitude(vel_ref,f_target)); + // 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_; + // project rotation angle to [-pi,pi] + if (q_err_.angle()*q_err_.angle()0.f){ + w_err_ = (2.f*M_PI_F-(float)fmod(q_err_.angle(),2.f*M_PI_F))*q_err_.axis(); + } + else{ + w_err_ = (-2.f*M_PI_F-(float)fmod(q_err_.angle(),2.f*M_PI_F))*q_err_.axis(); + } + } + */ + // ========================================================================== // get required attitude (assuming we can fly the target velocity), and error @@ -1239,12 +1344,15 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v w_err = (-2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); } } + + // ========================================= // apply PD control law on the body attitude // ========================================= Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref; + // ========================================== // input meant for tuning the INDI controller // ========================================== diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 839c78faf2..57a14e7608 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -276,8 +276,10 @@ private: Vector3f _compute_INDI_stage_2(Vector3f ctrl); Vector3f _compute_actuator_deflections(Vector3f ctrl); - // yaw controller - ECL_YawController _yaw_ctrl; + // helper methods + float _getClosest(float val1, float val2, float taget); // get float closest to target + float _findClosest(float arr[], int n, float target); // return element in arr closest to n + // control variables Vector _basis_coeffs_x = {}; // coefficients of the current path @@ -345,8 +347,9 @@ private: float _origin_E; float _origin_D; // wind shear parameters - float _shear_V_max; + float _shear_v_max; float _shear_alpha; + float _shear_energy; float _shear_h_ref; float _shear_heading; // loiter circle @@ -372,10 +375,11 @@ private: hrt_abstime _slip_last_valid{0}; ///< last time Aoa was received. Used to detect timeouts. float _slip{0.0f}; - // vectors defining the initial velocities, wind speed and shear strength - Vector _initial_energy_arr = {}; - Vector _V_max_arr = {}; - Vector _alpha_arr = {}; + // vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength + const static size_t _gridsize = 11; + float _energy_arr[_gridsize] = {14.f,16.f,18.f,20.f,22.f,24.f,26.f,28.f,30.f,32.f,34.f}; + float _v_max_arr[_gridsize] = {10.f,11.f,12.f,13.f,14.f,15.f,16.f,17.f,18.f,19.f,20.f}; + float _alpha_arr[_gridsize] = {0.1f,0.2f,0.3f,0.4f,0.5f,0.6f,0.7f,0.8f,0.9f,1.0f,1.1f}; // helper variables diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index a5c67e1767..f07c68e9fe 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 50.0f); +PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 40.0f); /** * Gyro control data maximum publication rate