FMCZ/stabilization
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added code handling IMU and started writing stabilization code

Browse Source
This commit is contained in:
Angoosh Leviocki
2025-08-09 00:54:26 +02:00
parent f24948bc09
commit 5d86bcfe26
220 changed files with 155202 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
Core/Src/madwick_example.c Normal file
View File

@@ -0,0 +1,34 @@
#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;
}