From 9e76223c344938709d927752bea5e8ae32568242 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 9 Jul 2017 14:06:18 +1000 Subject: [PATCH] AP_Arming: eliminate GCS_MAVLINK::send_statustext_all --- libraries/AP_Arming/AP_Arming.cpp | 62 +++++++++++++++---------------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 2d39b8c81c..9dd01a8a14 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -117,7 +117,7 @@ bool AP_Arming::barometer_checks(bool report) (checks_to_perform & ARMING_CHECK_BARO)) { if (!barometer.all_healthy()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Barometer not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Barometer not healthy"); } return false; } @@ -137,7 +137,7 @@ bool AP_Arming::airspeed_checks(bool report) } if (airspeed->enabled() && airspeed->use() && !airspeed->healthy()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Airspeed not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Airspeed not healthy"); } return false; } @@ -152,13 +152,13 @@ bool AP_Arming::logging_checks(bool report) (checks_to_perform & ARMING_CHECK_LOGGING)) { if (DataFlash_Class::instance()->logging_failed()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Logging failed"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Logging failed"); } return false; } if (!DataFlash_Class::instance()->CardInserted()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: No SD card"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: No SD card"); } return false; } @@ -173,25 +173,25 @@ bool AP_Arming::ins_checks(bool report) const AP_InertialSensor &ins = ahrs.get_ins(); if (!ins.get_gyro_health_all()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Gyros not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Gyros not healthy"); } return false; } if (!ins.gyro_calibrated_ok_all()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Gyros not calibrated"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Gyros not calibrated"); } return false; } if (!ins.get_accel_health_all()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Accels not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels not healthy"); } return false; } if (!ins.accel_calibrated_ok_all()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: 3D Accel calibration needed"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: 3D Accel calibration needed"); } return false; } @@ -199,7 +199,7 @@ bool AP_Arming::ins_checks(bool report) //check if accelerometers have calibrated and require reboot if (ins.accel_cal_requires_reboot()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Accels calibrated requires reboot"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels calibrated requires reboot"); } return false; } @@ -233,7 +233,7 @@ bool AP_Arming::ins_checks(bool report) } if (AP_HAL::millis() - last_accel_pass_ms[i] > 10000) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Accels inconsistent"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels inconsistent"); } return false; } @@ -257,7 +257,7 @@ bool AP_Arming::ins_checks(bool report) } if (AP_HAL::millis() - last_gyro_pass_ms[i] > 10000) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Gyros inconsistent"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Gyros inconsistent"); } return false; } @@ -280,14 +280,14 @@ bool AP_Arming::compass_checks(bool report) if (!_compass.healthy()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Compass not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass not healthy"); } return false; } // check compass learning is on or offsets have been set if (!_compass.learn_offsets_enabled() && !_compass.configured()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Compass not calibrated"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass not calibrated"); } return false; } @@ -295,7 +295,7 @@ bool AP_Arming::compass_checks(bool report) //check if compass is calibrating if (_compass.is_calibrating()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Arm: Compass calibration running"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Arm: Compass calibration running"); } return false; } @@ -303,7 +303,7 @@ bool AP_Arming::compass_checks(bool report) //check if compass has calibrated and requires reboot if (_compass.compass_cal_requires_reboot()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot"); } return false; } @@ -312,7 +312,7 @@ bool AP_Arming::compass_checks(bool report) Vector3f offsets = _compass.get_offsets(); if (offsets.length() > _compass.get_offsets_max()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Compass offsets too high"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass offsets too high"); } return false; } @@ -321,7 +321,7 @@ bool AP_Arming::compass_checks(bool report) float mag_field = _compass.get_field().length(); if (mag_field > AP_ARMING_COMPASS_MAGFIELD_MAX || mag_field < AP_ARMING_COMPASS_MAGFIELD_MIN) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Check mag field"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Check mag field"); } return false; } @@ -329,7 +329,7 @@ bool AP_Arming::compass_checks(bool report) // check all compasses point in roughly same direction if (!_compass.consistent()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,"PreArm: Compasses inconsistent"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compasses inconsistent"); } return false; } @@ -347,7 +347,7 @@ bool AP_Arming::gps_checks(bool report) if (home_status() == HOME_UNSET || gps.status() < AP_GPS::GPS_OK_FIX_3D) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Bad GPS Position"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Bad GPS Position"); } return false; } @@ -357,7 +357,7 @@ bool AP_Arming::gps_checks(bool report) uint8_t first_unconfigured = gps.first_unconfigured_gps(); if (first_unconfigured != AP_GPS::GPS_ALL_CONFIGURED) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS %d failing configuration checks", first_unconfigured + 1); gps.broadcast_first_configuration_failure_reason(); @@ -371,7 +371,7 @@ bool AP_Arming::gps_checks(bool report) float distance_m; if (!gps.all_consistent(distance_m)) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS positions differ by %4.1fm", (double)distance_m); } @@ -379,7 +379,7 @@ bool AP_Arming::gps_checks(bool report) } if (!gps.blend_health_check()) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: GPS blending unhealthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS blending unhealthy"); } return false; } @@ -395,7 +395,7 @@ bool AP_Arming::battery_checks(bool report) if (AP_Notify::flags.failsafe_battery) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Battery failsafe on"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Battery failsafe on"); } return false; } @@ -403,7 +403,7 @@ bool AP_Arming::battery_checks(bool report) for (int i = 0; i < _battery.num_instances(); i++) { if ((_min_voltage[i] > 0.0f) && (_battery.voltage(i) < _min_voltage[i])) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Battery %d voltage %.1f below minimum %.1f", + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Battery %d voltage %.1f below minimum %.1f", i+1, (double)_battery.voltage(i), (double)_min_voltage[i]); @@ -423,7 +423,7 @@ bool AP_Arming::hardware_safety_check(bool report) // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Hardware safety switch"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Hardware safety switch"); } return false; } @@ -439,7 +439,7 @@ bool AP_Arming::manual_transmitter_checks(bool report) if (AP_Notify::flags.failsafe_radio) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Radio failsafe on"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Radio failsafe on"); } return false; } @@ -459,7 +459,7 @@ bool AP_Arming::board_voltage_checks(bool report) if(!is_zero(hal.analogin->board_voltage()) && ((hal.analogin->board_voltage() < AP_ARMING_BOARD_VOLTAGE_MIN) || (hal.analogin->board_voltage() > AP_ARMING_BOARD_VOLTAGE_MAX))) { if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,"PreArm: Check board voltage"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check board voltage"); } return false; } @@ -494,7 +494,7 @@ bool AP_Arming::arm_checks(uint8_t method) if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_LOGGING)) { if (!DataFlash_Class::instance()->logging_started()) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Arm: Logging not started"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Arm: Logging not started"); return false; } } @@ -516,7 +516,7 @@ bool AP_Arming::arm(uint8_t method) if (checks_to_perform == ARMING_CHECK_NONE) { armed = true; arming_method = NONE; - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Throttle armed"); + gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed"); return true; } @@ -524,7 +524,7 @@ bool AP_Arming::arm(uint8_t method) armed = true; arming_method = method; - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Throttle armed"); + gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed"); //TODO: Log motor arming to the dataflash //Can't do this from this class until there is a unified logging library @@ -550,7 +550,7 @@ bool AP_Arming::disarm() } armed = false; - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Throttle disarmed"); + gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed"); //TODO: Log motor disarming to the dataflash //Can't do this from this class until there is a unified logging library.