/* USER CODE BEGIN Header */

/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2024 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/

/* USER CODE BEGIN Includes */

#include "stdio.h"
#include "string.h"
#include "AT24C08.h"
#include "FS-iA6B.h"
#include "MPU6050_IIR.h"

#define PWM_MAX 24999

#define PWM_MIN 12499

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/

/* USER CODE BEGIN PTD */

// tim10_100ms_count++;
// if(tim10_100ms_count == 100)
// {
// tim10_100ms_count = 0;
// tim10_100ms_flag = 1;
// }

extern uint8_t ibus_rx_buf[32]; // uart6

extern uint8_t ibus_rx_cplt_flag;

extern uint8_t tim10_20ms_flag;

//extern uint8_t tim10_100ms_flag;

extern uint8_t uart1_rx_data;

uint8_t hc06_tx_buf[40]; // uart1

uint8_t hc06_rx_buf[20];
uint8_t hc06_rx_cplt_flag;

float roll_in_kp, roll_out_kp;

float roll_in_ki, roll_out_ki;
float roll_in_kd, roll_out_kd;

float pitch_in_kp, pitch_out_kp;

float pitch_in_ki, pitch_out_ki;
float pitch_in_kd, pitch_out_kd;

float yaw_heading_kp, yaw_rate_kp;

float yaw_heading_ki, yaw_rate_ki;
float yaw_heading_kd, yaw_rate_kd;

unsigned short adcVal = 3252;

float batVolt ;

MPU6050_IIR_t MPU6050_IIR;

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/

/* USER CODE BEGIN PD */

void ESC_Calibration(void);
void Is_ibus_Received(void); // check connect Ibus
void Read_PID_EP_Start(void);
void RX_PID_Gain_Request(void);
void Encode_AHRS(unsigned char* hc06_tx_buf); // Read MPU + SetPoint -->
hc06_tx_buf
void Encode_PID_Gain(unsigned char* hc06_tx_buf, unsigned char id, float p, float
i, float d); // Read PID_EP --> hc06_tx_buf

//void Encode_GPS(unsigned char* hc06_tx_buf); // Read GPS + Battery + SwA,C -->

hc06_tx_buf
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/

/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

I2C_HandleTypeDef hi2c1;
I2C_HandleTypeDef hi2c3;

UART_HandleTypeDef huart1;

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/

void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
static void MX_I2C3_Init(void);
static void MX_TIM3_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART6_UART_Init(void);
static void MX_TIM10_Init(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/

/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */

/* MCU Configuration--------------------------------------------------------*/

/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();

/* USER CODE BEGIN Init */

/* USER CODE END Init */

/* Configure the system clock */

SystemClock_Config();

/* USER CODE BEGIN SysInit */

/* USER CODE END SysInit */

/* Initialize all configured peripherals */

MX_GPIO_Init();
MX_I2C1_Init();
MX_I2C3_Init();
MX_TIM3_Init();
MX_USART1_UART_Init();
MX_USART6_UART_Init();
MX_TIM10_Init();
/* USER CODE BEGIN 2 */

// Bat ngat UART6 - Ibus

LL_USART_EnableIT_RXNE(USART6);

// Bat ngat UART1 - HC06 - Debug

HAL_UART_Receive_IT(&huart1, &uart1_rx_data, 1);

// Bat TIMER10 - Chu ki debug

LL_TIM_EnableCounter(TIM10);
LL_TIM_EnableIT_UPDATE(TIM10);

// Bat TIMER3 - xung PWM - ESC - 2KHz(500us) - (0 ~ 49999)

LL_TIM_EnableCounter(TIM3);
 LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH4);
LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH3);
LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH2);
LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH1);

// ESC_Calibration();

MPU6050_IIR_Init_Benim(&hi2c3);// Initialize MPU6050

Calibrate_MPU6050_IIR(&hi2c3, &MPU6050_IIR);

