From 36237ae59645ae1b97823c1919c17b783856db11 Mon Sep 17 00:00:00 2001 From: Jon Trulson Date: Thu, 10 Sep 2015 13:49:07 -0600 Subject: [PATCH] lsm9ds0: throw exception(s) on fatal errors Signed-off-by: Jon Trulson Signed-off-by: Mihai Tudor Panu --- src/lsm9ds0/lsm9ds0.cxx | 61 +++++++++++++++++++++++++++-------------- 1 file changed, 40 insertions(+), 21 deletions(-) diff --git a/src/lsm9ds0/lsm9ds0.cxx b/src/lsm9ds0/lsm9ds0.cxx index 3b8d83d4..def2e448 100644 --- a/src/lsm9ds0/lsm9ds0.cxx +++ b/src/lsm9ds0/lsm9ds0.cxx @@ -89,7 +89,8 @@ bool LSM9DS0::init() // power up if (!setGyroscopePowerDown(false)) { - cerr << __FUNCTION__ << ": Unable to wake up gyro" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to wake up gyro"); return false; } @@ -97,21 +98,24 @@ bool LSM9DS0::init() if (!setGyroscopeEnableAxes(CTRL_REG1_G_YEN |CTRL_REG1_G_XEN | CTRL_REG1_G_ZEN)) { - cerr << __FUNCTION__ << ": Unable to enable gyro axes" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to enable gyro axes"); return false; } // set gyro ODR if (!setGyroscopeODR(G_ODR_95_25)) { - cerr << __FUNCTION__ << ": Unable to set gyro ODR" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to set gyro ODR"); return false; } // set gyro scale if (!setGyroscopeScale(G_FS_245)) { - cerr << __FUNCTION__ << ": Unable to set gyro scale" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to set gyro scale"); return false; } @@ -120,7 +124,8 @@ bool LSM9DS0::init() // power up and set ODR if (!setAccelerometerODR(XM_AODR_100)) { - cerr << __FUNCTION__ << ": Unable to set accel ODR" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to set accel ODR"); return false; } @@ -128,14 +133,16 @@ bool LSM9DS0::init() if (!setAccelerometerEnableAxes(CTRL_REG1_XM_AXEN |CTRL_REG1_XM_AYEN | CTRL_REG1_XM_AZEN)) { - cerr << __FUNCTION__ << ": Unable to enable accel axes" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to enable accel axes"); return false; } // set scaling rate if (!setAccelerometerScale(XM_AFS_2)) { - cerr << __FUNCTION__ << ": Unable to set accel scale" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to set accel scale"); return false; } @@ -144,7 +151,8 @@ bool LSM9DS0::init() // enable the temperature sensor if (!enableTemperatureSensor(true)) { - cerr << __FUNCTION__ << ": Unable to enable temp sensor" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to enable temp sensor"); return false; } @@ -153,35 +161,40 @@ bool LSM9DS0::init() // set mode (this also powers it up if not XM_MD_POWERDOWN) if (!setMagnetometerMode(XM_MD_CONTINUOUS)) { - cerr << __FUNCTION__ << ": Unable to set mag scale" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to set mag scale"); return false; } // turn LPM off if (!setMagnetometerLPM(false)) { - cerr << __FUNCTION__ << ": Unable to disable mag LPM" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to disable mag LPM"); return false; } // set resolution if (!setMagnetometerResolution(XM_RES_LOW)) { - cerr << __FUNCTION__ << ": Unable to set mag res" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to set mag res"); return false; } // set ODR if (!setMagnetometerODR(XM_ODR_12_5)) { - cerr << __FUNCTION__ << ": Unable to set mag ODR" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to set mag ODR"); return false; } // set scale if (!setMagnetometerScale(XM_MFS_2)) { - cerr << __FUNCTION__ << ": Unable to set mag scale" << endl; + throw std::runtime_error(string(__FUNCTION__) + + ": Unable to set mag scale"); return false; } @@ -280,7 +293,8 @@ uint8_t LSM9DS0::readReg(DEVICE_T dev, uint8_t reg) case DEV_GYRO: device = &m_i2cG; break; case DEV_XM: device = &m_i2cXM; break; default: - cerr << __FUNCTION__ << ": Internal error, invalid device" << endl; + throw std::logic_error(string(__FUNCTION__) + + ": Internal error, invalid device specified"); return 0; } @@ -296,7 +310,8 @@ void LSM9DS0::readRegs(DEVICE_T dev, uint8_t reg, uint8_t *buffer, int len) case DEV_GYRO: device = &m_i2cG; break; case DEV_XM: device = &m_i2cXM; break; default: - cerr << __FUNCTION__ << ": Internal error, invalid device" << endl; + throw std::logic_error(string(__FUNCTION__) + + ": Internal error, invalid device specified"); return; } @@ -314,15 +329,16 @@ bool LSM9DS0::writeReg(DEVICE_T dev, uint8_t reg, uint8_t val) case DEV_GYRO: device = &m_i2cG; break; case DEV_XM: device = &m_i2cXM; break; default: - cerr << __FUNCTION__ << ": Internal error, invalid device" << endl; + throw std::logic_error(string(__FUNCTION__) + + ": Internal error, invalid device specified"); return false; } mraa::Result rv; if ((rv = device->writeReg(reg, val)) != mraa::SUCCESS) { - cerr << __FUNCTION__ << ": failed:" << endl; - printError(rv); + throw std::runtime_error(std::string(__FUNCTION__) + + ": I2c.writeReg() failed"); return false; } @@ -399,7 +415,8 @@ bool LSM9DS0::setGyroscopeScale(G_FS_T scale) default: // should never occur, but... m_gyroScale = 0.0; // set a safe, though incorrect value - cerr << __FUNCTION__ << ": internal error, unsupported scale" << endl; + throw std::logic_error(string(__FUNCTION__) + + ": internal error, unsupported scale"); break; } @@ -472,7 +489,8 @@ bool LSM9DS0::setAccelerometerScale(XM_AFS_T scale) default: // should never occur, but... m_accelScale = 0.0; // set a safe, though incorrect value - cerr << __FUNCTION__ << ": internal error, unsupported scale" << endl; + throw std::logic_error(string(__FUNCTION__) + + ": internal error, unsupported scale"); break; } @@ -559,7 +577,8 @@ bool LSM9DS0::setMagnetometerScale(XM_MFS_T scale) default: // should never occur, but... m_magScale = 0.0; // set a safe, though incorrect value - cerr << __FUNCTION__ << ": internal error, unsupported scale" << endl; + throw std::logic_error(string(__FUNCTION__) + + ": internal error, unsupported scale"); break; }