自学教程：C++ HAL_ERROR1函数代码示例

bool RTIMUGD20HM303DLHC::setAccelCTRL4(){    unsigned char ctrl4;    switch (m_settings->m_GD20HM303DLHCAccelFsr) {    case LSM303DLHC_ACCEL_FSR_2:        m_accelScale = (RTFLOAT)0.001 / (RTFLOAT)16;        break;    case LSM303DLHC_ACCEL_FSR_4:        m_accelScale = (RTFLOAT)0.002 / (RTFLOAT)16;        break;    case LSM303DLHC_ACCEL_FSR_8:        m_accelScale = (RTFLOAT)0.004 / (RTFLOAT)16;        break;    case LSM303DLHC_ACCEL_FSR_16:        m_accelScale = (RTFLOAT)0.012 / (RTFLOAT)16;        break;    default:        HAL_ERROR1("Illegal LSM303DLHC accel FSR code %d/n", m_settings->m_GD20HM303DLHCAccelFsr);        return false;    }    ctrl4 = 0x80 + (m_settings->m_GD20HM303DLHCAccelFsr << 4);    return m_settings->HALWrite(m_accelSlaveAddr,  LSM303DLHC_CTRL4_A, ctrl4, "Failed to set LSM303DLHC CTRL4");}

bool RTIMUMPU9150::setGyroFsr(unsigned char fsr){    switch (fsr) {    case MPU9150_GYROFSR_250:        m_gyroFsr = fsr;        m_gyroScale = RTMATH_PI / (131.0 * 180.0);        return true;    case MPU9150_GYROFSR_500:        m_gyroFsr = fsr;        m_gyroScale = RTMATH_PI / (62.5 * 180.0);        return true;    case MPU9150_GYROFSR_1000:        m_gyroFsr = fsr;        m_gyroScale = RTMATH_PI / (32.8 * 180.0);        return true;    case MPU9150_GYROFSR_2000:        m_gyroFsr = fsr;        m_gyroScale = RTMATH_PI / (16.4 * 180.0);        return true;    default:        HAL_ERROR1("Illegal MPU9150 gyro fsr %d/n", fsr);        return false;    }}

bool RTIMULSM9DS0::setCompassCTRL6(){    unsigned char ctrl6;    //  convert FSR to uT    switch (m_settings->m_LSM9DS0CompassFsr) {    case LSM9DS0_COMPASS_FSR_2:        ctrl6 = 0;        m_compassScale = (RTFLOAT)0.008;        break;    case LSM9DS0_COMPASS_FSR_4:        ctrl6 = 0x20;        m_compassScale = (RTFLOAT)0.016;        break;    case LSM9DS0_COMPASS_FSR_8:        ctrl6 = 0x40;        m_compassScale = (RTFLOAT)0.032;        break;    case LSM9DS0_COMPASS_FSR_12:        ctrl6 = 0x60;        m_compassScale = (RTFLOAT)0.0479;        break;    default:        HAL_ERROR1("Illegal LSM9DS0 compass FSR code %d/n", m_settings->m_LSM9DS0CompassFsr);        return false;    }    return I2CWrite(m_accelCompassSlaveAddr,  LSM9DS0_CTRL6, ctrl6, "Failed to set LSM9DS0 compass CTRL6");}

bool RTIMUBMX055::setAccelFSR(){    unsigned char reg;    switch(m_settings->m_BMX055AccelFsr) {    case BMX055_ACCEL_FSR_2:        reg = 0x03;        m_accelScale = 0.00098 / 16.0;        break;    case BMX055_ACCEL_FSR_4:        reg = 0x05;        m_accelScale = 0.00195 / 16.0;        break;    case BMX055_ACCEL_FSR_8:        reg = 0x08;        m_accelScale = 0.00391 / 16.0;        break;    case BMX055_ACCEL_FSR_16:        reg = 0x0c;        m_accelScale = 0.00781 / 16.0;        break;    default:        HAL_ERROR1("Illegal BMX055 accel FSR code %d/n", m_settings->m_BMX055AccelFsr);        return false;    }    return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_RANGE, reg, "Failed to set BMX055 accel rate"));}