batVolt = adcVal * 0.003619f;

AT24C08_Init(&hi2c1);
Read_PID_EP_Start();

/* USER CODE END 2 */

/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{

RX_PID_Gain_Request();

MPU6050_IIR_Read_All(&hi2c3, &MPU6050_IIR);
MPU6050_IIR_Read_All_Benim(&hi2c3, &MPU6050_IIR);

if(tim10_20ms_flag == 1)// && tim10_100ms_flag != 1)

{
tim10_20ms_flag = 0;
Encode_AHRS(&hc06_tx_buf[0]);
HAL_UART_Transmit_IT(&huart1, &hc06_tx_buf[0], 20);
}

// else if(tim10_20ms_flag == 1 && tim10_100ms_flag == 1)

// {
// tim10_20ms_flag = 0;
// tim10_100ms_flag = 0;
// Encode_AHRS(&hc06_tx_buf[0]);
// Encode_GPS(&hc06_tx_buf[20]);
// HAL_UART_Transmit_IT(&huart1, &hc06_tx_buf[0], 40);
// }

// if(ibus_rx_cplt_flag == 1)
// {
// ibus_rx_cplt_flag = 0;
// if(iBus_Check_CHKSUM(&ibus_rx_buf[0], 32) == 1)
// {
// iBus_Parsing(&ibus_rx_buf[0], &iBus);
// if(iBus_isActiveFailsafe(&iBus) == 1)
// {
// LL_GPIO_SetOutputPin(GPIOC, LL_GPIO_PIN_13);
// HAL_Delay(100);
// LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);
// HAL_Delay(100);
// }
// else
// {
// LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);
// }
// printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
// iBus.RH, iBus.RV, iBus.LV, iBus.LH, iBus.SwA,
iBus.SwC, iBus.SwD, iBus.FailSafe);
// HAL_Delay(100);
// }
// }

// while(Is_iBus_Received() == 0)
// {
// LL_GPIO_SetOutputPin(GPIOC, LL_GPIO_PIN_13);
// HAL_Delay(100);
// LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);
// HAL_Delay(100);
// }
// if(iBus.SwC == 2000)
// {
// ESC_Calibration();
// while(iBus.SwC != 1000)
// {
// Is_iBus_Received();
// }
// }
// else if(iBus.SwC == 1500)
// {
// memset(&MPU6050_IIR, 0, sizeof(MPU6050_IIR));
// Calibrate_MPU6050_IIR(&hi2c3, &MPU6050_IIR);
// while(iBus.SwC != 1000)
// {
// Is_iBus_Received();
// LL_GPIO_SetOutputPin(GPIOC, LL_GPIO_PIN_13);
// HAL_Delay(100);
// LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);
// HAL_Delay(100);
// }
// }
//
//
// while(Is_iBus_Throttle_Min() == 0)
// {
// LL_GPIO_SetOutputPin(GPIOC, LL_GPIO_PIN_13);
// HAL_Delay(100);
// LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);
// HAL_Delay(100);
// }

/* USER CODE END WHILE */

/* USER CODE BEGIN 3 */

}
 /* USER CODE END 3 */
}

/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

