/*
 * 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;
}