/** * @brief Read gyroscope angular rate axis data. * * This function reads angular rate data for all 3 axes of an IMU-3000 * gyroscope. * * @param sensor Address of an initialized sensor device descriptor. * @param data The address of a vector storing sensor axis data. * @return bool true if the call succeeds, else false is returned. */ static bool imu3000_get_rotation(sensor_hal_t *hal, sensor_data_t *data) { struct { uint8_t msb; uint8_t lsb; } input[3]; size_t const count = sensor_bus_read(hal, IMU3000_GYRO_XOUT_H, input, sizeof(input)); /* Copy data values based on device orientation and signs. */ data->axis.x = hal->orientation.x.sign * ((int16_t)((input[hal->orientation.x.axis].msb << 8) | input[hal->orientation.x.axis].lsb)); data->axis.y = hal->orientation.y.sign * ((int16_t)((input[hal->orientation.y.axis].msb << 8) | input[hal->orientation.y.axis].lsb)); data->axis.z = hal->orientation.z.sign * ((int16_t)((input[hal->orientation.z.axis].msb << 8) | input[hal->orientation.z.axis].lsb)); /* Convert raw sensor samples to engineering units if requested. */ if (data->scaled) { data->axis.x /= (int32_t)scale_lsb_per_dps [sensor_fs_sel]; data->axis.y /= (int32_t)scale_lsb_per_dps [sensor_fs_sel]; data->axis.z /= (int32_t)scale_lsb_per_dps [sensor_fs_sel]; } return (count == sizeof(input)); }
/** * @brief Read BMA250 acceleration data. * * This function obtains accelerometer data for all three axes of the Bosch * device. The data is read from three device registers using a multi-byte * bus transfer. * * Along with the actual sensor data, the LSB byte contains a "new" flag * indicating if the data for this axis has been updated since the last * time the axis data was read. Reading either LSB or MSB data will * clear this flag. * * @param sensor Address of an initialized sensor device descriptor. * @param data The address of a vector storing sensor axis data. * @return bool true if the call succeeds, else false is returned. */ static bool bma250_get_accel(sensor_hal_t *hal, sensor_data_t *data) { size_t const count = sensor_bus_read(hal, hal->burst_addr, event_regs.acc, sizeof(event_regs.acc)); format_axis_data(hal, event_regs.acc, data); return (count == sizeof(event_regs.acc)); }
/** * @brief Read accelerometer data. * * This function obtains accelerometer data for all three axes of the Bosch * device. The data is read from six device registers using a multi-byte * bus transfer. The 10-bit raw results are then assembled from the two * register values for each axis, including extending the sign bit, to * form a signed 32-bit value. * * Along with the actual sensor data, the LSB byte contains a "new" flag * indicating if the data for this axis has been updated since the last * time the axis data was read. Reading either LSB or MSB data will * clear this flag. * * @param hal Address of an initialized sensor hardware descriptor. * @param data The address of a vector storing sensor axis data. * @return bool true if the call succeeds, else false is returned. */ static bool bma180_get_accel(sensor_hal_t *hal, sensor_data_t *data) { bma_axis_t axis[3]; size_t const count = sensor_bus_read(hal, hal->burst_addr, axis, sizeof(axis)); format_axis_data(hal, axis, data); return (count == sizeof(axis)); }
/** * @brief Read gyroscope integrated temperature sensor data * * This function reads IMU-3000 integrated temperature sensor data. * The temperature is always returned in scaled engineering units * (degrees Celsius). * * @param sensor Address of an initialized sensor device descriptor. * @param data The address where temperature samples are returned. * @return bool true if the call succeeds, else false is returned. */ static bool imu3000_get_temperature(sensor_hal_t *hal, sensor_data_t *data) { int16_t temp_data; size_t const count = sensor_bus_read(hal, IMU3000_TEMP_OUT_H, &temp_data, sizeof(temp_data)); data->temperature.value = (int16_t)be16_to_cpu(temp_data); if (data->scaled) { data->temperature.value -= TEMP_OFFSET; data->temperature.value /= TEMP_COUNTS_PER_DEG_C; data->temperature.value += TEMP_REF_DEG; } return (count == sizeof(temp_data)); }
/** * @brief Read SFH5712 light level data. * * This function reads the ambient light level data from the sensor * and places it in the user-provided sensor_data_t structure. * * @param sensor Address of an initialized sensor device descriptor. * @param data The address of a vector storing sensor measurement data. * @return bool true if the call succeeds, else false is returned. */ static bool sfh5712_get_light(sensor_hal_t *hal, sensor_data_t *data) { struct { uint8_t lsb; uint8_t msb; } light_data; /* Read and combine two light level data registers * NOTE: LSB register must be read first! */ size_t const count = sensor_bus_read(hal, SFH5712_ALS_DATA_LSB, (uint8_t *)&light_data, sizeof(light_data)); /* Device uses lux for internal values, so raw is the same as scaled */ data->light.value = (uint32_t)((light_data.msb << 8) | light_data.lsb); return (count == sizeof(light_data)); }
/** * @brief Read SFH7770 proximity data. * * This function reads the proximity data from the three channels of the sensor * and places it in the user-provided sensor_data_t structure. The * proximity value is not in standard units, so for "scaled" output * the values are encoded based on whether or not the proximity * reading exceeds the programmed threshold level for each channel (LED). * * The sensor has only a single threshold level, so only two possible * values are reported: PROXIMITY_NEAR (if threshold is exceeded) or * PROXIMITY_NONE. * * @param sensor Address of an initialized sensor device descriptor. * @param data The address of a vector storing sensor measurement data. * @return bool true if the call succeeds, else false is returned. */ static bool sfh7770_get_proximity(sensor_hal_t *hal, sensor_data_t *data) { struct { uint8_t als_ps_status; uint8_t ps_data_led1; uint8_t ps_data_led2; uint8_t ps_data_led3; } regs; /* Read three LED proximity measurements + status */ size_t const count = sensor_bus_read(hal, SFH7770_ALS_PS_STATUS, (uint8_t *)®s, sizeof(regs)); /* Fill in return values based on "scaled" or raw selection */ if (data->scaled) { /* Use internal device threshold status to determine values */ data->proximity.value[0] = (regs.als_ps_status & PS_LED1_THRESH) ? PROXIMITY_NEAR : PROXIMITY_NONE; data->proximity.value[1] = (regs.als_ps_status & PS_LED2_THRESH) ? PROXIMITY_NEAR : PROXIMITY_NONE; data->proximity.value[2] = (regs.als_ps_status & PS_LED3_THRESH) ? PROXIMITY_NEAR : PROXIMITY_NONE; } else { /* Use internal raw values */ data->proximity.value[0] = (int32_t)regs.ps_data_led1; data->proximity.value[1] = (int32_t)regs.ps_data_led2; data->proximity.value[2] = (int32_t)regs.ps_data_led3; } return (count == sizeof(regs)); }
/** * @brief Perform self-test function * * This function sets up and executes an built-in self test function * within the HMC5883L device. * * @param sensor Address of an initialized sensor descriptor. * @param test_code Address of a numeric code of which test to perform * This location will contain the specific test result code * when the function returns. * @return bool true if the call succeeds, else false is returned. */ bool hmc5883l_selftest(sensor_t *sensor, int *test_code, void *arg) { int count; uint8_t meas_mode; /* test measurement mode */ vector3_t data; bool status = true; sensor_hal_t *const hal = sensor->hal; struct { uint8_t config_reg_a; uint8_t config_reg_b; uint8_t mode_reg; } reg_set; /* Initialize result code */ *test_code = SENSOR_TEST_ERR_NONE; /* Save register values */ count = sensor_bus_read(hal, HMC5883L_CONFIG_REG_A, (uint8_t *)®_set, sizeof(reg_set)); if (count != sizeof(reg_set)) { *test_code = SENSOR_TEST_ERR_READ; return false; } /* Set range (sensitivity) */ sensor_bus_put(hal, HMC5883L_CONFIG_REG_B, HMC5883L_TEST_GAIN); /* Set test mode */ switch (*test_code) { /* which test was specified */ case SENSOR_TEST_DEFAULT: case SENSOR_TEST_BIAS_POS: meas_mode = MEAS_MODE_POS; /* positive bias measurement mode */ break; case SENSOR_TEST_BIAS_NEG: meas_mode = MEAS_MODE_NEG; /* negative bias measurement mode */ break; default: /* bad test code specified */ sensor_bus_put(hal, HMC5883L_CONFIG_REG_B, reg_set.config_reg_b); /* restore reg */ *test_code = SENSOR_TEST_ERR_FUNCTION; return false; } sensor_bus_put(hal, HMC5883L_CONFIG_REG_A, (DATA_RATE_15HZ | MEAS_AVG_1 | meas_mode)); delay_ms(SELF_TEST_DELAY_MSEC); /* Perform test measurement & check results */ sensor_bus_put(hal, HMC5883L_MODE_REG, MODE_SINGLE); /* single meas mode **/ if (hmc5883l_get_data(hal, &data) != true) { *test_code = SENSOR_TEST_ERR_READ; status = false; /* failed to read data registers */ } else { if (arg != NULL) { ((sensor_data_t *)arg)->scaled = false; /* only raw values */ ((sensor_data_t *)arg)->timestamp = 0; /* no timestamp */ ((sensor_data_t *)arg)->axis.x = (int32_t)data.x; /* copy values */ ((sensor_data_t *)arg)->axis.y = (int32_t)data.y; ((sensor_data_t *)arg)->axis.z = (int32_t)data.z; } /* Check range of readings */ if ((HMC5883L_TEST_X_MIN > data.x) || (data.x > HMC5883L_TEST_X_MAX) || (HMC5883L_TEST_Y_MIN > data.y) || (data.y > HMC5883L_TEST_Y_MAX) || (HMC5883L_TEST_Z_MIN > data.z) || (data.z > HMC5883L_TEST_Z_MAX)) { *test_code = SENSOR_TEST_ERR_RANGE; status = false; /* value out of range */ } } /* Restore registers */ count = sensor_bus_write(hal, HMC5883L_CONFIG_REG_A, (uint8_t *)®_set, sizeof(reg_set)); if (count != sizeof(reg_set)) { *test_code = SENSOR_TEST_ERR_WRITE; status = false; } return status; }
/** * @brief Honeywell HMC5883L magnetometer driver initialization. * * This is the main initialization function for the HMC5883L device. * * @param sensor Address of a sensor device descriptor. * @param resvd Reserved value. * @return bool true if the call succeeds, else false is returned. */ bool hmc5883l_init(sensor_t *sensor, int resvd) { bool status = false; sensor_hal_t *const hal = sensor->hal; sensor_data_t data; if (hmc5883l_device_id(hal, &data) && (HMC5883L_DEV_ID == data.device.id)) { /* Set the driver function table and capabilities pointer. */ static const sensor_device_t hmc5883l_device = { .func.read = hmc5883l_read, .func.ioctl = hmc5883l_ioctl, .func.selftest = hmc5883l_selftest, .func.calibrate = hmc5883l_calibrate, .caps.feature = SENSOR_CAPS_3_AXIS | SENSOR_CAPS_SELFTEST, .caps.vendor = SENSOR_VENDOR_HONEYWELL, .caps.units = SENSOR_UNITS_tesla, .caps.scale = SENSOR_SCALE_micro, .caps.name = "HMC5883L triaxial compass/magnetometer" }; sensor->drv = &hmc5883l_device; /* Enable sensor in continuous measurement mode */ sensor_bus_put(hal, HMC5883L_MODE_REG, MODE_CONTIN); /* Set the driver (device) default range, bandwidth, and * resolution. */ sensor_bus_put(hal, HMC5883L_CONFIG_REG_A, (DATA_RATE_15HZ | MEAS_MODE_NORM)); hal->range = range_table[index_130uT].range_units; hal->bandwidth = band_table[index_15hz].bandwidth_Hz; hal->resolution = HMC5883L_DATA_RESOLUTION; hmc5883l_set_range(hal, index_130uT); hmc5883l_set_bandwidth(hal, index_15hz); /* Initialize calibration data (offsets & sensitivity) from * NVRAM. */ nvram_read(0, &cal_data, sizeof(cal_data)); /* Clear all calibration values if any are invalid. */ #if defined(MATH_FIXED_POINT) if ((cal_data.x == -1) || (cal_data.y == -1) || (cal_data.z == -1)) { #else if (isnan(cal_data.offsets.x) || isnan(cal_data.offsets.y) || isnan(cal_data.offsets.z)) { #endif memset(&cal_data, 0x00, sizeof(cal_data)); } status = (STATUS_OK == hal->bus.status); } return status; } /** * @brief Read magnetometer device ID * * This function reads the magnetometer hardware identification registers * and returns these values to the addresses specified in the function * parameters. * * @param hal Address of an initialized sensor hardware descriptor. * @param data Address of sensor_data_t structure to return values. * @return bool true if the call succeeds, else false is returned. */ static bool hmc5883l_device_id(sensor_hal_t *hal, sensor_data_t *data) { uint32_t id_reg = 0; size_t const id_len = 3; size_t const count = sensor_bus_read (hal, HMC5883L_ID_REG_A, &id_reg, id_len); data->device.id = cpu_to_be32(id_reg) >> 8; data->device.version = 0; return (count == id_len); } /** * @brief Read magnetometer vector data. * * This function obtains magnetometer data for all three axes of the Honeywell * device. The data is read from six device registers using a multi-byte * bus transfer. The 13-bit raw results are then assembled from the two * register values for each axis, including extending the sign bit, to * form a signed 16-bit value. * * @param hal Address of an initialized sensor hardware descriptor. * @param data The address of a vector storing sensor axis data. * @return bool true if the call succeeds, else false is returned. */ static bool hmc5883l_get_data(sensor_hal_t *hal, vector3_t *data) { int16_t axis_data[3]; /* input data values: X,Y,Z */ /* Ensure that single-measurement mode is set and that bit MR7 is * cleared. Bit MR7 is set internally after each single-measurement * operation. */ sensor_bus_put(hal, HMC5883L_MODE_REG, MODE_SINGLE); do { /* Wait for the data ready signal. * * Instead of polling DRDY, as is done below, the device can be * placed in continuous measurement mode and this signal can be * configured to trigger an asynchronous interrupt. */ } while (gpio_pin_is_low(hal->mcu_sigint /* DRDY input */)); /* Get measurement data (consecutive big endian byte pairs: x, y, z). */ hmc588l_axis_t input[3]; size_t const count = sensor_bus_read (hal, HMC5883L_MAG_X_HI, (uint8_t *)input, sizeof(input)); if (count != sizeof(input)) { return false; } /* Note: Device data register order = x, z, y !! */ axis_data[0] = ((int16_t)((input[0].msb << 8) + input[0].lsb)); axis_data[2] = ((int16_t)((input[1].msb << 8) + input[1].lsb)); axis_data[1] = ((int16_t)((input[2].msb << 8) + input[2].lsb)); if ((axis_data[0] == DATA_OUTPUT_OVERFLOW) || (axis_data[1] == DATA_OUTPUT_OVERFLOW) || (axis_data[2] == DATA_OUTPUT_OVERFLOW)) { return false; } /* * Convert signed integer values to a real data type for calculations. * Use device orientation configuration to assign axes. */ const sensor_orient_t *const orient = &(hal->orientation); data->x = orient->x.sign * axis_data[orient->x.axis]; data->y = orient->y.sign * axis_data[orient->y.axis]; data->z = orient->z.sign * axis_data[orient->z.axis]; return true; }
/** * @brief InvenSense ITG-3200 gyroscope driver initialization. * * This is the main initialization function for the ITG-3200 device. * * @param sensor Address of a sensor device descriptor. * @param resvd Reserved value. * @return bool true if the sensor is ready for use, else false. */ bool itg3200_init(sensor_t *sensor, int resvd) { sensor_hal_t *const hal = sensor->hal; bool status = false; /* Set the driver function table and capabilities pointer. */ static const sensor_device_t itg3200_device = { .func.read = itg3200_read, .func.ioctl = itg3200_ioctl, .func.event = itg3200_event, .caps.feature = SENSOR_CAPS_3_AXIS | SENSOR_CAPS_AUX_TEMP, .caps.vendor = SENSOR_VENDOR_INVENSENSE, .caps.band_table = band_table, .caps.band_count = ARRAYSIZE(band_table), .caps.units = SENSOR_UNITS_deg_per_sec, .caps.name = "InvenSense ITG-3200 3-axis angular rate sensor" }; sensor->drv = &itg3200_device; /* Set the driver default, range, bandwidth, resolution, etc. */ hal->range = ITG3200_FS_RANGE; hal->bandwidth = 256; /* Hertz */ hal->sample_rate = 100; /* Hertz */ hal->resolution = ITG3200_RESOLUTION; hal->burst_addr = ITG3200_INT_STATUS; /* Apply default device settings & connect an interrupt handler. */ if (itg3200_default_init(hal) && sensor_irq_connect (hal->mcu_sigint, itg3200_isr, hal)) { status = true; } return status; } /** * @brief Invensense ITG3200 driver interrupt service routine. * * This is the interrupt service routine for enabled ITG3200 interrupt events. * Only the new data ("raw data ready") interrupt is supported. * * @param arg The address of the driver sensor_hal_t descriptor. * @return Nothing. */ static void itg3200_isr(volatile void *arg) { sensor_hal_t *const hal = (sensor_hal_t *)arg; int16_t input[3]; hal->bus.no_wait = true; itg3200_event_t regs; sensor_bus_read(hal, hal->burst_addr, ®s, sizeof(regs)); hal->bus.no_wait = false; if (STATUS_OK == hal->bus.status) { /* Assume new data to avoid an apparent race condition. The * interrupt status register sometimes has the new data flag * cleared before it is read above. If this happens, the sensor will * not generate any further new data interrupts until the device is * independently accessed. */ static sensor_event_data_t event_data = {.data.scaled = true}; event_data.data.timestamp = sensor_timestamp(); event_data.event = SENSOR_EVENT_NEW_DATA; input[0] = (regs.x_hi << 8) | regs.x_lo; input[1] = (regs.y_hi << 8) | regs.y_lo; input[2] = (regs.z_hi << 8) | regs.z_lo; event_data.data.axis.x = hal->orientation.x.sign * input[hal->orientation.x.axis]; event_data.data.axis.y = hal->orientation.y.sign * input[hal->orientation.y.axis]; event_data.data.axis.z = hal->orientation.z.sign * input[hal->orientation.z.axis]; event_data.data.axis.x /= SCALE_LSB_PER_DPS; event_data.data.axis.y /= SCALE_LSB_PER_DPS; event_data.data.axis.z /= SCALE_LSB_PER_DPS; /* Call application-supplied handler routine */ (event_cb.handler)(&event_data, event_cb.arg); } } /** * @brief Read gyroscope angular rate axis data. * * This function reads angular rate data for all 3 axes of an ITG-3200 * gyroscope. * * @param sensor Address of an initialized sensor device descriptor. * @param data The address of a vector storing sensor axis data. * @return bool true if the call succeeds, else false is returned. */ static bool itg3200_get_rotation(sensor_hal_t *hal, sensor_data_t *data) { struct { uint8_t msb; uint8_t lsb; } input[3]; size_t const count = sensor_bus_read(hal, ITG3200_GYRO_XOUT_H, input, sizeof(input)); /* Copy data values based on device orientation and signs. */ data->axis.x = hal->orientation.x.sign * ((int16_t)((input[hal->orientation.x.axis].msb << 8) | input[hal->orientation.x.axis].lsb)); data->axis.y = hal->orientation.y.sign * ((int16_t)((input[hal->orientation.y.axis].msb << 8) | input[hal->orientation.y.axis].lsb)); data->axis.z = hal->orientation.z.sign * ((int16_t)((input[hal->orientation.z.axis].msb << 8) | input[hal->orientation.z.axis].lsb)); /* Convert raw sensor samples to engineering units if requested. */ if (data->scaled) { data->axis.x /= SCALE_LSB_PER_DPS; data->axis.y /= SCALE_LSB_PER_DPS; data->axis.z /= SCALE_LSB_PER_DPS; } return (count == sizeof(input)); } /** * @brief Read gyroscope integrated temperature sensor data * * This function reads ITG-3200 integrated temperature sensor data. * The temperature is always returned in scaled engineering units * (degrees Celsius). * * @param sensor Address of an initialized sensor device descriptor. * @param data The address where temperature samples are returned. * @return bool true if the call succeeds, else false is returned. */ static bool itg3200_get_temperature(sensor_hal_t *hal, sensor_data_t *data) { int16_t temp_data; size_t const count = sensor_bus_read(hal, ITG3200_TEMP_OUT_H, &temp_data, sizeof(temp_data)); data->temperature.value = (int16_t)be16_to_cpu(temp_data); if (data->scaled) { data->temperature.value -= TEMP_OFFSET; data->temperature.value /= TEMP_COUNTS_PER_DEG_C; data->temperature.value += TEMP_REF_DEG; } return (count == sizeof(temp_data)); }
/** * @brief InvenSense IMU-3000 motion processor driver initialization. * * This is the main initialization function for the IMU-3000 device. * * @param sensor Address of a sensor device descriptor. * @param resvd Reserved value. * @return bool true if the sensor is ready for use, else false. */ bool imu3000_init(sensor_t *sensor, int resvd) { sensor_hal_t *const hal = sensor->hal; sensor_hal_t *const aux = (sensor_hal_t *)sensor->aux; bool status = false; /* Set the driver function table and capabilities pointer. */ static const sensor_device_t imu3000_device = { .func.read = imu3000_read, .func.ioctl = imu3000_ioctl, .func.event = imu3000_event, .caps.feature = SENSOR_CAPS_3_AXIS | SENSOR_CAPS_AUX_TEMP | SENSOR_CAPS_AUX_ACCEL, .caps.vendor = SENSOR_VENDOR_INVENSENSE, .caps.range_table = range_table, .caps.band_table = band_table, .caps.range_count = ARRAYSIZE(range_table), .caps.band_count = ARRAYSIZE(band_table), .caps.units = SENSOR_UNITS_deg_per_sec, .caps.name = "InvenSense IMU-3000 Motion Processing Unit" }; sensor->drv = &imu3000_device; /* Set the driver default, range, bandwidth, resolution, etc. */ hal->range = range_table [sensor_fs_sel].range_units; hal->bandwidth = band_table [sensor_dlpf_cfg].bandwidth_Hz; hal->sample_rate = 100; hal->resolution = IMU3000_RESOLUTION; /* Apply default device settings */ if (imu3000_default_init(hal, aux) != true) { return status; /* return error */ } /* Set start addr for burst read (used during interrupt). */ hal->burst_addr = IMU3000_INT_STATUS; /* Connect interrupt event handler. */ if (sensor_irq_connect(hal->mcu_sigint, imu3000_isr, hal)) { status = true; } return status; } /** * @brief Invensense IMU3000 driver interrupt service routine. * * This is the interrupt service routine for enabled IMU3000 interrupt events. * Only the new data ("raw data ready") interrupt is supported. * * @param arg The address of the driver sensor_hal_t descriptor. * @return Nothing. */ static void imu3000_isr(volatile void *arg) { sensor_hal_t *const hal = (sensor_hal_t *)arg; int16_t input[3]; hal->bus.no_wait = true; imu3000_event_t regs; sensor_bus_read(hal, hal->burst_addr, ®s, sizeof(regs)); hal->bus.no_wait = false; if (STATUS_OK == hal->bus.status) { /* Assume new data to avoid an apparent race condition. The * interupt status register sometimes has the new data flag * cleared before it is read above. If this happens, the sensor will * not generate any further new data interrupts until the device is * independently accessed. */ static sensor_event_data_t event_data = {.data.scaled = true}; event_data.data.timestamp = sensor_timestamp(); event_data.event = SENSOR_EVENT_NEW_DATA; input[0] = (regs.x_hi << 8) | regs.x_lo; input[1] = (regs.y_hi << 8) | regs.y_lo; input[2] = (regs.z_hi << 8) | regs.z_lo; event_data.data.axis.x = hal->orientation.x.sign * input[hal->orientation.x.axis]; event_data.data.axis.y = hal->orientation.y.sign * input[hal->orientation.y.axis]; event_data.data.axis.z = hal->orientation.z.sign * input[hal->orientation.z.axis]; const int32_t scaling = (int32_t)scale_lsb_per_dps[sensor_fs_sel]; event_data.data.axis.x /= scaling; event_data.data.axis.y /= scaling; event_data.data.axis.z /= scaling; /* Call application-supplied handler routine */ (event_cb.handler)(&event_data, event_cb.arg); } } /** * @brief Enable/disable IMU3000 sensor event * * @param sensor Address of an initialized sensor device descriptor * @param callback Application-defined event callback handler descriptor * @param enable Enable flag: true = enable event, false = disable event * @return bool true if the call succeeds, else false is returned */ static bool imu3000_event(sensor_t *sensor, sensor_event_t sensor_event, sensor_event_callback_t *callback, bool enable) { sensor_hal_t *const hal = sensor->hal; bool status = false; if (sensor_event & SENSOR_EVENT_NEW_DATA) { if (callback) { event_cb = *callback; } if (enable) { /* Enable new data int, latch until any reg is read, * active high */ sensor_bus_put(hal, IMU3000_INT_CFG, INT_CFG_RAW_RDY_EN | INT_CFG_ANYRD_2CLEAR); } else { /* Disable new data int */ sensor_reg_bitclear(hal, IMU3000_INT_CFG, INT_CFG_LATCH_INT_EN); } status = true; } return status; }
/** * @brief Bosch BMA250 accelerometer driver initialization. * * This is the main initialization function for the BMA250 device. * * @param sensor Address of a sensor device descriptor. * @param resvd Reserved value. * @return bool true if the call succeeds, else false is returned. */ bool bma250_init(sensor_t *sensor, int resvd) { bool status = false; sensor_hal_t *const hal = sensor->hal; if (BMA250_ID_VAL == sensor_bus_get(hal, BMA250_CHIP_ID)) { /* Set the driver function table and capabilities pointer. */ static const sensor_device_t bma250_device = { /* Bosch BMA250 Driver Entry Points & Capabilities */ .func.read = bma250_read, .func.ioctl = bma250_ioctl, .func.selftest = bma250_selftest, .func.event = bma250_event, .caps.feature = SENSOR_CAPS_3_AXIS | SENSOR_CAPS_SELFTEST | SENSOR_CAPS_HI_G_EVENT | SENSOR_CAPS_LO_G_EVENT | SENSOR_CAPS_TAP_EVENT | SENSOR_CAPS_TILT_EVENT | SENSOR_CAPS_AUX_TEMP, .caps.vendor = SENSOR_VENDOR_BOSCH, .caps.range_table = range_table, .caps.band_table = band_table, .caps.range_count = ARRAYSIZE(range_table), .caps.band_count = ARRAYSIZE(band_table), .caps.units = SENSOR_UNITS_g0, .caps.scale = SENSOR_SCALE_milli, .caps.name = "BMA250 Digital, triaxial acceleration sensor" }; sensor->drv = &bma250_device; /* Set the driver (device) default range, bandwidth, & * resolution. * * \internal * Per the BMA250 Datasheet the default range and bandwidth * after device reset are +/- 2g and 1kHz respectively. */ bma250_set_state(sensor, SENSOR_STATE_RESET); hal->range = 2000; hal->bandwidth = 1000; hal->resolution = BMA250_DATA_RESOLUTION; hal->burst_addr = BMA250_NEW_DATA_X; /* Install an interrupt handler. */ if ((STATUS_OK == hal->bus.status) && sensor_irq_connect(hal->mcu_sigint, bma250_isr, hal)) { status = true; } } return status; } /** * @brief Bosch BMA250 driver interrupt service routine. * * This is the common interrupt service routine for all enabled BMA250 interrupt * events. Five different types of interrupts can be programmed. All interrupt * criteria are combined and drive the interrupt pad with a Boolean \c OR * condition. * * Interrupt criteria are tested against values from the BMA250 digital filter * output. All thresholds are scaled using the current device range. Timings * for high and low acceleration are absolute values (1 LSB of HG_dur and LG_dur * registers corresponds to 1 millisecond, +/- 10%). Timing for the any-motion * interrupt and alert detection are proportional to the bandwidth setting. * * This routine handles interrupts generated when low-g, high-g, any-motion, * alert, and new data criteria are satisfied and the corresponding event * notification is enabled in the device. * * The BMA250 device does not provide any way to definitively identify an * any-motion interrupt once it has occurred. So, if a handler has been * installed for that event, it will always be called by this routine, * and the SENSOR_EVENT_MOTION indicator will be set in the event type field. * * @param arg The address of the driver sensor_hal_t descriptor. * @return Nothing. */ static void bma250_isr(volatile void *arg) { sensor_hal_t *const hal = (sensor_hal_t *)arg; hal->bus.no_wait = true; sensor_bus_read(hal, hal->burst_addr, &event_regs, sizeof(event_regs)); hal->bus.no_wait = false; if (STATUS_OK == hal->bus.status) { sensor_event_data_t event_data = {.data.scaled = true}; event_data.data.timestamp = sensor_timestamp(); event_data.event = SENSOR_EVENT_UNKNOWN; format_axis_data(hal, event_regs.acc, &(event_data.data)); if (event_regs.status_field.data_int) { event_data.event |= SENSOR_EVENT_NEW_DATA; (event_cb[0].handler)(&event_data, event_cb[0].arg); } if (event_regs.status_field.slope_int) { event_data.event |= SENSOR_EVENT_MOTION; (event_cb[1].handler)(&event_data, event_cb[1].arg); } if (event_regs.status_field.low_int) { event_data.event |= SENSOR_EVENT_LOW_G; (event_cb[2].handler)(&event_data, event_cb[2].arg); } if (event_regs.status_field.high_int) { event_data.event |= SENSOR_EVENT_HIGH_G; (event_cb[3].handler)(&event_data, event_cb[3].arg); } if (event_regs.status_field.s_tap_int) { event_data.event |= SENSOR_EVENT_S_TAP; (event_cb[4].handler)(&event_data, event_cb[4].arg); } if (event_regs.status_field.d_tap_int) { event_data.event |= SENSOR_EVENT_D_TAP; (event_cb[4].handler)(&event_data, event_cb[4].arg); } } }
/** * @brief Bosch BMA150 accelerometer driver initialization. * * This is the main initialization function for the BMA150 device. * * @param sensor Address of a sensor device descriptor. * @param resvd Reserved value. * @return bool true if the call succeeds, else false is returned. */ bool bma150_init(sensor_t *sensor, int resvd) { bool status = false; sensor_hal_t *const hal = sensor->hal; if (BMA150_ID_VAL == sensor_bus_get(hal, BMA150_CHIP_ID)) { /* Set the driver function table and capabilities pointer. */ static const sensor_device_t bma150_device = { .func.read = bma150_read, .func.ioctl = bma150_ioctl, .func.selftest = bma150_selftest, .func.event = bma150_event, .caps.feature = SENSOR_CAPS_3_AXIS | SENSOR_CAPS_SELFTEST | SENSOR_CAPS_HI_G_EVENT | SENSOR_CAPS_LO_G_EVENT | SENSOR_CAPS_AUX_TEMP, .caps.vendor = SENSOR_VENDOR_BOSCH, .caps.range_table = range_table, .caps.band_table = band_table, .caps.range_count = ARRAYSIZE(range_table), .caps.band_count = ARRAYSIZE(band_table), .caps.units = SENSOR_UNITS_g0, .caps.scale = SENSOR_SCALE_milli, .caps.name = "BMA150 Digital, triaxial acceleration sensor" }; sensor->drv = &bma150_device; /* Set the driver (device) default range, bandwidth, & * resolution. * * Per the BMA150 Datasheet the default range and bandwidth * after device reset are +/- 4g and 1500 Hz. respectively. So, * if that's the desired initialization state for this device, *then * don't use software to set these values in the driver _init() * routine. */ hal->range = 4000; hal->bandwidth = 1500; hal->resolution = BMA150_DATA_RESOLUTION; /* Set the device burst read base address. */ hal->burst_addr = BMA150_ACC_X_LSB; /* Initialize the control registers. */ sensor_bus_put(hal, BMA150_CTRL1, 0); sensor_bus_put(hal, BMA150_CTRL2, 0); sensor_bus_put(hal, BMA150_CTRL5, 0); /* Set the interrupt handler. */ if ((STATUS_OK == hal->bus.status) && sensor_irq_connect(hal->mcu_sigint, bma150_isr, hal)) { status = true; } } return status; } /** * @brief Bosch BMA150 driver interrupt service routine. * * This is the common interrupt service routine for all enabled BMA150 interrupt * events. Five different types of interrupts can be programmed. All interrupt * criteria are combined and drive the interrupt pad with a Boolean \c OR * condition. * * Interrupt events may be set to an inconsistent state by device EEPROM * changes. BMA150 interrupts should be disabled at the host microcontroller * when device EEPROM writes are performed. * * Interrupt criteria are tested against values from the BMA150 digital filter * output. All thresholds are scaled using the current device range. Timings * for high and low acceleration are absolute values (1 LSB of HG_dur and LG_dur * registers corresponds to 1 millisecond, +/- 10%). Timing for the any-motion * interrupt and alert detection are proportional to the bandwidth setting. * * This routine handles interrupts generated when low-g, high-g, any-motion, * alert, and new data criteria are satisfied and the corresponding event * notification is enabled in the device. * * The BMA150 device does not provide any way to definitively identify an * any-motion interrupt once it has occurred. So, if a handler has been * installed for that event, it will always be called by this routine, * and the SENSOR_EVENT_MOTION indicator will be set in the event type field. * * @param arg The address of the driver sensor_hal_t descriptor. * @return Nothing. */ static void bma150_isr(volatile void *arg) { sensor_hal_t *const hal = (sensor_hal_t *)arg; hal->bus.no_wait = true; sensor_bus_read(hal, hal->burst_addr, &event_regs, sizeof(event_regs)); hal->bus.no_wait = false; if (STATUS_OK == hal->bus.status) { static sensor_event_data_t event_data = {.data.scaled = true}; event_data.data.timestamp = sensor_timestamp(); event_data.event = SENSOR_EVENT_MOTION; /* * Test the "new data" bit in the sample outputs prior to * converting axis data to sign extended 10-bit values and * buffering the result. */ bool const new_data = event_regs.acc[2].field.new_data; format_axis_data(hal, event_regs.acc, &(event_data.data)); if (event_regs.status_field.LG_issued) { event_data.event = SENSOR_EVENT_LOW_G; (event_cb[2].handler)(&event_data, event_cb[2].arg); } else if (event_regs.status_field.HG_issued) { event_data.event = SENSOR_EVENT_HIGH_G; (event_cb[3].handler)(&event_data, event_cb[3].arg); } else if (new_data) { event_data.event = SENSOR_EVENT_NEW_DATA; (event_cb[0].handler)(&event_data, event_cb[0].arg); } else { /* Assume the any-motion event triggered the interrupt. */ (event_cb[1].handler)(&event_data, event_cb[1].arg); } } } /** * @brief Read BMA150 device ID and revision numbers. * * This function reads the accelerometer hardware identification registers * and returns these values in the specified data structure. * * @param hal Address of an initialized sensor hardware descriptor. * @param data Address of sensor_data_t structure to return values. * @return bool true if the call succeeds, else false is returned. */ static bool bma150_device_id(sensor_hal_t *hal, sensor_data_t *data) { data->device.id = sensor_bus_get(hal, BMA150_CHIP_ID); data->device.version = sensor_bus_get(hal, BMA150_CHIP_VERSION); return true; }
/** * @brief Osram SFH7770 light & proximity sensor driver initialization. * * This is the main initialization function for the SFH7770 device. * * @param sensor Address of a sensor device descriptor. * @param resvd Reserved value. * @return bool true if the call succeeds, else false is returned. */ bool sfh7770_init(sensor_t *sensor, int resvd) { bool status = false; sensor_hal_t *const hal = sensor->hal; /* Proximity threshold values from NVRAM */ struct { uint8_t ps_thr_led1; uint8_t ps_thr_led2; uint8_t ps_thr_led3; } prox_thresholds; /* Read and check part ID register */ uint8_t part_id = sensor_bus_get(hal, SFH7770_PART_ID); if (part_id == (SFH7770_PART_ID_VAL | SFH7770_PART_REV_VAL)) { /* Set the driver function table and capabilities pointer. */ static const sensor_device_t sfh7770_device = { .func.read = sfh7770_read, .func.ioctl = sfh7770_ioctl, .func.calibrate = sfh7770_calibrate, .func.event = sfh7770_event, #if 0 .caps.feature = XXX #endif .caps.vendor = SENSOR_VENDOR_OSRAM, .caps.units = SENSOR_UNITS_lux, .caps.scale = SENSOR_SCALE_one, .caps.name = "SFH7770 Ambient Light & Proximity Sensor" }; sensor->drv = &sfh7770_device; hal->resolution = SFH7770_DATA_RESOLUTION; /* Set the device burst read starting register address. */ hal->burst_addr = SFH7770_ALS_DATA_LSB; /* Reset device during first init call */ if (!sfh7770_initialized) { sensor_bus_put(hal, SFH7770_ALS_CONTROL, ALS_CONTROL_SW_RESET); } /* Init light sensor functions if specified */ if (sensor->type & SENSOR_TYPE_LIGHT) { /* Set light sensor mode & interval */ sensor_bus_put(hal, SFH7770_ALS_CONTROL, ALS_MODE_FREE_RUNNING); sensor_bus_put(hal, SFH7770_ALS_INTERVAL, ALS_INTERVAL_500MS); } /* Init proximity sensor functions if specified */ if (sensor->type & SENSOR_TYPE_PROXIMITY) { /* Set proximity sensor mode & interval */ sensor_bus_put(hal, SFH7770_PS_CONTROL, PS_MODE_FREE_RUNNING); sensor_bus_put(hal, SFH7770_PS_INTERVAL, PS_INTERVAL_100MS); /* Specify which LEDs are active */ uint8_t const active_leds = LED_ACTIVE_ALL; /* XXX one of: */ /* XXX LED_ACTIVE_1 - LED1 only (default) */ /* XXX LED_ACTIVE_1_2 - LED1 & LED2 */ /* XXX LED_ACTIVE_1_3 - LED1 & LED3 */ /* XXX LED_ACTIVE_ALL - all LEDs */ /* Set LED current levels */ uint8_t const led1_curr = I_LED_50MA; /* LED1 current */ uint8_t const led2_curr = I_LED_50MA; /* LED2 current */ uint8_t const led3_curr = I_LED_50MA; /* LED3 current */ sensor_bus_put(hal, SFH7770_I_LED_1_2, (active_leds | (led2_curr << I_LED2_SHIFT) | led1_curr)); sensor_bus_put(hal, SFH7770_I_LED_3, led3_curr); /* Apply stored proximity thresholds from nvram */ nvram_read(SFH7770_NVRAM_OFFSET, &prox_thresholds, sizeof(prox_thresholds)); sensor_bus_write(hal, (SFH7770_PS_THR_LED1), &prox_thresholds, sizeof(prox_thresholds)); } if (!sfh7770_initialized) { /* Set interrupt output polarity & mode(active-low, * latched). */ sensor_bus_put(hal, SFH7770_INT_SET, 0); /* Set up interrupt handler */ if (STATUS_OK == hal->bus.status) { sensor_irq_connect(hal->mcu_sigint, sfh7770_isr, hal); } } sfh7770_initialized = true; status = true; } return status; } /** * @brief Osram SFH7770 driver interrupt service routine. * * This is the common interrupt service routine for all enabled SFH7770 * interrupt events. Three different types of interrupts can be programmed: * high light level, low light level, and near proximity. All share the * same interrupt pin and therefore the same ISR entry. * * @param arg The address of the driver sensor_hal_t descriptor. * @return Nothing. */ static void sfh7770_isr(volatile void *arg) { sensor_hal_t *const hal = (sensor_hal_t *)arg; struct { /* Interrupt register data */ uint8_t als_data_lsb; /* light meas data - least signif byte */ uint8_t als_data_msb; /* light meas data - most signif byte */ uint8_t als_ps_status; /* light & prox sensor status */ uint8_t ps_data_led1; /* proximity meas data - LED 1 */ uint8_t ps_data_led2; /* proximity meas data - LED 2 */ uint8_t ps_data_led3; /* proximity meas data - LED 3 */ uint8_t int_set; /* interrupt status */ } regs; /* Do not wait for a busy bus when reading data. */ hal->bus.no_wait = true; sensor_bus_read(hal, hal->burst_addr, (uint8_t *)®s, sizeof(regs)); hal->bus.no_wait = false; if (STATUS_OK == hal->bus.status) { static sensor_event_data_t event_data = {.data.scaled = true}; event_data.data.timestamp = sensor_timestamp(); event_data.event = SENSOR_EVENT_UNKNOWN; /* * Determine the interrupt source then combine measurement * register values into a single 16-bit measurement value. */ uint8_t const int_source = (regs.int_set & INT_SOURCE_MASK); uint16_t const light_level = ((regs.als_data_msb << 8) | regs.als_data_lsb); switch (int_source) { case INT_SOURCE_ALS: /* Determine if low or high light interrupt */ if (light_level >= high_light_threshold) { event_data.event = SENSOR_EVENT_HIGH_LIGHT; event_data.data.light.value = light_level; (event_cb[2].handler)(&event_data, event_cb[2].arg); } else if (light_level <= low_light_threshold) { event_data.event = SENSOR_EVENT_LOW_LIGHT; event_data.data.light.value = light_level; (event_cb[1].handler)(&event_data, event_cb[1].arg); } return; case INT_SOURCE_LED1: case INT_SOURCE_LED2: case INT_SOURCE_LED3: event_data.event = SENSOR_EVENT_NEAR_PROXIMITY; if (int_source == INT_SOURCE_LED1) { event_data.channel = 1; } else if (int_source == INT_SOURCE_LED2) { event_data.channel = 2; } else { /* INT_SOURCE_LED3 */ event_data.channel = 3; } /* Use internal device threshold status to * determine scaled values. */ event_data.data.proximity.value[0] = (regs.als_ps_status & PS_LED1_THRESH) ? PROXIMITY_NEAR : PROXIMITY_NONE; event_data.data.proximity.value[1] = (regs.als_ps_status & PS_LED2_THRESH) ? PROXIMITY_NEAR : PROXIMITY_NONE; event_data.data.proximity.value[2] = (regs.als_ps_status & PS_LED3_THRESH) ? PROXIMITY_NEAR : PROXIMITY_NONE; (event_cb[0].handler)(&event_data, event_cb[0].arg); } } } /** * @brief Read SFH7770 device ID and revision numbers. * * This function reads the sensor hardware identification registers * and returns these values in the specified data structure. * * @param hal Address of an initialized sensor hardware descriptor. * @param data Address of sensor_data_t structure to return values. * @return bool true if the call succeeds, else false is returned. */ static bool sfh7770_device_id(sensor_hal_t *hal, sensor_data_t *data) { uint8_t const part_id = sensor_bus_get(hal, SFH7770_PART_ID); data->device.id = (uint32_t)(part_id & PART_ID_MASK) >> PART_ID_SHIFT; data->device.version = (uint32_t)(part_id & PART_REV_MASK); return true; }
/** * @brief AKM AK8975 magnetometer driver initialization. * * This is the main initialization function for the AK8975 device. * * @param sensor Address of a sensor device descriptor. * @param resvd Reserved value. * @return bool true if the call succeeds, else false is returned. */ bool ak8975_init(sensor_t *sensor, int resvd) { sensor_hal_t *const hal = sensor->hal; /* Set the driver function table and capabilities pointer. */ static const sensor_device_t ak8975_device = { .func.read = ak8975_read, .func.ioctl = ak8975_ioctl, .func.selftest = ak8975_selftest, .func.calibrate = ak8975_calibrate, .caps.feature = SENSOR_CAPS_3_AXIS | SENSOR_CAPS_SELFTEST, .caps.vendor = SENSOR_VENDOR_AKM, .caps.units = SENSOR_UNITS_tesla, .caps.scale = SENSOR_SCALE_micro, .caps.name = "AK8975 Digital, triaxial compass/magnetometer" }; sensor->drv = &ak8975_device; /* Set the driver default, range, bandwidth, resolution, &c. */ hal->resolution = AK8975_DATA_RESOLUTION; /* Initialize calibration data from NVRAM storage. */ nvram_read(0 /* + NVRAM_BASE_ADDR */, &calibrated_offsets, sizeof(calibrated_offsets)); /* Clear all calibration values if any are invalid */ #if defined(MATH_FIXED_POINT) if ((calibrated_offsets.x == -1) || (calibrated_offsets.y == -1) || (calibrated_offsets.z == -1)) { #else if (isnan(calibrated_offsets.x) || isnan(calibrated_offsets.y) || isnan(calibrated_offsets.z)) { #endif memset(&calibrated_offsets, 0x00, sizeof(calibrated_offsets)); } return (AK8975_WIA_VALUE == sensor_bus_get(hal, AK8975_REG_WIA)); } /** * @brief Read magnetometer device ID * * This function reads the magnetometer hardware identification registers * and returns these values to the addresses specified in the function * parameters. * * @param hal Address of an initialized sensor hardware descriptor. * @param data Address of sensor_data_t structure to return values. * @return bool true if the call succeeds, else false is returned. */ static bool ak8975_device_id(sensor_hal_t *hal, sensor_data_t *data) { data->device.id = (uint32_t)sensor_bus_get(hal, AK8975_REG_WIA); data->device.version = 0; return true; } /** * @ brief Test for magnetic sensor overflow * * AK8975/B has the limitation for measurement range that the sum of the * absolute values of each axis should be smaller than 2400 microtesla: * * |x| + |y| + |z| < 2400 uT * * When the magnetic field exceeds this limitation, data stored at measurement * data are not correct. This is called "Magnetic Sensor Overflow". * * When magnetic sensor overflow occurs, HOFL but turns to "1". When the next * measurement starts, it returns to "0". * * @return bool true if Magnetic Sensor Overflow criteria are satisfied. */ static inline bool ak8975_check_overflow(const vector3_t *data) { static scalar_t const MEASURE_MAX = (1 << (AK8975_DATA_RESOLUTION - 1)) / MICRO_TESLA_PER_COUNT; /* \todo Use register instead of calculating overflow? * return (AK8975_HOFL & sensor_bus_get(hal, AK8975_REG_ST2)); */ return ((abs(data->x) + abs(data->y) + abs(data->z)) >= MEASURE_MAX); } /** * @brief Read magnetometer vector data. * * This function obtains magnetometer data for all three axes of the AKM * device. The data is read from six device registers using a multi-byte * bus transfer. The 13-bit raw results are then assembled from the two * register values for each axis, including extending the sign bit, to * form a signed 16-bit value. * * @param hal Address of an initialized sensor hardware descriptor. * @param data The address of a vector storing sensor axis data. * @return bool true if the call succeeds, else false is returned. */ static bool ak8975_get_data(sensor_hal_t *hal, uint8_t mode, vector3_t *data) { /* Put sensor in single measurement or self-test mode */ sensor_bus_put(hal, AK8975_REG_CNTL1, mode); /* Wait for the data ready signal. */ do { /* In single measurement or self-test mode, when measurement * data is stored and ready to be read, DRDY bit in ST1 register * turns to "1". This is called "Data Ready". DRDY pin is in the * same state as DRDY bit. * * Instead of polling DRDY, as is done below, this signal out of * the sensor and into the microcontroller can be used to * trigger * an asynchronous interrupt. */ } while (gpio_pin_is_low(hal->mcu_sigint /* DRDY input */)); /* Get measurement data (consecutive little endian byte pairs: x, y, z). */ struct { uint8_t lsb; uint8_t msb; } input[3]; size_t const count = sensor_bus_read(hal, AK8975_REG_HXL, input, sizeof(input)); if (count != sizeof(input)) { return false; } /* Convert to a real data (non-integer) type for calculations. */ data->x = hal->orientation.x.sign * ((int16_t)((input[hal->orientation.x.axis].msb << 8) + input[hal->orientation.x.axis].lsb)); data->y = hal->orientation.y.sign * ((int16_t)((input[hal->orientation.y.axis].msb << 8) + input[hal->orientation.y.axis].lsb)); data->z = hal->orientation.z.sign * ((int16_t)((input[hal->orientation.z.axis].msb << 8) + input[hal->orientation.z.axis].lsb)); return true; }