ALPAKA/TeleStern
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

Add rotation of value vector for magnetometer

Browse Source
This commit is contained in:
Jannis Bergmann 2024-07-03 00:10:09 +02:00
parent 499e91b6ec
commit e480df7c9b
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/drivers/ao_mmc5983.c
View File

@ -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);
}