41 lines
599 B
C
41 lines
599 B
C
/*
|
|
* stabilize.c
|
|
*
|
|
* Created on: Aug 9, 2025
|
|
* Author: angoosh
|
|
*/
|
|
|
|
#include "stabilize.h"
|
|
|
|
Stabilize_Typedef STAB;
|
|
|
|
void Stabilize_init(){
|
|
STAB.pitch_gain = 1;
|
|
STAB.roll_gain = 1;
|
|
STAB.yaw_gain = 1;
|
|
}
|
|
|
|
int Stabilize_Roll(int servo){
|
|
int stab_servo = 0;
|
|
|
|
stab_servo = servo + (IMU.roll * STAB.roll_gain);
|
|
|
|
return stab_servo;
|
|
}
|
|
|
|
int Stabilize_Pitch(int servo){
|
|
int stab_servo = 0;
|
|
|
|
stab_servo = servo + (IMU.pitch * STAB.pitch_gain);
|
|
|
|
return stab_servo;
|
|
}
|
|
|
|
int Stabilize_Yaw(int servo){
|
|
int stab_servo = 0;
|
|
|
|
stab_servo = servo + (IMU.yaw * STAB.yaw_gain);
|
|
|
|
return stab_servo;
|
|
}
|