35 lines
1.3 KiB
C
35 lines
1.3 KiB
C
#include "madgwick.h"
|
|
#include "stdint.h"
|
|
#include "stdio.h"
|
|
|
|
#define MADGWICK_BETA 0.1f
|
|
#define MADGWICK_SAMPLE_RATE 100.0f
|
|
#define DEG2RAD 3.14f/180.0f
|
|
|
|
madgwick_handle_t madgwick_handle;
|
|
madgwick_quat_data_t quat_data;
|
|
|
|
int main(){
|
|
madgwick_cfg_t madgwick_cfg;
|
|
madgwick_cfg.beta = MADGWICK_BETA;
|
|
madgwick_cfg.sample_freq = MADGWICK_SAMPLE_RATE;
|
|
madgwick_handle = madgwick_init(&madgwick_cfg);
|
|
|
|
madgwick_update_6dof(madgwick_handle,
|
|
0.6 * DEG2RAD,
|
|
1 * DEG2RAD,
|
|
3 * DEG2RAD,
|
|
0.43,
|
|
0.56,
|
|
1.3);
|
|
|
|
madgwick_get_quaternion(madgwick_handle, &quat_data);
|
|
float roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
|
|
float pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
|
|
float yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);
|
|
|
|
printf("Roll: %f\nPitch: %f\nYaw: %f\n", roll, pitch, yaw);
|
|
|
|
return 0;
|
|
}
|