void orient_update() {
updateinterval ++;
if (updateinterval % 20 == 0) {
int16_t ax = accelerometer_readSigned16(ADXL345_REG_DATAX0);
int16_t ay = accelerometer_readSigned16(ADXL345_REG_DATAY0);
int16_t az = accelerometer_readSigned16(ADXL345_REG_DATAZ0);
if (ax != last_ax ||
ay != last_ay ||
az != last_az) {
comm_send_acceleration(ax, ay, az);
last_ax = ax;
last_ay = ay;
last_az = az;
}
}
if (updateinterval % 3 == 1) {
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, L3GD20_ADDRESS, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(endpoint_id, L3GD20_REGISTER_OUT_X_L | 0x80);
platform_i2c_send_stop(endpoint_id);
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, L3GD20_ADDRESS, PLATFORM_I2C_DIRECTION_RECEIVER);
uint8_t b0 = platform_i2c_recv_byte(endpoint_id, 1);
uint8_t b1 = platform_i2c_recv_byte(endpoint_id, 1);
uint8_t b2 = platform_i2c_recv_byte(endpoint_id, 1);
uint8_t b3 = platform_i2c_recv_byte(endpoint_id, 1);
uint8_t b4 = platform_i2c_recv_byte(endpoint_id, 1);
uint8_t b5 = platform_i2c_recv_byte(endpoint_id, 0);
platform_i2c_send_stop(endpoint_id);
int16_t gx = comm_fromUnsigned16((b1 << 8) | b0);
int16_t gy = comm_fromUnsigned16((b3 << 8) | b2);
int16_t gz = comm_fromUnsigned16((b5 << 8) | b4);
acc_gx = ((acc_gx * 3) / 4) + (gx / 4);
acc_gy = ((acc_gy * 3) / 4) + (gy / 4);
acc_gz = ((acc_gz * 3) / 4) + (gz / 4);
}
if (updateinterval % 20 == 10) {
if (acc_gx != last_gx ||
acc_gy != last_gy ||
acc_gz != last_gz) {
comm_send_rotation(acc_gx, acc_gy, acc_gz);
last_gx = acc_gx;
last_gy = acc_gy;
last_gz = acc_gz;
}
}
}
int16_t accelerometer_readSigned16(int reg) {
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, ADXL345_ADDRESS, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(endpoint_id, reg);
platform_i2c_send_stop(endpoint_id);
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, ADXL345_ADDRESS, PLATFORM_I2C_DIRECTION_RECEIVER);
uint8_t b0 = platform_i2c_recv_byte(endpoint_id, 1);
uint8_t b1 = platform_i2c_recv_byte(endpoint_id, 0);
platform_i2c_send_stop(endpoint_id);
uint16_t output = (b1 << 8) | b0;
int16_t output2 = comm_fromUnsigned16(output);
/*
if (output < 32767)
output2 = + (int)output;
else
output2 = - (int)(65536 - output);
*/
return output2;
}
irom uint8_t u8g_com_esp8266_ssd_i2c_fn(u8g_t * u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr)
{
switch (msg) {
case U8G_COM_MSG_INIT:
// we assume that the i2c bus was already initialized
//u8g_i2c_init(u8g->pin_list[U8G_PI_I2C_OPTION]);
platform_i2c_setup(4, 5);
break;
case U8G_COM_MSG_STOP:
break;
case U8G_COM_MSG_RESET:
/* Currently disabled, but it could be enable. Previous restrictions have been removed */
digitalWrite(D4, arg_val);
break;
case U8G_COM_MSG_CHIP_SELECT:
u8g->pin_list[U8G_PI_A0_STATE] = 0;
u8g->pin_list[U8G_PI_SET_A0] = 1; /* force a0 to set again, also forces start condition */
if (arg_val == 0) {
/* disable chip, send stop condition */
platform_i2c_send_stop();
} else {
/* enable, do nothing: any byte writing will trigger the i2c start */
}
break;
case U8G_COM_MSG_WRITE_BYTE:
//u8g->pin_list[U8G_PI_SET_A0] = 1;
if (u8g_com_esp8266_ssd_start_sequence(u8g) == 0)
return platform_i2c_send_stop(), 0;
// ignore return value -> tolerate missing ACK
if (platform_i2c_send_byte(arg_val) == 0) ;
break;
case U8G_COM_MSG_WRITE_SEQ:
case U8G_COM_MSG_WRITE_SEQ_P:
//u8g->pin_list[U8G_PI_SET_A0] = 1;
if (u8g_com_esp8266_ssd_start_sequence(u8g) == 0)
return platform_i2c_send_stop(), 0;
{
register uint8_t *ptr = arg_ptr;
while (arg_val > 0) {
// ignore return value -> tolerate missing ACK
if (platform_i2c_send_byte(*ptr++) == 0) ;
arg_val--;
}
}
break;
case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */
u8g->pin_list[U8G_PI_A0_STATE] = arg_val;
u8g->pin_list[U8G_PI_SET_A0] = 1; /* force a0 to set again */
break;
}
return 1;
}
static uint8_t r8u(uint32_t id, uint8_t reg) {
uint8_t ret;
platform_i2c_send_start(id);
platform_i2c_send_address(id, i2c_addr, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(id, reg);
platform_i2c_send_stop(id);
platform_i2c_send_start(id);
platform_i2c_send_address(id, i2c_addr, PLATFORM_I2C_DIRECTION_RECEIVER);
ret = platform_i2c_recv_byte(id, 0);
platform_i2c_send_stop(id);
return ret;
}
uint8_t gyro_read_all(int reg) {
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, L3GD20_ADDRESS, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(endpoint_id, reg);
platform_i2c_send_stop(endpoint_id);
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, L3GD20_ADDRESS, PLATFORM_I2C_DIRECTION_RECEIVER);
uint8_t b0 = platform_i2c_recv_byte(endpoint_id, 0);
platform_i2c_send_stop(endpoint_id);
return b0;
}
void accelerometer_start() {
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, ADXL345_ADDRESS, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(endpoint_id, ADXL345_REG_POWER_CTL);
platform_i2c_send_byte(endpoint_id, 0x08);
platform_i2c_send_stop(endpoint_id);
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, ADXL345_ADDRESS, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(endpoint_id, ADXL345_REG_BW_RATE);
platform_i2c_send_byte(endpoint_id, ADXL345_DATARATE_50_HZ);
platform_i2c_send_stop(endpoint_id);
}
static int l3g4200d_read(lua_State* L) {
uint8_t data[6];
int x,y,z;
int i;
platform_i2c_send_start(i2c_id);
platform_i2c_send_address(i2c_id, i2c_addr, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(i2c_id, 0xA8);
platform_i2c_send_start(i2c_id);
platform_i2c_send_address(i2c_id, i2c_addr, PLATFORM_I2C_DIRECTION_RECEIVER);
for (i=0; i<5; i++) {
data[i] = platform_i2c_recv_byte(i2c_id, 1);
}
data[5] = platform_i2c_recv_byte(i2c_id, 0);
platform_i2c_send_stop(i2c_id);
x = (int16_t) ((data[1] << 8) | data[0]);
y = (int16_t) ((data[3] << 8) | data[2]);
z = (int16_t) ((data[5] << 8) | data[4]);
lua_pushinteger(L, x);
lua_pushinteger(L, y);
lua_pushinteger(L, z);
return 3;
}
static void w8u(uint32_t id, uint8_t reg, uint8_t val) {
platform_i2c_send_start(i2c_id);
platform_i2c_send_address(i2c_id, i2c_addr, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(i2c_id, reg);
platform_i2c_send_byte(i2c_id, val);
platform_i2c_send_stop(i2c_id);
}
// Lua: i2c.stop( id )
static int i2c_stop( lua_State *L )
{
unsigned id = luaL_checkinteger( L, 1 );
MOD_CHECK_ID( i2c, id );
platform_i2c_send_stop( id );
return 0;
}
void gyro_start() {
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, L3GD20_ADDRESS, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(endpoint_id, L3GD20_REGISTER_CTRL_REG1);
platform_i2c_send_byte(endpoint_id, 0x0F);
platform_i2c_send_stop(endpoint_id);
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, L3GD20_ADDRESS, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(endpoint_id, L3GD20_REGISTER_CTRL_REG2);
platform_i2c_send_byte(endpoint_id, 0x08);
platform_i2c_send_stop(endpoint_id);
platform_i2c_send_start(endpoint_id);
platform_i2c_send_address(endpoint_id, L3GD20_ADDRESS, PLATFORM_I2C_DIRECTION_TRANSMITTER);
platform_i2c_send_byte(endpoint_id, L3GD20_REGISTER_CTRL_REG4);
platform_i2c_send_byte(endpoint_id, 0x00);
platform_i2c_send_stop(endpoint_id);
}
