Commit 93c69287 authored by Alex Wang Fu's avatar Alex Wang Fu
Browse files

i2c works

parent 5ec52292
#include "ports/atmel-samd/common-hal/motor_control/Accel.h"
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include "ports/atmel-samd/common-hal/motor_control/Accel.h"
extern void common_hal_time_delay_ms(uint32_t);
/*
Input: spi_sensor_t object
// Accelerometer value read registers
#define _REG_OUT_X_L 0x28
#define _REG_OUT_X_H 0x29
#define _REG_OUT_Y_L 0x2A
#define _REG_OUT_Y_H 0x2B
#define _REG_OUT_Z_L 0x2C
#define _REG_OUT_Z_H 0x2D
#define _REG_OUTADC1_L 0x08
#define _REG_WHOAMI 0x0F
#define _REG_TEMPCFG 0x1F
#define _REG_CTRL1 0x20
#define _REG_CTRL3 0x22
#define _REG_CTRL4 0x23
#define _REG_CTRL5 0x24
#define _REG_INT1SRC 0x31
#define _REG_CLICKCFG 0x38
#define _REG_CLICKSRC 0x39
#define _REG_CLICKTHS 0x3A
#define _REG_TIMELIMIT 0x3B
#define _REG_TIMELATENCY 0x3C
#define _REG_TIMEWINDOW 0x3D
#define RANGE_16_G 0x03 // +/- 16g
#define RANGE_8_G 0x02 // +/- 8g
#define RANGE_4_G 0x01 // +/- 4g
#define RANGE_2_G 0x00 // +/- 2g (default value)
#define DATARATE_1344_HZ 0x09 // 1.344 KHz
#define DATARATE_400_HZ 0x07 // 400Hz
#define DATARATE_200_HZ 0x06 // 200Hz
#define DATARATE_100_HZ 0x05 // 100Hz
#define DATARATE_50_HZ 0x04 // 50Hz
#define DATARATE_25_HZ 0x03 // 25Hz
#define DATARATE_10_HZ 0x02 // 10 Hz
#define DATARATE_1_HZ 0x01 // 1 Hz
#define DATARATE_POWERDOWN 0
#define DATARATE_LOWPOWER_1K6HZ 0x1000
#define DATARATE_LOWPOWER_5KHZ 0x1001
#define STANDARD_GRAVITY 9.806
#define LEFT_WING_I2CADDR 0x18
#define RIGHT_WING_I2CADDR 0x19
typedef struct {
i2c_sensor_t i2c_sensor;
float divider;
} i2c_accel_sensor_t;
i2c_accel_sensor_t left_wing;
i2c_accel_sensor_t right_wing;
void i2c_accel_init(i2c_accel_sensor_t* sensor, i2c_sensor_t i2c_sensor) {
sensor->i2c_sensor = i2c_sensor;
/*
* Corresponds to `accel = adafruit_lis3dh.LIS3DH_I2C(i2c)` which goes into
* `class LIS3DH_I2C(LIS3DH)`'s `__init__` which will eventually
* call `__init__` in `i2c_device.py`
*
* All init does is update object state and call `__probe_for_device()`
*
* `__probe_for_device()` just calls `busio_i2c_writeto`, which is a wrapper
* for `writeto` in `shared-bindings/busio/I2C.c` which calls
* `common_hal_busio_i2c_write` (in common-hal/busio/I2C.c) with an empty
* buffer.
*/
uint8_t trash;
common_hal_busio_i2c_write(i2c_sensor.i2c, i2c_sensor.addr, &trash, 0, true);
/*
* Next, we enter the constructor for `LIS3DH` in `adafruit_lis3dh.py`
*/
uint8_t device_id = i2c_read_register_byte(i2c_sensor, _REG_WHOAMI);
common_hal_time_delay_ms(1);
// Check device ID
if(device_id != 0x33) {
printf("Failed to find LIS3DH!\n");
return;
}
// Reboot
i2c_write_register_byte(i2c_sensor, _REG_CTRL5, 0x80);
// Take 5 ms
common_hal_time_delay_ms(5);
// Enable all axes, normal mode
i2c_write_register_byte(i2c_sensor, _REG_CTRL1, 0x07);
common_hal_time_delay_ms(1);
// Set 400Hz data rate
uint8_t ctl1 = i2c_read_register_byte(i2c_sensor, _REG_CTRL1);
common_hal_time_delay_ms(1);
ctl1 &= ~(0xF0);
ctl1 |= (DATARATE_400_HZ << 4);
i2c_write_register_byte(i2c_sensor, _REG_CTRL1, ctl1);
common_hal_time_delay_ms(1);
// High res & BDU enabled.
i2c_write_register_byte(i2c_sensor, _REG_CTRL4, 0x88);
common_hal_time_delay_ms(1);
// Enable ADCs.
i2c_write_register_byte(i2c_sensor, _REG_TEMPCFG, 0x80);
common_hal_time_delay_ms(1);
// Latch interrupt for INT1.
i2c_write_register_byte(i2c_sensor, _REG_CTRL5, 0x08);
common_hal_time_delay_ms(1);
// Get range
uint8_t range = i2c_read_register_byte(i2c_sensor, _REG_CTRL4);
range = (range >> 4) & 0x03;
switch(range) {
case RANGE_16_G:
sensor->divider = 1365;
break;
case RANGE_8_G:
sensor->divider = 4096;
break;
case RANGE_4_G:
sensor->divider = 8190;
break;
case RANGE_2_G:
sensor->divider = 16380;
break;
}
}
void init_accels(void) {
i2c_sensor_t left_i2c_sensor = i2c_sensor_create(LEFT_WING_I2CADDR);
i2c_sensor_t right_i2c_sensor = i2c_sensor_create(RIGHT_WING_I2CADDR);
i2c_accel_init(&left_wing, left_i2c_sensor);
i2c_accel_init(&right_wing, right_i2c_sensor);
}
/*
Input: spi_sensor_t object
*/
acceleration read_bird_accelerometer(spi_sensor_t *device) {
uint16_t divider = 1;
uint8_t accel_range = get_spi_range(device);
acceleration accel;
switch(accel_range) {
case RANGE_16_G:
divider = 1365;
break;
case RANGE_8_G:
divider = 4096;
break;
case RANGE_4_G:
divider = 8190;
break;
case RANGE_2_G:
divider = 16380;
break;
}
uint16_t divider = 1;
uint8_t accel_range = get_spi_range(device);
acceleration accel;
switch (accel_range) {
case RANGE_16_G:
divider = 1365;
break;
case RANGE_8_G:
divider = 4096;
break;
case RANGE_4_G:
divider = 8190;
break;
case RANGE_2_G:
divider = 16380;
break;
}
// Values are stored in two registers as left-justified two's complement
// read into int16_t as these are stored as two's complement
// read high and low bytes, and shift right 4 (12b high-resolution mode)
int16_t raw_x = read_spi_byte(device, _REG_OUT_X_H | 0x80);
raw_x <<= 8;
raw_x |= read_spi_byte(device, _REG_OUT_X_L | 0x80);
// Values are stored in two registers as left-justified two's complement
// read into int16_t as these are stored as two's complement
// read high and low bytes, and shift right 4 (12b high-resolution mode)
int16_t raw_x = read_spi_byte(device, _REG_OUT_X_H | 0x80);
raw_x <<= 8;
raw_x |= read_spi_byte(device, _REG_OUT_X_L | 0x80);
int16_t raw_y = read_spi_byte(device, _REG_OUT_Y_H | 0x80);
raw_y <<= 8;
raw_y |= read_spi_byte(device, _REG_OUT_Y_L | 0x80);
int16_t raw_y = read_spi_byte(device, _REG_OUT_Y_H | 0x80);
raw_y <<= 8;
raw_y |= read_spi_byte(device, _REG_OUT_Y_L | 0x80);
int16_t raw_z = read_spi_byte(device, _REG_OUT_Z_H | 0x80);
raw_z <<= 8;
raw_z |= read_spi_byte(device, _REG_OUT_Z_L | 0x80);
int16_t raw_z = read_spi_byte(device, _REG_OUT_Z_H | 0x80);
raw_z <<= 8;
raw_z |= read_spi_byte(device, _REG_OUT_Z_L | 0x80);
// convert from Gs to m / s ^ 2 and adjust for the range
accel.x = ((float)raw_x / divider) * STANDARD_GRAVITY;
accel.y = ((float)raw_y / divider) * STANDARD_GRAVITY;
accel.z = ((float)raw_z / divider) * STANDARD_GRAVITY;
// convert from Gs to m / s ^ 2 and adjust for the range
accel.x = ((float)raw_x / divider) * STANDARD_GRAVITY;
accel.y = ((float)raw_y / divider) * STANDARD_GRAVITY;
accel.z = ((float)raw_z / divider) * STANDARD_GRAVITY;
return accel;
return accel;
}
acceleration read_i2c_accelerometer(i2c_sensor_t *device){
uint16_t divider = 1;
uint8_t accel_range = get_i2c_range(device);
acceleration accel;
switch(accel_range) {
case RANGE_16_G:
divider = 1365;
break;
case RANGE_8_G:
divider = 4096;
break;
case RANGE_4_G:
divider = 8190;
break;
case RANGE_2_G:
divider = 16380;
break;
}
void reset_i2c_accelerometer(i2c_accel_sensor_t device) {
i2c_write_register_byte(device.i2c_sensor, _REG_CTRL5, 0x80);
// Values are stored in two registers as left-justified two's complement
// read into int16_t as these are stored as two's complement
// read high and low bytes, and shift right 4 (12b high-resolution mode)
int16_t raw_x = read_i2c_byte(device, _REG_OUT_X_H | 0x80);
raw_x <<= 8;
raw_x |= read_i2c_byte(device, _REG_OUT_X_L | 0x80);
common_hal_time_delay_ms(5);
}
acceleration read_i2c_accelerometer(i2c_accel_sensor_t device) {
/*
* Logic for this comes from `def acceleration(self):` in `adafruit_lis3dh.py`
*/
acceleration accel;
int16_t raw_y = read_i2c_byte(device, _REG_OUT_Y_H | 0x80);
raw_y <<= 8;
raw_y |= read_i2c_byte(device, _REG_OUT_Y_L | 0x80);
float divider = device.divider;
uint8_t buf[6];
i2c_read_register(device.i2c_sensor, _REG_OUT_X_L | 0x80, buf, 6);
int16_t raw_z = read_i2c_byte(device, _REG_OUT_Z_H | 0x80);
raw_z <<= 8;
raw_z |= read_i2c_byte(device, _REG_OUT_Z_L | 0x80);
int16_t raw_x = buf[0] | (buf[1] << 8);
int16_t raw_y = buf[2] | (buf[3] << 8);
int16_t raw_z = buf[4] | (buf[5] << 8);
// x, y, z = struct.unpack("<hhh", self._read_register(_REG_OUT_X_L | 0x80, 6));
printf("divider: %f, raw x: %d raw y: %d raw z: %d \n", (double) divider, raw_x, raw_y, raw_z);
// convert from Gs to m / s ^ 2 and adjust for the range
accel.x = ((float)raw_x / divider) * STANDARD_GRAVITY;
accel.y = ((float)raw_y / divider) * STANDARD_GRAVITY;
accel.z = ((float)raw_z / divider) * STANDARD_GRAVITY;
// convert from Gs to m / s ^ 2 and adjust for the range
accel.x = ((float)raw_x / divider) * STANDARD_GRAVITY;
accel.y = ((float)raw_y / divider) * STANDARD_GRAVITY;
accel.z = ((float)raw_z / divider) * STANDARD_GRAVITY;
return accel;
return accel;
}
/*
*/
acceleration get_raw_accel(ACCEL_TYPE side){
acceleration accel;
if (side == CENTER){
spi_sensor_t device = get_bird_sensor();
accel = read_bird_accelerometer(&device);
}
else if(side==RIGHTWING){
i2c_sensor_t wing_device = get_wing_sensor(RIGHT);
accel = read_i2c_accelerometer(&wing_device);
}
else if (side==LEFTWING) {
i2c_sensor_t wing_device = get_wing_sensor(LEFT);
accel = read_i2c_accelerometer(&wing_device);
}
else
printf("Invalid accelerometer choice. Please select LEFTWING(0), CENTER(1) or RIGHTWING(2)");
return accel;
*/
acceleration get_raw_accel(ACCEL_TYPE side) {
acceleration accel;
if (side == CENTER) {
spi_sensor_t device = get_bird_sensor();
accel = read_bird_accelerometer(&device);
} else if (side == RIGHTWING) {
accel = read_i2c_accelerometer(right_wing);
} else if (side == LEFTWING) {
accel = read_i2c_accelerometer(left_wing);
} else
printf("Invalid accelerometer choice. Please select LEFTWING(0), CENTER(1) "
"or RIGHTWING(2)");
return accel;
}
void accel_reset(ACCEL_TYPE side){
if (side == CENTER){
void accel_reset(ACCEL_TYPE side) {
if (side == CENTER) {
spi_sensor_t device = get_bird_sensor();
reset_bird_accel(&device);
}
/*else if(side==RIGHTWING){
i2c_sensor_t wing_device = get_wing_sensor(RIGHT);
reset_wing_accel(&wing_device);
}
else if (side==LEFTWING) {
i2c_sensor_t wing_device = get_wing_sensor(LEFT);
reset_wing_accel(&wing_device);
}*/
else if(side==RIGHTWING){
reset_i2c_accelerometer(left_wing);
}
else if (side==LEFTWING) {
reset_i2c_accelerometer(left_wing);
}
}
void test_spi_reset(void){
for(uint8_t i = 0; i<1000; i++){
if(!i/3){
void test_spi_reset(void) {
for (uint8_t i = 0; i < 1000; i++) {
if (!i / 3) {
printf("Resetting device: \n");
accel_reset(CENTER);
acceleration postReset = get_raw_accel(CENTER);
printf("X: %f, Y: %f, Z: %f \n", (double)postReset.x, (double)postReset.y, (double)postReset.z);
printf("X: %f, Y: %f, Z: %f \n", (double)postReset.x, (double)postReset.y,
(double)postReset.z);
}
acceleration aOut = get_raw_accel(CENTER);
printf("X: %f, Y: %f, Z: %f \n", (double)aOut.x, (double)aOut.y, (double)aOut.z);
printf("X: %f, Y: %f, Z: %f \n", (double)aOut.x, (double)aOut.y,
(double)aOut.z);
}
}
void test_spi_reading(void){
while(1){
void test_spi_reading(void) {
while (1) {
acceleration aOut = get_raw_accel(CENTER);
printf("X: %f, Y: %f, Z: %f \n", (double)aOut.x, (double)aOut.y, (double)aOut.z);
printf("X: %f, Y: %f, Z: %f \n", (double)aOut.x, (double)aOut.y,
(double)aOut.z);
common_hal_time_delay_ms(5);
}
}
\ No newline at end of file
......@@ -9,27 +9,10 @@
#include <stdlib.h>
#include <math.h>
// Accelerometer value read registers
#define _REG_OUT_X_L 0x28
#define _REG_OUT_X_H 0x29
#define _REG_OUT_Y_L 0x2A
#define _REG_OUT_Y_H 0x2B
#define _REG_OUT_Z_L 0x2C
#define _REG_OUT_Z_H 0x2D
// Accelerometer range possibilities
#define RANGE_16_G 0x03 // +/- 16g
#define RANGE_8_G 0x02 // +/- 8g
#define RANGE_4_G 0x01 // +/- 4g
#define RANGE_2_G 0x00 // +/- 2g (default value)
#define LEFTWING 0
#define CENTER 1
#define RIGHTWING 2
// Relevant constants
#define STANDARD_GRAVITY 9.806
typedef struct {
float x;
float y;
......@@ -40,6 +23,8 @@ typedef uint8_t ACCEL_TYPE;
acceleration get_raw_accel (uint8_t);
void accel_reset(uint8_t);
void init_accels(void);
void test_spi_reading(void);
void test_spi_reset(void);
......
#include "ports/atmel-samd/common-hal/motor_control/I2C.h"
#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>
#include <math.h>
// Register addresses:
#define _REG_OUTADC1_L 0x08
#define _REG_WHOAMI 0x0F
#define _REG_TEMPCFG 0x1F
#define _REG_CTRL1 0x20
#define _REG_CTRL3 0x22
#define _REG_CTRL4 0x23
#define _REG_CTRL5 0x24
#define _REG_INT1SRC 0x31
#define _REG_CLICKCFG 0x38
#define _REG_CLICKSRC 0x39
#define _REG_CLICKTHS 0x3A
#define _REG_TIMELIMIT 0x3B
#define _REG_TIMELATENCY 0x3C
#define _REG_TIMEWINDOW 0x3D
// Register value constants:
#define RANGE_16_G 0x03 // +/- 16g
#define RANGE_8_G 0x02 // +/- 8g
#define RANGE_4_G 0x01 // +/- 4g
#define RANGE_2_G 0x00 // +/- 2g (default value)
#define DATARATE_1344_HZ 0x09 // 1.344 KHz
#define DATARATE_400_HZ 0x07 // 400Hz
#define DATARATE_200_HZ 0x06 // 200Hz
#define DATARATE_100_HZ 0x05 // 100Hz
#define DATARATE_50_HZ 0x04 // 50Hz
#define DATARATE_25_HZ 0x03 // 25Hz
#define DATARATE_10_HZ 0x02 // 10 Hz
#define DATARATE_1_HZ 0x01 // 1 Hz
#define DATARATE_POWERDOWN 0
#define DATARATE_LOWPOWER_1K6HZ 0x1000
#define DATARATE_LOWPOWER_5KHZ 0x1001
// I2C Control Pins
#define CS_LEFT &pin_PA18
#define CS_RIGHT &pin_PA16
#define I2C_SCL (&pin_PA13)
#define I2C_SDA (&pin_PA12)
#define STOP 1
#define NO_STOP 0
i2c_sensor_t sensor_right;
i2c_sensor_t sensor_left;
void init_i2c(i2c_sensor_t *device, uint8_t device_addr){
// init i2c bus using busio library
uint32_t frequency = 100000;
uint8_t timeout = 0; // TODO: determine appropriate value for this
common_hal_busio_i2c_construct(&device->bus, I2C_SCL, I2C_SDA, frequency, timeout);
// construct i2c devices from shared-bindings/adafruit_bus_device/I2C_Device
// TODO: how to determine device address from generalized info??
common_hal_adafruit_bus_device_i2cdevice_construct(&device->sensor, &device->bus, device_addr);
#include "I2C.h"
#include "shared-bindings/busio/I2C.h"
#include "peripherals/samd/pins.h"
#define I2CBAUD 100000
#define I2CTIMEOUT 255
extern void common_hal_time_delay_ms(uint32_t);
static const mcu_pin_obj_t *sda_pin = &pin_PA12;
static const mcu_pin_obj_t *scl_pin = &pin_PA13;
static bool i2c_initted = false;
static busio_i2c_obj_t i2c;
static void i2c_init(void) {
if(i2c_initted) {
return;
}
/*
* Corresponds to `i2c = busio.I2C(board.SCL, board.SDA)`
* which calls `busio_i2c_make_new` in `I2C.c` which is a wrapper for
* `common_hal_busio_i2c_construct`
*/
common_hal_busio_i2c_construct(&i2c, scl_pin, sda_pin, I2CBAUD, I2CTIMEOUT);
i2c_initted = true;
}
static void write(busio_i2c_obj_t *device, uint16_t addr, uint8_t *data, size_t len, bool transmit_stop_bit){
common_hal_busio_i2c_write(device, addr, data, len, transmit_stop_bit);
i2c_sensor_t i2c_sensor_create(uint8_t addr) {
i2c_init();
i2c_sensor_t sensor;
sensor.addr = addr;
sensor.i2c = &i2c;
return sensor;
}
static void readInto(busio_i2c_obj_t *device, uint16_t addr, uint8_t *data, size_t len){
common_hal_busio_i2c_read(device, addr, data, len);
void i2c_read_register(i2c_sensor_t device, uint8_t reg, uint8_t* buf, uint8_t len) {
/*
* `_read_register` in `class LIS3DH_I2C(LIS3DH)` of `adafruit_lis3dh.py`
* first calls `write` of `I2CDevice` in `i2c_device.py` which calls `writeto`
* which is hooked up to `busio_i2c_writeto` in `shared-bindings/busio/I2C.c`
* which eventually calls `common_hal_busio_i2c_write`
*/
common_hal_busio_i2c_write(device.i2c, device.addr, &reg, 1, true);
/*
* After, that in `_read_register`, we go to `readinto` which goes into
* `readfrom_into` which goes into `busio_i2c_readfrom_into` which goes into
* `readfrom` which calls `common_hal_busio_i2c_read`
*/
common_hal_busio_i2c_read(device.i2c, device.addr, buf, len);
}
// translated directly from original Python https://github.com/adafruit/Adafruit_CircuitPython_LIS3DH/blob/main/adafruit_lis3dh.py
// i2c driver line 405
static void _write_register_byte(i2c_sensor_t *device, uint8_t reg, uint8_t value){
/* device->gbuf[0] = reg & 0xFF;
device->gbuf[1] = value & 0xFF;
common_hal_adafruit_bus_device_i2cdevice_lock(&device->sensor);
write(&device->bus, reg, &device->gbuf, 2, STOP); //TODO: should STOP be enabled or is that the wrong thing?
common_hal_adafruit_bus_device_i2cdevice_unlock(&device->sensor);
*/
}
// translated directly from original Python https://github.com/adafruit/Adafruit_CircuitPython_LIS3DH/blob/main/adafruit_lis3dh.py
// i2c driver line 398
static uint8_t _read_register_byte(i2c_sensor_t *device, uint8_t reg){
device->gbuf[0] = reg & 0xFF;
// enter device, write desired reg to read from, read value of reg, and exit device
common_hal_adafruit_bus_device_i2cdevice_lock(&device->sensor);
write(&device->bus, reg, device->gbuf, 1, STOP);
readInto(&device->bus, reg, device->gbuf, 1);
common_hal_adafruit_bus_device_i2cdevice_unlock(&device->sensor);
return *(uint8_t *)device->gbuf;