Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: AK8963: enhance the readability

Browse Source 
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

42
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,8 +261,7 @@ 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;
@ -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,7 +335,8 @@ 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_x_accum += _mag_x;
_mag_y_accum += _mag_y; _mag_y_accum += _mag_y;
_mag_z_accum += _mag_z; _mag_z_accum += _mag_z;
@ -345,7 +347,6 @@ bool AP_Compass_AK8963::_collect_samples()
_mag_z_accum /= 2; _mag_z_accum /= 2;
_accum_count = 5; _accum_count = 5;
} }
}
return true; return true;
} }
@ -364,9 +365,11 @@ 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()) {
_sem_failure_count = 0;
return true;
}
if (!got) {
if (!hal.scheduler->system_initializing() ) { if (!hal.scheduler->system_initializing() ) {
_sem_failure_count++; _sem_failure_count++;
if (_sem_failure_count > 100) { if (_sem_failure_count > 100) {
@ -375,11 +378,8 @@ bool AP_Compass_AK8963::_sem_take_nonblocking()
"AP_Compass_AK8963")); "AP_Compass_AK8963"));
} }
} }
return false; /* never reached */
} else { return false;
_sem_failure_count = 0;
}
return got;
} }
void AP_Compass_AK8963::_dump_registers() void AP_Compass_AK8963::_dump_registers()
@ -478,7 +478,10 @@ 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) {
return false;
}
mag_x = (float) int16_val(rx, 1); mag_x = (float) int16_val(rx, 1);
mag_y = (float) int16_val(rx, 2); mag_y = (float) int16_val(rx, 2);
mag_z = (float) int16_val(rx, 3); mag_z = (float) int16_val(rx, 3);
@ -488,9 +491,6 @@ bool AP_AK8963_SerialBus_MPU9250::read_raw(float &mag_x, float &mag_y, float &ma
} }
return true; return true;
} else {
return false;
}
} }
AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore() AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore()
@ -545,7 +545,10 @@ 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) {
return false;
}
mag_x = (float) int16_val(rx, 1); mag_x = (float) int16_val(rx, 1);
mag_y = (float) int16_val(rx, 2); mag_y = (float) int16_val(rx, 2);
mag_z = (float) int16_val(rx, 3); mag_z = (float) int16_val(rx, 3);
@ -555,9 +558,6 @@ bool AP_AK8963_SerialBus_I2C::read_raw(float &mag_x, float &mag_y, float &mag_z)
} }
return true; return true;
} else {
return false;
}
} }
AP_HAL::Semaphore * AP_AK8963_SerialBus_I2C::get_semaphore() AP_HAL::Semaphore * AP_AK8963_SerialBus_I2C::get_semaphore()