/** Configure the main internal regulator output voltage

*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

/** Initializes the RCC Oscillators according to the specified parameters

* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 12;
RCC_OscInitStruct.PLL.PLLN = 96;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}

/** Initializes the CPU, AHB and APB buses clocks

*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)

{
Error_Handler();
}
}

/**
* @brief I2C1 Initialization Function
* @param None
* @retval None
*/
static void MX_I2C1_Init(void)
{

/* USER CODE BEGIN I2C1_Init 0 */

/* USER CODE END I2C1_Init 0 */

 /* USER CODE BEGIN I2C1_Init 1 */

/* USER CODE END I2C1_Init 1 */

hi2c1.Instance = I2C1;
hi2c1.Init.ClockSpeed = 100000;
hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN I2C1_Init 2 */

/* USER CODE END I2C1_Init 2 */

/**
* @brief I2C3 Initialization Function
* @param None
* @retval None
*/
static void MX_I2C3_Init(void)
{

/* USER CODE BEGIN I2C3_Init 0 */

/* USER CODE END I2C3_Init 0 */

/* USER CODE BEGIN I2C3_Init 1 */

/* USER CODE END I2C3_Init 1 */

hi2c3.Instance = I2C3;
hi2c3.Init.ClockSpeed = 100000;
hi2c3.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c3.Init.OwnAddress1 = 0;
hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c3.Init.OwnAddress2 = 0;
hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c3) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN I2C3_Init 2 */

/* USER CODE END I2C3_Init 2 */

/**
* @brief TIM3 Initialization Function
 * @param None
* @retval None
*/
static void MX_TIM3_Init(void)
{

/* USER CODE BEGIN TIM3_Init 0 */

/* USER CODE END TIM3_Init 0 */

LL_TIM_InitTypeDef TIM_InitStruct = {0};

LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};

LL_GPIO_InitTypeDef GPIO_InitStruct = {0};

/* Peripheral clock enable */

LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);

/* USER CODE BEGIN TIM3_Init 1 */

/* USER CODE END TIM3_Init 1 */

TIM_InitStruct.Prescaler = 0;
TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
TIM_InitStruct.Autoreload = 49999;
TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
LL_TIM_Init(TIM3, &TIM_InitStruct);
LL_TIM_EnableARRPreload(TIM3);
LL_TIM_SetClockSource(TIM3, LL_TIM_CLOCKSOURCE_INTERNAL);
LL_TIM_OC_EnablePreload(TIM3, LL_TIM_CHANNEL_CH1);
TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1;
TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE;
TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE;
TIM_OC_InitStruct.CompareValue = 12499;
TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH;
LL_TIM_OC_Init(TIM3, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct);
LL_TIM_OC_EnableFast(TIM3, LL_TIM_CHANNEL_CH1);
LL_TIM_OC_EnablePreload(TIM3, LL_TIM_CHANNEL_CH2);
TIM_OC_InitStruct.CompareValue = 24499;
LL_TIM_OC_Init(TIM3, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct);
LL_TIM_OC_DisableFast(TIM3, LL_TIM_CHANNEL_CH2);
LL_TIM_OC_EnablePreload(TIM3, LL_TIM_CHANNEL_CH3);
TIM_OC_InitStruct.CompareValue = 37499;
LL_TIM_OC_Init(TIM3, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct);
LL_TIM_OC_EnableFast(TIM3, LL_TIM_CHANNEL_CH3);
LL_TIM_OC_EnablePreload(TIM3, LL_TIM_CHANNEL_CH4);
TIM_OC_InitStruct.CompareValue = 49999;
LL_TIM_OC_Init(TIM3, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct);
LL_TIM_OC_EnableFast(TIM3, LL_TIM_CHANNEL_CH4);
LL_TIM_SetTriggerOutput(TIM3, LL_TIM_TRGO_RESET);
LL_TIM_DisableMasterSlaveMode(TIM3);
/* USER CODE BEGIN TIM3_Init 2 */

/* USER CODE END TIM3_Init 2 */

LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB);
/**TIM3 GPIO Configuration
PA6 ------> TIM3_CH1
PA7 ------> TIM3_CH2
PB0 ------> TIM3_CH3
 PB1 ------> TIM3_CH4
*/
GPIO_InitStruct.Pin = LL_GPIO_PIN_6|LL_GPIO_PIN_7;
GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
GPIO_InitStruct.Alternate = LL_GPIO_AF_2;
LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

GPIO_InitStruct.Pin = LL_GPIO_PIN_0|LL_GPIO_PIN_1;
GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
GPIO_InitStruct.Alternate = LL_GPIO_AF_2;
LL_GPIO_Init(GPIOB, &GPIO_InitStruct);

