uint8_t lcdGetChar(void) {
uint8_t ret;
twiStart(LCD_ADDRESS+I2C_READ);
ret = twiReadAck();
twiReadNak();
twiStop();
return ret;
}
uint8_t twiReadFrom(uint8_t address, uint8_t* data, bool repeated, bool stop){
//send START condition, abort if error
if(!twiStart(repeated))
return 1;
//send ADDRESS
//shift the address to the left and add write bit, send.
twiWriteByte((MPU9150_ADDRESS << 1) + TW_WRITE);
//check for address send f**k-up
if(TW_STATUS != TW_MT_SLA_ACK)
return 2;
twiReadAck(data);
if(stop)
twiStop();
return 0;
}
Error gyroRead(Vector3f *v) {
// Simple Software Low-Pass
static double gyroSumX = 0, gyroSumY = 0, gyroSumZ = 0;
static double gyroFilterX = 0, gyroFilterY = 0, gyroFilterZ = 0;
if (v == NULL) {
return ARGUMENT_ERROR;
}
if (twiStart(GYRO_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(GYROREG_OUTXL | 0x80)) { // Auto Increment
return TWI_WRITE_ERROR;
}
if (twiRepStart(GYRO_ADDRESS | TWI_READ)) {
return TWI_NO_ANSWER;
}
uint8_t xl = twiReadAck();
uint8_t xh = twiReadAck();
uint8_t yl = twiReadAck();
uint8_t yh = twiReadAck();
uint8_t zl = twiReadAck();
uint8_t zh = twiReadNak();
int16_t x = *(int8_t *)(&xh);
x *= (1 << 8);
x |= xl;
int16_t y = *(int8_t *)(&yh);
y *= (1 << 8);
y |= yl;
int16_t z = *(int8_t *)(&zh);
z *= (1 << 8);
z |= zl;
switch (gyroRange) {
case r250DPS:
v->x = (((double)x) * 250 / 0x8000);
v->y = (((double)y) * 250 / 0x8000);
v->z = (((double)z) * 250 / 0x8000);
break;
case r500DPS:
v->x = (((double)x) * 500 / 0x8000);
v->y = (((double)y) * 500 / 0x8000);
v->z = (((double)z) * 500 / 0x8000);
break;
case r2000DPS:
v->x = (((double)x) * 2000 / 0x8000);
v->y = (((double)y) * 2000 / 0x8000);
v->z = (((double)z) * 2000 / 0x8000);
break;
default:
return ARGUMENT_ERROR;
}
gyroSumX = gyroSumX - gyroFilterX + v->x;
gyroFilterX = gyroSumX / GYROFILTERFACTOR;
v->x = gyroFilterX;
gyroSumY = gyroSumY - gyroFilterY + v->y;
gyroFilterY = gyroSumY / GYROFILTERFACTOR;
v->y = gyroFilterY;
gyroSumZ = gyroSumZ - gyroFilterZ + v->z;
gyroFilterZ = gyroSumZ / GYROFILTERFACTOR;
v->z = gyroFilterZ;
return SUCCESS;
}
Error magRead(Vector3f *v) {
if (v == NULL) {
return ARGUMENT_ERROR;
}
if (twiStart(MAG_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(MAGREG_XH)) {
return TWI_WRITE_ERROR;
}
if (twiRepStart(MAG_ADDRESS | TWI_READ)) {
return TWI_NO_ANSWER;
}
uint8_t xh = twiReadAck();
uint8_t xl = twiReadAck();
uint8_t zh = twiReadAck();
uint8_t zl = twiReadAck();
uint8_t yh = twiReadAck();
uint8_t yl = twiReadNak();
int16_t x = *(int8_t *)(&xh);
x *= (1 << 8);
x |= xl;
int16_t y = *(int8_t *)(&yh);
y *= (1 << 8);
y |= yl;
int16_t z = *(int8_t *)(&zh);
z *= (1 << 8);
z |= zl;
switch (magRange) {
case r1g3:
v->x = (((double)x) * 1.3 / MAG_NORMALIZE);
v->y = (((double)y) * 1.3 / MAG_NORMALIZE);
v->z = (((double)z) * 1.3 / MAG_NORMALIZE);
break;
case r1g9:
v->x = (((double)x) * 1.9 / MAG_NORMALIZE);
v->y = (((double)y) * 1.9 / MAG_NORMALIZE);
v->z = (((double)z) * 1.9 / MAG_NORMALIZE);
break;
case r2g5:
v->x = (((double)x) * 2.5 / MAG_NORMALIZE);
v->y = (((double)y) * 2.5 / MAG_NORMALIZE);
v->z = (((double)z) * 2.5 / MAG_NORMALIZE);
break;
case r4g0:
v->x = (((double)x) * 4.0 / MAG_NORMALIZE);
v->y = (((double)y) * 4.0 / MAG_NORMALIZE);
v->z = (((double)z) * 4.0 / MAG_NORMALIZE);
break;
case r4g7:
v->x = (((double)x) * 4.7 / MAG_NORMALIZE);
v->y = (((double)y) * 4.7 / MAG_NORMALIZE);
v->z = (((double)z) * 4.7 / MAG_NORMALIZE);
break;
case r5g6:
v->x = (((double)x) * 5.6 / MAG_NORMALIZE);
v->y = (((double)y) * 5.6 / MAG_NORMALIZE);
v->z = (((double)z) * 5.6 / MAG_NORMALIZE);
break;
case r8g1:
v->x = (((double)x) * 8.1 / MAG_NORMALIZE);
v->y = (((double)y) * 8.1 / MAG_NORMALIZE);
v->z = (((double)z) * 8.1 / MAG_NORMALIZE);
break;
default:
return ARGUMENT_ERROR;
}
return SUCCESS;
}