bool RTIMULSM9DS1::setCompassCTRL2(){    unsigned char ctrl2;    //  convert FSR to uT    switch (m_settings->m_LSM9DS1CompassFsr) {    case LSM9DS1_COMPASS_FSR_4:        ctrl2 = 0;        m_compassScale = (RTFLOAT)0.014;        break;    case LSM9DS1_COMPASS_FSR_8:        ctrl2 = 0x20;        m_compassScale = (RTFLOAT)0.029;        break;    case LSM9DS1_COMPASS_FSR_12:        ctrl2 = 0x40;        m_compassScale = (RTFLOAT)0.043;        break;    case LSM9DS1_COMPASS_FSR_16:        ctrl2 = 0x60;        m_compassScale = (RTFLOAT)0.058;        break;    default:        HAL_ERROR1("Illegal LSM9DS1 compass FSR code %d/n", m_settings->m_LSM9DS1CompassFsr);        return false;    }    return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL2, ctrl2, "Failed to set LSM9DS1 compass CTRL6");}

bool RTIMULSM9DS0::setGyroCTRL2(){    if ((m_settings->m_LSM9DS0GyroHpf < LSM9DS0_GYRO_HPF_0) || (m_settings->m_LSM9DS0GyroHpf > LSM9DS0_GYRO_HPF_9)) {        HAL_ERROR1("Illegal LSM9DS0 gyro high pass filter code %d/n", m_settings->m_LSM9DS0GyroHpf);        return false;    }    return I2CWrite(m_gyroSlaveAddr,  LSM9DS0_GYRO_CTRL2, m_settings->m_LSM9DS0GyroHpf, "Failed to set LSM9DS0 gyro CTRL2");}

bool RTIMUGD20HM303DLHC::setGyroCTRL2(){    if ((m_settings->m_GD20HM303DLHCGyroHpf < L3GD20H_HPF_0) || (m_settings->m_GD20HM303DLHCGyroHpf > L3GD20H_HPF_9)) {        HAL_ERROR1("Illegal L3GD20H high pass filter code %d/n", m_settings->m_GD20HM303DLHCGyroHpf);        return false;    }    return m_settings->HALWrite(m_gyroSlaveAddr,  L3GD20H_CTRL2, m_settings->m_GD20HM303DLHCGyroHpf, "Failed to set L3GD20H CTRL2");}

bool RTIMUMPU9150::setCompassRate(int rate){    if ((rate < MPU9150_COMPASSRATE_MIN) || (rate > MPU9150_COMPASSRATE_MAX)) {        HAL_ERROR1("Illegal compass rate %d/n", rate);        return false;    }    m_compassRate = rate;    return true;}

bool RTIMULSM9DS1::setAccelCTRL6(){    unsigned char ctrl6;    if ((m_settings->m_LSM9DS1AccelSampleRate < 0) || (m_settings->m_LSM9DS1AccelSampleRate > 6)) {        HAL_ERROR1("Illegal LSM9DS1 accel sample rate code %d/n", m_settings->m_LSM9DS1AccelSampleRate);        return false;    }    ctrl6 = (m_settings->m_LSM9DS1AccelSampleRate << 5);    if ((m_settings->m_LSM9DS1AccelLpf < 0) || (m_settings->m_LSM9DS1AccelLpf > 3)) {        HAL_ERROR1("Illegal LSM9DS1 accel low pass fiter code %d/n", m_settings->m_LSM9DS1AccelLpf);        return false;    }    switch (m_settings->m_LSM9DS1AccelFsr) {    case LSM9DS1_ACCEL_FSR_2:        m_accelScale = (RTFLOAT)0.000061;        break;    case LSM9DS1_ACCEL_FSR_4:        m_accelScale = (RTFLOAT)0.000122;        break;    case LSM9DS1_ACCEL_FSR_8:        m_accelScale = (RTFLOAT)0.000244;        break;    case LSM9DS1_ACCEL_FSR_16:        m_accelScale = (RTFLOAT)0.000732;        break;    default:        HAL_ERROR1("Illegal LSM9DS1 accel FSR code %d/n", m_settings->m_LSM9DS1AccelFsr);        return false;    }    ctrl6 |= (m_settings->m_LSM9DS1AccelLpf) | (m_settings->m_LSM9DS1AccelFsr << 3);    return m_settings->HALWrite(m_accelGyroSlaveAddr,  LSM9DS1_CTRL6, ctrl6, "Failed to set LSM9DS1 accel CTRL6");}

bool RTIMUHal::HALRead(unsigned char slaveAddr, unsigned char length,                    unsigned char *data, const char *errorMsg){    int tries, result, total;    unsigned char rxBuff[MAX_READ_LEN + 1];    struct spi_ioc_transfer rdIOC;    if (m_busIsI2C) {        if (!I2CSelectSlave(slaveAddr, errorMsg))            return false;        total = 0;        tries = 0;        while ((total < length) && (tries < 5)) {            result = read(m_I2C, data + total, length - total);            if (result < 0) {                if (strlen(errorMsg) > 0)                    HAL_ERROR2("I2C read error from %d - %s/n", slaveAddr, errorMsg);                return false;            }            total += result;            if (total == length)                break;            delayMs(10);            tries++;        }        if (total < length) {            if (strlen(errorMsg) > 0)                HAL_ERROR2("I2C read from %d failed - %s/n", slaveAddr, errorMsg);            return false;        }    } else {        memset(&rdIOC, 0, sizeof(rdIOC));        rdIOC.tx_buf = 0;        rdIOC.rx_buf = (unsigned long) rxBuff;        rdIOC.len = length;        if (ioctl(m_SPI, SPI_IOC_MESSAGE(1), &rdIOC) < 0) {            if (strlen(errorMsg) > 0)                HAL_ERROR1("SPI read error from - %s/n", errorMsg);            return false;        }        memcpy(data, rxBuff, length);    }    return true;}

bool RTIMUHal::HALRead(unsigned char slaveAddr, unsigned char length,                    unsigned char *data, const char *errorMsg){    if (m_busIsI2C) {        if (I2Cdev::readBytes(slaveAddr, length, data, 10) == length)             return true;        if (strlen(errorMsg) > 0)            HAL_ERROR1("I2C read failed - %s/n", errorMsg);        return false;    } else {        SPI.beginTransaction(m_SPISettings);        digitalWrite(m_SPISelect, LOW);        for (int i = 0; i < length; i++)            data[i] = SPI.transfer(0);        digitalWrite(m_SPISelect, HIGH);        SPI.endTransaction();        return true;    }}

bool RTIMUMPU9255::setAccelLpf(unsigned char lpf){    switch (lpf) {    case MPU9255_ACCEL_LPF_1130:    case MPU9255_ACCEL_LPF_460:    case MPU9255_ACCEL_LPF_184:    case MPU9255_ACCEL_LPF_92:    case MPU9255_ACCEL_LPF_41:    case MPU9255_ACCEL_LPF_20:    case MPU9255_ACCEL_LPF_10:    case MPU9255_ACCEL_LPF_5:        m_accelLpf = lpf;        return true;    default:        HAL_ERROR1("Illegal MPU9255 accel lpf %d/n", lpf);        return false;    }}

bool RTIMUHal::I2CSelectSlave(unsigned char slaveAddr, const char *errorMsg){    if (m_currentSlave == slaveAddr)        return true;    if (!HALOpen()) {        HAL_ERROR1("Failed to open I2C port - %s/n", errorMsg);        return false;    }    if (ioctl(m_I2C, I2C_SLAVE, slaveAddr) < 0) {        HAL_ERROR2("I2C slave select %d failed - %s/n", slaveAddr, errorMsg);        return false;    }    m_currentSlave = slaveAddr;    return true;}