/**
* @brief TIM10 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM10_Init(void)
{

/* USER CODE BEGIN TIM10_Init 0 */

/* USER CODE END TIM10_Init 0 */

LL_TIM_InitTypeDef TIM_InitStruct = {0};

/* Peripheral clock enable */

LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM10);

/* TIM10 interrupt Init */

NVIC_SetPriority(TIM1_UP_TIM10_IRQn,
NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0));
NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);

/* USER CODE BEGIN TIM10_Init 1 */

/* USER CODE END TIM10_Init 1 */

TIM_InitStruct.Prescaler = 24999;
TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
TIM_InitStruct.Autoreload = 1;
TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
LL_TIM_Init(TIM10, &TIM_InitStruct);
LL_TIM_EnableARRPreload(TIM10);
/* USER CODE BEGIN TIM10_Init 2 */

/* USER CODE END TIM10_Init 2 */

/**
* @brief USART1 Initialization Function
 * @param None
* @retval None
*/
static void MX_USART1_UART_Init(void)
{

/* USER CODE BEGIN USART1_Init 0 */

/* USER CODE END USART1_Init 0 */

/* USER CODE BEGIN USART1_Init 1 */

/* USER CODE END USART1_Init 1 */

huart1.Instance = USART1;
huart1.Init.BaudRate = 115200;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX_RX;
huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart1.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART1_Init 2 */

/* USER CODE END USART1_Init 2 */

/**
* @brief USART6 Initialization Function
* @param None
* @retval None
*/
static void MX_USART6_UART_Init(void)
{

/* USER CODE BEGIN USART6_Init 0 */

/* USER CODE END USART6_Init 0 */

LL_USART_InitTypeDef USART_InitStruct = {0};

LL_GPIO_InitTypeDef GPIO_InitStruct = {0};

/* Peripheral clock enable */

LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_USART6);

LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA);
/**USART6 GPIO Configuration
PA11 ------> USART6_TX
PA12 ------> USART6_RX
*/
GPIO_InitStruct.Pin = LL_GPIO_PIN_11|USART6_RX_IA6B_Pin;
GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
 GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
GPIO_InitStruct.Alternate = LL_GPIO_AF_8;
LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

/* USART6 interrupt Init */

NVIC_SetPriority(USART6_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0,
0));
NVIC_EnableIRQ(USART6_IRQn);

/* USER CODE BEGIN USART6_Init 1 */

/* USER CODE END USART6_Init 1 */

USART_InitStruct.BaudRate = 115200;
USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B;
USART_InitStruct.StopBits = LL_USART_STOPBITS_1;
USART_InitStruct.Parity = LL_USART_PARITY_NONE;
USART_InitStruct.TransferDirection = LL_USART_DIRECTION_RX;
USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE;
USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16;
LL_USART_Init(USART6, &USART_InitStruct);
LL_USART_ConfigAsyncMode(USART6);
LL_USART_Enable(USART6);
/* USER CODE BEGIN USART6_Init 2 */

/* USER CODE END USART6_Init 2 */

/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
LL_GPIO_InitTypeDef GPIO_InitStruct = {0};
/* USER CODE BEGIN MX_GPIO_Init_1 */
/* USER CODE END MX_GPIO_Init_1 */

/* GPIO Ports Clock Enable */

LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOC);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOH);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA);
LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB);

/**/
LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);

/**/
GPIO_InitStruct.Pin = LL_GPIO_PIN_13;
GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
LL_GPIO_Init(GPIOC, &GPIO_InitStruct);

/* USER CODE BEGIN MX_GPIO_Init_2 */

/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) // UART callback -

