zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: Console output can be disabled

apm_2208
murata 3 years ago committed by Andrew Tridgell
parent
e007b21f9e
commit
324f5e3ac9
11 changed files with 42 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      libraries/AP_InertialSensor/AP_InertialSensor.cpp
  2. 4
      libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
  3. 8
      libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
  4. 20
      libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
  5. 4
      libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
  6. 8
      libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
  7. 8
      libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
  8. 2
      libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
  9. 8
      libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp
  10. 2
      libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp
  11. 2
      libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp

18
libraries/AP_InertialSensor/AP_InertialSensor.cpp
Unescape View File

@ -1166,7 +1166,7 @@ AP_InertialSensor::detect_backends(void)
#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 #if CONFIG_HAL_BOARD == HAL_BOARD_ESP32
ADD_BACKEND(AP_InertialSensor_NONE::detect(*this, INS_NONE_SENSOR_A)); ADD_BACKEND(AP_InertialSensor_NONE::detect(*this, INS_NONE_SENSOR_A));
#else #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"); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: unable to initialise driver");
AP_BoardConfig::config_error("INS: unable to initialise driver"); AP_BoardConfig::config_error("INS: unable to initialise driver");
#endif #endif
@ -1446,7 +1446,7 @@ AP_InertialSensor::_init_gyro()
AP_Notify::flags.initialising = true; AP_Notify::flags.initialising = true;
// cold start // cold start
hal.console->printf("Init Gyro"); DEV_PRINTF("Init Gyro");
/* /*
we do the gyro calibration with no board rotation. This avoids 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)); memset(diff_norm, 0, sizeof(diff_norm));
hal.console->printf("*"); DEV_PRINTF("*");
for (uint8_t k=0; k<num_gyros; k++) { for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k].zero(); 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 // we've kept the user waiting long enough - use the best pair we
// found so far // found so far
hal.console->printf("\n"); DEV_PRINTF("\n");
for (uint8_t k=0; k<num_gyros; k++) { for (uint8_t k=0; k<num_gyros; k++) {
if (!converged[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, (unsigned)k,
(double)ToDeg(best_diff[k]), (double)ToDeg(best_diff[k]),
(double)GYRO_INIT_MAX_DIFF_DPS); (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) || if (fabsf(_trim_rad.x) > radians(HAL_INS_TRIM_LIMIT_DEG) ||
fabsf(_trim_rad.y) > 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)) { 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 _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)); memset(diff_norm, 0, sizeof(diff_norm));
hal.console->printf("*"); DEV_PRINTF("*");
for (uint8_t k=0; k<num_accels; k++) { for (uint8_t k=0; k<num_accels; k++) {
accel_sum[k].zero(); accel_sum[k].zero();
@ -2350,7 +2350,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
_board_orientation = saved_orientation; _board_orientation = saved_orientation;
if (result == MAV_RESULT_ACCEPTED) { if (result == MAV_RESULT_ACCEPTED) {
hal.console->printf("\nPASSED\n"); DEV_PRINTF("\nPASSED\n");
for (uint8_t k=0; k<num_accels; k++) { for (uint8_t k=0; k<num_accels; k++) {
// remove rotated gravity // remove rotated gravity
new_accel_offset[k] -= rotated_gravity; new_accel_offset[k] -= rotated_gravity;
@ -2366,7 +2366,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
// force trim to zero // force trim to zero
AP::ahrs().set_trim(Vector3f(0, 0, 0)); AP::ahrs().set_trim(Vector3f(0, 0, 0));
} else { } else {
hal.console->printf("\nFAILED\n"); DEV_PRINTF("\nFAILED\n");
// restore old values // restore old values
for (uint8_t k=0; k<num_accels; k++) { for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = saved_offsets[k]; _accel_offset[k] = saved_offsets[k];

4
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
Unescape View File

@ -161,7 +161,7 @@ bool AP_InertialSensor_BMI055::accel_init()
goto failed; goto failed;
} }
hal.console->printf("BMI055: found accel\n"); DEV_PRINTF("BMI055: found accel\n");
dev_accel->get_semaphore()->give(); dev_accel->get_semaphore()->give();
return true; return true;
@ -215,7 +215,7 @@ bool AP_InertialSensor_BMI055::gyro_init()
goto failed; goto failed;
} }
hal.console->printf("BMI055: found gyro\n"); DEV_PRINTF("BMI055: found gyro\n");
dev_gyro->get_semaphore()->give(); dev_gyro->get_semaphore()->give();
return true; return true;

8
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
Unescape View File

@ -184,7 +184,7 @@ bool AP_InertialSensor_BMI088::setup_accel_config(void)
} }
} }
done_accel_config = true; 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; return true;
} }
@ -218,10 +218,10 @@ bool AP_InertialSensor_BMI088::accel_init()
} }
if (!setup_accel_config()) { 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; return true;
} }
@ -272,7 +272,7 @@ bool AP_InertialSensor_BMI088::gyro_init()
return false; return false;
} }
hal.console->printf("BMI088: found gyro\n"); DEV_PRINTF("BMI088: found gyro\n");
return true; return true;
} }

20
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
Unescape 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); r = _dev->write_register(BMI160_REG_INT_EN_1, BMI160_INT_FWM_EN);
if (!r) { 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; return false;
} }
hal.scheduler->delay(1); hal.scheduler->delay(1);
r = _dev->write_register(BMI160_REG_INT_MAP_1, BMI160_INT_MAP_INT1_FWM); r = _dev->write_register(BMI160_REG_INT_MAP_1, BMI160_INT_MAP_INT1_FWM);
if (!r) { if (!r) {
hal.console->printf("BMI160: Unable to configure interrupt mapping\n"); DEV_PRINTF("BMI160: Unable to configure interrupt mapping\n");
return false; return false;
} }
hal.scheduler->delay(1); hal.scheduler->delay(1);
@ -308,14 +308,14 @@ bool AP_InertialSensor_BMI160::_configure_int1_pin()
r = _dev->write_register(BMI160_REG_INT_OUT_CTRL, r = _dev->write_register(BMI160_REG_INT_OUT_CTRL,
BMI160_INT1_OUTPUT_EN | BMI160_INT1_LVL); BMI160_INT1_OUTPUT_EN | BMI160_INT1_LVL);
if (!r) { if (!r) {
hal.console->printf("BMI160: Unable to configure interrupt output\n"); DEV_PRINTF("BMI160: Unable to configure interrupt output\n");
return false; return false;
} }
hal.scheduler->delay(1); hal.scheduler->delay(1);
_int1_pin = hal.gpio->channel(BMI160_INT1_GPIO); _int1_pin = hal.gpio->channel(BMI160_INT1_GPIO);
if (_int1_pin == nullptr) { 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; return false;
} }
_int1_pin->mode(HAL_GPIO_INPUT); _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, r = _dev->write_register(BMI160_REG_FIFO_CONFIG_0,
sizeof(struct RawData) / 4); sizeof(struct RawData) / 4);
if (!r) { 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; return false;
} }
hal.scheduler->delay(1); hal.scheduler->delay(1);
@ -339,7 +339,7 @@ bool AP_InertialSensor_BMI160::_configure_fifo()
r = _dev->write_register(BMI160_REG_FIFO_CONFIG_1, r = _dev->write_register(BMI160_REG_FIFO_CONFIG_1,
BMI160_FIFO_ACC_EN | BMI160_FIFO_GYR_EN); BMI160_FIFO_ACC_EN | BMI160_FIFO_GYR_EN);
if (!r) { if (!r) {
hal.console->printf("BMI160: Unable to enable FIFO\n"); DEV_PRINTF("BMI160: Unable to enable FIFO\n");
return false; return false;
} }
hal.scheduler->delay(1); 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); r = _dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH);
if (!r) { if (!r) {
hal.console->printf("BMI160: Unable to flush FIFO\n"); DEV_PRINTF("BMI160: Unable to flush FIFO\n");
return false; return false;
} }
@ -399,7 +399,7 @@ read_fifo_read_data:
/* Read again just once */ /* Read again just once */
if (excess && num_samples) { 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))); (uint8_t)(excess / sizeof(struct RawData)));
_dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH); _dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH);
excess = 0; excess = 0;
@ -434,7 +434,7 @@ read_fifo_read_data:
read_fifo_end: read_fifo_end:
if (!r) { 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(); ret = _hardware_init();
if (!ret) { if (!ret) {
hal.console->printf("BMI160: failed to init\n"); DEV_PRINTF("BMI160: failed to init\n");
} }
return ret; return ret;

4
libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
Unescape View File

@ -533,7 +533,7 @@ bool AP_InertialSensor_BMI270::hardware_init()
if ((status & 1) == 1) { if ((status & 1) == 1) {
ret = true; ret = true;
hal.console->printf("BMI270 initialized after %d retries\n", i+1); DEV_PRINTF("BMI270 initialized after %d retries\n", i+1);
break; break;
} }
} }
@ -551,7 +551,7 @@ bool AP_InertialSensor_BMI270::init()
ret = hardware_init(); ret = hardware_init();
if (!ret) { if (!ret) {
hal.console->printf("BMI270: failed to init\n"); DEV_PRINTF("BMI270: failed to init\n");
} }
return ret; return ret;

