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

Fix issues with nans

Browse Source
This commit is contained in:
JaeyoungLim 2025-08-02 11:12:27 +02:00
parent 10d094ae97
commit c92bde66fb
3 changed files with 1200 additions and 1115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/simulation/gazebo-classic/sitl_gazebo-classic

@ -1 +1 @@
Subproject commit 70683dc759cbd3ebccfa78429b564f8b1fb5d149
Subproject commit 3cb9b94c80d0b55ece8088adfdeb594d7ffac81a

2250
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
View File

File diff suppressed because it is too large Load Diff

63
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
View File

@ -114,15 +114,15 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
// Subscriptions
// Subscriptions
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // vehicle status
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed
// uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)}; // angle of attack
// uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; // angle of sideslip
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // linear acceleration
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position
uORB::Subscription _home_position_sub{ORB_ID(home_position)}; // home position
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position
uORB::Subscription _home_position_sub{ORB_ID(home_position)}; // home position
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude
// uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel
uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)}; // vehicle status flags
uORB::Subscription _soaring_estimator_shear_sub{ORB_ID(soaring_estimator_shear)}; // shear params for trajectory selection
@ -130,7 +130,7 @@ private:
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)};
// Publishers
// Publishers
uORB::Publication<vehicle_torque_setpoint_s> _torque_sp_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::Publication<vehicle_thrust_setpoint_s> _thrust_sp_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
@ -144,7 +144,7 @@ private:
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
uORB::Publication<debug_value_s> _debug_value_pub{ORB_ID(debug_value)};
// Message structs
// Message structs
vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
vehicle_torque_setpoint_s _actuators {}; // actuator commands
vehicle_thrust_setpoint_s _thrust_sp {};
@ -276,30 +276,42 @@ private:
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
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
Vector3f _compute_wind_estimate_EKF(); // compute a wind estimate to be used only as a measurement for the shear estimator
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_d2_dt2_basis_funs(float t=0); // compute the vector of basis function curvatures at normalized time t in [0,1]
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_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
Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref);
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);
// helper methods
void _reverse(char* str, int len); // reverse a string of length 'len'
void _reverse(char *str, int len); // reverse a string of length 'len'
int _int_to_str(int x, char str[], int d); // convert an integer x into a string of length d
void _float_to_str(float n, char* res, int afterpoint); // convert float to string
void _float_to_str(float n, char *res, int afterpoint); // convert float to string
// control variables
@ -321,9 +333,9 @@ private:
Vector3f _omega; // angular rate vector
Vector3f _alpha; // angular acceleration vector
float _k_ail;
float _k_ele;
float _k_d_roll;
float _k_d_pitch;
float _k_ele;
float _k_d_roll;
float _k_d_pitch;
hrt_abstime _last_run{0};
// controller frequency
@ -334,9 +346,10 @@ private:
math::LowPassFilter2p<float> _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // force command
math::LowPassFilter2p<float> _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // body rates
// smoothing filter to reject HF noise in control output
const float _cutoff_frequency_smoothing = 20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
const float _cutoff_frequency_smoothing =
20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
math::LowPassFilter2p<float> _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}}; // force command stage 1
math::LowPassFilter2p<float> _lp_filter_rud {_sample_frequency, 10}; // rudder command
math::LowPassFilter2p<float> _lp_filter_rud {_sample_frequency, 10.f}; // rudder command
// Low-Pass filters stage 2
const float _cutoff_frequency_2 = 20.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF"
math::LowPassFilter2p<float> _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // filter to match acceleration processing delay