diff --git a/src/drivers/ao_adxl375.c b/src/drivers/ao_adxl375.c index 70bf92a..3b2b471 100644 --- a/src/drivers/ao_adxl375.c +++ b/src/drivers/ao_adxl375.c @@ -85,13 +85,42 @@ ao_adxl375_reg_write(uint8_t addr, uint8_t value) static void ao_adxl375_value(struct ao_adxl375_sample *value) { - uint8_t d[7]; + uint8_t d[7]; + float magic_number = (float)0.70710678118654752440084436210484; // 1/sqrt(2); could use double for higher percision + struct ao_adxl375_sample sample; + struct ao_adxl375_sample result; + d[0] = AO_ADXL375_DATAX0 | AO_ADXL375_READ | AO_ADXL375_MULTI_BYTE; ao_adxl375_start(); ao_spi_duplex(d, d, 7, AO_ADXL375_SPI_INDEX); ao_adxl375_stop(); - memcpy(value, &d[1], 6); + + memcpy(&sample, &d[1], 6); + +// rotating x and y by 45 degree + + /* Bias by 32768 to convert from uint16_t to int16_t */ +// sample.x = (int16_t) ((((uint16_t) raw.x0 << 8) | raw.x1) - 32768); +// sample.y = (int16_t) ((((uint16_t) raw.y0 << 8) | raw.y1) - 32768); + +// sample.x = (int16_t) ((((uint16_t) d[2] << 8) | d[3]) - 0); +// sample.y = (int16_t) ((((uint16_t) d[4] << 8) | d[5]) - 0); +// sample.z = (int16_t) ((((uint16_t) d[6] << 8) | d[7]) - 0); + + // rotate value vector by 45 degrees mathimatical positive around z-axis + result.x = (int16_t)((sample.x - sample.y) * magic_number); + result.y = (int16_t)((sample.x + sample.y) * magic_number); + result.z = sample.z; + +// result.x = 0; +// result.y = 1; +// result.z = sample.z; + + + memcpy(value, &result, 6); + +// memcpy(value, &d[1], 6); } struct ao_adxl375_total {