Bluetooth
{
static unsigned char cnt = 0;

if(huart->Instance == USART1)
{
HAL_UART_Receive_IT(&huart1, &uart1_rx_data, 1);

switch(cnt)
{
case 0:
if(uart1_rx_data == 0x47)
{
hc06_rx_buf[cnt] = uart1_rx_data;
cnt++;
}
break;
case 1:
if(uart1_rx_data == 0x53)
{
hc06_rx_buf[cnt] = uart1_rx_data;
cnt++;
}
else
cnt = 0;
break;
case 19:
hc06_rx_buf[cnt] = uart1_rx_data;
cnt = 0;
hc06_rx_cplt_flag = 1;
break;
default:
hc06_rx_buf[cnt] = uart1_rx_data;
cnt++;
break;
}
}
}

void ESC_Calibration(void) // hieu chinh ESC

{
TIM3 -> CCR1 = PWM_MAX;
TIM3 -> CCR2 = PWM_MAX;
TIM3 -> CCR3 = PWM_MAX;
TIM3 -> CCR4 = PWM_MAX;
HAL_Delay(7000);
TIM3 -> CCR1 = PWM_MIN;
TIM3 -> CCR2 = PWM_MIN;
TIM3 -> CCR3 = PWM_MIN;
TIM3 -> CCR4 = PWM_MIN;
HAL_Delay(8000);
}

int Is_iBus_Received(void) // check connect Ibus

{
 if(ibus_rx_cplt_flag == 1)
{
ibus_rx_cplt_flag = 0;
if(iBus_Check_CHKSUM(&ibus_rx_buf[0], 32) == 1)
{
iBus_Parsing(&ibus_rx_buf[0], &iBus);
return 1;
}
}

return 0;
}

void Encode_AHRS(unsigned char* hc06_tx_buf) // Read MPU + SetPoint --> hc06_tx_buf

