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

AP_Compass: AK8963: enhance the readability 

Reduce the deepness of indentation and fix coding style.
This commit is contained in:
dgrat 2015-07-09 16:06:59 +02:00 committed by Andrew Tridgell
parent c627f84fa8
commit d941174978
1 changed files with 63 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -247,6 +247,7 @@ void AP_Compass_AK8963::read()
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
field.rotate(ROTATION_YAW_90); field.rotate(ROTATION_YAW_90);
#endif #endif
publish_field(field, _compass_instance); publish_field(field, _compass_instance);
} }
@ -260,20 +261,19 @@ void AP_Compass_AK8963::_update()
return; return;
} }
switch (_state) switch (_state) {
{ case STATE_SAMPLE:
case STATE_SAMPLE: if (!_collect_samples()) {
if (!_collect_samples()) { _state = STATE_ERROR;
_state = STATE_ERROR; }
} break;
break; case STATE_ERROR:
case STATE_ERROR: if (_bus->start_conversion()) {
if (_bus->start_conversion()) { _state = STATE_SAMPLE;
_state = STATE_SAMPLE; }
} break;
break; default:
default: break;
break;
} }
_last_update_timestamp = hal.scheduler->micros(); _last_update_timestamp = hal.scheduler->micros();
@ -283,7 +283,7 @@ void AP_Compass_AK8963::_update()
bool AP_Compass_AK8963::_check_id() bool AP_Compass_AK8963::_check_id()
{ {
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
uint8_t deviceid; uint8_t deviceid = 0;
_bus->register_read(AK8963_WIA, &deviceid, 0x01); /* Read AK8963's id */ _bus->register_read(AK8963_WIA, &deviceid, 0x01); /* Read AK8963's id */
if (deviceid == AK8963_Device_ID) { if (deviceid == AK8963_Device_ID) {
@ -310,7 +310,8 @@ bool AP_Compass_AK8963::_calibrate()
{ {
uint8_t cntl1 = _bus->register_read(AK8963_CNTL1); uint8_t cntl1 = _bus->register_read(AK8963_CNTL1);
_bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution); /* Enable FUSE-mode in order to be able to read calibreation data */ /* Enable FUSE-mode in order to be able to read calibration data */
_bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution);
uint8_t response[3]; uint8_t response[3];
_bus->register_read(AK8963_ASAX, response, 3); _bus->register_read(AK8963_ASAX, response, 3);
@ -334,17 +335,17 @@ bool AP_Compass_AK8963::_collect_samples()
if (!_bus->read_raw(_mag_x, _mag_y, _mag_z)) { if (!_bus->read_raw(_mag_x, _mag_y, _mag_z)) {
return false; return false;
} else { }
_mag_x_accum += _mag_x;
_mag_y_accum += _mag_y; _mag_x_accum += _mag_x;
_mag_z_accum += _mag_z; _mag_y_accum += _mag_y;
_accum_count++; _mag_z_accum += _mag_z;
if (_accum_count == 10) { _accum_count++;
_mag_x_accum /= 2; if (_accum_count == 10) {
_mag_y_accum /= 2; _mag_x_accum /= 2;
_mag_z_accum /= 2; _mag_y_accum /= 2;
_accum_count = 5; _mag_z_accum /= 2;
} _accum_count = 5;
} }
return true; return true;
@ -364,22 +365,21 @@ bool AP_Compass_AK8963::_sem_take_nonblocking()
{ {
static int _sem_failure_count = 0; static int _sem_failure_count = 0;
bool got = _bus_sem->take_nonblocking(); if (_bus_sem->take_nonblocking()) {
if (!got) {
if (!hal.scheduler->system_initializing()) {
_sem_failure_count++;
if (_sem_failure_count > 100) {
hal.scheduler->panic(PSTR("PANIC: failed to take _bus->sem "
"100 times in a row, in "
"AP_Compass_AK8963"));
}
}
return false; /* never reached */
} else {
_sem_failure_count = 0; _sem_failure_count = 0;
return true;
} }
return got;
if (!hal.scheduler->system_initializing() ) {
_sem_failure_count++;
if (_sem_failure_count > 100) {
hal.scheduler->panic(PSTR("PANIC: failed to take _bus->sem "
"100 times in a row, in "
"AP_Compass_AK8963"));
}
}
return false;
} }
void AP_Compass_AK8963::_dump_registers() void AP_Compass_AK8963::_dump_registers()
@ -478,19 +478,19 @@ bool AP_AK8963_SerialBus_MPU9250::read_raw(float &mag_x, float &mag_y, float &ma
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx]))
if(!(st2 & 0x08)) { if (st2 & 0x08) {
mag_x = (float) int16_val(rx, 1);
mag_y = (float) int16_val(rx, 2);
mag_z = (float) int16_val(rx, 3);
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
return false;
}
return true;
} else {
return false; return false;
} }
mag_x = (float) int16_val(rx, 1);
mag_y = (float) int16_val(rx, 2);
mag_z = (float) int16_val(rx, 3);
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
return false;
}
return true;
} }
AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore() AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore()
@ -545,19 +545,19 @@ bool AP_AK8963_SerialBus_I2C::read_raw(float &mag_x, float &mag_y, float &mag_z)
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx]))
if(!(st2 & 0x08)) { if (st2 & 0x08) {
mag_x = (float) int16_val(rx, 1);
mag_y = (float) int16_val(rx, 2);
mag_z = (float) int16_val(rx, 3);
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
return false;
}
return true;
} else {
return false; return false;
} }
mag_x = (float) int16_val(rx, 1);
mag_y = (float) int16_val(rx, 2);
mag_z = (float) int16_val(rx, 3);
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
return false;
}
return true;
} }
AP_HAL::Semaphore * AP_AK8963_SerialBus_I2C::get_semaphore() AP_HAL::Semaphore * AP_AK8963_SerialBus_I2C::get_semaphore()