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

AP_Compass: add register to checked ones and remove single-use goto

Browse Source 
Also fix comment on TMRC register setting
This commit is contained in:
Pierre Kancir 2019-02-20 14:24:01 +01:00 committed by Andrew Tridgell
parent d6563bff7d
commit d332773689
1 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Compass/AP_Compass_RM3100.cpp
View File

@ -122,13 +122,14 @@ bool AP_Compass_RM3100::init()
ccy1 != CCP1_DEFAULT || ccy0 != CCP0_DEFAULT || ccy1 != CCP1_DEFAULT || ccy0 != CCP0_DEFAULT ||
ccz1 != CCP1_DEFAULT || ccz0 != CCP0_DEFAULT) { ccz1 != CCP1_DEFAULT || ccz0 != CCP0_DEFAULT) {
// couldn't read one of the cycle count registers or didn't recognize the default cycle count values // couldn't read one of the cycle count registers or didn't recognize the default cycle count values
goto fail; dev->get_semaphore()->give();
return false;
} }
dev->setup_checked_registers(8); dev->setup_checked_registers(8);
dev->write_register(RM3100_TMRC_REG, TMRC, true); // cycle count z dev->write_register(RM3100_TMRC_REG, TMRC, true); // CMM data rate
dev->write_register(RM3100_CMM_REG, CMM, false); // CMM configuration dev->write_register(RM3100_CMM_REG, CMM, true); // CMM configuration
dev->write_register(RM3100_CCX1_REG, CCP1, true); // cycle count x dev->write_register(RM3100_CCX1_REG, CCP1, true); // cycle count x
dev->write_register(RM3100_CCX0_REG, CCP0, true); // cycle count x dev->write_register(RM3100_CCX0_REG, CCP0, true); // cycle count x
dev->write_register(RM3100_CCY1_REG, CCP1, true); // cycle count y dev->write_register(RM3100_CCY1_REG, CCP1, true); // cycle count y
@ -146,7 +147,7 @@ bool AP_Compass_RM3100::init()
/* register the compass instance in the frontend */ /* register the compass instance in the frontend */
compass_instance = register_compass(); compass_instance = register_compass();
printf("Found a RM3100 at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance); hal.console->printf("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance);
set_rotation(compass_instance, rotation); set_rotation(compass_instance, rotation);
@ -162,10 +163,6 @@ bool AP_Compass_RM3100::init()
FUNCTOR_BIND_MEMBER(&AP_Compass_RM3100::timer, void)); FUNCTOR_BIND_MEMBER(&AP_Compass_RM3100::timer, void));
return true; return true;
fail:
dev->get_semaphore()->give();
return false;
} }
void AP_Compass_RM3100::timer() void AP_Compass_RM3100::timer()