diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index a3b25145a9..99cfe6a0de 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -869,7 +869,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_PREFLIGHT_CALIBRATION: - if (AP_Math::is_equal(packet.param1,1.0f)) { + if (is_equal(packet.param1,1.0f)) { ins.init_gyro(); if (ins.gyro_calibrated_ok_all()) { ahrs.reset_gyro_drift(); @@ -877,13 +877,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else { result = MAV_RESULT_FAILED; } - } else if (AP_Math::is_equal(packet.param3,1.0f)) { + } else if (is_equal(packet.param3,1.0f)) { init_barometer(); result = MAV_RESULT_ACCEPTED; - } else if (AP_Math::is_equal(packet.param4,1.0f)) { + } else if (is_equal(packet.param4,1.0f)) { trim_radio(); result = MAV_RESULT_ACCEPTED; - } else if (AP_Math::is_equal(packet.param5,1.0f)) { + } else if (is_equal(packet.param5,1.0f)) { float trim_roll, trim_pitch; AP_InertialSensor_UserInteract_MAVLink interact(this); if (g.skip_gyro_cal) { @@ -905,12 +905,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: - if (AP_Math::is_equal(packet.param1,2.0f)) { + if (is_equal(packet.param1,2.0f)) { // save first compass's offsets compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } - if (AP_Math::is_equal(packet.param1,5.0f)) { + if (is_equal(packet.param1,5.0f)) { // save secondary compass's offsets compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; @@ -967,15 +967,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (AP_Math::is_equal(packet.param1,1.0f) || AP_Math::is_equal(packet.param1,3.0f)) { + if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader - hal.scheduler->reboot(AP_Math::is_equal(packet.param1,3.0f)); + hal.scheduler->reboot(is_equal(packet.param1,3.0f)); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { - if (AP_Math::is_equal(packet.param1,1.0f)) { + if (is_equal(packet.param1,1.0f)) { gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); result = MAV_RESULT_ACCEPTED; } diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index e64ffb2cbb..932880eca0 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -337,7 +337,7 @@ struct PACKED log_Sonar { static void Log_Write_Sonar() { uint16_t turn_time = 0; - if (!AP_Math::is_zero(obstacle.turn_angle)) { + if (!is_zero(obstacle.turn_angle)) { turn_time = hal.scheduler->millis() - obstacle.detected_time_ms; } struct log_Sonar pkt = { diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index f62c406cda..08fdb167e3 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -40,7 +40,7 @@ static bool auto_check_trigger(void) return true; } - if (g.auto_trigger_pin == -1 && AP_Math::is_zero(g.auto_kickstart)) { + if (g.auto_trigger_pin == -1 && is_zero(g.auto_kickstart)) { // no trigger configured - let's go! auto_triggered = true; return true; @@ -52,7 +52,7 @@ static bool auto_check_trigger(void) return true; } - if (!AP_Math::is_zero(g.auto_kickstart)) { + if (!is_zero(g.auto_kickstart)) { float xaccel = ins.get_accel().x; if (xaccel >= g.auto_kickstart) { gcs_send_text_fmt(PSTR("Triggered AUTO xaccel=%.1f"), (double)xaccel); diff --git a/APMrover2/test.pde b/APMrover2/test.pde index 1e48c1d1c0..0fea8aba4a 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -480,7 +480,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) float dist_cm = sonar.distance_cm(0); float voltage = sonar.voltage_mv(0); - if (AP_Math::is_zero(sonar_dist_cm_min)) { + if (is_zero(sonar_dist_cm_min)) { sonar_dist_cm_min = dist_cm; voltage_min = voltage; } @@ -491,7 +491,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) dist_cm = sonar.distance_cm(1); voltage = sonar.voltage_mv(1); - if (AP_Math::is_zero(sonar2_dist_cm_min)) { + if (is_zero(sonar2_dist_cm_min)) { sonar2_dist_cm_min = dist_cm; voltage2_min = voltage; }