fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:53: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/Auto/FlightTaskAuto.cpp
+5 -12
View File
@@ -97,12 +97,7 @@ bool FlightTaskAuto::updateInitialize()
// require valid reference and valid target
ret = ret && _evaluateGlobalReference() && _evaluateTriplets();
// require valid position
ret = ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_position(2))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1))
&& PX4_ISFINITE(_velocity(2));
ret = ret && _position.isAllFinite() && _velocity.isAllFinite();
return ret;
}
@@ -360,7 +355,7 @@ bool FlightTaskAuto::_evaluateTriplets()
if (!PX4_ISFINITE(_sub_triplet_setpoint.get().current.lat)
|| !PX4_ISFINITE(_sub_triplet_setpoint.get().current.lon)) {
// No position provided in xy. Lock position
if (!PX4_ISFINITE(_lock_position_xy(0))) {
if (!_lock_position_xy.isAllFinite()) {
tmp_target(0) = _lock_position_xy(0) = _position(0);
tmp_target(1) = _lock_position_xy(1) = _position(1);
@@ -388,9 +383,7 @@ bool FlightTaskAuto::_evaluateTriplets()
const bool prev_next_validity_changed = (_prev_was_valid != _sub_triplet_setpoint.get().previous.valid)
|| (_next_was_valid != _sub_triplet_setpoint.get().next.valid);
if (PX4_ISFINITE(_triplet_target(0))
&& PX4_ISFINITE(_triplet_target(1))
&& PX4_ISFINITE(_triplet_target(2))
if (_triplet_target.isAllFinite()
&& fabsf(_triplet_target(0) - tmp_target(0)) < 0.001f
&& fabsf(_triplet_target(1) - tmp_target(1)) < 0.001f
&& fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f
@@ -402,7 +395,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_triplet_target = tmp_target;
_target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius;
if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) {
if (!Vector2f(_triplet_target).isAllFinite()) {
// Horizontal target is not finite.
_triplet_target(0) = _position(0);
_triplet_target(1) = _position(1);
@@ -536,7 +529,7 @@ void FlightTaskAuto::_set_heading_from_mode()
break;
}
if (PX4_ISFINITE(v.norm_squared())) {
if (v.isAllFinite()) {
// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
// radius, lock yaw to current yaw.
// This prevents excessive yawing.
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();
}
/**
src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp
+3 -3
View File
@@ -51,13 +51,13 @@ bool FlightTaskFailsafe::update()
{
bool ret = FlightTask::update();
if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
if (matrix::Vector2f(_position).isAllFinite()) {
// stay at current position setpoint
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.f;
_acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.f;
} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
// don't move along xy
} else if (matrix::Vector2f(_velocity).isAllFinite()) {
// don't move horizontally
_position_setpoint(0) = _position_setpoint(1) = NAN;
_acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN;
}
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
+3 -3
View File
@@ -45,14 +45,14 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
_stick_acceleration_xy.resetPosition();
if (PX4_ISFINITE(last_setpoint.velocity[0]) && PX4_ISFINITE(last_setpoint.velocity[1])) {
if (Vector2f(last_setpoint.velocity).isAllFinite()) {
_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.velocity));
} else {
_stick_acceleration_xy.resetVelocity(_velocity.xy());
}
if (PX4_ISFINITE(last_setpoint.acceleration[0]) && PX4_ISFINITE(last_setpoint.acceleration[1])) {
if (Vector2f(last_setpoint.acceleration).isAllFinite()) {
_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
}
@@ -80,7 +80,7 @@ bool FlightTaskManualAcceleration::update()
_yaw_setpoint = NAN;
// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
if (Vector2f(_position_setpoint).isAllFinite()) {
// vehicle is steady
_yawspeed_setpoint += _weathervane.getWeathervaneYawrate();
}
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
+7 -12
View File
@@ -45,10 +45,7 @@ bool FlightTaskManualPosition::updateInitialize()
{
bool ret = FlightTaskManualAltitude::updateInitialize();
// require valid position / velocity in xy
return ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1));
return ret && Vector2f(_position).isAllFinite() && Vector2f(_velocity).isAllFinite();
}
bool FlightTaskManualPosition::activate(const trajectory_setpoint_s &last_setpoint)
@@ -112,16 +109,14 @@ void FlightTaskManualPosition::_updateXYlock()
const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON;
const bool stopped = (_param_mpc_hold_max_xy.get() < FLT_EPSILON || vel_xy_norm < _param_mpc_hold_max_xy.get());
if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint(0) = _position(0);
_position_setpoint(1) = _position(1);
if (apply_brake && stopped && !Vector2f(_position_setpoint).isAllFinite()) {
_position_setpoint.xy() = _position.xy();
} else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) {
} else if (Vector2f(_position_setpoint).isAllFinite() && apply_brake) {
// Position is locked but check if a reset event has happened.
// We will shift the setpoints.
if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counter) {
_position_setpoint(0) = _position(0);
_position_setpoint(1) = _position(1);
_position_setpoint.xy() = _position.xy();
_reset_counter = _sub_vehicle_local_position.get().xy_reset_counter;
}
@@ -144,8 +139,8 @@ void FlightTaskManualPosition::_updateSetpoints()
if (_weathervane.isActive()) {
_yaw_setpoint = NAN;
// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. aren't NAN)
if (Vector2f(_position_setpoint).isAllFinite()) {
// vehicle is steady
_yawspeed_setpoint += _weathervane.getWeathervaneYawrate();
}
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+1 -6
View File
@@ -169,12 +169,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
// need a valid position and velocity
ret = ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_position(2))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1))
&& PX4_ISFINITE(_velocity(2));
ret = ret && _position.isAllFinite() && _velocity.isAllFinite();
Vector3f pos_prev{last_setpoint.position};
Vector3f vel_prev{last_setpoint.velocity};
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+2 -2
View File
@@ -177,7 +177,7 @@ void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt)
{
const bool moving = _velocity_setpoint.norm_squared() > FLT_EPSILON;
const bool position_locked = PX4_ISFINITE(_position_setpoint(0)) || PX4_ISFINITE(_position_setpoint(1));
const bool position_locked = Vector2f(_position_setpoint).isAllFinite();
// lock position
if (!moving && !position_locked) {
@@ -189,7 +189,7 @@ void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector
_position_setpoint.setNaN();
// avoid velocity setpoint jump caused by ignoring remaining position error
if (PX4_ISFINITE(vel_sp_feedback(0)) && PX4_ISFINITE(vel_sp_feedback(1))) {
if (vel_sp_feedback.isAllFinite()) {
_velocity_setpoint = vel_sp_feedback;
}
}
src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
+1 -6
View File
@@ -64,12 +64,7 @@ bool Sticks::checkAndUpdateStickInputs()
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
// valid stick inputs are required
const bool valid_sticks = PX4_ISFINITE(_positions(0))
&& PX4_ISFINITE(_positions(1))
&& PX4_ISFINITE(_positions(2))
&& PX4_ISFINITE(_positions(3));
_input_available = valid_sticks;
_input_available = _positions.isAllFinite();
} else {
failsafe_flags_s failsafe_flags;