8
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
Unescape View File

@ -1004,7 +1004,7 @@ bool AP_InertialSensor_Invensense::_hardware_init(void)
_dev->set_speed(AP_HAL::Device::SPEED_HIGH); _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
if (tries == 5) { if (tries == 5) {
hal.console->printf("Failed to boot Invensense 5 times\n"); DEV_PRINTF("Failed to boot Invensense 5 times\n");
return false; return false;
} }
@ -1055,7 +1055,7 @@ int AP_Invensense_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
uint8_t size) uint8_t size)
{ {
if (_registered) { 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; 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) int AP_Invensense_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
{ {
if (_registered) { 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; 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) int AP_Invensense_AuxiliaryBusSlave::read(uint8_t *buf)
{ {
if (!_registered) { 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; return -1;
} }

8
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
Unescape View File

@ -724,7 +724,7 @@ bool AP_InertialSensor_Invensensev2::_hardware_init(void)
_dev->set_speed(AP_HAL::Device::SPEED_HIGH); _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
if (tries == 5) { if (tries == 5) {
hal.console->printf("Failed to boot Invensense 5 times\n"); DEV_PRINTF("Failed to boot Invensense 5 times\n");
return false; return false;
} }
@ -771,7 +771,7 @@ int AP_Invensensev2_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *bu
uint8_t size) uint8_t size)
{ {
if (_registered) { 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; 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) int AP_Invensensev2_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
{ {
if (_registered) { 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; 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) int AP_Invensensev2_AuxiliaryBusSlave::read(uint8_t *buf)
{ {
if (!_registered) { 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; return -1;
} }

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
Unescape View File

@ -492,7 +492,7 @@ bool AP_InertialSensor_LSM9DS0::_hardware_init()
#endif #endif
} }
if (tries == 5) { 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; goto fail_tries;
} }

8
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp
Unescape View File

@ -255,7 +255,7 @@ bool AP_InertialSensor_LSM9DS1::_hardware_init()
whoami = _register_read(LSM9DS1XG_WHO_AM_I); whoami = _register_read(LSM9DS1XG_WHO_AM_I);
if (whoami != 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; goto fail_whoami;
} }
@ -284,7 +284,7 @@ bool AP_InertialSensor_LSM9DS1::_hardware_init()
#endif #endif
} }
if (tries == 5) { 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; 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 = { }; struct sensor_raw_data raw_data_temp = { };
if (!_dev->transfer(&_reg, 1, (uint8_t *) &raw_data_temp, sizeof(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; return;
} }
@ -485,7 +485,7 @@ void AP_InertialSensor_LSM9DS1::_read_data_transaction_g(uint16_t samples)
struct sensor_raw_data raw_data_temp = { }; struct sensor_raw_data raw_data_temp = { };
if (!_dev->transfer(&_reg, 1, (uint8_t *) &raw_data_temp, sizeof(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; return;
} }

2
libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp
Unescape View File

@ -261,7 +261,7 @@ void AP_InertialSensor_NONE::timer_update(void)
static uint64_t last_msg_sent = 0; static uint64_t last_msg_sent = 0;
if (now > last_msg_sent + 2000000) { //2sec= 2000ms = 2000000us if (now > last_msg_sent + 2000000) { //2sec= 2000ms = 2000000us
//gcs().send_text(MAV_SEVERITY_WARNING, "NO IMU FOUND"); //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; last_msg_sent = now;
} }
if (now >= next_accel_sample) { if (now >= next_accel_sample) {

2
libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
Unescape View File

@ -291,7 +291,7 @@ bool AP_InertialSensor_RST::_init_accel(void)
_dev_accel->read_registers(ACCEL_WHO_AM_I, &whoami, sizeof(whoami)); _dev_accel->read_registers(ACCEL_WHO_AM_I, &whoami, sizeof(whoami));
if (whoami != ACCEL_WHO_I_AM) { 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); printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami);
goto fail_whoami; goto fail_whoami;
} }

Write Preview
Loading…
Cancel
Save