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;
}
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;
}
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;
}
}
}
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;
}
// Lua: read = i2c.read( id, size )
static int i2c_read( lua_State *L )
{
unsigned id = luaL_checkinteger( L, 1 );
u32 size = ( u32 )luaL_checkinteger( L, 2 ), i;
luaL_Buffer b;
int data;
MOD_CHECK_ID( i2c, id );
if( size == 0 )
return 0;
luaL_buffinit( L, &b );
for( i = 0; i < size; i ++ )
if( ( data = platform_i2c_recv_byte( id, i < size - 1 ) ) == -1 )
break;
else
luaL_addchar( &b, ( char )data );
luaL_pushresult( &b );
return 1;
}
