mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 12:33:34 +08:00
cleaned up code
This commit is contained in:
MarvinHarms
+1
-1
Submodule Tools/sitl_gazebo updated: d3cb480a15...a3eef9d3a1
@@ -418,7 +418,6 @@ FixedwingPositionINDIControl::soaring_estimator_shear_poll()
|
||||
_shear_aspd = _soaring_estimator_shear.aspd;
|
||||
}
|
||||
|
||||
// TODO: include some safety guards for large drifts, e.g. limit the maximum heading shift
|
||||
}
|
||||
}
|
||||
|
||||
@@ -717,7 +716,7 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
|
||||
// =======================================================================
|
||||
|
||||
|
||||
// go back to safety mode loiter circle
|
||||
// go back to safety mode loiter circle in 30m height
|
||||
if(error){
|
||||
// 100m radius circle trajec
|
||||
_basis_coeffs_x(0) = 0.000038f;
|
||||
@@ -1180,6 +1179,7 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T)
|
||||
Rotation(2,0) *= -1.f;
|
||||
Rotation(2,1) *= -1.f;
|
||||
Rotation(2,2) *= -1.f;
|
||||
// plausibility check
|
||||
/*
|
||||
float determinant = Rotation(0,0)*(Rotation(1,1)*Rotation(2,2)-Rotation(2,1)*Rotation(1,2)) -
|
||||
Rotation(1,0)*(Rotation(0,1)*Rotation(2,2)-Rotation(2,1)*Rotation(0,2)) +
|
||||
@@ -1285,15 +1285,6 @@ Quatf
|
||||
FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f)
|
||||
{
|
||||
Vector3f vel_air = vel - _wind_estimate;
|
||||
// catch case, where the aircraft needs to accelerate downwards
|
||||
// only become inverted, if we need to accelerate a lot:
|
||||
/*
|
||||
Vector3f f_body = Dcmf(_att).transpose()*f;
|
||||
bool inverted = false;
|
||||
if(f_body(2)>9.81*_mass){
|
||||
inverted = true;
|
||||
}
|
||||
*/
|
||||
// compute force component projected onto lift axis
|
||||
Vector3f vel_normalized = vel_air.normalized();
|
||||
Vector3f f_lift = f - (f*vel_normalized)*vel_normalized;
|
||||
@@ -1356,15 +1347,12 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
|
||||
// apply PD control law on the body position
|
||||
// =========================================
|
||||
Vector3f acc_command = R_ib*(_K_x*R_bi*(pos_ref-_pos) + _K_v*R_bi*(vel_ref-_vel) + _K_a*R_bi*(acc_ref-_acc)) + acc_ref;
|
||||
// add gravity
|
||||
//acc_command(2) += 9.81f;
|
||||
//acc_filtered(2) += 9.81f;
|
||||
|
||||
// ==================================
|
||||
// compute expected aerodynamic force
|
||||
// ==================================
|
||||
Vector3f f_current;
|
||||
Vector3f vel_body = R_bi*(_vel - _wind_estimate); //TODO: correct to airspeed!!!
|
||||
Vector3f vel_body = R_bi*(_vel - _wind_estimate);
|
||||
float AoA = atan2f(vel_body(2), vel_body(0)) + _aoa_offset;
|
||||
float C_l = _C_L0 + _C_L1*AoA;
|
||||
float C_d = _C_D0 + _C_D1*AoA + _C_D2*powf(AoA,2);
|
||||
@@ -1399,8 +1387,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
|
||||
f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1));
|
||||
f_command(2) = _lp_filter_ctrl0[2].apply(f_command(2));
|
||||
_f_command = f_command;
|
||||
// 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));
|
||||
// limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa)
|
||||
|
||||
// ====================================================================
|
||||
// saturate force command to avoid overly agressive maneuvers and stall
|
||||
@@ -1423,42 +1410,6 @@ 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
|
||||
// ==========================================================================
|
||||
@@ -1486,7 +1437,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
|
||||
// =========================================
|
||||
Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref;
|
||||
|
||||
|
||||
if (sqrtf(w_err*w_err)>M_PI_F){
|
||||
PX4_ERR("rotation angle larger than pi: \t%.2f, \t%.2f, \t%.2f", (double)sqrtf(w_err*w_err), (double)q_err.angle(), (double)(q_err.axis()*q_err.axis()));
|
||||
}
|
||||
@@ -1557,7 +1507,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
|
||||
rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(2)*scaler + _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2))* scaler*scaler;
|
||||
|
||||
|
||||
|
||||
return rot_acc_command;
|
||||
}
|
||||
|
||||
@@ -1568,24 +1517,29 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl)
|
||||
Dcmf R_ib(_att);
|
||||
Vector3f vel_body = R_ib.transpose()*(_vel-_wind_estimate);
|
||||
float q = fmaxf(0.5f*sqrtf(vel_body*vel_body)*vel_body(0), 0.5f*_stall_speed*_stall_speed); // dynamic pressure, saturates at stall speed
|
||||
|
||||
// compute moments
|
||||
Vector3f moment;
|
||||
moment(0) = _k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - _k_d_roll*q*_omega(0);
|
||||
moment(1) = _k_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH] - _k_d_pitch*q*_omega(1);
|
||||
moment(2) = 0.f;
|
||||
|
||||
// introduce artificial time delay that is also present in acceleration
|
||||
Vector3f moment_filtered;
|
||||
moment_filtered(0) = _lp_filter_delay[0].apply(moment(0));
|
||||
moment_filtered(1) = _lp_filter_delay[1].apply(moment(1));
|
||||
moment_filtered(2) = _lp_filter_delay[2].apply(moment(2));
|
||||
|
||||
// No filter for alpha, since it is already filtered...
|
||||
Vector3f alpha_filtered = _alpha;
|
||||
Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered;
|
||||
_m_command = R_ib.transpose()*moment_command;
|
||||
|
||||
// perform dynamic inversion
|
||||
Vector3f deflection;
|
||||
deflection(0) = (moment_command(0) + _k_d_roll*q*_omega(0))/fmaxf((_k_ail*q),0.0001f);
|
||||
deflection(1) = (moment_command(1) + _k_d_pitch*q*_omega(1))/fmaxf((_k_ele*q),0.0001f);
|
||||
|
||||
// overwrite rudder deflection with NDI turn coordination (no INDI)
|
||||
deflection(2) = ctrl(2);
|
||||
|
||||
|
||||
@@ -269,12 +269,12 @@ private:
|
||||
//
|
||||
void status_publish();
|
||||
|
||||
const static size_t _num_basis_funs = 16; // number of basis functions used for the trajectory approximation
|
||||
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_loiter_trajectory(); // select the correct loiter trajectory based on available energy
|
||||
void _select_soaring_trajectory(); // select the correct loiter trajectory based on available energy
|
||||
void _compute_trajectory_transform(); // compute the transform between trajectory frame and ENU frame (soaring frame) based on shear params
|
||||
void _select_loiter_trajectory(); // select the correct loiter trajectory based on available energy
|
||||
void _select_soaring_trajectory(); // select the correct loiter trajectory based on available energy
|
||||
void _read_trajectory_coeffs_csv(char *filename); // read in the correct coefficients of the appropriate trajectory
|
||||
float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position
|
||||
Vector3f _compute_wind_estimate(); // compute a wind estimate to be used only inside the controller
|
||||
@@ -282,16 +282,16 @@ private:
|
||||
void _set_wind_estimate(Vector3f wind);
|
||||
void _set_wind_estimate_EKF(Vector3f wind);
|
||||
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_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]
|
||||
void _load_basis_coefficients(); // load the coefficients of the current path approximation
|
||||
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
|
||||
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
|
||||
Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel
|
||||
void _load_basis_coefficients(); // load the coefficients of the current path approximation
|
||||
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
|
||||
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
|
||||
Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel
|
||||
Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref);
|
||||
Vector3f _compute_INDI_stage_2(Vector3f ctrl);
|
||||
Vector3f _compute_actuator_deflections(Vector3f ctrl);
|
||||
|
||||
@@ -550,6 +550,9 @@ bool FixedwingShearEstimator::check_feasibility()
|
||||
float
|
||||
FixedwingShearEstimator::_getClosest(float val1, float val2, float target)
|
||||
{
|
||||
// =============================================================================
|
||||
// METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/"
|
||||
// =============================================================================
|
||||
if (target - val1 >= val2 - target)
|
||||
return val2;
|
||||
else
|
||||
@@ -559,6 +562,9 @@ FixedwingShearEstimator::_getClosest(float val1, float val2, float target)
|
||||
float
|
||||
FixedwingShearEstimator::_findClosest(float arr[], int n, float target)
|
||||
{
|
||||
// =============================================================================
|
||||
// METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/"
|
||||
// =============================================================================
|
||||
// Corner cases
|
||||
if (target <= arr[0])
|
||||
return arr[0];
|
||||
|
||||
@@ -66,9 +66,6 @@ private:
|
||||
uORB::SubscriptionCallbackWorkItem _soaring_controller_wind_sub{this, ORB_ID(soaring_controller_wind)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
// Subscriptions
|
||||
//uORB::Subscription _soaring_controller_wind_sub{ORB_ID(soaring_controller_wind)};
|
||||
|
||||
// Publishers
|
||||
uORB::Publication<soaring_estimator_shear_s> _soaring_estimator_shear_pub{ORB_ID(soaring_estimator_shear)};
|
||||
|
||||
Reference in New Issue
Block a user