diff --git a/src/drivers/ao_mmc5983.c b/src/drivers/ao_mmc5983.c index 02df7a2..7c99b0e 100644 --- a/src/drivers/ao_mmc5983.c +++ b/src/drivers/ao_mmc5983.c @@ -232,11 +232,18 @@ static struct ao_mmc5983_raw raw; static void ao_mmc5983_sample(struct ao_mmc5983_sample *s) { + struct ao_mmc5983_sample sample; + float magic_number = (float)0.70710678118654752440084436210484; // 1/sqrt(2); could use double for higher percision + ao_mmc5983_raw(&raw); /* Bias by 32768 to convert from uint16_t to int16_t */ - s->x = (int16_t) ((((uint16_t) raw.x0 << 8) | raw.x1) - 32768); - s->y = (int16_t) ((((uint16_t) raw.y0 << 8) | raw.y1) - 32768); + sample.x = (int16_t) ((((uint16_t) raw.x0 << 8) | raw.x1) - 32768); + sample.y = (int16_t) ((((uint16_t) raw.y0 << 8) | raw.y1) - 32768); + + // rotate value vector by 45 degrees mathimatical negative around z-axis + s->x = (int16_t)((sample.x + sample.y) * magic_number); + s->y = (int16_t)((sample.x - sample.y) * magic_number); s->z = (int16_t) ((((uint16_t) raw.z0 << 8) | raw.z1) - 32768); }