From 324f5e3ac97808dd7b553eedcf32f3bcad382754 Mon Sep 17 00:00:00 2001 From: murata Date: Mon, 21 Mar 2022 18:25:18 +0900 Subject: [PATCH] AP_InertialSensor: Console output can be disabled --- .../AP_InertialSensor/AP_InertialSensor.cpp | 18 ++++++++--------- .../AP_InertialSensor_BMI055.cpp | 4 ++-- .../AP_InertialSensor_BMI088.cpp | 8 ++++---- .../AP_InertialSensor_BMI160.cpp | 20 +++++++++---------- .../AP_InertialSensor_BMI270.cpp | 4 ++-- .../AP_InertialSensor_Invensense.cpp | 8 ++++---- .../AP_InertialSensor_Invensensev2.cpp | 8 ++++---- .../AP_InertialSensor_LSM9DS0.cpp | 2 +- .../AP_InertialSensor_LSM9DS1.cpp | 8 ++++---- .../AP_InertialSensor_NONE.cpp | 2 +- .../AP_InertialSensor_RST.cpp | 2 +- 11 files changed, 42 insertions(+), 42 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index a331dcfc46..54b0f0c971 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1166,7 +1166,7 @@ AP_InertialSensor::detect_backends(void) #if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 ADD_BACKEND(AP_InertialSensor_NONE::detect(*this, INS_NONE_SENSOR_A)); #else - hal.console->printf("INS: unable to initialise driver\n"); + DEV_PRINTF("INS: unable to initialise driver\n"); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: unable to initialise driver"); AP_BoardConfig::config_error("INS: unable to initialise driver"); #endif @@ -1446,7 +1446,7 @@ AP_InertialSensor::_init_gyro() AP_Notify::flags.initialising = true; // cold start - hal.console->printf("Init Gyro"); + DEV_PRINTF("Init Gyro"); /* we do the gyro calibration with no board rotation. This avoids @@ -1497,7 +1497,7 @@ AP_InertialSensor::_init_gyro() memset(diff_norm, 0, sizeof(diff_norm)); - hal.console->printf("*"); + DEV_PRINTF("*"); for (uint8_t k=0; kprintf("\n"); + DEV_PRINTF("\n"); for (uint8_t k=0; kprintf("gyro[%u] did not converge: diff=%f dps (expected < %f)\n", + DEV_PRINTF("gyro[%u] did not converge: diff=%f dps (expected < %f)\n", (unsigned)k, (double)ToDeg(best_diff[k]), (double)GYRO_INIT_MAX_DIFF_DPS); @@ -2164,7 +2164,7 @@ void AP_InertialSensor::_acal_save_calibrations() if (fabsf(_trim_rad.x) > radians(HAL_INS_TRIM_LIMIT_DEG) || fabsf(_trim_rad.y) > radians(HAL_INS_TRIM_LIMIT_DEG) || fabsf(_trim_rad.z) > radians(HAL_INS_TRIM_LIMIT_DEG)) { - hal.console->printf("ERR: Trim over maximum of %.1f degrees!!", float(HAL_INS_TRIM_LIMIT_DEG)); + DEV_PRINTF("ERR: Trim over maximum of %.1f degrees!!", float(HAL_INS_TRIM_LIMIT_DEG)); _new_trim = false; //we have either got faulty level during acal or highly misaligned accelerometers } @@ -2302,7 +2302,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal() memset(diff_norm, 0, sizeof(diff_norm)); - hal.console->printf("*"); + DEV_PRINTF("*"); for (uint8_t k=0; kprintf("\nPASSED\n"); + DEV_PRINTF("\nPASSED\n"); for (uint8_t k=0; kprintf("\nFAILED\n"); + DEV_PRINTF("\nFAILED\n"); // restore old values for (uint8_t k=0; kprintf("BMI055: found accel\n"); + DEV_PRINTF("BMI055: found accel\n"); dev_accel->get_semaphore()->give(); return true; @@ -215,7 +215,7 @@ bool AP_InertialSensor_BMI055::gyro_init() goto failed; } - hal.console->printf("BMI055: found gyro\n"); + DEV_PRINTF("BMI055: found gyro\n"); dev_gyro->get_semaphore()->give(); return true; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp index 60f67d41a1..b71420f3e1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp @@ -184,7 +184,7 @@ bool AP_InertialSensor_BMI088::setup_accel_config(void) } } done_accel_config = true; - hal.console->printf("BMI088: accel config OK (%u tries)\n", (unsigned)accel_config_count); + DEV_PRINTF("BMI088: accel config OK (%u tries)\n", (unsigned)accel_config_count); return true; } @@ -218,10 +218,10 @@ bool AP_InertialSensor_BMI088::accel_init() } if (!setup_accel_config()) { - hal.console->printf("BMI08x: delaying accel config\n"); + DEV_PRINTF("BMI08x: delaying accel config\n"); } - hal.console->printf("BMI08x: found accel\n"); + DEV_PRINTF("BMI08x: found accel\n"); return true; } @@ -272,7 +272,7 @@ bool AP_InertialSensor_BMI088::gyro_init() return false; } - hal.console->printf("BMI088: found gyro\n"); + DEV_PRINTF("BMI088: found gyro\n"); return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index a4fb2e7e80..aff58e5b35 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -293,14 +293,14 @@ bool AP_InertialSensor_BMI160::_configure_int1_pin() r = _dev->write_register(BMI160_REG_INT_EN_1, BMI160_INT_FWM_EN); if (!r) { - hal.console->printf("BMI160: Unable to enable FIFO watermark interrupt engine\n"); + DEV_PRINTF("BMI160: Unable to enable FIFO watermark interrupt engine\n"); return false; } hal.scheduler->delay(1); r = _dev->write_register(BMI160_REG_INT_MAP_1, BMI160_INT_MAP_INT1_FWM); if (!r) { - hal.console->printf("BMI160: Unable to configure interrupt mapping\n"); + DEV_PRINTF("BMI160: Unable to configure interrupt mapping\n"); return false; } hal.scheduler->delay(1); @@ -308,14 +308,14 @@ bool AP_InertialSensor_BMI160::_configure_int1_pin() r = _dev->write_register(BMI160_REG_INT_OUT_CTRL, BMI160_INT1_OUTPUT_EN | BMI160_INT1_LVL); if (!r) { - hal.console->printf("BMI160: Unable to configure interrupt output\n"); + DEV_PRINTF("BMI160: Unable to configure interrupt output\n"); return false; } hal.scheduler->delay(1); _int1_pin = hal.gpio->channel(BMI160_INT1_GPIO); if (_int1_pin == nullptr) { - hal.console->printf("BMI160: Couldn't request data ready GPIO channel\n"); + DEV_PRINTF("BMI160: Couldn't request data ready GPIO channel\n"); return false; } _int1_pin->mode(HAL_GPIO_INPUT); @@ -331,7 +331,7 @@ bool AP_InertialSensor_BMI160::_configure_fifo() r = _dev->write_register(BMI160_REG_FIFO_CONFIG_0, sizeof(struct RawData) / 4); if (!r) { - hal.console->printf("BMI160: Unable to configure FIFO watermark level\n"); + DEV_PRINTF("BMI160: Unable to configure FIFO watermark level\n"); return false; } hal.scheduler->delay(1); @@ -339,7 +339,7 @@ bool AP_InertialSensor_BMI160::_configure_fifo() r = _dev->write_register(BMI160_REG_FIFO_CONFIG_1, BMI160_FIFO_ACC_EN | BMI160_FIFO_GYR_EN); if (!r) { - hal.console->printf("BMI160: Unable to enable FIFO\n"); + DEV_PRINTF("BMI160: Unable to enable FIFO\n"); return false; } hal.scheduler->delay(1); @@ -348,7 +348,7 @@ bool AP_InertialSensor_BMI160::_configure_fifo() r = _dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH); if (!r) { - hal.console->printf("BMI160: Unable to flush FIFO\n"); + DEV_PRINTF("BMI160: Unable to flush FIFO\n"); return false; } @@ -399,7 +399,7 @@ read_fifo_read_data: /* Read again just once */ if (excess && num_samples) { - hal.console->printf("BMI160: dropping %u samples from fifo\n", + DEV_PRINTF("BMI160: dropping %u samples from fifo\n", (uint8_t)(excess / sizeof(struct RawData))); _dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH); excess = 0; @@ -434,7 +434,7 @@ read_fifo_read_data: read_fifo_end: if (!r) { - hal.console->printf("BMI160: error on reading FIFO\n"); + DEV_PRINTF("BMI160: error on reading FIFO\n"); } } @@ -510,7 +510,7 @@ bool AP_InertialSensor_BMI160::_init() ret = _hardware_init(); if (!ret) { - hal.console->printf("BMI160: failed to init\n"); + DEV_PRINTF("BMI160: failed to init\n"); } return ret; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp index 9800db8a26..f6c09848cd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp @@ -533,7 +533,7 @@ bool AP_InertialSensor_BMI270::hardware_init() if ((status & 1) == 1) { ret = true; - hal.console->printf("BMI270 initialized after %d retries\n", i+1); + DEV_PRINTF("BMI270 initialized after %d retries\n", i+1); break; } } @@ -551,7 +551,7 @@ bool AP_InertialSensor_BMI270::init() ret = hardware_init(); if (!ret) { - hal.console->printf("BMI270: failed to init\n"); + DEV_PRINTF("BMI270: failed to init\n"); } return ret; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index a811bb845d..354be1eda5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -1004,7 +1004,7 @@ bool AP_InertialSensor_Invensense::_hardware_init(void) _dev->set_speed(AP_HAL::Device::SPEED_HIGH); if (tries == 5) { - hal.console->printf("Failed to boot Invensense 5 times\n"); + DEV_PRINTF("Failed to boot Invensense 5 times\n"); return false; } @@ -1055,7 +1055,7 @@ int AP_Invensense_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) { if (_registered) { - hal.console->printf("Error: can't passthrough when slave is already configured\n"); + DEV_PRINTF("Error: can't passthrough when slave is already configured\n"); return -1; } @@ -1081,7 +1081,7 @@ int AP_Invensense_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf, int AP_Invensense_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val) { if (_registered) { - hal.console->printf("Error: can't passthrough when slave is already configured\n"); + DEV_PRINTF("Error: can't passthrough when slave is already configured\n"); return -1; } @@ -1104,7 +1104,7 @@ int AP_Invensense_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val) int AP_Invensense_AuxiliaryBusSlave::read(uint8_t *buf) { if (!_registered) { - hal.console->printf("Error: can't read before configuring slave\n"); + DEV_PRINTF("Error: can't read before configuring slave\n"); return -1; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index f5da4c7e07..30daf0f3cd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -724,7 +724,7 @@ bool AP_InertialSensor_Invensensev2::_hardware_init(void) _dev->set_speed(AP_HAL::Device::SPEED_HIGH); if (tries == 5) { - hal.console->printf("Failed to boot Invensense 5 times\n"); + DEV_PRINTF("Failed to boot Invensense 5 times\n"); return false; } @@ -771,7 +771,7 @@ int AP_Invensensev2_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *bu uint8_t size) { if (_registered) { - hal.console->printf("Error: can't passthrough when slave is already configured\n"); + DEV_PRINTF("Error: can't passthrough when slave is already configured\n"); return -1; } @@ -797,7 +797,7 @@ int AP_Invensensev2_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *bu int AP_Invensensev2_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val) { if (_registered) { - hal.console->printf("Error: can't passthrough when slave is already configured\n"); + DEV_PRINTF("Error: can't passthrough when slave is already configured\n"); return -1; } @@ -820,7 +820,7 @@ int AP_Invensensev2_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t va int AP_Invensensev2_AuxiliaryBusSlave::read(uint8_t *buf) { if (!_registered) { - hal.console->printf("Error: can't read before configuring slave\n"); + DEV_PRINTF("Error: can't read before configuring slave\n"); return -1; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index f3eac505bb..a872ddc7b5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -492,7 +492,7 @@ bool AP_InertialSensor_LSM9DS0::_hardware_init() #endif } if (tries == 5) { - hal.console->printf("Failed to boot LSM9DS0 5 times\n\n"); + DEV_PRINTF("Failed to boot LSM9DS0 5 times\n\n"); goto fail_tries; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp index 57a55e551b..295f1ebcca 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp @@ -255,7 +255,7 @@ bool AP_InertialSensor_LSM9DS1::_hardware_init() whoami = _register_read(LSM9DS1XG_WHO_AM_I); if (whoami != WHO_AM_I) { - hal.console->printf("LSM9DS1: unexpected acc/gyro WHOAMI 0x%x\n", whoami); + DEV_PRINTF("LSM9DS1: unexpected acc/gyro WHOAMI 0x%x\n", whoami); goto fail_whoami; } @@ -284,7 +284,7 @@ bool AP_InertialSensor_LSM9DS1::_hardware_init() #endif } if (tries == 5) { - hal.console->printf("Failed to boot LSM9DS1 5 times\n\n"); + DEV_PRINTF("Failed to boot LSM9DS1 5 times\n\n"); goto fail_tries; } @@ -448,7 +448,7 @@ void AP_InertialSensor_LSM9DS1::_read_data_transaction_x(uint16_t samples) struct sensor_raw_data raw_data_temp = { }; if (!_dev->transfer(&_reg, 1, (uint8_t *) &raw_data_temp, sizeof(raw_data_temp))) { - hal.console->printf("LSM9DS1: error reading accelerometer\n"); + DEV_PRINTF("LSM9DS1: error reading accelerometer\n"); return; } @@ -485,7 +485,7 @@ void AP_InertialSensor_LSM9DS1::_read_data_transaction_g(uint16_t samples) struct sensor_raw_data raw_data_temp = { }; if (!_dev->transfer(&_reg, 1, (uint8_t *) &raw_data_temp, sizeof(raw_data_temp))) { - hal.console->printf("LSM9DS1: error reading gyroscope\n"); + DEV_PRINTF("LSM9DS1: error reading gyroscope\n"); return; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp index 7501902df8..e51a97d618 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp @@ -261,7 +261,7 @@ void AP_InertialSensor_NONE::timer_update(void) static uint64_t last_msg_sent = 0; if (now > last_msg_sent + 2000000) { //2sec= 2000ms = 2000000us //gcs().send_text(MAV_SEVERITY_WARNING, "NO IMU FOUND"); - hal.console->printf("INS: NO IMU FOUND\n"); + DEV_PRINTF("INS: NO IMU FOUND\n"); last_msg_sent = now; } if (now >= next_accel_sample) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp index e1f186870f..761e2e9100 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp @@ -291,7 +291,7 @@ bool AP_InertialSensor_RST::_init_accel(void) _dev_accel->read_registers(ACCEL_WHO_AM_I, &whoami, sizeof(whoami)); if (whoami != ACCEL_WHO_I_AM) { - hal.console->printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami); + DEV_PRINTF("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami); printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami); goto fail_whoami; }