42 lines
906 B
C
42 lines
906 B
C
#pragma once
|
|
|
|
#include <stdint.h>
|
|
|
|
typedef struct
|
|
{
|
|
uint8_t frac_bits;
|
|
int32_t kp;
|
|
int32_t ki;
|
|
int32_t integral_x;
|
|
int32_t integral_y;
|
|
int32_t integral_z;
|
|
int32_t q0;
|
|
int32_t q1;
|
|
int32_t q2;
|
|
int32_t q3;
|
|
} mahony_filter_t;
|
|
|
|
void mahony_filter_init(mahony_filter_t *filter, uint8_t frac_bits);
|
|
void mahony_filter_reset(mahony_filter_t *filter);
|
|
void mahony_filter_set_gains(mahony_filter_t *filter, int32_t kp, int32_t ki);
|
|
void mahony_filter_update_imu(
|
|
mahony_filter_t *filter,
|
|
int32_t gx,
|
|
int32_t gy,
|
|
int32_t gz,
|
|
int32_t ax,
|
|
int32_t ay,
|
|
int32_t az,
|
|
int32_t dt);
|
|
void mahony_filter_get_quaternion(
|
|
const mahony_filter_t *filter,
|
|
int32_t *q0,
|
|
int32_t *q1,
|
|
int32_t *q2,
|
|
int32_t *q3);
|
|
void mahony_filter_get_yaw_pitch_roll(
|
|
const mahony_filter_t *filter,
|
|
int32_t *yaw,
|
|
int32_t *pitch,
|
|
int32_t *roll);
|