mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
AP_Compass: compiler warnings: apply is_zero(float) or is_equal(float)
This commit is contained in:
parent
820199635b
commit
ad11a57f57
@ -235,7 +235,7 @@ void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS
|
||||
*/
|
||||
bool AP_Camera::update_location(const struct Location &loc)
|
||||
{
|
||||
if (_trigg_dist == 0.0f) {
|
||||
if (AP_Math::is_zero(_trigg_dist)) {
|
||||
return false;
|
||||
}
|
||||
if (_last_location.lat == 0 && _last_location.lng == 0) {
|
||||
|
@ -284,7 +284,7 @@ bool AP_Compass_AK8963_MPU9250::read_raw()
|
||||
_mag_y = (float) int16_val(rx, 2);
|
||||
_mag_z = (float) int16_val(rx, 3);
|
||||
|
||||
if (_mag_x == 0 && _mag_y == 0 && _mag_z == 0) {
|
||||
if (AP_Math::is_zero(_mag_x) && AP_Math::is_zero(_mag_y) && AP_Math::is_zero(_mag_z)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -60,7 +60,7 @@ void AP_Compass_Backend::apply_corrections(Vector3f &mag, uint8_t i)
|
||||
being applied so it can be logged correctly
|
||||
*/
|
||||
mag += offsets;
|
||||
if(_compass._motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _compass._thr_or_curr != 0.0f) {
|
||||
if(_compass._motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && !AP_Math::is_zero(_compass._thr_or_curr)) {
|
||||
state.motor_offset = mot * _compass._thr_or_curr;
|
||||
mag += state.motor_offset;
|
||||
} else {
|
||||
|
@ -533,7 +533,7 @@ bool Compass::configured(uint8_t i)
|
||||
}
|
||||
|
||||
// exit immediately if all offsets are zero
|
||||
if (get_offsets(i).length() == 0.0f) {
|
||||
if (AP_Math::is_zero(get_offsets(i).length())) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -575,7 +575,7 @@ void Compass::setHIL(float roll, float pitch, float yaw)
|
||||
// create a rotation matrix for the given attitude
|
||||
R.from_euler(roll, pitch, yaw);
|
||||
|
||||
if (_hil.last_declination != get_declination()) {
|
||||
if (!AP_Math::is_equal(_hil.last_declination,get_declination())) {
|
||||
_setup_earth_field();
|
||||
_hil.last_declination = get_declination();
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user