{
hc06_tx_buf[0] = 0x46;
hc06_tx_buf[1] = 0x43;

hc06_tx_buf[2] = 0x10;

hc06_tx_buf[3] = (short)(MPU6050_IIR.angle_pitch_output*100);
hc06_tx_buf[4] = ((short)(MPU6050_IIR.angle_pitch_output*100))>>8;

hc06_tx_buf[5] = (short)(MPU6050_IIR.angle_roll_output*100);
hc06_tx_buf[6] = ((short)(MPU6050_IIR.angle_roll_output*100))>>8;

hc06_tx_buf[7] = (unsigned short)(MPU6050_IIR.angle_yaw_rate_output*100);

hc06_tx_buf[8] = ((unsigned short)
(MPU6050_IIR.angle_yaw_rate_output*100))>>8;

hc06_tx_buf[9] = (short)(0.0 *10);

hc06_tx_buf[10] = ((short)(0.0 *10))>>8;

hc06_tx_buf[11] = (short)((iBus.RH-1500)*0.1f*100);
hc06_tx_buf[12] = ((short)((iBus.RH-1500)*0.1f*100))>>8;

hc06_tx_buf[13] = (short)((iBus.RV-1500)*0.1f*100);
hc06_tx_buf[14] = ((short)((iBus.RV-1500)*0.1f*100))>>8;

hc06_tx_buf[15] = (unsigned short)((iBus.LH-1000)*0.36f*100);

hc06_tx_buf[16] = ((unsigned short)((iBus.LH-1000)*0.36f*100))>>8;

hc06_tx_buf[17] = 0x00;
hc06_tx_buf[18] = 0x00;

hc06_tx_buf[19] = 0xff;

for(int i=0;i<19;i++) hc06_tx_buf[19] = hc06_tx_buf[19] - hc06_tx_buf[i];

//void Encode_GPS(unsigned char* hc06_tx_buf) // Read GPS + Battery + SwA,C -->

hc06_tx_buf
//{
// hc06_tx_buf[0] = 0x46;
// hc06_tx_buf[1] = 0x43;
// hc06_tx_buf[2] = 0x11;

// hc06_tx_buf[3] = 0;
// hc06_tx_buf[4] = 0>>8;
// hc06_tx_buf[5] = 0>>16;
// hc06_tx_buf[6] = 0>>24;

// hc06_tx_buf[7] = 0;
// hc06_tx_buf[8] = 0>>8;
// hc06_tx_buf[9] = 0>>16;
// hc06_tx_buf[10] = 0>>24;

// hc06_tx_buf[11] = (unsigned short)(batVolt*100);

// hc06_tx_buf[12] = ((unsigned short)(batVolt*100))>>8;

// hc06_tx_buf[13] = iBus.SwA == 1000 ? 0 : 1;

// hc06_tx_buf[14] = iBus.SwC == 1000 ? 0 : iBus.SwC == 1500 ? 1 : 2;

// hc06_tx_buf[15] = iBus_isActiveFailsafe(&iBus);

// hc06_tx_buf[16] = 0x00;
// hc06_tx_buf[17] = 0x00;
// hc06_tx_buf[18] = 0x00;

// hc06_tx_buf[19] = 0xff;

// for(int i=0;i<19;i++) hc06_tx_buf[19] = hc06_tx_buf[19] - hc06_tx_buf[i];

//}

void Encode_PID_Gain(unsigned char* hc06_tx_buf, unsigned char id, float p, float

i, float d) // Read PID_EP --> hc06_tx_buf
{
hc06_tx_buf[0] = 0x46;
hc06_tx_buf[1] = 0x43;

hc06_tx_buf[2] = id;

// Kp
unsigned char* p_bytes = (unsigned char*)&p;
hc06_tx_buf[3] = p_bytes[0];
hc06_tx_buf[4] = p_bytes[1];
hc06_tx_buf[5] = p_bytes[2];
hc06_tx_buf[6] = p_bytes[3];

// Ki
unsigned char* i_bytes = (unsigned char*)&i;
hc06_tx_buf[7] = i_bytes[0];
hc06_tx_buf[8] = i_bytes[1];
hc06_tx_buf[9] = i_bytes[2];
hc06_tx_buf[10] = i_bytes[3];

// Kd
unsigned char* d_bytes = (unsigned char*)&d;
hc06_tx_buf[11] = d_bytes[0];
hc06_tx_buf[12] = d_bytes[1];
hc06_tx_buf[13] = d_bytes[2];
hc06_tx_buf[14] = d_bytes[3];

hc06_tx_buf[15] = 0x00;
 hc06_tx_buf[16] = 0x00;
hc06_tx_buf[17] = 0x00;
hc06_tx_buf[18] = 0x00;

hc06_tx_buf[19] = 0xff;

for(int i=0;i<19;i++) hc06_tx_buf[19] = hc06_tx_buf[19] - hc06_tx_buf[i];

}

void Read_PID_EP_Start(void)
{
if(EP_PIDGain_Read(0, &roll_in_kp, &roll_in_ki, &roll_in_kd) != 0 ||
EP_PIDGain_Read(1, &roll_out_kp, &roll_out_ki, &roll_out_kd) != 0 ||
EP_PIDGain_Read(2, &pitch_in_kp, &pitch_in_ki, &pitch_in_kd) != 0 ||
EP_PIDGain_Read(3, &pitch_out_kp, &pitch_out_ki, &pitch_out_kd) != 0
||
EP_PIDGain_Read(4, &yaw_heading_kp, &yaw_heading_ki,
&yaw_heading_kd) != 0 ||
EP_PIDGain_Read(5, &yaw_rate_kp, &yaw_rate_ki, &yaw_rate_kd) != 0)
{
LL_GPIO_SetOutputPin(GPIOC, LL_GPIO_PIN_13);
HAL_Delay(100);
LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);
HAL_Delay(100);
}
else
{
Encode_PID_Gain(&hc06_tx_buf[0], 0, roll_in_kp, roll_in_ki, roll_in_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0], 20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 1, roll_out_kp, roll_out_ki, roll_out_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0], 20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 2, pitch_in_kp, pitch_in_ki, pitch_in_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0], 20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 3, pitch_out_kp, pitch_out_ki,
pitch_out_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0], 20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 4, yaw_heading_kp, yaw_heading_ki,
yaw_heading_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0], 20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 5, yaw_rate_kp, yaw_rate_ki, yaw_rate_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0], 20, 10);
}
}

