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

Use isAllFinite() in all places that check finiteness on entire vectors or matrices

Browse Source
This commit is contained in:
Matthias Grob
2022-10-19 19:09:20 +02:00
parent 93de9567a5
commit 5ca28dd6dc
38 changed files with 128 additions and 252 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
+4 -12
View File
@@ -67,8 +67,7 @@ bool FlightTaskAutoFollowTarget::activate(const trajectory_setpoint_s &last_setp
{
bool ret = FlightTask::activate(last_setpoint);
if (!PX4_ISFINITE(_position_setpoint(0)) || !PX4_ISFINITE(_position_setpoint(1))
|| !PX4_ISFINITE(_position_setpoint(2))) {
if (!_position_setpoint.isAllFinite()) {
_position_setpoint = _position;
}
@@ -128,18 +127,12 @@ void FlightTaskAutoFollowTarget::updateTargetPositionVelocityFilter(const follow
const Vector3f pos_ned_est{follow_target_estimator.pos_est};
const Vector3f vel_ned_est{follow_target_estimator.vel_est};
const bool filtered_target_pos_is_finite = PX4_ISFINITE(_target_position_velocity_filter.getState()(0))
&& PX4_ISFINITE(_target_position_velocity_filter.getState()(1))
&& PX4_ISFINITE(_target_position_velocity_filter.getState()(2));
const bool filtered_target_vel_is_finite = PX4_ISFINITE(_target_position_velocity_filter.getRate()(0))
&& PX4_ISFINITE(_target_position_velocity_filter.getRate()(1))
&& PX4_ISFINITE(_target_position_velocity_filter.getRate()(2));
const bool target_estimator_timed_out = ((follow_target_estimator.timestamp - _last_valid_target_estimator_timestamp) >
TARGET_ESTIMATOR_TIMEOUT_US);
_last_valid_target_estimator_timestamp = follow_target_estimator.timestamp;
if (!filtered_target_pos_is_finite || !filtered_target_vel_is_finite || target_estimator_timed_out) {
if (!_target_position_velocity_filter.getState().isAllFinite()
|| !_target_position_velocity_filter.getRate().isAllFinite() || target_estimator_timed_out) {
_target_position_velocity_filter.reset(pos_ned_est, vel_ned_est);
} else {
@@ -364,8 +357,7 @@ bool FlightTaskAutoFollowTarget::update()
const Vector2f orbit_total_accel = orbit_radial_accel + orbit_tangential_accel;
// Position + Velocity + Acceleration setpoint
if (PX4_ISFINITE(drone_desired_position(0)) && PX4_ISFINITE(drone_desired_position(1))
&& PX4_ISFINITE(drone_desired_position(2))) {
if (drone_desired_position.isAllFinite()) {
if (fabsf(drone_desired_position(2) - _position(2)) < ALT_ACCEPTANCE_THRESHOLD) {
// Drone is close enough to the altitude target : Apply Horizontal + Velocity Control
_position_setpoint = drone_desired_position;
src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp
+6 -10
View File
@@ -210,12 +210,8 @@ bool TargetEstimator::measurement_can_be_fused(const Vector3f &current_measureme
const Vector3f &previous_measurement,
uint64_t last_fusion_timestamp, float min_delta_t) const
{
const bool measurement_valid = PX4_ISFINITE(current_measurement(0)) && PX4_ISFINITE(current_measurement(1))
&& PX4_ISFINITE(current_measurement(2));
const bool sensor_data_changed = Vector3f(current_measurement - previous_measurement).longerThan(2.0f * FLT_EPSILON)
|| !PX4_ISFINITE(previous_measurement(0)) || !PX4_ISFINITE(previous_measurement(1))
|| !PX4_ISFINITE(previous_measurement(2));
|| !previous_measurement.isAllFinite();
// This is required as a throttle
const bool fusion_old_enough = hrt_absolute_time() - last_fusion_timestamp >
@@ -225,7 +221,7 @@ bool TargetEstimator::measurement_can_be_fused(const Vector3f &current_measureme
const bool fusion_too_old = hrt_absolute_time() - last_fusion_timestamp >
2 * min_delta_t * 1000;
return measurement_valid && fusion_old_enough && (sensor_data_changed || fusion_too_old);
return current_measurement.isAllFinite() && fusion_old_enough && (sensor_data_changed || fusion_too_old);
// return measurement_valid;
}
@@ -242,7 +238,7 @@ void TargetEstimator::measurement_update(follow_target_s follow_target)
if (_last_follow_target_timestamp == 0) {
_filter_states.pos_ned_est = pos_measured;
if (PX4_ISFINITE(vel_measured(0)) && PX4_ISFINITE(vel_measured(1)) && PX4_ISFINITE(vel_measured(2))) {
if (vel_measured.isAllFinite()) {
_filter_states.vel_ned_est = vel_measured;
} else {
@@ -316,11 +312,11 @@ void TargetEstimator::prediction_update(float deltatime)
const Vector3f vel_ned_est_prev = _filter_states.vel_ned_est;
const Vector3f acc_ned_est_prev = _filter_states.acc_ned_est;
if (PX4_ISFINITE(vel_ned_est_prev(0)) && PX4_ISFINITE(vel_ned_est_prev(1)) && PX4_ISFINITE(vel_ned_est_prev(2))) {
if (vel_ned_est_prev.isAllFinite()) {
_filter_states.pos_ned_est += deltatime * vel_ned_est_prev + 0.5f * acc_ned_est_prev * deltatime * deltatime;
}
if (PX4_ISFINITE(acc_ned_est_prev(0)) && PX4_ISFINITE(acc_ned_est_prev(1)) && PX4_ISFINITE(acc_ned_est_prev(2))) {
if (acc_ned_est_prev.isAllFinite()) {
_filter_states.vel_ned_est += deltatime * acc_ned_est_prev;
}
}
@@ -329,7 +325,7 @@ Vector3<double> TargetEstimator::get_lat_lon_alt_est() const
{
Vector3<double> lat_lon_alt{(double)NAN, (double)NAN, (double)NAN};
if (PX4_ISFINITE(_filter_states.pos_ned_est(0)) && PX4_ISFINITE(_filter_states.pos_ned_est(0))) {
if (Vector2f(_filter_states.pos_ned_est).isAllFinite()) {
_reference_position.reproject(_filter_states.pos_ned_est(0), _filter_states.pos_ned_est(1), lat_lon_alt(0),
lat_lon_alt(1));
lat_lon_alt(2) = -(double)_filter_states.pos_ned_est(2) + (double)_vehicle_local_position.ref_alt;
src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp
+1 -3
View File
@@ -91,9 +91,7 @@ struct filter_states_s {
*/
bool is_finite()
{
return PX4_ISFINITE(pos_ned_est(0)) && PX4_ISFINITE(pos_ned_est(1)) && PX4_ISFINITE(pos_ned_est(2)) &&
PX4_ISFINITE(vel_ned_est(0)) && PX4_ISFINITE(vel_ned_est(1)) && PX4_ISFINITE(vel_ned_est(2)) &&
PX4_ISFINITE(acc_ned_est(0)) && PX4_ISFINITE(acc_ned_est(1)) && PX4_ISFINITE(acc_ned_est(2));
return pos_ned_est.isAllFinite() && vel_ned_est.isAllFinite() && acc_ned_est.isAllFinite();
}
/**