Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: Console output can be disabled

This commit is contained in:
murata 2022-03-21 18:25:18 +09:00 committed by Andrew Tridgell
parent e007b21f9e
commit 324f5e3ac9
11 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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; k<num_gyros; k++) {
gyro_sum[k].zero();
@ -1550,10 +1550,10 @@ AP_InertialSensor::_init_gyro()
// we've kept the user waiting long enough - use the best pair we
// found so far
hal.console->printf("\n");
DEV_PRINTF("\n");
for (uint8_t k=0; k<num_gyros; k++) {
if (!converged[k]) {
hal.console->printf("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; k<num_accels; k++) {
accel_sum[k].zero();
@ -2350,7 +2350,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
_board_orientation = saved_orientation;
if (result == MAV_RESULT_ACCEPTED) {
hal.console->printf("\nPASSED\n");
DEV_PRINTF("\nPASSED\n");
for (uint8_t k=0; k<num_accels; k++) {
// remove rotated gravity
new_accel_offset[k] -= rotated_gravity;
@ -2366,7 +2366,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
// force trim to zero
AP::ahrs().set_trim(Vector3f(0, 0, 0));
} else {
hal.console->printf("\nFAILED\n");
DEV_PRINTF("\nFAILED\n");
// restore old values
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = saved_offsets[k];

4
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
View File

@ -161,7 +161,7 @@ bool AP_InertialSensor_BMI055::accel_init()
goto failed;
}
hal.console->printf("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;

8
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
View File

@ -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;
}

20
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
View File

@ -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;

4
libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
View File

@ -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;

8
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -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;
}

8
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -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;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -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;
}

8
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp
View File

@ -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;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp
View File

@ -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) {

2
libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
View File

@ -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;
}