Compare commits

...

3 Commits

3 changed files with 58 additions and 6 deletions

View File

@ -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 {

View File

@ -60,12 +60,20 @@ _ao_bmi088_acc_sample_read(struct ao_bmi088_acc_sample *sample)
{
uint8_t dummy;
uint8_t addr = BMI088_ACC_DATA | 0x80;
struct ao_bmi088_acc_sample raw;
float magic_number = (float)0.70710678118654752440084436210484; // 1/sqrt(2); could use double for higher percision
ao_bmi088_acc_start();
ao_spi_send(&addr, 1, AO_BMI088_SPI_BUS);
ao_spi_recv(&dummy, 1, AO_BMI088_SPI_BUS); /* part sends garbage for first byte */
ao_spi_recv(sample, sizeof(struct ao_bmi088_acc_sample), AO_BMI088_SPI_BUS);
ao_spi_recv(&raw, sizeof(struct ao_bmi088_acc_sample), AO_BMI088_SPI_BUS);
ao_bmi088_acc_end();
// rotate value vector by 45 degrees mathimatical positice around z-axis
sample->x = (int16_t)((raw.x + raw.y) * magic_number);
sample->y = (int16_t)((raw.x - raw.y) * magic_number);
sample->z = raw.z;
}
static void
@ -98,7 +106,15 @@ _ao_bmi088_gyr_reg_write(uint8_t addr, uint8_t value)
static void
_ao_bmi088_gyr_sample_read(struct ao_bmi088_gyr_sample *sample)
{
_ao_bmi088_gyr_read(BMI088_GYRO_DATA, sample, sizeof (struct ao_bmi088_gyr_sample));
struct ao_bmi088_gyr_sample raw;
float magic_number = (float)0.70710678118654752440084436210484; // 1/sqrt(2); could use double for higher percision
_ao_bmi088_gyr_read(BMI088_GYRO_DATA, &raw, sizeof (struct ao_bmi088_gyr_sample));
// rotate value vector by 45 degrees mathimatical negative around z-axis
sample->x = (int16_t)((raw.x + raw.y) * magic_number);
sample->y = (int16_t)((raw.x - raw.y) * magic_number);
sample->z = raw.z;
}
static void

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