void RX_PID_Gain_Request(void)
{
if(hc06_rx_cplt_flag == 1)
{
hc06_rx_cplt_flag = 0;

if(iBus.SwA == 1000)
{
unsigned char chksum = 0xff;
for(int i=0;i<19;i++) chksum = chksum - hc06_rx_buf[i];

if(chksum == hc06_rx_buf[19])
{
LL_GPIO_SetOutputPin(GPIOC, LL_GPIO_PIN_13);
HAL_Delay(100);
 LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);

switch(hc06_rx_buf[2])
{
case 0:
roll_in_kp = *(float*)&hc06_rx_buf[3];
roll_in_ki = *(float*)&hc06_rx_buf[7];
roll_in_kd = *(float*)&hc06_rx_buf[11];
EP_PIDGain_Write(hc06_rx_buf[2], roll_in_kp,
roll_in_ki, roll_in_kd);
EP_PIDGain_Read(hc06_rx_buf[2], &roll_in_kp,
&roll_in_ki, &roll_in_kd);
Encode_PID_Gain(&hc06_tx_buf[0], hc06_rx_buf[2],
roll_in_kp, roll_in_ki, roll_in_kd);
HAL_UART_Transmit_IT(&huart1, &hc06_tx_buf[0], 20);
break;
case 1:
roll_out_kp = *(float*)&hc06_rx_buf[3];
roll_out_ki = *(float*)&hc06_rx_buf[7];
roll_out_kd = *(float*)&hc06_rx_buf[11];
EP_PIDGain_Write(hc06_rx_buf[2], roll_out_kp,
roll_out_ki, roll_out_kd);
EP_PIDGain_Read(hc06_rx_buf[2], &roll_out_kp,
&roll_out_ki, &roll_out_kd);
Encode_PID_Gain(&hc06_tx_buf[0], hc06_rx_buf[2],
roll_out_kp, roll_out_ki, roll_out_kd);
HAL_UART_Transmit_IT(&huart1, &hc06_tx_buf[0], 20);
break;
case 2:
pitch_in_kp = *(float*)&hc06_rx_buf[3];
pitch_in_ki = *(float*)&hc06_rx_buf[7];
pitch_in_kd = *(float*)&hc06_rx_buf[11];
EP_PIDGain_Write(hc06_rx_buf[2], pitch_in_kp,
pitch_in_ki, pitch_in_kd);
EP_PIDGain_Read(hc06_rx_buf[2], &pitch_in_kp,
&pitch_in_ki, &pitch_in_kd);
Encode_PID_Gain(&hc06_tx_buf[0], hc06_rx_buf[2],
pitch_in_kp, pitch_in_ki, pitch_in_kd);
HAL_UART_Transmit_IT(&huart1, &hc06_tx_buf[0], 20);
break;
case 3:
pitch_out_kp = *(float*)&hc06_rx_buf[3];
pitch_out_ki = *(float*)&hc06_rx_buf[7];
pitch_out_kd = *(float*)&hc06_rx_buf[11];
EP_PIDGain_Write(hc06_rx_buf[2], pitch_out_kp,
pitch_out_ki, pitch_out_kd);
EP_PIDGain_Read(hc06_rx_buf[2], &pitch_out_kp,
&pitch_out_ki, &pitch_out_kd);
Encode_PID_Gain(&hc06_tx_buf[0], hc06_rx_buf[2],
pitch_out_kp, pitch_out_ki, pitch_out_kd);
HAL_UART_Transmit_IT(&huart1, &hc06_tx_buf[0], 20);
break;
case 4:
yaw_heading_kp = *(float*)&hc06_rx_buf[3];
yaw_heading_ki = *(float*)&hc06_rx_buf[7];
yaw_heading_kd = *(float*)&hc06_rx_buf[11];
EP_PIDGain_Write(hc06_rx_buf[2], yaw_heading_kp,
yaw_heading_ki, yaw_heading_kd);
EP_PIDGain_Read(hc06_rx_buf[2], &yaw_heading_kp,
&yaw_heading_ki, &yaw_heading_kd);
Encode_PID_Gain(&hc06_tx_buf[0], hc06_rx_buf[2],
yaw_heading_kp, yaw_heading_ki, yaw_heading_kd);
HAL_UART_Transmit_IT(&huart1, &hc06_tx_buf[0], 20);
break;
case 5:
yaw_rate_kp = *(float*)&hc06_rx_buf[3];
yaw_rate_ki = *(float*)&hc06_rx_buf[7];
yaw_rate_kd = *(float*)&hc06_rx_buf[11];
EP_PIDGain_Write(hc06_rx_buf[2], yaw_rate_kp,
yaw_rate_ki, yaw_rate_kd);
EP_PIDGain_Read(hc06_rx_buf[2], &yaw_rate_kp,
&yaw_rate_ki, &yaw_rate_kd);
Encode_PID_Gain(&hc06_tx_buf[0], hc06_rx_buf[2],
yaw_rate_kp, yaw_rate_ki, yaw_rate_kd);
HAL_UART_Transmit_IT(&huart1, &hc06_tx_buf[0], 20);
break;
case 0x10:
switch(hc06_rx_buf[3])
{
case 0:
Encode_PID_Gain(&hc06_tx_buf[0],
hc06_rx_buf[3], roll_in_kp, roll_in_ki, roll_in_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
break;
case 1:
Encode_PID_Gain(&hc06_tx_buf[0],
hc06_rx_buf[3], roll_out_kp, roll_out_ki, roll_out_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
break;
case 2:
Encode_PID_Gain(&hc06_tx_buf[0],
hc06_rx_buf[3], pitch_in_kp, pitch_in_ki, pitch_in_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
break;
case 3:
Encode_PID_Gain(&hc06_tx_buf[0],
hc06_rx_buf[3], pitch_out_kp, pitch_out_ki, pitch_out_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
break;
case 4:
Encode_PID_Gain(&hc06_tx_buf[0],
hc06_rx_buf[3], yaw_heading_kp, yaw_heading_ki, yaw_heading_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
break;
case 5:
Encode_PID_Gain(&hc06_tx_buf[0],
hc06_rx_buf[3], yaw_rate_kp, yaw_rate_ki, yaw_rate_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
break;
case 6:
Encode_PID_Gain(&hc06_tx_buf[0], 0,
roll_in_kp, roll_in_ki, roll_in_kd);
 HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 1,
roll_out_kp, roll_out_ki, roll_out_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 2,
pitch_in_kp, pitch_in_ki, pitch_in_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 3,
pitch_out_kp, pitch_out_ki, pitch_out_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 4,
yaw_heading_kp, yaw_heading_ki, yaw_heading_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
Encode_PID_Gain(&hc06_tx_buf[0], 5,
yaw_rate_kp, yaw_rate_ki, yaw_rate_kd);
HAL_UART_Transmit(&huart1, &hc06_tx_buf[0],
20, 10);
break;
}
break;

}
}
}
}
}

/* USER CODE END 4 */

/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
LL_GPIO_SetOutputPin(GPIOC, LL_GPIO_PIN_13);
HAL_Delay(100);
LL_GPIO_ResetOutputPin(GPIOC, LL_GPIO_PIN_13);
HAL_Delay(100);
}
/* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
 * @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */