From aa047d6b5c3f5ce8efc3db12e2e02b4f8b5e30e7 Mon Sep 17 00:00:00 2001 From: Jon Trulson Date: Sat, 19 Aug 2017 15:27:32 -0600 Subject: [PATCH] bno055: enhance error detection and propagation This patch reworks error handling in the C driver to more reliably detect errors, and for C++, throw exceptions when they are detected. The C++ API is unchanged aside from the fact that more methods will throw an exception on errors now. This addresses the error handling deficiencies reported in Issue #593. Signed-off-by: Jon Trulson Signed-off-by: Abhishek Malik --- src/bno055/bno055.c | 449 ++++++++++++++++++++++++++++-------------- src/bno055/bno055.cxx | 140 ++++++++++--- src/bno055/bno055.h | 158 +++++++++------ src/bno055/bno055.hpp | 28 +++ 4 files changed, 541 insertions(+), 234 deletions(-) diff --git a/src/bno055/bno055.c b/src/bno055/bno055.c index 37ebac10..8b27911a 100644 --- a/src/bno055/bno055.c +++ b/src/bno055/bno055.c @@ -51,23 +51,26 @@ static void _clear_data(const bno055_context dev) } // load fusion data -static void _update_fusion_data(const bno055_context dev) +static upm_result_t _update_fusion_data(const bno055_context dev) { assert(dev != NULL); - // bail if we are in config mode, or aren't in a fusion mode... + // bail (with success code) if we are in config mode, or aren't in + // a fusion mode... if (dev->currentMode == BNO055_OPERATION_MODE_CONFIGMODE || dev->currentMode < BNO055_OPERATION_MODE_IMU) - return; + return UPM_SUCCESS; - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; // FIXME/MAYBE? - abort early if SYS calibration is == 0? const int fusionBytes = 26; uint8_t buf[fusionBytes]; - bno055_read_regs(dev, BNO055_REG_EUL_HEADING_LSB, buf, fusionBytes); + if (bno055_read_regs(dev, BNO055_REG_EUL_HEADING_LSB, buf, fusionBytes)) + return UPM_ERROR_OPERATION_FAILED; dev->eulHeading = INT16_TO_FLOAT(buf[0], buf[1]); dev->eulRoll = INT16_TO_FLOAT(buf[2], buf[3]); @@ -85,23 +88,27 @@ static void _update_fusion_data(const bno055_context dev) dev->grvX = INT16_TO_FLOAT(buf[20], buf[21]); dev->grvY = INT16_TO_FLOAT(buf[22], buf[23]); dev->grvZ = INT16_TO_FLOAT(buf[24], buf[25]); + + return UPM_SUCCESS; } // update non-fusion data -static void _update_non_fusion_data(const bno055_context dev) +static upm_result_t _update_non_fusion_data(const bno055_context dev) { assert(dev != NULL); - // bail if we are in config mode... + // bail (with success code) if we are in config mode... if (dev->currentMode == BNO055_OPERATION_MODE_CONFIGMODE) - return; + return UPM_SUCCESS; - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; const int nonFusionBytes = 18; uint8_t buf[nonFusionBytes]; - bno055_read_regs(dev, BNO055_REG_ACC_DATA_X_LSB, buf, nonFusionBytes); + if (bno055_read_regs(dev, BNO055_REG_ACC_DATA_X_LSB, buf, nonFusionBytes)) + return UPM_ERROR_OPERATION_FAILED; dev->accX = INT16_TO_FLOAT(buf[0], buf[1]); dev->accY = INT16_TO_FLOAT(buf[2], buf[3]); @@ -114,6 +121,8 @@ static void _update_non_fusion_data(const bno055_context dev) dev->gyrX = INT16_TO_FLOAT(buf[12], buf[13]); dev->gyrY = INT16_TO_FLOAT(buf[14], buf[15]); dev->gyrZ = INT16_TO_FLOAT(buf[16], buf[17]); + + return UPM_SUCCESS; } // init @@ -163,7 +172,14 @@ bno055_context bno055_init(int bus, uint8_t addr) // check the chip id. This has to be done after forcibly setting // page 0, as that is the only page where the chip id is present. - uint8_t chipID = bno055_get_chip_id(dev); + uint8_t chipID = 0; + if (bno055_get_chip_id(dev, &chipID)) + { + printf("%s: Could not read chip id\n", __FUNCTION__); + bno055_close(dev); + return NULL; + } + if (chipID != BNO055_CHIPID) { printf("%s: Invalid chip ID. Expected 0x%02x, got 0x%02x\n", @@ -172,34 +188,41 @@ bno055_context bno055_init(int bus, uint8_t addr) return NULL; } - // if the above two accesses succeeded, the rest should succeed - + upm_result_t urv = UPM_SUCCESS; // set config mode - bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE); + urv += bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE); // default to internal clock - bno055_set_clock_external(dev, false); + urv += bno055_set_clock_external(dev, false); // we specifically avoid doing a reset so that if the device is // already calibrated, it will remain so. // we always use C for temperature - bno055_set_temperature_units_celsius(dev); + urv += bno055_set_temperature_units_celsius(dev); // default to accelerometer temp - bno055_set_temperature_source(dev, BNO055_TEMP_SOURCE_ACC); + urv += bno055_set_temperature_source(dev, BNO055_TEMP_SOURCE_ACC); // set accel units to m/s^2 - bno055_set_accelerometer_units(dev, false); + urv += bno055_set_accelerometer_units(dev, false); // set gyro units to degrees - bno055_set_gyroscope_units(dev, false); + urv += bno055_set_gyroscope_units(dev, false); // set Euler units to degrees - bno055_set_euler_units(dev, false); + urv += bno055_set_euler_units(dev, false); // by default, we set the operating mode to the NDOF fusion mode - bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_NDOF); + urv += bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_NDOF); + + // if any of those failed, bail + if (urv != UPM_SUCCESS) + { + printf("%s: Initial device configuration failed\n", __FUNCTION__); + bno055_close(dev); + return NULL; + } return dev; } @@ -220,34 +243,41 @@ upm_result_t bno055_update(const bno055_context dev) { assert(dev != NULL); - upm_result_t rv = UPM_SUCCESS; - if ((rv = bno055_set_page(dev, 0, false))) - return rv; + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; // temperature first, always in Celsius - dev->temperature = (float)((int8_t)bno055_read_reg(dev, - BNO055_REG_TEMPERATURE)); + uint8_t tempreg = 0; + if (bno055_read_reg(dev, BNO055_REG_TEMPERATURE, &tempreg)) + return UPM_ERROR_OPERATION_FAILED; - _update_fusion_data(dev); - _update_non_fusion_data(dev); + dev->temperature = (float)((int8_t)tempreg); - return rv; + if (_update_fusion_data(dev)) + return UPM_ERROR_OPERATION_FAILED; + if (_update_non_fusion_data(dev)) + return UPM_ERROR_OPERATION_FAILED; + + return UPM_SUCCESS; } -uint8_t bno055_read_reg(const bno055_context dev, uint8_t reg) +upm_result_t bno055_read_reg(const bno055_context dev, uint8_t reg, + uint8_t *retval) { assert(dev != NULL); int rv = mraa_i2c_read_byte_data(dev->i2c, reg); - if (rv < 0) { - printf("%s: mraa_i2c_read_byte_data() failed, returning 0\n", + printf("%s: mraa_i2c_read_byte_data() failed\n", __FUNCTION__); - return 0; + return UPM_ERROR_OPERATION_FAILED; } - return (uint8_t)rv; + if (retval) + *retval = (uint8_t)(rv & 0xff); + + return UPM_SUCCESS; } upm_result_t bno055_read_regs(const bno055_context dev, uint8_t reg, @@ -256,7 +286,11 @@ upm_result_t bno055_read_regs(const bno055_context dev, uint8_t reg, assert(dev != NULL); if (mraa_i2c_read_bytes_data(dev->i2c, reg, buffer, len) < 0) + { + printf("%s: mraa_i2c_read_bytes() failed\n", + __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; + } return UPM_SUCCESS; } @@ -267,7 +301,11 @@ upm_result_t bno055_write_reg(const bno055_context dev, assert(dev != NULL); if (mraa_i2c_write_byte_data(dev->i2c, val, reg)) + { + printf("%s: mraa_i2c_write_byte_data() failed\n", + __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; + } return UPM_SUCCESS; } @@ -284,62 +322,82 @@ upm_result_t bno055_write_regs(const bno055_context dev, uint8_t reg, buf[i+1] = buffer[i]; if (mraa_i2c_write(dev->i2c, buf, len + 1)) + { + printf("%s: mraa_i2c_write() failed\n", + __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; + } return UPM_SUCCESS; } -uint8_t bno055_get_chip_id(const bno055_context dev) +upm_result_t bno055_get_chip_id(const bno055_context dev, uint8_t *chip_id) { assert(dev != NULL); - bno055_set_page(dev, 0, false); - return bno055_read_reg(dev, BNO055_REG_CHIP_ID); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; + + return bno055_read_reg(dev, BNO055_REG_CHIP_ID, chip_id); } -uint8_t bno055_get_acc_id(const bno055_context dev) +upm_result_t bno055_get_acc_id(const bno055_context dev, uint8_t *chip_id) { assert(dev != NULL); - bno055_set_page(dev, 0, false); - return bno055_read_reg(dev, BNO055_REG_ACC_ID); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; + + return bno055_read_reg(dev, BNO055_REG_ACC_ID, chip_id); } -uint8_t bno055_get_mag_id(const bno055_context dev) +upm_result_t bno055_get_mag_id(const bno055_context dev, uint8_t *chip_id) { assert(dev != NULL); - bno055_set_page(dev, 0, false); - return bno055_read_reg(dev, BNO055_REG_MAG_ID); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; + + return bno055_read_reg(dev, BNO055_REG_MAG_ID, chip_id); } -uint8_t bno055_get_gyr_id(const bno055_context dev) +upm_result_t bno055_get_gyr_id(const bno055_context dev, uint8_t *chip_id) { assert(dev != NULL); - bno055_set_page(dev, 0, false); - return bno055_read_reg(dev, BNO055_REG_GYR_ID); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; + + return bno055_read_reg(dev, BNO055_REG_GYR_ID, chip_id); } -uint16_t bno055_get_sw_revision(const bno055_context dev) +upm_result_t bno055_get_sw_revision(const bno055_context dev, uint16_t *sw_rev) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t lsb, msb; - lsb = bno055_read_reg(dev, BNO055_REG_SW_REV_ID_LSB); - msb = bno055_read_reg(dev, BNO055_REG_SW_REV_ID_MSB); + uint8_t lsb = 0, msb = 0; + if (bno055_read_reg(dev, BNO055_REG_SW_REV_ID_LSB, &lsb)) + return UPM_ERROR_OPERATION_FAILED; + if (bno055_read_reg(dev, BNO055_REG_SW_REV_ID_MSB, &msb)) + return UPM_ERROR_OPERATION_FAILED; - return (uint16_t)(lsb | (msb << 8)); + if (sw_rev) + *sw_rev = (uint16_t)(lsb | (msb << 8)); + + return UPM_SUCCESS; } -uint8_t bno055_get_bootloader_id(const bno055_context dev) +upm_result_t bno055_get_bootloader_id(const bno055_context dev, uint8_t *bl_id) { assert(dev != NULL); - bno055_set_page(dev, 0, false); - return bno055_read_reg(dev, BNO055_REG_BL_REV_ID); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; + + return bno055_read_reg(dev, BNO055_REG_BL_REV_ID, bl_id); } upm_result_t bno055_set_page(const bno055_context dev, uint8_t page, @@ -356,64 +414,84 @@ upm_result_t bno055_set_page(const bno055_context dev, uint8_t page, } if (force || page != dev->currentPage) - bno055_write_reg(dev, BNO055_REG_PAGE_ID, page); + { + if (bno055_write_reg(dev, BNO055_REG_PAGE_ID, page)) + return UPM_ERROR_OPERATION_FAILED; + } dev->currentPage = page; return UPM_SUCCESS; } -void bno055_set_clock_external(const bno055_context dev, bool extClock) +upm_result_t bno055_set_clock_external(const bno055_context dev, + bool extClock) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; // first we need to be in config mode BNO055_OPERATION_MODES_T currentMode = dev->currentMode; - bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE); + if (bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t reg = bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER, ®)) + return UPM_ERROR_OPERATION_FAILED; if (extClock) reg |= BNO055_SYS_TRIGGER_CLK_SEL; else reg &= ~BNO055_SYS_TRIGGER_CLK_SEL; - bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg); + if (bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg)) + return UPM_ERROR_OPERATION_FAILED; // now reset our operating mode - bno055_set_operation_mode(dev, currentMode); + if (bno055_set_operation_mode(dev, currentMode)) + return UPM_ERROR_OPERATION_FAILED; + + return UPM_SUCCESS; } -void bno055_set_temperature_source(const bno055_context dev, - BNO055_TEMP_SOURCES_T src) +upm_result_t bno055_set_temperature_source(const bno055_context dev, + BNO055_TEMP_SOURCES_T src) { assert(dev != NULL); - bno055_set_page(dev, 0, false); - bno055_write_reg(dev, BNO055_REG_TEMP_SOURCE, src); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; + + return bno055_write_reg(dev, BNO055_REG_TEMP_SOURCE, src); } -void bno055_set_temperature_units_celsius(const bno055_context dev) +upm_result_t bno055_set_temperature_units_celsius(const bno055_context dev) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t reg = bno055_read_reg(dev, BNO055_REG_UNIT_SEL); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_UNIT_SEL, ®)) + return UPM_ERROR_OPERATION_FAILED; reg &= ~BNO055_UNIT_SEL_TEMP_UNIT; - bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); + return bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); } -void bno055_set_accelerometer_units(const bno055_context dev, bool mg) +upm_result_t bno055_set_accelerometer_units(const bno055_context dev, bool mg) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t reg = bno055_read_reg(dev, BNO055_REG_UNIT_SEL); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_UNIT_SEL, ®)) + return UPM_ERROR_OPERATION_FAILED; if (mg) { @@ -426,16 +504,19 @@ void bno055_set_accelerometer_units(const bno055_context dev, bool mg) dev->accUnitScale = 100.0; } - bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); + return bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); } -void bno055_set_gyroscope_units(const bno055_context dev, bool radians) +upm_result_t bno055_set_gyroscope_units(const bno055_context dev, bool radians) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t reg = bno055_read_reg(dev, BNO055_REG_UNIT_SEL); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_UNIT_SEL, ®)) + return UPM_ERROR_OPERATION_FAILED; if (radians) { @@ -448,16 +529,19 @@ void bno055_set_gyroscope_units(const bno055_context dev, bool radians) dev->gyrUnitScale = 16.0; } - bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); + return bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); } -void bno055_set_euler_units(const bno055_context dev, bool radians) +upm_result_t bno055_set_euler_units(const bno055_context dev, bool radians) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t reg = bno055_read_reg(dev, BNO055_REG_UNIT_SEL); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_UNIT_SEL, ®)) + return UPM_ERROR_OPERATION_FAILED; if (radians) { @@ -470,41 +554,51 @@ void bno055_set_euler_units(const bno055_context dev, bool radians) dev->eulUnitScale = 16.0; } - bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); + return bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); } -void bno055_set_operation_mode(const bno055_context dev, - BNO055_OPERATION_MODES_T mode) +upm_result_t bno055_set_operation_mode(const bno055_context dev, + BNO055_OPERATION_MODES_T mode) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; // we clear all of our loaded data on mode changes _clear_data(dev); - uint8_t reg = bno055_read_reg(dev, BNO055_REG_OPER_MODE); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_OPER_MODE, ®)) + return UPM_ERROR_OPERATION_FAILED; reg &= ~(_BNO055_OPR_MODE_OPERATION_MODE_MASK << _BNO055_OPR_MODE_OPERATION_MODE_SHIFT); reg |= (mode << _BNO055_OPR_MODE_OPERATION_MODE_SHIFT); - bno055_write_reg(dev, BNO055_REG_OPER_MODE, reg); + if (bno055_write_reg(dev, BNO055_REG_OPER_MODE, reg)) + return UPM_ERROR_OPERATION_FAILED; + dev->currentMode = mode; upm_delay_us(30); + + return UPM_SUCCESS; } -void bno055_get_calibration_status(const bno055_context dev, - int *mag, int *acc, - int *gyr, int *sys) +upm_result_t bno055_get_calibration_status(const bno055_context dev, + int *mag, int *acc, + int *gyr, int *sys) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t reg = bno055_read_reg(dev, BNO055_REG_CALIB_STAT); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_CALIB_STAT, ®)) + return UPM_ERROR_OPERATION_FAILED; if (mag) *mag = (reg >> _BNO055_CALIB_STAT_MAG_SHIFT) @@ -521,6 +615,8 @@ void bno055_get_calibration_status(const bno055_context dev, if (sys) *sys = (reg >> _BNO055_CALIB_STAT_SYS_SHIFT) & _BNO055_CALIB_STAT_SYS_MASK; + + return UPM_SUCCESS; } bool bno055_is_fully_calibrated(const bno055_context dev) @@ -529,7 +625,9 @@ bool bno055_is_fully_calibrated(const bno055_context dev) int mag, acc, gyr, sys; - bno055_get_calibration_status(dev, &mag, &acc, &gyr, &sys); + // fail on error + if (bno055_get_calibration_status(dev, &mag, &acc, &gyr, &sys)) + return false; // all of them equal to 3 means fully calibrated if (mag == 3 && acc == 3 && gyr == 3 && sys == 3) @@ -538,94 +636,131 @@ bool bno055_is_fully_calibrated(const bno055_context dev) return false; } -void bno055_reset_system(const bno055_context dev) +upm_result_t bno055_reset_system(const bno055_context dev) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t reg = bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER, ®)) + return UPM_ERROR_OPERATION_FAILED; reg |= BNO055_SYS_TRIGGER_RST_SYS; - bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg); + if (bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg)) + return UPM_ERROR_OPERATION_FAILED; + upm_delay(1); + + return UPM_SUCCESS; } -void bno055_reset_interrupt_status(const bno055_context dev) +upm_result_t bno055_reset_interrupt_status(const bno055_context dev) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - uint8_t reg = bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER, ®)) + return UPM_ERROR_OPERATION_FAILED; reg |= BNO055_SYS_TRIGGER_RST_INT; - bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg); + return bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg); } -uint8_t bno055_get_interrupt_status(const bno055_context dev) +upm_result_t bno055_get_interrupt_status(const bno055_context dev, + uint8_t *istat) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - return bno055_read_reg(dev, BNO055_REG_INT_STA); + return bno055_read_reg(dev, BNO055_REG_INT_STA, istat); } -uint8_t bno055_get_interrupt_enable(const bno055_context dev) +upm_result_t bno055_get_interrupt_enable(const bno055_context dev, + uint8_t *ienable) { assert(dev != NULL); - bno055_set_page(dev, 1, false); + if (bno055_set_page(dev, 1, false)) + return UPM_ERROR_OPERATION_FAILED; - return bno055_read_reg(dev, BNO055_REG_INT_EN); + return bno055_read_reg(dev, BNO055_REG_INT_EN, ienable); } -void bno055_set_interrupt_enable(const bno055_context dev, uint8_t enables) +upm_result_t bno055_set_interrupt_enable(const bno055_context dev, + uint8_t enables) { assert(dev != NULL); - bno055_set_page(dev, 1, false); + if (bno055_set_page(dev, 1, false)) + return UPM_ERROR_OPERATION_FAILED; - bno055_write_reg(dev, BNO055_REG_INT_EN, enables); + return bno055_write_reg(dev, BNO055_REG_INT_EN, enables); } -uint8_t bno055_get_interrupt_mask(const bno055_context dev) +upm_result_t bno055_get_interrupt_mask(const bno055_context dev, + uint8_t *imask) { assert(dev != NULL); - bno055_set_page(dev, 1, false); + if (bno055_set_page(dev, 1, false)) + return UPM_ERROR_OPERATION_FAILED; - return bno055_read_reg(dev, BNO055_REG_INT_MSK); + return bno055_read_reg(dev, BNO055_REG_INT_MSK, imask); } -void bno055_set_interrupt_mask(const bno055_context dev, uint8_t mask) +upm_result_t bno055_set_interrupt_mask(const bno055_context dev, uint8_t mask) { assert(dev != NULL); - bno055_set_page(dev, 1, false); + if (bno055_set_page(dev, 1, false)) + return UPM_ERROR_OPERATION_FAILED; - bno055_write_reg(dev, BNO055_REG_INT_MSK, mask); + return bno055_write_reg(dev, BNO055_REG_INT_MSK, mask); } -BNO055_SYS_STATUS_T bno055_get_system_status(const bno055_context dev) +upm_result_t bno055_get_system_status(const bno055_context dev, + BNO055_SYS_STATUS_T *sys_stat) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - return (BNO055_SYS_STATUS_T)bno055_read_reg(dev, BNO055_REG_SYS_STATUS); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_SYS_STATUS, ®)) + return UPM_ERROR_OPERATION_FAILED; + + if (sys_stat) + *sys_stat = (BNO055_SYS_STATUS_T)reg; + + return UPM_SUCCESS; } -BNO055_SYS_ERR_T bno055_get_system_error(const bno055_context dev) +upm_result_t bno055_get_system_error(const bno055_context dev, + BNO055_SYS_ERR_T *sys_err) { assert(dev != NULL); - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; - return (BNO055_SYS_ERR_T)bno055_read_reg(dev, BNO055_REG_SYS_ERROR); + uint8_t reg = 0; + if (bno055_read_reg(dev, BNO055_REG_SYS_ERROR, ®)) + return UPM_ERROR_OPERATION_FAILED; + + if (sys_err) + *sys_err = (BNO055_SYS_ERR_T)reg; + + return UPM_SUCCESS; } upm_result_t bno055_read_calibration_data(const bno055_context dev, @@ -649,17 +784,21 @@ upm_result_t bno055_read_calibration_data(const bno055_context dev, } // should be at page 0, but lets make sure - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; // first we need to go back into config mode BNO055_OPERATION_MODES_T currentMode = dev->currentMode; - bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE); + if (bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE)) + return UPM_ERROR_OPERATION_FAILED; - bno055_read_regs(dev, BNO055_REG_ACC_OFFSET_X_LSB, data, - BNO055_CALIBRATION_DATA_SIZE); + if (bno055_read_regs(dev, BNO055_REG_ACC_OFFSET_X_LSB, data, + BNO055_CALIBRATION_DATA_SIZE)) + return UPM_ERROR_OPERATION_FAILED; // now reset our operating mode - bno055_set_operation_mode(dev, currentMode); + if (bno055_set_operation_mode(dev, currentMode)) + return UPM_ERROR_OPERATION_FAILED; return UPM_SUCCESS; } @@ -680,18 +819,22 @@ upm_result_t bno055_write_calibration_data(const bno055_context dev, } // should be at page 0, but lets make sure - bno055_set_page(dev, 0, false); + if (bno055_set_page(dev, 0, false)) + return UPM_ERROR_OPERATION_FAILED; // first we need to go back into config mode BNO055_OPERATION_MODES_T currentMode = dev->currentMode; - bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE); + if (bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE)) + return UPM_ERROR_OPERATION_FAILED; // write the data - bno055_write_regs(dev, BNO055_REG_ACC_OFFSET_X_LSB, data, - BNO055_CALIBRATION_DATA_SIZE); + if (bno055_write_regs(dev, BNO055_REG_ACC_OFFSET_X_LSB, data, + BNO055_CALIBRATION_DATA_SIZE)) + return UPM_ERROR_OPERATION_FAILED; // now reset our operating mode - bno055_set_operation_mode(dev, currentMode); + if (bno055_set_operation_mode(dev, currentMode)) + return UPM_ERROR_OPERATION_FAILED; return UPM_SUCCESS; } @@ -817,55 +960,59 @@ void bno055_get_gyroscope(const bno055_context dev, *z = dev->gyrZ / dev->gyrUnitScale; } -void bno055_set_acceleration_config(const bno055_context dev, - BNO055_ACC_RANGE_T range, - BNO055_ACC_BW_T bw, - BNO055_ACC_PWR_MODE_T pwr) +upm_result_t bno055_set_acceleration_config(const bno055_context dev, + BNO055_ACC_RANGE_T range, + BNO055_ACC_BW_T bw, + BNO055_ACC_PWR_MODE_T pwr) { assert(dev != NULL); - bno055_set_page(dev, 1, false); + if (bno055_set_page(dev, 1, false)) + return UPM_ERROR_OPERATION_FAILED; uint8_t reg = ((range << _BNO055_ACC_CONFIG_ACC_RANGE_SHIFT) | (bw << _BNO055_ACC_CONFIG_ACC_BW_SHIFT) | (pwr << _BNO055_ACC_CONFIG_ACC_PWR_MODE_SHIFT)); - bno055_write_reg(dev, BNO055_REG_ACC_CONFIG, reg); + return bno055_write_reg(dev, BNO055_REG_ACC_CONFIG, reg); } -void bno055_set_magnetometer_config(const bno055_context dev, - BNO055_MAG_ODR_T odr, - BNO055_MAG_OPR_T opr, - BNO055_MAG_POWER_T pwr) +upm_result_t bno055_set_magnetometer_config(const bno055_context dev, + BNO055_MAG_ODR_T odr, + BNO055_MAG_OPR_T opr, + BNO055_MAG_POWER_T pwr) { assert(dev != NULL); - bno055_set_page(dev, 1, false); + if (bno055_set_page(dev, 1, false)) + return UPM_ERROR_OPERATION_FAILED; uint8_t reg = ((odr << _BNO055_MAG_CONFIG_MAG_ODR_SHIFT) | (opr << _BNO055_MAG_CONFIG_MAG_OPR_MODE_SHIFT) | (pwr << _BNO055_MAG_CONFIG_MAG_POWER_MODE_SHIFT)); - bno055_write_reg(dev, BNO055_REG_MAG_CONFIG, reg); + return bno055_write_reg(dev, BNO055_REG_MAG_CONFIG, reg); } -void bno055_set_gyroscope_config(const bno055_context dev, - BNO055_GYR_RANGE_T range, - BNO055_GYR_BW_T bw, - BNO055_GYR_POWER_MODE_T pwr) +upm_result_t bno055_set_gyroscope_config(const bno055_context dev, + BNO055_GYR_RANGE_T range, + BNO055_GYR_BW_T bw, + BNO055_GYR_POWER_MODE_T pwr) { assert(dev != NULL); - bno055_set_page(dev, 1, false); + if (bno055_set_page(dev, 1, false)) + return UPM_ERROR_OPERATION_FAILED; uint8_t reg = ((range << _BNO055_GYR_CONFIG0_GYR_RANGE_SHIFT) | (bw << _BNO055_GYR_CONFIG0_GYR_BW_SHIFT)); - bno055_write_reg(dev, BNO055_REG_GYR_CONFIG0, reg); + if (bno055_write_reg(dev, BNO055_REG_GYR_CONFIG0, reg)) + return UPM_ERROR_OPERATION_FAILED; reg = (pwr << _BNO055_GYR_CONFIG1_GYR_POWER_MODE_SHIFT); - bno055_write_reg(dev, BNO055_REG_GYR_CONFIG1, reg); + return bno055_write_reg(dev, BNO055_REG_GYR_CONFIG1, reg); } upm_result_t bno055_install_isr(const bno055_context dev, diff --git a/src/bno055/bno055.cxx b/src/bno055/bno055.cxx index 8ad6533a..2f9a53f8 100644 --- a/src/bno055/bno055.cxx +++ b/src/bno055/bno055.cxx @@ -61,7 +61,12 @@ void BNO055::update() uint8_t BNO055::readReg(uint8_t reg) { - return bno055_read_reg(m_bno055, reg); + uint8_t rv = 0; + if (bno055_read_reg(m_bno055, reg, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_read_reg() failed"); + + return rv; } void BNO055::readRegs(uint8_t reg, uint8_t *buffer, int len) @@ -87,32 +92,62 @@ void BNO055::writeRegs(uint8_t reg, uint8_t *buffer, int len) uint8_t BNO055::getChipID() { - return bno055_get_chip_id(m_bno055); + uint8_t rv = 0; + if (bno055_get_chip_id(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_chip_id() failed"); + + return rv; } uint8_t BNO055::getACCID() { - return bno055_get_acc_id(m_bno055); + uint8_t rv = 0; + if (bno055_get_acc_id(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_acc_id() failed"); + + return rv; } uint8_t BNO055::getMAGID() { - return bno055_get_mag_id(m_bno055); + uint8_t rv = 0; + if (bno055_get_mag_id(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_mag_id() failed"); + + return rv; } uint8_t BNO055::getGYRID() { - return bno055_get_gyr_id(m_bno055); + uint8_t rv = 0; + if (bno055_get_gyr_id(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_gyr_id() failed"); + + return rv; } uint16_t BNO055::getSWRevID() { - return bno055_get_sw_revision(m_bno055); + uint16_t rv = 0; + if (bno055_get_sw_revision(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_sw_revision() failed"); + + return rv; } uint8_t BNO055::getBootLoaderID() { - return bno055_get_bootloader_id(m_bno055); + uint8_t rv = 0; + if (bno055_get_bootloader_id(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_bootloader_id() failed"); + + return rv; } void BNO055::setPage(uint8_t page, bool force) @@ -124,37 +159,51 @@ void BNO055::setPage(uint8_t page, bool force) void BNO055::setClockExternal(bool extClock) { - bno055_set_clock_external(m_bno055, extClock); + if (bno055_set_clock_external(m_bno055, extClock)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_clock_external() failed"); } void BNO055::setTemperatureSource(BNO055_TEMP_SOURCES_T src) { - bno055_set_temperature_source(m_bno055, src); + if (bno055_set_temperature_source(m_bno055, src)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_temperature_source() failed"); } void BNO055::setAccelerometerUnits(bool mg) { - bno055_set_accelerometer_units(m_bno055, mg); + if (bno055_set_accelerometer_units(m_bno055, mg)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_accelerometer_units() failed"); } void BNO055::setGyroscopeUnits(bool radians) { - bno055_set_gyroscope_units(m_bno055, radians); + if (bno055_set_gyroscope_units(m_bno055, radians)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_gyroscope_units() failed"); } void BNO055::setEulerUnits(bool radians) { - bno055_set_euler_units(m_bno055, radians); + if (bno055_set_euler_units(m_bno055, radians)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_euler_units() failed"); } void BNO055::setOperationMode(BNO055_OPERATION_MODES_T mode) { - bno055_set_operation_mode(m_bno055, mode); + if (bno055_set_operation_mode(m_bno055, mode)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_operation_mode() failed"); } void BNO055::getCalibrationStatus(int *mag, int *acc, int *gyr, int *sys) { - bno055_get_calibration_status(m_bno055, mag, acc, gyr, sys); + if (bno055_get_calibration_status(m_bno055, mag, acc, gyr, sys)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_calibration_status() failed"); } vector BNO055::getCalibrationStatus() @@ -172,47 +221,80 @@ bool BNO055::isFullyCalibrated() void BNO055::resetSystem() { - bno055_reset_system(m_bno055); + if (bno055_reset_system(m_bno055)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_reset_system() failed"); } void BNO055::resetInterruptStatus() { - bno055_reset_interrupt_status(m_bno055); + if (bno055_reset_interrupt_status(m_bno055)) + throw std::runtime_error(string(__FUNCTION__) + + ": reset_interrupt_status() failed"); } uint8_t BNO055::getInterruptStatus() { - return bno055_get_interrupt_status(m_bno055); + uint8_t rv = 0; + if (bno055_get_interrupt_status(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055get_interrupt_status() failed"); + + return rv; } uint8_t BNO055::getInterruptEnable() { - return bno055_get_interrupt_enable(m_bno055); + uint8_t rv = 0; + if (bno055_get_interrupt_enable(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_interrupt_enable() failed"); + + return rv; } void BNO055::setInterruptEnable(uint8_t enables) { - return bno055_set_interrupt_enable(m_bno055, enables); + if (bno055_set_interrupt_enable(m_bno055, enables)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_interrupt_enable() failed"); } uint8_t BNO055::getInterruptMask() { - return bno055_get_interrupt_mask(m_bno055); + uint8_t rv = 0; + if (bno055_get_interrupt_mask(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_interrupt_mask() failed"); + + return rv; } void BNO055::setInterruptMask(uint8_t mask) { - return bno055_set_interrupt_mask(m_bno055, mask); + if (bno055_set_interrupt_mask(m_bno055, mask)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_interrupt_mask() failed"); } BNO055_SYS_STATUS_T BNO055::getSystemStatus() { - return bno055_get_system_status(m_bno055); + BNO055_SYS_STATUS_T rv; + if (bno055_get_system_status(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_system_status() failed"); + + return rv; } BNO055_SYS_ERR_T BNO055::getSystemError() { - return bno055_get_system_error(m_bno055); + BNO055_SYS_ERR_T rv; + if (bno055_get_system_error(m_bno055, &rv)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_get_system_error() failed"); + + return rv; } std::vector BNO055::readCalibrationData() @@ -334,21 +416,27 @@ void BNO055::setAccelerationConfig(BNO055_ACC_RANGE_T range, BNO055_ACC_BW_T bw, BNO055_ACC_PWR_MODE_T pwr) { - bno055_set_acceleration_config(m_bno055, range, bw, pwr); + if (bno055_set_acceleration_config(m_bno055, range, bw, pwr)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_acceleration_config() failed"); } void BNO055::setMagnetometerConfig(BNO055_MAG_ODR_T odr, BNO055_MAG_OPR_T opr, BNO055_MAG_POWER_T pwr) { - bno055_set_magnetometer_config(m_bno055, odr, opr, pwr); + if (bno055_set_magnetometer_config(m_bno055, odr, opr, pwr)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_magnetometer_config() failed"); } void BNO055::setGyroscopeConfig(BNO055_GYR_RANGE_T range, BNO055_GYR_BW_T bw, BNO055_GYR_POWER_MODE_T pwr) { - bno055_set_gyroscope_config(m_bno055, range, bw, pwr); + if (bno055_set_gyroscope_config(m_bno055, range, bw, pwr)) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_set_gyroscope_config() failed"); } void BNO055::installISR(int gpio, mraa_gpio_edge_t level, diff --git a/src/bno055/bno055.h b/src/bno055/bno055.h index 496d5c46..38244a57 100644 --- a/src/bno055/bno055.h +++ b/src/bno055/bno055.h @@ -145,49 +145,61 @@ extern "C" { * Return the chip ID. * * @param dev The device context. - * @return The chip ID (BNO055_CHIPID). + * @param chip_id The chip ID (BNO055_CHIPID). + * @return UPM result. */ - uint8_t bno055_get_chip_id(const bno055_context dev); + upm_result_t bno055_get_chip_id(const bno055_context dev, + uint8_t *chip_id); /** * Return the accelerometer chip ID. * * @param dev The device context. - * @return The chip ID. + * @param chip_id The chip ID + * @return UPM result. */ - uint8_t bno055_get_acc_id(const bno055_context dev); + upm_result_t bno055_get_acc_id(const bno055_context dev, + uint8_t *chip_id); /** * Return the magnetometer chip ID. * * @param dev The device context. - * @return The chip ID. + * @param chip_id The chip ID + * @return UPM result. */ - uint8_t bno055_get_mag_id(const bno055_context dev); + upm_result_t bno055_get_mag_id(const bno055_context dev, + uint8_t *chip_id); /** * Return the gyroscope chip ID. * * @param dev The device context. - * @return The chip ID. + * @param chip_id The chip ID + * @return UPM result. */ - uint8_t bno055_get_gyr_id(const bno055_context dev); + upm_result_t bno055_get_gyr_id(const bno055_context dev, + uint8_t *chip_id); /** * Return the fusion firmware revison. * * @param dev The device context. - * @return The firmware revison. + * @param sw_rev The firmware revison. + * @return UPM result. */ - uint16_t bno055_get_sw_revision(const bno055_context dev); + upm_result_t bno055_get_sw_revision(const bno055_context dev, + uint16_t *sw_rev); /** * Return the bootloader ID. * * @param dev The device context. - * @return The bootloader ID. + * @param bl_id The bootloader ID. + * @return UPM result. */ - uint8_t bno055_get_bootloader_id(const bno055_context dev); + upm_result_t bno055_get_bootloader_id(const bno055_context dev, + uint8_t *bl_id); /** * Enable or disables the use of the external clock. The Adafriut @@ -196,8 +208,10 @@ extern "C" { * * @param dev The device context. * @param extClock true to use external clock, false otherwise. + * @return UPM result. */ - void bno055_set_clock_external(const bno055_context dev, bool extClock); + upm_result_t bno055_set_clock_external(const bno055_context dev, + bool extClock); /** * Select the temperature source. This can be the accelerometer @@ -206,16 +220,18 @@ extern "C" { * * @param dev The device context. * @param src One of the TEMP_SOURCES_T values. + * @return UPM result. */ - void bno055_set_temperature_source(const bno055_context dev, - BNO055_TEMP_SOURCES_T src); + upm_result_t bno055_set_temperature_source(const bno055_context dev, + BNO055_TEMP_SOURCES_T src); /** * Set the temperature units of the device to Celsius (always). * * @param dev The device context. + * @return UPM result. */ - void bno055_set_temperature_units_celsius(const bno055_context dev); + upm_result_t bno055_set_temperature_units_celsius(const bno055_context dev); /** * Set the operating mode for the device. This places the device @@ -226,9 +242,10 @@ extern "C" { * * @param dev The device context. * @param mode One of the OPERATION_MODES_T values. + * @return UPM result. */ - void bno055_set_operation_mode(const bno055_context dev, - BNO055_OPERATION_MODES_T mode); + upm_result_t bno055_set_operation_mode(const bno055_context dev, + BNO055_OPERATION_MODES_T mode); /** * Reboot the sensor. This is equivalent to a power on reset. @@ -236,8 +253,9 @@ extern "C" { * re-calibrated. * * @param dev The device context. + * @return UPM result. */ - void bno055_reset_system(const bno055_context dev); + upm_result_t bno055_reset_system(const bno055_context dev); /** * Read the calibration status registers and return them. The @@ -248,10 +266,11 @@ extern "C" { * @param acc The calibration status of the accelerometer. * @param mag The calibration status of the gyroscope. * @param mag The calibration status of the overall system. + * @return UPM result. */ - void bno055_get_calibration_status(const bno055_context dev, - int *mag, int *acc, - int *gyr, int *sys); + upm_result_t bno055_get_calibration_status(const bno055_context dev, + int *mag, int *acc, + int *gyr, int *sys); /** * Read the calibration status registers and return true or false, @@ -437,11 +456,12 @@ extern "C" { * @param range One of the BNO055_ACC_RANGE_T values. * @param bw One of the BNO055_ACC_BW_T values. * @param pwr One of the BNO055_ACC_PWR_MODE_T values. + * @return UPM result. */ - void bno055_set_acceleration_config(const bno055_context dev, - BNO055_ACC_RANGE_T range, - BNO055_ACC_BW_T bw, - BNO055_ACC_PWR_MODE_T pwr); + upm_result_t bno055_set_acceleration_config(const bno055_context dev, + BNO055_ACC_RANGE_T range, + BNO055_ACC_BW_T bw, + BNO055_ACC_PWR_MODE_T pwr); /** * Set the output data rate, operating mode and power mode of the @@ -451,11 +471,12 @@ extern "C" { * @param odr One of the BNO055_MAG_ODR_T values. * @param opr One of the BNO055_MAG_OPR_T values. * @param pwr One of the BNO055_MAG_POWER_T values. + * @return UPM result. */ - void bno055_set_magnetometer_config(const bno055_context dev, - BNO055_MAG_ODR_T odr, - BNO055_MAG_OPR_T opr, - BNO055_MAG_POWER_T pwr); + upm_result_t bno055_set_magnetometer_config(const bno055_context dev, + BNO055_MAG_ODR_T odr, + BNO055_MAG_OPR_T opr, + BNO055_MAG_POWER_T pwr); /** * Set the range, bandwidth and power modes of the gyroscope. In @@ -465,11 +486,12 @@ extern "C" { * @param range One of the BNO055_GYR_RANGE_T values. * @param bw One of the BNO055_GYR_BW_T values. * @param pwr One of the BNO055_GYR_POWER_MODE_T values. + * @return UPM result. */ - void bno055_set_gyroscope_config(const bno055_context dev, - BNO055_GYR_RANGE_T range, - BNO055_GYR_BW_T bw, - BNO055_GYR_POWER_MODE_T pwr); + upm_result_t bno055_set_gyroscope_config(const bno055_context dev, + BNO055_GYR_RANGE_T range, + BNO055_GYR_BW_T bw, + BNO055_GYR_POWER_MODE_T pwr); /** * Set the unit of measurement for the accelerometer related @@ -478,8 +500,10 @@ extern "C" { * * @param dev The device context. * @param mg true for mg, false for m/s^2. + * @return UPM result. */ - void bno055_set_accelerometer_units(const bno055_context dev, bool mg); + upm_result_t bno055_set_accelerometer_units(const bno055_context dev, + bool mg); /** * Set the unit of measurement for the gyroscope related sensor @@ -488,8 +512,10 @@ extern "C" { * * @param dev The device context. * @param radians true for radians, false for degrees. + * @return UPM result. */ - void bno055_set_gyroscope_units(const bno055_context dev, bool radians); + upm_result_t bno055_set_gyroscope_units(const bno055_context dev, + bool radians); /** * Set the unit of measurement for the Euler Angle related sensor @@ -498,34 +524,41 @@ extern "C" { * * @param dev The device context. * @param radians true for radians, false for degrees. + * @return UPM result. */ - void bno055_set_euler_units(const bno055_context dev, bool radians); + upm_result_t bno055_set_euler_units(const bno055_context dev, + bool radians); /** * Reset all interrupt status bits and interrupt output. * * @param dev The device context. + * @return UPM result. */ - void bno055_reset_interrupt_status(const bno055_context dev); + upm_result_t bno055_reset_interrupt_status(const bno055_context dev); /** - * Return the interrupt status register. This is a bitmask of the + * Get the interrupt status register. This is a bitmask of the * INT_STA_BITS_T bits. * * @param dev The device context. - * @return a bitmask of INT_STA_BITS_T bits. + * @param istat A bitmask of INT_STA_BITS_T bits. + * @return UPM result. */ - uint8_t bno055_get_interrupt_status(const bno055_context dev); + upm_result_t bno055_get_interrupt_status(const bno055_context dev, + uint8_t *istat); /** - * Return the interrupt enables register. This is a bitmask of the + * Get the interrupt enables register. This is a bitmask of the * INT_STA_BITS_T bits. * * @param dev The device context. - * @return a bitmask of INT_STA_BITS_T bits currently set in the + * @param ienable A bitmask of INT_STA_BITS_T bits currently set in the * enable register. + * @return UPM result. */ - uint8_t bno055_get_interrupt_enable(const bno055_context dev); + upm_result_t bno055_get_interrupt_enable(const bno055_context dev, + uint8_t *ienable); /** * Set the interrupt enable register. This is composed of a @@ -533,22 +566,25 @@ extern "C" { * * @param dev The device context. * @param enables a bitmask of INT_STA_BITS_T bits to enable + * @return UPM result. */ - void bno055_set_interrupt_enable(const bno055_context dev, - uint8_t enables); + upm_result_t bno055_set_interrupt_enable(const bno055_context dev, + uint8_t enables); /** - * Return the interrupt mask register. This is a bitmask of the + * Get the interrupt mask register. This is a bitmask of the * INT_STA_BITS_T bits. The interrupt mask is used to mask off * enabled interrupts from generating a hardware interrupt. The * interrupt status register can still be used to detect masked * interrupts if they are enabled. * * @param dev The device context. - * @return a bitmask of INT_STA_BITS_T bits currently set in the + * @param imask a bitmask of INT_STA_BITS_T bits currently set in the * interrupt mask register. + * @return UPM result. */ - uint8_t bno055_get_interrupt_mask(const bno055_context dev); + upm_result_t bno055_get_interrupt_mask(const bno055_context dev, + uint8_t *imask); /** * Set the interrupt mask register. This is a bitmask of the @@ -560,17 +596,21 @@ extern "C" { * @param dev The device context. * @param mask A bitmask of INT_STA_BITS_T bits to set in the interrupt * mask register. + * @return UPM result. */ - void bno055_set_interrupt_mask(const bno055_context dev, uint8_t mask); + upm_result_t bno055_set_interrupt_mask(const bno055_context dev, + uint8_t mask); /** - * Return the value of the system status register. This method + * Get the value of the system status register. This method * can be used to determine the overall status of the device. * * @param dev The device context. - * @return One of the SYS_STATUS_T values. + * @param sys_stat One of the SYS_STATUS_T values. + * @return UPM result. */ - BNO055_SYS_STATUS_T bno055_get_system_status(const bno055_context dev); + upm_result_t bno055_get_system_status(const bno055_context dev, + BNO055_SYS_STATUS_T *sys_stat); /** * Return the value of the system error register. This mathod can @@ -578,9 +618,11 @@ extern "C" { * conditions. * * @param dev The device context. - * @return One of the SYS_ERR_T values. + * @param sys_stat One of the SYS_ERR_T values. + * @return UPM result. */ - BNO055_SYS_ERR_T bno055_get_system_error(const bno055_context dev); + upm_result_t bno055_get_system_error(const bno055_context dev, + BNO055_SYS_ERR_T *sys_err); /** * install an interrupt handler. @@ -625,9 +667,11 @@ extern "C" { * * @param dev The device context. * @param reg The register to read - * @return The value of the register + * @param retval The returned value of the register + * @return UPM result. */ - uint8_t bno055_read_reg(const bno055_context dev, uint8_t reg); + upm_result_t bno055_read_reg(const bno055_context dev, uint8_t reg, + uint8_t *retval); /** * Read contiguous registers into a buffer. This is a low level diff --git a/src/bno055/bno055.hpp b/src/bno055/bno055.hpp index c7b5722a..73ce7af2 100644 --- a/src/bno055/bno055.hpp +++ b/src/bno055/bno055.hpp @@ -138,6 +138,7 @@ namespace upm { * Return the chip ID. * * @return The chip ID (BNO055_CHIPID). + * @throws std::runtime_error on failure. */ uint8_t getChipID(); @@ -145,6 +146,7 @@ namespace upm { * Return the accelerometer chip ID. * * @return The chip ID. + * @throws std::runtime_error on failure. */ uint8_t getACCID(); @@ -152,6 +154,7 @@ namespace upm { * Return the magnetometer chip ID. * * @return The chip ID. + * @throws std::runtime_error on failure. */ uint8_t getMAGID(); @@ -159,6 +162,7 @@ namespace upm { * Return the gyroscope chip ID. * * @return The chip ID. + * @throws std::runtime_error on failure. */ uint8_t getGYRID(); @@ -166,6 +170,7 @@ namespace upm { * Return the fusion firmware revison. * * @return The firmware revison. + * @throws std::runtime_error on failure. */ uint16_t getSWRevID(); @@ -173,6 +178,7 @@ namespace upm { * Return the bootloader ID. * * @return The bootloader ID. + * @throws std::runtime_error on failure. */ uint8_t getBootLoaderID(); @@ -182,6 +188,7 @@ namespace upm { * stable. By default, the internal clock is used. * * @param extClock true to use external clock, false otherwise. + * @throws std::runtime_error on failure. */ void setClockExternal(bool extClock); @@ -191,6 +198,7 @@ namespace upm { * used as the source. * * @param src One of the BNO055_TEMP_SOURCES_T values. + * @throws std::runtime_error on failure. */ void setTemperatureSource(BNO055_TEMP_SOURCES_T src); @@ -202,6 +210,7 @@ namespace upm { * configuration operations. See the datasheet for details. * * @param mode One of the BNO055_OPERATION_MODES_T values. + * @throws std::runtime_error on failure. */ void setOperationMode(BNO055_OPERATION_MODES_T mode); @@ -209,6 +218,7 @@ namespace upm { * Reboot the sensor. This is equivalent to a power on reset. * All calibration data will be lost, and the device must be * recalibrated. + * @throws std::runtime_error on failure. */ void resetSystem(); @@ -220,6 +230,7 @@ namespace upm { * @param acc The calibration status of the accelerometer. * @param mag The calibration status of the gyroscope. * @param mag The calibration status of the overall system. + * @throws std::runtime_error on failure. */ void getCalibrationStatus(int *mag, int *acc, int *gyr, int *sys); @@ -230,6 +241,7 @@ namespace upm { * * @return An integer vector containing the values in the * order: mag, acc, gyr, and sys. + * @throws std::runtime_error on failure. */ std::vector getCalibrationStatus(); @@ -467,6 +479,7 @@ namespace upm { * @param range One of the BNO055_ACC_RANGE_T values. * @param bw One of the BNO055_ACC_BW_T values. * @param pwr One of the BNO055_ACC_PWR_MODE_T values. + * @throws std::runtime_error on failure. */ void setAccelerationConfig(BNO055_ACC_RANGE_T range, BNO055_ACC_BW_T bw, @@ -479,6 +492,7 @@ namespace upm { * @param odr One of the BNO055_MAG_ODR_T values. * @param opr One of the BNO055_MAG_OPR_T values. * @param pwr One of the BNO055_MAG_POWER_T values. + * @throws std::runtime_error on failure. */ void setMagnetometerConfig(BNO055_MAG_ODR_T odr, BNO055_MAG_OPR_T opr, @@ -491,6 +505,7 @@ namespace upm { * @param range One of the BNO055_GYR_RANGE_T values. * @param bw One of the BNO055_GYR_BW_T values. * @param pwr One of the BNO055_GYR_POWER_MODE_T values. + * @throws std::runtime_error on failure. */ void setGyroscopeConfig(BNO055_GYR_RANGE_T range, BNO055_GYR_BW_T bw, @@ -502,6 +517,7 @@ namespace upm { * per-second squared (m/s^2). The default is m/s^2. * * @param mg true for mg, false for m/s^2. + * @throws std::runtime_error on failure. */ void setAccelerometerUnits(bool mg=false); @@ -511,6 +527,7 @@ namespace upm { * degrees. * * @param radians true for radians, false for degrees. + * @throws std::runtime_error on failure. */ void setGyroscopeUnits(bool radians=false); @@ -520,11 +537,14 @@ namespace upm { * degrees. * * @param radians true for radians, false for degrees. + * @throws std::runtime_error on failure. */ void setEulerUnits(bool radians=false); /** * Reset all interrupt status bits and interrupt output. + * + * @throws std::runtime_error on failure. */ void resetInterruptStatus(); @@ -533,6 +553,7 @@ namespace upm { * BNO055_INT_STA_BITS_T bits. * * @return a bitmask of BNO055_INT_STA_BITS_T bits. + * @throws std::runtime_error on failure. */ uint8_t getInterruptStatus(); @@ -542,6 +563,7 @@ namespace upm { * * @return a bitmask of BNO055_INT_STA_BITS_T bits currently set in the * enable register. + * @throws std::runtime_error on failure. */ uint8_t getInterruptEnable(); @@ -550,6 +572,7 @@ namespace upm { * bitmask of the BNO055_INT_STA_BITS_T bits. * * @param enables a bitmask of BNO055_INT_STA_BITS_T bits to enable + * @throws std::runtime_error on failure. */ void setInterruptEnable(uint8_t enables); @@ -562,6 +585,7 @@ namespace upm { * * @return a bitmask of BNO055_INT_STA_BITS_T bits currently set in the * interrupt mask register. + * @throws std::runtime_error on failure. */ uint8_t getInterruptMask(); @@ -574,6 +598,7 @@ namespace upm { * * @param mask A bitmask of BNO055_INT_STA_BITS_T bits to set in * the interrupt mask register. + * @throws std::runtime_error on failure. */ void setInterruptMask(uint8_t mask); @@ -582,6 +607,7 @@ namespace upm { * can be used to determine the overall status of the device. * * @return One of the BNO055_SYS_STATUS_T values. + * @throws std::runtime_error on failure. */ BNO055_SYS_STATUS_T getSystemStatus(); @@ -591,6 +617,7 @@ namespace upm { * conditions. * * @return One of the BNO055_SYS_ERR_T values. + * @throws std::runtime_error on failure. */ BNO055_SYS_ERR_T getSystemError(); @@ -643,6 +670,7 @@ namespace upm { * * @param reg The register to read * @return The value of the register + * @throws std::runtime_error on failure. */ uint8_t readReg(uint8_t reg);