From a19d4b65fc68e8ada4cf279507c40152c4116ae4 Mon Sep 17 00:00:00 2001 From: murata Date: Mon, 21 Mar 2022 18:24:45 +0900 Subject: [PATCH] AP_Compass: Console output can be disabled --- libraries/AP_Compass/AP_Compass.cpp | 2 +- libraries/AP_Compass/AP_Compass_AK09916.cpp | 10 +++++----- libraries/AP_Compass/AP_Compass_AK8963.cpp | 10 +++++----- libraries/AP_Compass/AP_Compass_BMM150.cpp | 4 ++-- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 10 +++++----- libraries/AP_Compass/AP_Compass_LSM303D.cpp | 4 ++-- libraries/AP_Compass/AP_Compass_LSM9DS1.cpp | 19 +++++++++---------- libraries/AP_Compass/AP_Compass_RM3100.cpp | 2 +- 8 files changed, 30 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 492027b96b..8a01cc08ab 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1461,7 +1461,7 @@ void Compass::_detect_backends(void) if (_backend_count == 0 || _compass_count == 0) { - hal.console->printf("No Compass backends available\n"); + DEV_PRINTF("No Compass backends available\n"); } } diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 9924e36b34..5ea8d381d1 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -155,7 +155,7 @@ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtrread_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2)) { // a device is replying on the AK09916 I2C address, don't // load the ICM20948 - hal.console->printf("ICM20948: AK09916 bus conflict\n"); + DEV_PRINTF("ICM20948: AK09916 bus conflict\n"); goto fail; } @@ -230,7 +230,7 @@ bool AP_Compass_AK09916::init() _bus->get_semaphore()->take_blocking(); if (!_bus->configure()) { - hal.console->printf("AK09916: Could not configure the bus\n"); + DEV_PRINTF("AK09916: Could not configure the bus\n"); goto fail; } @@ -239,7 +239,7 @@ bool AP_Compass_AK09916::init() } if (!_check_id()) { - hal.console->printf("AK09916: Wrong id\n"); + DEV_PRINTF("AK09916: Wrong id\n"); goto fail; } @@ -247,12 +247,12 @@ bool AP_Compass_AK09916::init() _bus->setup_checked_registers(1); if (!_setup_mode()) { - hal.console->printf("AK09916: Could not setup mode\n"); + DEV_PRINTF("AK09916: Could not setup mode\n"); goto fail; } if (!_bus->start_measurements()) { - hal.console->printf("AK09916: Could not start measurements\n"); + DEV_PRINTF("AK09916: Could not start measurements\n"); goto fail; } diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index d78f9220f5..9667090280 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -135,27 +135,27 @@ bool AP_Compass_AK8963::init() _bus->get_semaphore()->take_blocking(); if (!_bus->configure()) { - hal.console->printf("AK8963: Could not configure the bus\n"); + DEV_PRINTF("AK8963: Could not configure the bus\n"); goto fail; } if (!_check_id()) { - hal.console->printf("AK8963: Wrong id\n"); + DEV_PRINTF("AK8963: Wrong id\n"); goto fail; } if (!_calibrate()) { - hal.console->printf("AK8963: Could not read calibration data\n"); + DEV_PRINTF("AK8963: Could not read calibration data\n"); goto fail; } if (!_setup_mode()) { - hal.console->printf("AK8963: Could not setup mode\n"); + DEV_PRINTF("AK8963: Could not setup mode\n"); goto fail; } if (!_bus->start_measurements()) { - hal.console->printf("AK8963: Could not start measurements\n"); + DEV_PRINTF("AK8963: Could not start measurements\n"); goto fail; } diff --git a/libraries/AP_Compass/AP_Compass_BMM150.cpp b/libraries/AP_Compass/AP_Compass_BMM150.cpp index 4202e07b7d..7d26f6ed29 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.cpp +++ b/libraries/AP_Compass/AP_Compass_BMM150.cpp @@ -118,7 +118,7 @@ bool AP_Compass_BMM150::_load_trim_values() } } if (-1 == tries) { - hal.console->printf("BMM150: Failed to load trim registers\n"); + DEV_PRINTF("BMM150: Failed to load trim registers\n"); return false; } @@ -175,7 +175,7 @@ bool AP_Compass_BMM150::init() break; } if (boot_tries == 0) { - hal.console->printf("BMM150: Wrong chip ID 0x%02x should be 0x%02x\n", val, CHIP_ID_VAL); + DEV_PRINTF("BMM150: Wrong chip ID 0x%02x should be 0x%02x\n", val, CHIP_ID_VAL); } } if (-1 == boot_tries) { diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 4f237bd204..4b09f8a7ab 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -152,7 +152,7 @@ bool AP_Compass_HMC5843::init() AP_HAL::Semaphore *bus_sem = _bus->get_semaphore(); if (!bus_sem) { - hal.console->printf("HMC5843: Unable to get bus semaphore\n"); + DEV_PRINTF("HMC5843: Unable to get bus semaphore\n"); return false; } bus_sem->take_blocking(); @@ -161,7 +161,7 @@ bool AP_Compass_HMC5843::init() _bus->set_retries(10); if (!_bus->configure()) { - hal.console->printf("HMC5843: Could not configure the bus\n"); + DEV_PRINTF("HMC5843: Could not configure the bus\n"); goto errout; } @@ -170,7 +170,7 @@ bool AP_Compass_HMC5843::init() } if (!_calibrate()) { - hal.console->printf("HMC5843: Could not calibrate sensor\n"); + DEV_PRINTF("HMC5843: Could not calibrate sensor\n"); goto errout; } @@ -179,7 +179,7 @@ bool AP_Compass_HMC5843::init() } if (!_bus->start_measurements()) { - hal.console->printf("HMC5843: Could not start measurements on bus\n"); + DEV_PRINTF("HMC5843: Could not start measurements on bus\n"); goto errout; } @@ -210,7 +210,7 @@ bool AP_Compass_HMC5843::init() _bus->register_periodic_callback(13333, FUNCTOR_BIND_MEMBER(&AP_Compass_HMC5843::_timer, void)); - hal.console->printf("HMC5843 found on bus 0x%x\n", (unsigned)_bus->get_bus_id()); + DEV_PRINTF("HMC5843 found on bus 0x%x\n", (unsigned)_bus->get_bus_id()); return true; diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp index 1e62f6e6cf..ec5910e8b7 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp @@ -222,7 +222,7 @@ bool AP_Compass_LSM303D::_read_sample() } rx; if (_register_read(ADDR_CTRL_REG7) != _reg7_expected) { - hal.console->printf("LSM303D _read_data_transaction_accel: _reg7_expected unexpected\n"); + DEV_PRINTF("LSM303D _read_data_transaction_accel: _reg7_expected unexpected\n"); return false; } @@ -316,7 +316,7 @@ bool AP_Compass_LSM303D::_hardware_init() } } if (tries == 5) { - hal.console->printf("Failed to boot LSM303D 5 times\n"); + DEV_PRINTF("Failed to boot LSM303D 5 times\n"); goto fail_tries; } diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index f40ea2e05a..3fb407e2d7 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp @@ -84,23 +84,23 @@ bool AP_Compass_LSM9DS1::init() AP_HAL::Semaphore *bus_sem = _dev->get_semaphore(); if (!bus_sem) { - hal.console->printf("LSM9DS1: Unable to get bus semaphore\n"); + DEV_PRINTF("LSM9DS1: Unable to get bus semaphore\n"); return false; } bus_sem->take_blocking(); if (!_check_id()) { - hal.console->printf("LSM9DS1: Could not check id\n"); + DEV_PRINTF("LSM9DS1: Could not check id\n"); goto errout; } if (!_configure()) { - hal.console->printf("LSM9DS1: Could not check id\n"); + DEV_PRINTF("LSM9DS1: Could not check id\n"); goto errout; } if (!_set_scale()) { - hal.console->printf("LSM9DS1: Could not set scale\n"); + DEV_PRINTF("LSM9DS1: Could not set scale\n"); goto errout; } @@ -127,15 +127,14 @@ errout: void AP_Compass_LSM9DS1::_dump_registers() { - hal.console->printf("LSMDS1 registers\n"); + DEV_PRINTF("LSMDS1 registers\n"); for (uint8_t reg = LSM9DS1M_OFFSET_X_REG_L_M; reg <= LSM9DS1M_INT_THS_H_M; reg++) { - uint8_t v = _register_read(reg); - hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v); + DEV_PRINTF("%02x:%02x ", (unsigned)reg, (unsigned)_register_read(reg)); if ((reg - (LSM9DS1M_OFFSET_X_REG_L_M-1)) % 16 == 0) { - hal.console->printf("\n"); + DEV_PRINTF("\n"); } } - hal.console->printf("\n"); + DEV_PRINTF("\n"); } void AP_Compass_LSM9DS1::_update(void) @@ -174,7 +173,7 @@ bool AP_Compass_LSM9DS1::_check_id(void) uint8_t value = _register_read(LSM9DS1M_WHO_AM_I); if (value != WHO_AM_I_MAG) { - hal.console->printf("LSM9DS1: unexpected WHOAMI 0x%x\n", (unsigned)value); + DEV_PRINTF("LSM9DS1: unexpected WHOAMI 0x%x\n", (unsigned)value); return false; } diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp index f3423c1348..8df7c182df 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.cpp +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -149,7 +149,7 @@ bool AP_Compass_RM3100::init() } set_dev_id(compass_instance, dev->get_bus_id()); - hal.console->printf("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance); + DEV_PRINTF("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance); set_rotation(compass_instance, rotation);