fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 12:10:34 +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-18 16:57:01 +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/airspeed_selector/AirspeedValidator.cpp
+1 -2
View File
@@ -130,8 +130,7 @@ AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vec
_scale_check_TAS(segment_index) = airspeed_true_raw;
// run check if all segments are filled
if (PX4_ISFINITE(_scale_check_groundspeed.norm_squared())) {
if (_scale_check_groundspeed.isAllFinite()) {
float ground_speed_sum = 0.f;
float TAS_sum = 0.f;
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+2 -6
View File
@@ -441,9 +441,7 @@ bool AttitudeEstimatorQ::init_attitude_q()
_q.normalize();
if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f) {
if (_q.isAllFinite() && _q.length() > 0.95f && _q.length() < 1.05f) {
_initialized = true;
} else {
@@ -552,9 +550,7 @@ bool AttitudeEstimatorQ::update(float dt)
// Normalize quaternion
_q.normalize();
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
if (!_q.isAllFinite()) {
// Reset quaternion to last good state
_q = q_last;
_rates.zero();
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+1 -2
View File
@@ -752,8 +752,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
_vehicle_angular_velocity_sub.copy(&angular_velocity);
const bool condition_angular_velocity_time_valid = angular_velocity.timestamp != 0
&& now < angular_velocity.timestamp + 1_s;
const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
const bool condition_angular_velocity_finite = matrix::Vector3f(angular_velocity.xyz).isAllFinite();
const bool angular_velocity_invalid = !condition_angular_velocity_time_valid
|| !condition_angular_velocity_finite;
src/modules/commander/accelerometer_calibration.cpp
+1 -5
View File
@@ -558,11 +558,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
calibration::Accelerometer calibration{arp.device_id};
if (!calibrated || (offset.norm() > CONSTANTS_ONE_G)
|| !PX4_ISFINITE(offset(0))
|| !PX4_ISFINITE(offset(1))
|| !PX4_ISFINITE(offset(2))) {
if (!calibrated || (offset.norm() > CONSTANTS_ONE_G) || !offset.isAllFinite()) {
PX4_ERR("accel %d quick calibrate failed", accel_index);
} else {
src/modules/commander/gyro_calibration.cpp
+1 -3
View File
@@ -219,9 +219,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* maximum allowable calibration error */
static constexpr float maxoff = math::radians(0.6f);
if (!PX4_ISFINITE(worker_data.offset[0](0)) ||
!PX4_ISFINITE(worker_data.offset[0](1)) ||
!PX4_ISFINITE(worker_data.offset[0](2)) ||
if (!worker_data.offset[0].isAllFinite() ||
fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) {
calibration_log_critical(mavlink_log_pub, "motion, retrying..");
src/modules/ekf2/EKF/estimator_interface.h
+1 -1
View File
@@ -121,7 +121,7 @@ public:
void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; }
// return true if the attitude is usable
bool attitude_valid() const { return PX4_ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
bool attitude_valid() const { return _output_new.quat_nominal.isAllFinite() && _control_status.flags.tilt_align; }
// get vehicle landed status data
bool get_in_air_status() const { return _control_status.flags.in_air; }
src/modules/ekf2/EKF/optflow_fusion.cpp
+1 -1
View File
@@ -346,7 +346,7 @@ void Ekf::fuseOptFlow()
bool Ekf::calcOptFlowBodyRateComp()
{
bool is_body_rate_comp_available = false;
const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2));
const bool use_flow_sensor_gyro = _flow_sample_delayed.gyro_xyz.isAllFinite();
if (use_flow_sensor_gyro) {
src/modules/ekf2/EKF/optical_flow_control.cpp
+1 -2
View File
@@ -88,8 +88,7 @@ void Ekf::controlOpticalFlowFusion()
const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
// don't allow invalid flow gyro_xyz to propagate
if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
if (!_flow_sample_delayed.gyro_xyz.isAllFinite()) {
_flow_sample_delayed.gyro_xyz.zero();
}
src/modules/ekf2/EKF2.cpp
+6 -18
View File
@@ -1741,7 +1741,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
// if error estimates are unavailable, use parameter defined defaults
// check for valid velocity data
if (PX4_ISFINITE(ev_odom.velocity[0]) && PX4_ISFINITE(ev_odom.velocity[1]) && PX4_ISFINITE(ev_odom.velocity[2])) {
if (Vector3f(ev_odom.velocity).isAllFinite()) {
bool velocity_valid = true;
switch (ev_odom.velocity_frame) {
@@ -1770,11 +1770,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
const float evv_noise_var = sq(_param_ekf2_evv_noise.get());
// velocity measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() &&
PX4_ISFINITE(ev_odom.velocity_variance[0]) &&
PX4_ISFINITE(ev_odom.velocity_variance[1]) &&
PX4_ISFINITE(ev_odom.velocity_variance[2])) {
if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.velocity_variance).isAllFinite()) {
ev_data.velVar(0) = fmaxf(evv_noise_var, ev_odom.velocity_variance[0]);
ev_data.velVar(1) = fmaxf(evv_noise_var, ev_odom.velocity_variance[1]);
ev_data.velVar(2) = fmaxf(evv_noise_var, ev_odom.velocity_variance[2]);
@@ -1788,8 +1784,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
}
// check for valid position data
if (PX4_ISFINITE(ev_odom.position[0]) && PX4_ISFINITE(ev_odom.position[1]) && PX4_ISFINITE(ev_odom.position[2])) {
if (Vector3f(ev_odom.position).isAllFinite()) {
bool position_valid = true;
// switch (ev_odom.pose_frame) {
@@ -1814,11 +1809,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
const float evp_noise_var = sq(_param_ekf2_evp_noise.get());
// position measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() &&
PX4_ISFINITE(ev_odom.position_variance[0]) &&
PX4_ISFINITE(ev_odom.position_variance[1]) &&
PX4_ISFINITE(ev_odom.position_variance[2])
) {
if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.position_variance).isAllFinite()) {
ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]);
ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]);
ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]);
@@ -1832,8 +1823,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
}
// check for valid orientation data
if ((PX4_ISFINITE(ev_odom.q[0]) && PX4_ISFINITE(ev_odom.q[1])
&& PX4_ISFINITE(ev_odom.q[2]) && PX4_ISFINITE(ev_odom.q[3]))
if ((Quatf(ev_odom.q).isAllFinite())
&& ((fabsf(ev_odom.q[0]) > 0.f) || (fabsf(ev_odom.q[1]) > 0.f)
|| (fabsf(ev_odom.q[2]) > 0.f) || (fabsf(ev_odom.q[3]) > 0.f))
) {
@@ -1896,9 +1886,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
.quality = optical_flow.quality,
};
if (PX4_ISFINITE(optical_flow.pixel_flow[0]) &&
PX4_ISFINITE(optical_flow.pixel_flow[1]) &&
flow.dt < 1) {
if (Vector2f(optical_flow.pixel_flow).isAllFinite() && flow.dt < 1) {
// Save sensor limits reported by the optical flow sensor
_ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
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;
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+3 -6
View File
@@ -2135,8 +2135,7 @@ FixedwingPositionControl::Run()
_pos_sp_triplet.current.lon = static_cast<double>(NAN);
_pos_sp_triplet.current.alt = NAN;
if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1])
&& PX4_ISFINITE(trajectory_setpoint.position[2])) {
if (Vector3f(trajectory_setpoint.position).isAllFinite()) {
if (_global_local_proj_ref.isInitialized()) {
double lat;
double lon;
@@ -2150,16 +2149,14 @@ FixedwingPositionControl::Run()
}
if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1])
&& PX4_ISFINITE(trajectory_setpoint.velocity[2])) {
if (Vector3f(trajectory_setpoint.velocity).isAllFinite()) {
valid_setpoint = true;
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0];
_pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1];
_pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2];
if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1])
&& PX4_ISFINITE(trajectory_setpoint.acceleration[2])) {
if (Vector3f(trajectory_setpoint.acceleration).isAllFinite()) {
Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized();
Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);
src/modules/gimbal/output.cpp
+3 -3
View File
@@ -213,12 +213,12 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
float dt = math::constrain((t - _last_update) * 1.e-6f, 0.001f, 1.f);
const bool q_setpoint_valid = PX4_ISFINITE(_q_setpoint[0]) && PX4_ISFINITE(_q_setpoint[1])
&& PX4_ISFINITE(_q_setpoint[2]) && PX4_ISFINITE(_q_setpoint[3]);
const matrix::Quatf q_setpoint(_q_setpoint);
const bool q_setpoint_valid = q_setpoint.isAllFinite();
matrix::Eulerf euler_gimbal{};
if (q_setpoint_valid) {
euler_gimbal = matrix::Quatf{_q_setpoint};
euler_gimbal = q_setpoint;
}
for (int i = 0; i < 3; ++i) {
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
+1 -2
View File
@@ -263,8 +263,7 @@ void LandingTargetEstimator::_update_topics()
return;
}
if (!PX4_ISFINITE((float)_uwbDistance.position[0]) || !PX4_ISFINITE((float)_uwbDistance.position[1]) ||
!PX4_ISFINITE((float)_uwbDistance.position[2])) {
if (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) {
PX4_WARN("Position is corrupt!");
return;
}
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+5 -9
View File
@@ -581,9 +581,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
}
// publish local position
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
if (Vector3f(_x(X_x), _x(X_y), _x(X_z)).isAllFinite() &&
Vector3f(_x(X_vx), _x(X_vy), _x(X_vz)).isAllFinite()) {
_pub_lpos.get().timestamp_sample = _timeStamp;
_pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY;
@@ -644,10 +643,8 @@ void BlockLocalPositionEstimator::publishOdom()
const Vector<float, n_x> &xLP = _xLowPass.getState();
// publish vehicle odometry
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
if (Vector3f(_x(X_x), _x(X_y), _x(X_z)).isAllFinite() &&
Vector3f(_x(X_vx), _x(X_vy), _x(X_vz)).isAllFinite()) {
_pub_odom.get().timestamp_sample = _timeStamp;
_pub_odom.get().pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
@@ -795,8 +792,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
}
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
PX4_ISFINITE(xLP(X_vz))) {
Vector3f(xLP(X_vx), xLP(X_vy), xLP(X_vz)).isAllFinite()) {
_pub_gpos.get().timestamp_sample = _timeStamp;
_pub_gpos.get().lat = lat;
_pub_gpos.get().lon = lon;
src/modules/mag_bias_estimator/MagBiasEstimator.cpp
+1 -1
View File
@@ -194,7 +194,7 @@ void MagBiasEstimator::Run()
const Vector3f &bias = _bias_estimator[mag_index].getBias();
const Vector3f bias_rate = (bias - bias_prev) / dt;
if (!PX4_ISFINITE(bias(0)) || !PX4_ISFINITE(bias(1)) || !PX4_ISFINITE(bias(2)) || bias.longerThan(5.f)) {
if (!bias.isAllFinite() || bias.longerThan(5.f)) {
_reset_field_estimator[mag_index] = true;
_valid[mag_index] = false;
_time_valid[mag_index] = 0;
src/modules/mavlink/mavlink_receiver.cpp
+19 -26
View File
@@ -792,10 +792,10 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
sensor_optical_flow.quality = flow.quality;
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro;
sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro;
sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro;
const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
if (integrated_gyro.isAllFinite()) {
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
sensor_optical_flow.delta_angle_available = true;
}
@@ -838,10 +838,10 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
sensor_optical_flow.quality = flow.quality;
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro;
sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro;
sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro;
const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
if (integrated_gyro.isAllFinite()) {
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
sensor_optical_flow.delta_angle_available = true;
}
@@ -1340,16 +1340,16 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odom.timestamp_sample = _mavlink_timesync.sync_stamp(odom_in.time_usec);
const matrix::Vector3f odom_in_p(odom_in.x, odom_in.y, odom_in.z);
// position x/y/z (m)
if (PX4_ISFINITE(odom_in.x) && PX4_ISFINITE(odom_in.y) && PX4_ISFINITE(odom_in.z)) {
if (odom_in_p.isAllFinite()) {
// frame_id: Coordinate frame of reference for the pose data.
switch (odom_in.frame_id) {
case MAV_FRAME_LOCAL_NED:
// NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = odom_in.x;
odom.position[1] = odom_in.y;
odom.position[2] = odom_in.z;
odom_in_p.copyTo(odom.position);
break;
case MAV_FRAME_LOCAL_ENU:
@@ -1363,9 +1363,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
case MAV_FRAME_LOCAL_FRD:
// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth.
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD;
odom.position[0] = odom_in.x;
odom.position[1] = odom_in.y;
odom.position[2] = odom_in.z;
odom_in_p.copyTo(odom.position);
break;
case MAV_FRAME_LOCAL_FLU:
@@ -1408,10 +1406,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
}
// q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame
if (PX4_ISFINITE(odom_in.q[0])
&& PX4_ISFINITE(odom_in.q[1])
&& PX4_ISFINITE(odom_in.q[2])
&& PX4_ISFINITE(odom_in.q[3])) {
if (matrix::Quatf(odom_in.q).isAllFinite()) {
odom.q[0] = odom_in.q[0];
odom.q[1] = odom_in.q[1];
@@ -1428,16 +1423,16 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
}
}
const matrix::Vector3f odom_in_v(odom_in.vx, odom_in.vy, odom_in.vz);
// velocity vx/vy/vz (m/s)
if (PX4_ISFINITE(odom_in.vx) && PX4_ISFINITE(odom_in.vy) && PX4_ISFINITE(odom_in.vz)) {
if (odom_in_v.isAllFinite()) {
// child_frame_id: Coordinate frame of reference for the velocity in free space (twist) data.
switch (odom_in.child_frame_id) {
case MAV_FRAME_LOCAL_NED:
// NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
odom.velocity[0] = odom_in.vx;
odom.velocity[1] = odom_in.vy;
odom.velocity[2] = odom_in.vz;
odom_in_v.copyTo(odom.velocity);
break;
case MAV_FRAME_LOCAL_ENU:
@@ -1451,9 +1446,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
case MAV_FRAME_LOCAL_FRD:
// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD;
odom.velocity[0] = odom_in.vx;
odom.velocity[1] = odom_in.vy;
odom.velocity[2] = odom_in.vz;
odom_in_v.copyTo(odom.velocity);
break;
case MAV_FRAME_LOCAL_FLU:
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+14 -17
View File
@@ -268,15 +268,14 @@ PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehic
{
PositionControlStates states;
const Vector2f position_xy(vehicle_local_position.x, vehicle_local_position.y);
// only set position states if valid and finite
if (PX4_ISFINITE(vehicle_local_position.x) && PX4_ISFINITE(vehicle_local_position.y)
&& vehicle_local_position.xy_valid) {
states.position(0) = vehicle_local_position.x;
states.position(1) = vehicle_local_position.y;
if (vehicle_local_position.xy_valid && position_xy.isAllFinite()) {
states.position.xy() = position_xy;
} else {
states.position(0) = NAN;
states.position(1) = NAN;
states.position(0) = states.position(1) = NAN;
}
if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) {
@@ -286,18 +285,16 @@ PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehic
states.position(2) = NAN;
}
if (PX4_ISFINITE(vehicle_local_position.vx) && PX4_ISFINITE(vehicle_local_position.vy)
&& vehicle_local_position.v_xy_valid) {
states.velocity(0) = vehicle_local_position.vx;
states.velocity(1) = vehicle_local_position.vy;
states.acceleration(0) = _vel_x_deriv.update(vehicle_local_position.vx);
states.acceleration(1) = _vel_y_deriv.update(vehicle_local_position.vy);
const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy);
if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) {
states.velocity.xy() = velocity_xy;
states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0));
states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1));
} else {
states.velocity(0) = NAN;
states.velocity(1) = NAN;
states.acceleration(0) = NAN;
states.acceleration(1) = NAN;
states.velocity(0) = states.velocity(1) = NAN;
states.acceleration(0) = states.acceleration(1) = NAN;
// reset derivatives to prevent acceleration spikes when regaining velocity
_vel_x_deriv.reset();
@@ -599,7 +596,7 @@ trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const
trajectory_setpoint_s failsafe_setpoint = PositionControl::empty_trajectory_setpoint;
failsafe_setpoint.timestamp = now;
if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) {
if (Vector2f(states.velocity).isAllFinite()) {
// don't move along xy
failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = 0.f;
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+1 -4
View File
@@ -117,10 +117,7 @@ bool PositionControl::update(const float dt)
}
// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
return valid;
return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite();
}
void PositionControl::_positionControl()
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+1 -1
View File
@@ -234,7 +234,7 @@ void VehicleAcceleration::Run()
while (_sensor_sub.update(&sensor_data)) {
const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z};
if (math::isFinite(accel_raw)) {
if (accel_raw.isAllFinite()) {
// Apply calibration and filter
// - calibration offsets, scale factors, and thermal scale (if available)
// - estimated in run bias (if available)
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+3 -9
View File
@@ -499,10 +499,7 @@ Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
// start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection)
Vector3f angular_velocity_uncalibrated{_calibration.Uncorrect(_angular_velocity + _bias)};
if (PX4_ISFINITE(angular_velocity_uncalibrated(0))
&& PX4_ISFINITE(angular_velocity_uncalibrated(1))
&& PX4_ISFINITE(angular_velocity_uncalibrated(2))) {
if (angular_velocity_uncalibrated.isAllFinite()) {
return angular_velocity_uncalibrated;
}
}
@@ -517,10 +514,7 @@ Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const
// start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection)
Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration};
if (PX4_ISFINITE(angular_acceleration(0))
&& PX4_ISFINITE(angular_acceleration(1))
&& PX4_ISFINITE(angular_acceleration(2))) {
if (angular_acceleration.isAllFinite()) {
return angular_acceleration;
}
}
@@ -849,7 +843,7 @@ void VehicleAngularVelocity::Run()
sensor_gyro_s sensor_data;
while (_sensor_sub.update(&sensor_data)) {
if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) {
if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) {
if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {
_timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz;
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
+1 -5
View File
@@ -125,11 +125,7 @@ void VehicleOpticalFlow::Run()
// delta angle
// - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available
if (sensor_optical_flow.delta_angle_available
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[0])
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[1])
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[2])
) {
if (sensor_optical_flow.delta_angle_available && Vector3f(sensor_optical_flow.delta_angle).isAllFinite()) {
// passthrough integrated gyro if available
_delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle};
_delta_angle_available = true;
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+16 -26
View File
@@ -634,15 +634,15 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
odom.timestamp_sample = hrt_absolute_time(); // _mavlink_timesync.sync_stamp(odom_in.time_usec);
// position x/y/z (m)
if (PX4_ISFINITE(odom_in.x) && PX4_ISFINITE(odom_in.y) && PX4_ISFINITE(odom_in.z)) {
matrix::Vector3f odom_in_p(odom_in.x, odom_in.y, odom_in.z);
if (odom_in_p.isAllFinite()) {
// frame_id: Coordinate frame of reference for the pose data.
switch (odom_in.frame_id) {
case MAV_FRAME_LOCAL_NED:
// NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = odom_in.x;
odom.position[1] = odom_in.y;
odom.position[2] = odom_in.z;
odom_in_p.copyTo(odom.position);
break;
case MAV_FRAME_LOCAL_ENU:
@@ -656,9 +656,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
case MAV_FRAME_LOCAL_FRD:
// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth.
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD;
odom.position[0] = odom_in.x;
odom.position[1] = odom_in.y;
odom.position[2] = odom_in.z;
odom_in_p.copyTo(odom.position);
break;
case MAV_FRAME_LOCAL_FLU:
@@ -701,11 +699,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
}
// q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame
if (PX4_ISFINITE(odom_in.q[0])
&& PX4_ISFINITE(odom_in.q[1])
&& PX4_ISFINITE(odom_in.q[2])
&& PX4_ISFINITE(odom_in.q[3])) {
if (matrix::Quatf(odom_in.q).isAllFinite()) {
odom.q[0] = odom_in.q[0];
odom.q[1] = odom_in.q[1];
odom.q[2] = odom_in.q[2];
@@ -722,15 +716,15 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
}
// velocity vx/vy/vz (m/s)
if (PX4_ISFINITE(odom_in.vx) && PX4_ISFINITE(odom_in.vy) && PX4_ISFINITE(odom_in.vz)) {
matrix::Vector3f odom_in_v(odom_in.vx, odom_in.vy, odom_in.vz);
if (odom_in_v.isAllFinite()) {
// child_frame_id: Coordinate frame of reference for the velocity in free space (twist) data.
switch (odom_in.child_frame_id) {
case MAV_FRAME_LOCAL_NED:
// NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
odom.velocity[0] = odom_in.vx;
odom.velocity[1] = odom_in.vy;
odom.velocity[2] = odom_in.vz;
odom_in_v.copyTo(odom.velocity);
break;
case MAV_FRAME_LOCAL_ENU:
@@ -744,9 +738,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
case MAV_FRAME_LOCAL_FRD:
// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD;
odom.velocity[0] = odom_in.vx;
odom.velocity[1] = odom_in.vy;
odom.velocity[2] = odom_in.vz;
odom_in_v.copyTo(odom.velocity);
break;
case MAV_FRAME_LOCAL_FLU:
@@ -762,9 +754,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
case MAV_FRAME_BODY_FRD:
// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD;
odom.velocity[0] = odom_in.vx;
odom.velocity[1] = odom_in.vy;
odom.velocity[2] = odom_in.vz;
odom_in_v.copyTo(odom.velocity);
break;
default:
@@ -862,10 +852,10 @@ void SimulatorMavlink::handle_message_optical_flow(const mavlink_message_t *msg)
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
sensor_optical_flow.quality = flow.quality;
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro;
sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro;
sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro;
matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
if (integrated_gyro.isAllFinite()) {
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
sensor_optical_flow.delta_angle_available = true;
}