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

added trajectroy loader

Browse Source
This commit is contained in:
Marvin Harms
2022-07-26 12:24:29 +02:00
parent 49263efed6
commit bb1acb0ce3
3 changed files with 137 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+125 -17
View File
@@ -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_<type>_<V_max>_<alpha>_<energy>_
with
<type> in {nominal, robust}
<V_max> in [08,24]
<alpha> in [0.10, 1.00]
<energy> 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_<type>_<energy>_<V_max>_<alpha>
with
<type> in {nominal, robust}
<energy> in [E_min, E_max]
<V_max> in [8,24]
<alpha> 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()<M_PI_F*M_PI_F){
w_err_ = -q_err_.angle()*q_err_.axis();
}
else{
if (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
// ==========================================
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+11 -7
View File
@@ -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<float, _num_basis_funs> _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<float, 10> _initial_energy_arr = {};
Vector<float, 10> _V_max_arr = {};
Vector<float, 10> _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
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
+1 -1
View File
@@ -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