main.c

Go to the documentation of this file.

/* USER CODE BEGIN Header */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os.h"
 
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "kalmanFilter.h"
#include "mekf.h"
#include "OptimalAngle.h"
#include "z_qflash_W25QXXX.h"
#include "attitude_estimation.h"
#include "buzzer.h"
#include "profiler.h"
/* USER CODE END Includes */
 
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
 
/* USER CODE END PTD */
 
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
 
/* USER CODE END PD */
 
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
 
/* USER CODE END PM */
 
/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2;
ADC_HandleTypeDef hadc3;
 
FDCAN_HandleTypeDef hfdcan1;
 
QSPI_HandleTypeDef hqspi;
MDMA_HandleTypeDef hmdma_quadspi_fifo_th;
 
SPI_HandleTypeDef hspi1;
SPI_HandleTypeDef hspi2;
SPI_HandleTypeDef hspi3;
 
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim6;
TIM_HandleTypeDef htim7;
TIM_HandleTypeDef htim13;
TIM_HandleTypeDef htim15;
 
UART_HandleTypeDef huart4;
UART_HandleTypeDef huart1;
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart1_tx;
 
PCD_HandleTypeDef hpcd_USB_OTG_FS;
 
/* Definitions for Sensor_Task */
osThreadId_t Sensor_TaskHandle;
const osThreadAttr_t Sensor_Task_attributes = {
  .name = "Sensor_Task",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityRealtime7,
};
/* Definitions for FSM */
osThreadId_t FSMHandle;
const osThreadAttr_t FSM_attributes = {
  .name = "FSM",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Kalman_mkf */
osThreadId_t Kalman_mkfHandle;
const osThreadAttr_t Kalman_mkf_attributes = {
  .name = "Kalman_mkf",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for MPC_Task */
osThreadId_t MPC_TaskHandle;
const osThreadAttr_t MPC_Task_attributes = {
  .name = "MPC_Task",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for ReceiveVel_Task */
osThreadId_t ReceiveVel_TaskHandle;
const osThreadAttr_t ReceiveVel_Task_attributes = {
  .name = "ReceiveVel_Task",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* USER CODE BEGIN PV */
//---------------------------------------------------------------------------------------State Machine tied variables
flight_phase_t flight_phase = CALIBRATING; //Variable for initializing our state machine
 
flight_fsm_t flight_state; //state machine
//---------------------------------------------------------------------------------------State Machine tied variables
 
//---------------------------------------------------------------------------------------------Sensor and buzzer tied vars
buzzer_t *bz ={0};
struct bmp3_dev bmp390_1 = {0}, bmp390_2 = {0}; //our barometer structs (main and backup)
 
stmdev_ctx_t imu_1 = {0}, imu_2 = {0}; //IMU structs (main and backup)
 
struct bmp3_data barometer_data_1 = {-1, -1}; //variable we use to get data from the main baro sensors
struct bmp3_data barometer_data_2 = {-1, -1}; //variable we use to get data from the backup baro sensors
 
static int16_t data_raw_angular_rate_1[3] = {0}; //variable we use to get angular acc from the imu sensor
static int16_t data_raw_acceleration_1[3] = {0}; //variable we use to get linear acc from the imu sensor
 
static float_t acceleration_mg_1[3] = {0}; //variable in which we save linear acc in milli-gs
static float_t angular_rate_mdps_1[3] = {0}; //variable in which we save angular acc in milli-gs
 
stmdev_ctx_t lis2mdl_dev_ctx; //main magnetometer sensor object
stmdev_ctx_t lis2mdl_dev_ctx_2; //backup magnetometer sensor object
int16_t raw_mag_data[3]; // X, Y, Z //variable we use to get magnetic values from the magnetometer
//---------------------------------------------------------------------------------------------Sensor and buzzer tied vars
 
 
#pragma pack(push, 1)
typedef struct {
    float_t acc_x;  
    float_t acc_y;  
    float_t acc_z;  
} Linear_IMU_Package;
#pragma pack(pop)
 
 
 
#pragma pack(push, 1)
typedef struct {
    float_t dps_x;      
    float_t dps_y;      
    float_t dps_z;      
 
    float_t mag_x;      
    float_t mag_y;      
    float_t mag_z;      
 
    float_t temperature;
    float_t pressure;   
 
    float_t altitude;   
    float_t velocity;   
    float_t phase;      
    float_t deg;        
} Data_Package_no_Linear_IMU;
#pragma pack(pop)
 
 
#pragma pack(push, 1)
typedef struct {
    float_t acc_x;              
    float_t acc_y;              
    float_t acc_z;              
 
    float_t dps_x;              
    float_t dps_y;              
    float_t dps_z;              
 
    float_t mag_x;              
    float_t mag_y;              
    float_t mag_z;              
 
    float_t temperature;        
    float_t pressure;           
    float_t altitude;           
 
    float_t velocity;           
    uint8_t phase;              
 
    float_t Battery_Voltage; 
    float_t deg;                
 
    uint8_t parachute_1;        
    uint8_t parachute_2;        
} Data_Package_For_Marconi;
#pragma pack(pop)
 
 
 
float_t altitude = 0.0; //variable in which we save altitude
float_t velocity = 0.0; //variable in which we store kalman adjusted velocity
float_t Pressure_1 = 0.0; //variable holding the pressure we measured at ground level
float_t Temperature_1 = 0.0; //variable holding the temperature we measured at ground level
Data_Package_For_Marconi message = {0}; //variable holding the values we will send to the Marconi
uint8_t drogue = 0; // flag indicating whether the drogue parachutes are open or not
uint8_t mainp = 0; // flag indicating whether the main parachutes are open or not
float max_alt = 0; //variable holding our highest measured altitude so far
float max_vel =0; //variable holding our highest measured speed so far
bool first_measure = true; //flag allowing us to understand if we're handling the first measure or not, useful for saving pressure and temperature and ground level
 
 
//-------------------------------------------------------------------------------------------------------- MSA's variables, you must have a call with them before every flight to decide what values to put there
double q[] = {0.7061377159181262, -0.03700710955926802, 0.03700710955926802, -0.7061377159181262};// quaternione di rotazione del razzo, da inizializzare con MSA, prodotto in output dal controllore (q_new precedente)
double beta[] = {0, 0, 0}; //bias del gyroscopio, sentirsi con MSA per settare i valori
static double P[] __attribute__((aligned(4))) = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0, 1.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0, 1.0, 0.0, 0.0, 0.0,0.0, 0.0, 0.0, 1.0, 0.0, 0.0,0.0, 0.0, 0.0, 0.0, 1.0, 0.0,0.0, 0.0, 0.0, 0.0, 0.0, 1.0};// covarianza del gyroscopio + magnetometro
double omega[] = {3.760494829166693e-05, -1.4118446451604494e-07, 0.0003572545679652845}; //gyro dal sensore
double b_B[] = {0,0,0};//{6669.23590038,-23984.08306486,39107.60772102}; //magnetometro dal sensore
//double height_AGL = 0;// altezza dall'x_est //COmmented because temporarly unused
double q_new[] = {0,0,0,0};// risultato del mekf, quaternione nuovo
double beta_new[] = {0,0,0};// risultato del mekf, da sovrascrivere al beta vecchio
static double P_new[] __attribute__((aligned(4)))= {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};// come il precedente
//double x_pred[] = {5000,300}; //x_est dell'iterazione precedente -> inizializzati prima della gara con i dati forniti da MSA
double P_2[] = {5,0,0,5}; //Da settare con i dati di noise del GNSS
double z[] = {5003.24140978,329.14706741};//dati in input, altitudine (da calcolare con la pressione) e velocita  del barometro e GNSS
double alpha = 0.0; //angolo rispetto alla verticale del razzo
double a_m = -12.09; //accelerazione longitudinale
double Ts = 0.01; // periodo del filtro di Kalman -> vogliamo che il filtro si esegua ogni 10 ms
const double Q[] =  {2769,1846,1846,1385}; //per calcolare covarianza del noise(legato a P2)
const double R[] = {5.54e-3,0,0,0.121}; // per calcolare covarianza del noise(legato a P2) //
double x_est[] = {5000,330}; // risultati del filtro di Kalman , altitudine e velocita filtrate
double time_step = 0.5; // periodo di esecuzione dell'MPC
double sim_time_step = 0.5; // periodo di simulazione interna
double current_delta = 0; // ultimo grado inviato agli airbrakes
double target_apogee = 8900; //apogeo che vogliamo
double tolerance = 1; // tolleranza sull'angolo, angolo minimo che possiamo inviare
double tilt_angle = 0.24556997475102937; //Angolo rispetto alla verticale del razzo
const double mach_states[] = {0.1,0.2,0.3,0.4,0.5};// legato alla lookup table per il ct
const double deployment_states[] ={0.0,40.5000,69.7500,94.5000,108.0000,173.8000}; //lookup table
static double cd_table[] ={0.4510,0.3940,0.3610,0.3390,0.3320,0.5430,0.4960,0.4690,0.4250,0.4170,0.6240,0.5790,0.5550,0.5350,0.5280, 0.7450,0.7140,0.6780,0.6760,0.6720,0.7520,0.7230,0.6980,0.6850,0.6820,0.85,0.81,0.8,0.8,0.8}; //lookup table
double z_component = 0.0; //component use to calculate the alpha, will be reintroduced alongside the magnetometers
//-----------------------------------------------------------------------------------------------------------------
 
//------------------------------------------ Handles of the tasks
TaskHandle_t xReceived_VelHandle = NULL;
TaskHandle_t xMPCTaskHandle = NULL;
TaskHandle_t xSensorsTaskHandle = NULL;
TaskHandle_t xKalman_TaskHandle = NULL;
//------------------------------------------ Handles of the tasks
 
//----------------------------------------- Thread ids of the tasks
osThreadId_t xReceived_VelID =NULL;
osThreadId_t xMPCTaskID = NULL;
osThreadId_t xSensorsTaskID = NULL;
osThreadId_t xKalman_TaskID = NULL;
//----------------------------------------- Thread ids of the tasks
 
 
//---------------------------------------------- Flags for thread management
#define SENSOR_FLAG   (1U << 0)   // 0x0001
#define MPC_FLAG      (1U << 1)   // 0x0002
#define RECEIVED_VEL_FLAG  (1U << 2)
#define KALMAN_FLAG   (1U << 3)
#define FSM_FLAG (1U << 4)
#define UART_RX_ERROR_FLAG (1U << 5)
//---------------------------------------------- Flags for thread management
 
//----------------------------------------------------------------------------GNSS velocity/Marconi Rx tied vars
uint8_t vel[4]; //variable in which we receive the gps measured velocity
float gps_vel = 0.0f; //variable in which we store the gps measured velocity
//----------------------------------------------------------------------------GNSS velocity/Marconi Rx tied vars
 
uint8_t array[63]; //variable used to transform the message struct into an array of byte and send it over UART with correct byte order
 
 
//---------------------------------------------------------------------------Controller tied flags
bool gnss_flag = false; //flag indicating whether we just received a velocity from the gnss or not, it influences which Kalman filter we use
bool flag_kf = false; //flag indicating whether we are in a phase of the flight state machine which requires to use the Kalman filter or not
bool flag_attitude = false; //flag indicating whether we are in a phase of the flight state machine which requires to use the attitude controller or not
bool flag_MPC = false; //flag indicating whether we are in a phase of the flight state machine which requires to use the MPC or not
//---------------------------------------------------------------------------Controller tied flags
 
//-----------------------------------------------------------------------------------------------------Flash tied variables
bool flag_flash = false; //flag indicating whether we write data in the flash or not //TODO : set to flight conditions later (flase)
uint32_t flash_address = 4; // variable in which we save the address of the winbond in which we save the data
uint8_t data_buffer[4224] ={0}; //data buffer that we use to transform our structs into single byte to preserve order while saving in the winbond
uint8_t cpy_buffer[2112] = {0}; //buffer used to copy the structure and saving it into the winbond
uint32_t num_of_lin_acc_saved = 0; // variable keeping track of the saved linear accelerations so far
uint32_t num_of_saved_rest_packages = 0; // variable keeping track of the saved structures not containing linear acceleration so far
uint32_t num_of_saved_rest_packages_locked = 0; // counter of how many packages minus linear accelerations have been saved in the temporary buffer before we save them in the flash (we save at 4 and 8 then flush at 8)
uint32_t num_of_lin_acc_saved_flash = 0; // variable keeping track of the saved linear acceleration so far in the winbond
uint32_t num_of_saved_rest_packages_flash = 0; // variable keeping track of the saved structures not containing linear acceleration so far in the winbond
float initial_flag_value = 0.0f; //flag indicating us whether we need to flush the winbond at startup or not
//-----------------------------------------------------------------------------------------------------Flash tied variables
 
 
//-------------------------------------------------------------------------------------Voltage calculation
float Vin =0; //Variable used to calculate the voltage of the battery
float Vr =0;  //Variable used to calculate the voltage of the battery
uint32_t adcVal = 0; //variable holding the voltage of the battery
//-------------------------------------------------------------------------------------Voltage calculation
 
 
/* USER CODE END PV */
 
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void PeriphCommonClock_Config(void);
static void MPU_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_MDMA_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USB_OTG_FS_PCD_Init(void);
static void MX_FDCAN1_Init(void);
static void MX_QUADSPI_Init(void);
static void MX_SPI1_Init(void);
static void MX_SPI2_Init(void);
static void MX_SPI3_Init(void);
static void MX_TIM2_Init(void);
static void MX_TIM15_Init(void);
static void MX_UART4_Init(void);
static void MX_ADC1_Init(void);
static void MX_ADC2_Init(void);
static void MX_ADC3_Init(void);
static void MX_TIM7_Init(void);
static void MX_TIM6_Init(void);
static void MX_TIM13_Init(void);
void StartSensor_Task(void *argument);
void StartFSM(void *argument);
void StartKalman_mkf(void *argument);
void StartMPC_Task(void *argument);
void StartReceiveVel_Task(void *argument);
 
/* USER CODE BEGIN PFP */
 
/* USER CODE END PFP */
 
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
 
note_t Partition[] = {{NOTE_Gb5,TEMPO_BASE_MS*2}, {NOTE_Gb5,TEMPO_BASE_MS},{NOTE_D6,TEMPO_BASE_MS},{NOTE_Db6,TEMPO_BASE_MS*2},//{REST,TEMPO_BASE_MS*3},
        {NOTE_Db6,TEMPO_BASE_MS*2},{NOTE_Db6,TEMPO_BASE_MS},{NOTE_D6,TEMPO_BASE_MS},{NOTE_B5,TEMPO_BASE_MS*2},//{REST,TEMPO_BASE_MS*2},
        {NOTE_B5,TEMPO_BASE_MS*2},{NOTE_B5,TEMPO_BASE_MS},{NOTE_A5,TEMPO_BASE_MS},{NOTE_B5,TEMPO_BASE_MS*2},{NOTE_B5,TEMPO_BASE_MS*2},{NOTE_B5,TEMPO_BASE_MS},{NOTE_A5,TEMPO_BASE_MS},{NOTE_B5,TEMPO_BASE_MS},{NOTE_A5,TEMPO_BASE_MS},{NOTE_Gb5,TEMPO_BASE_MS*5}  };
 
 
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
    if (huart->Instance == USART1) {
 
        osThreadFlagsSet(ReceiveVel_TaskHandle, RECEIVED_VEL_FLAG);
 
 
    }
}
 
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart){
    if (huart->Instance == USART1){
        uint32_t error = HAL_UART_GetError(huart);
        __HAL_UART_CLEAR_OREFLAG(huart);
        HAL_UART_AbortReceive(huart); // This should handle associated DMA abort too
 
 
        if (ReceiveVel_TaskHandle != NULL) { // Ensure task handle is valid
            osThreadFlagsSet(ReceiveVel_TaskHandle, UART_RX_ERROR_FLAG);
        }
    }
}
 
 
/* USER CODE END 0 */
 
int main(void)
{
 
  /* USER CODE BEGIN 1 */
 
  /* USER CODE END 1 */
 
  /* MPU Configuration--------------------------------------------------------*/
  MPU_Config();
 
  /* 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();
 
  /* Configure the peripherals common clocks */
  PeriphCommonClock_Config();
 
  /* USER CODE BEGIN SysInit */
  initDWT();
  /* USER CODE END SysInit */
 
  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_MDMA_Init();
  MX_USART1_UART_Init();
  MX_USB_OTG_FS_PCD_Init();
  MX_FDCAN1_Init();
  MX_QUADSPI_Init();
  MX_SPI1_Init();
  MX_SPI2_Init();
  MX_SPI3_Init();
  MX_TIM2_Init();
  MX_TIM15_Init();
  MX_UART4_Init();
  MX_ADC1_Init();
  MX_ADC2_Init();
  MX_ADC3_Init();
  MX_TIM7_Init();
  MX_TIM6_Init();
  MX_TIM13_Init();
  /* USER CODE BEGIN 2 */
  //----------------------------------------------------------------------------------------------------------------------------------------
//TODO CODICE PER INIZIALIZZARE I  MAGNETOMETRI, COMMENTATO FINQUANDO AVREMO UNA SCHEDA CON I MAGNETOMETRI GIRATI NEL VERSO GIUSTO
  //uint8_t result = lis2mdl_init(&lis2mdl_dev_ctx);
  //uint8_t result_7 = lis2mdl_init_2(&lis2mdl_dev_ctx_2);
//TODO: SETTAGGIO DELL'SPI IN 4WAY PIUTTOSTO CHE 3WAY, SERVE A GESTIRE LA QUESTIONE DEL REGISTRO DEL MAGNETOMETRO, SE GIA STA SU 4WAY NO NEED
  //  HAL_SPI_DeInit(&hspi2);
  //  HAL_Delay(10);
  //  hspi2.Init.Direction = SPI_DIRECTION_2LINES;
  //  HAL_SPI_Init(&hspi2);
  //----------------------------------------------------------------------------------------------------------------------------------------
 
  LED_ON(GPIO_LED_FLASH_GPIO_Port, GPIO_LED_FLASH_Pin);
 
 
 
  IMU_CS_HIGH;
  MAG_CS_HIGH;
  BMP390_CS_HIGH;
 
  HAL_Delay(100);
 
 
  uint8_t result_4 = init_bmp390_B(&bmp390_2);
 
  uint8_t result_2 = init_imuB(&imu_2, LSM6DSO32_16g, LSM6DSO32_2000dps, LSM6DSO32_XL_ODR_6667Hz_HIGH_PERF, LSM6DSO32_GY_ODR_208Hz_HIGH_PERF);
 
  //TODO: the result of the init of the backup of the imu and of the barometer are currently not checked, we can add a very similar snippet of code to the lines 494 to 498 but using a different LED
 
  uint8_t result_3 = init_bmp390_p(&bmp390_1);
 
  uint8_t result_1 = init_imup(&imu_1, LSM6DSO32_16g, LSM6DSO32_2000dps, LSM6DSO32_XL_ODR_6667Hz_HIGH_PERF, LSM6DSO32_GY_ODR_208Hz_HIGH_PERF);
 
 
  if(result_1 != 0 || result_3 != 0){
    LED_ON(GPIO_LED2_GPIO_Port, GPIO_LED2_Pin);
  }
 
  calibrateIMU(&imu_1, 1000, HWOFFSET);
  calibrateIMU(&imu_2, 1000, HWOFFSET);
 
 
  buzzer_init(bz, &htim15, TIM_CHANNEL_2);
 
  musica_maestro(bz,Partition,sizeof(Partition)/sizeof(note_t));
 
 
  if (QFlash_Init() != 0) {
          // Handle initialization error
          Error_Handler();
  }
 
 
  uint8_t small_buffer[4];
  if (QFlash_Read(0, (uint8_t*)small_buffer, 4) != HAL_OK) {
      // Handle read error
      Error_Handler();
  }
 
  memcpy(&initial_flag_value, (uint8_t*)small_buffer, 4);
 
  if(initial_flag_value != 1.0) {
      HAL_TIM_PWM_Start(&htim15, TIM_CHANNEL_2);
 
      for(int i = 0; i < 10; i++) {
          __HAL_TIM_SET_PRESCALER(&htim15, NOTE_B5);
          HAL_Delay(100);
          __HAL_TIM_SET_PRESCALER(&htim15, 0);
          HAL_Delay(100);
          __HAL_TIM_SET_PRESCALER(&htim15, NOTE_B5);
          HAL_Delay(100);
          __HAL_TIM_SET_PRESCALER(&htim15, 0);
 
          HAL_Delay(700);
      }
 
      __HAL_TIM_SET_PRESCALER(&htim15, NOTE_B5);
      HAL_Delay(1000);
 
      HAL_TIM_PWM_Stop(&htim15, TIM_CHANNEL_2);
 
      QFlash_ChipErase();
 
      HAL_TIM_PWM_Start(&htim15, TIM_CHANNEL_2);
 
      __HAL_TIM_SET_PRESCALER(&htim15, NOTE_B5);
      HAL_Delay(700);
 
      HAL_TIM_PWM_Stop(&htim15, TIM_CHANNEL_2);
  }
 
  LED_ON(GPIO_LED1_GPIO_Port, GPIO_LED1_Pin);
  flight_state.flight_state = flight_phase;
 
 
 
  /* USER CODE END 2 */
 
  /* Init scheduler */
  osKernelInitialize();
 
  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */
 
  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */
 
  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */
 
  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */
 
  /* Create the thread(s) */
  /* creation of Sensor_Task */
  Sensor_TaskHandle = osThreadNew(StartSensor_Task, NULL, &Sensor_Task_attributes);
 
  /* creation of FSM */
  FSMHandle = osThreadNew(StartFSM, NULL, &FSM_attributes);
 
  /* creation of Kalman_mkf */
  Kalman_mkfHandle = osThreadNew(StartKalman_mkf, NULL, &Kalman_mkf_attributes);
 
  /* creation of MPC_Task */
  MPC_TaskHandle = osThreadNew(StartMPC_Task, NULL, &MPC_Task_attributes);
 
  /* creation of ReceiveVel_Task */
  ReceiveVel_TaskHandle = osThreadNew(StartReceiveVel_Task, NULL, &ReceiveVel_Task_attributes);
 
  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */
 
  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
 
  /* USER CODE END RTOS_EVENTS */
 
  /* Start scheduler */
  osKernelStart();
 
  /* We should never get here as control is now taken by the scheduler */
 
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
 
  while (1)
  {
    /* USER CODE END WHILE */
 
    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}
 
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
 
  HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY);
 
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
 
  while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}
 
  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 = 1;
  RCC_OscInitStruct.PLL.PLLN = 60;
  RCC_OscInitStruct.PLL.PLLP = 2;
  RCC_OscInitStruct.PLL.PLLQ = 15;
  RCC_OscInitStruct.PLL.PLLR = 2;
  RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;
  RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;
  RCC_OscInitStruct.PLL.PLLFRACN = 0;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
 
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
                              |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
  RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2;
 
  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
  {
    Error_Handler();
  }
}
 
void PeriphCommonClock_Config(void)
{
  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
 
  PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC;
  PeriphClkInitStruct.PLL2.PLL2M = 1;
  PeriphClkInitStruct.PLL2.PLL2N = 10;
  PeriphClkInitStruct.PLL2.PLL2P = 2;
  PeriphClkInitStruct.PLL2.PLL2Q = 2;
  PeriphClkInitStruct.PLL2.PLL2R = 2;
  PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_3;
  PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOMEDIUM;
  PeriphClkInitStruct.PLL2.PLL2FRACN = 0;
  PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLL2;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
}
 
static void MX_ADC1_Init(void)
{
 
  /* USER CODE BEGIN ADC1_Init 0 */
 
  /* USER CODE END ADC1_Init 0 */
 
  ADC_MultiModeTypeDef multimode = {0};
  ADC_ChannelConfTypeDef sConfig = {0};
 
  /* USER CODE BEGIN ADC1_Init 1 */
 
  /* USER CODE END ADC1_Init 1 */
 
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
  hadc1.Init.Resolution = ADC_RESOLUTION_16B;
  hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;
  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  hadc1.Init.LowPowerAutoWait = DISABLE;
  hadc1.Init.ContinuousConvMode = DISABLE;
  hadc1.Init.NbrOfConversion = 1;
  hadc1.Init.DiscontinuousConvMode = DISABLE;
  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc1.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DR;
  hadc1.Init.Overrun = ADC_OVR_DATA_PRESERVED;
  hadc1.Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE;
  hadc1.Init.OversamplingMode = DISABLE;
  hadc1.Init.Oversampling.Ratio = 1;
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    Error_Handler();
  }
 
  multimode.Mode = ADC_MODE_INDEPENDENT;
  if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
  {
    Error_Handler();
  }
 
  sConfig.Channel = ADC_CHANNEL_11;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.OffsetNumber = ADC_OFFSET_NONE;
  sConfig.Offset = 0;
  sConfig.OffsetSignedSaturation = DISABLE;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC1_Init 2 */
 
  /* USER CODE END ADC1_Init 2 */
 
}
 
static void MX_ADC2_Init(void)
{
 
  /* USER CODE BEGIN ADC2_Init 0 */
 
  /* USER CODE END ADC2_Init 0 */
 
  ADC_ChannelConfTypeDef sConfig = {0};
 
  /* USER CODE BEGIN ADC2_Init 1 */
 
  /* USER CODE END ADC2_Init 1 */
 
  hadc2.Instance = ADC2;
  hadc2.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
  hadc2.Init.Resolution = ADC_RESOLUTION_16B;
  hadc2.Init.ScanConvMode = ADC_SCAN_DISABLE;
  hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  hadc2.Init.LowPowerAutoWait = DISABLE;
  hadc2.Init.ContinuousConvMode = DISABLE;
  hadc2.Init.NbrOfConversion = 1;
  hadc2.Init.DiscontinuousConvMode = DISABLE;
  hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc2.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DR;
  hadc2.Init.Overrun = ADC_OVR_DATA_PRESERVED;
  hadc2.Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE;
  hadc2.Init.OversamplingMode = DISABLE;
  hadc2.Init.Oversampling.Ratio = 1;
  if (HAL_ADC_Init(&hadc2) != HAL_OK)
  {
    Error_Handler();
  }
 
  sConfig.Channel = ADC_CHANNEL_9;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.OffsetNumber = ADC_OFFSET_NONE;
  sConfig.Offset = 0;
  sConfig.OffsetSignedSaturation = DISABLE;
  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC2_Init 2 */
 
  /* USER CODE END ADC2_Init 2 */
 
}
 
static void MX_ADC3_Init(void)
{
 
  /* USER CODE BEGIN ADC3_Init 0 */
 
  /* USER CODE END ADC3_Init 0 */
 
  ADC_ChannelConfTypeDef sConfig = {0};
 
  /* USER CODE BEGIN ADC3_Init 1 */
 
  /* USER CODE END ADC3_Init 1 */
 
  hadc3.Instance = ADC3;
  hadc3.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
  hadc3.Init.Resolution = ADC_RESOLUTION_12B;
  hadc3.Init.ScanConvMode = ADC_SCAN_DISABLE;
  hadc3.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  hadc3.Init.LowPowerAutoWait = DISABLE;
  hadc3.Init.ContinuousConvMode = DISABLE;
  hadc3.Init.NbrOfConversion = 1;
  hadc3.Init.DiscontinuousConvMode = DISABLE;
  hadc3.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc3.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DR;
  hadc3.Init.Overrun = ADC_OVR_DATA_PRESERVED;
  hadc3.Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE;
  hadc3.Init.OversamplingMode = DISABLE;
  hadc3.Init.Oversampling.Ratio = 1;
  if (HAL_ADC_Init(&hadc3) != HAL_OK)
  {
    Error_Handler();
  }
 
  sConfig.Channel = ADC_CHANNEL_11;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.OffsetNumber = ADC_OFFSET_NONE;
  sConfig.Offset = 0;
  sConfig.OffsetSignedSaturation = DISABLE;
  if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC3_Init 2 */
 
  /* USER CODE END ADC3_Init 2 */
 
}
 
static void MX_FDCAN1_Init(void)
{
 
  /* USER CODE BEGIN FDCAN1_Init 0 */
 
  /* USER CODE END FDCAN1_Init 0 */
 
  /* USER CODE BEGIN FDCAN1_Init 1 */
 
  /* USER CODE END FDCAN1_Init 1 */
  hfdcan1.Instance = FDCAN1;
  hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
  hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
  hfdcan1.Init.AutoRetransmission = DISABLE;
  hfdcan1.Init.TransmitPause = DISABLE;
  hfdcan1.Init.ProtocolException = DISABLE;
  hfdcan1.Init.NominalPrescaler = 16;
  hfdcan1.Init.NominalSyncJumpWidth = 1;
  hfdcan1.Init.NominalTimeSeg1 = 1;
  hfdcan1.Init.NominalTimeSeg2 = 1;
  hfdcan1.Init.DataPrescaler = 1;
  hfdcan1.Init.DataSyncJumpWidth = 1;
  hfdcan1.Init.DataTimeSeg1 = 1;
  hfdcan1.Init.DataTimeSeg2 = 1;
  hfdcan1.Init.MessageRAMOffset = 0;
  hfdcan1.Init.StdFiltersNbr = 0;
  hfdcan1.Init.ExtFiltersNbr = 0;
  hfdcan1.Init.RxFifo0ElmtsNbr = 0;
  hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
  hfdcan1.Init.RxFifo1ElmtsNbr = 0;
  hfdcan1.Init.RxFifo1ElmtSize = FDCAN_DATA_BYTES_8;
  hfdcan1.Init.RxBuffersNbr = 0;
  hfdcan1.Init.RxBufferSize = FDCAN_DATA_BYTES_8;
  hfdcan1.Init.TxEventsNbr = 0;
  hfdcan1.Init.TxBuffersNbr = 0;
  hfdcan1.Init.TxFifoQueueElmtsNbr = 0;
  hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
  hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8;
  if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN FDCAN1_Init 2 */
 
  /* USER CODE END FDCAN1_Init 2 */
 
}
 
static void MX_QUADSPI_Init(void)
{
 
  /* USER CODE BEGIN QUADSPI_Init 0 */
 
  /* USER CODE END QUADSPI_Init 0 */
 
  /* USER CODE BEGIN QUADSPI_Init 1 */
 
  /* USER CODE END QUADSPI_Init 1 */
  /* QUADSPI parameter configuration*/
  hqspi.Instance = QUADSPI;
  hqspi.Init.ClockPrescaler = 1;
  hqspi.Init.FifoThreshold = 32;
  hqspi.Init.SampleShifting = QSPI_SAMPLE_SHIFTING_NONE;
  hqspi.Init.FlashSize = 25;
  hqspi.Init.ChipSelectHighTime = QSPI_CS_HIGH_TIME_1_CYCLE;
  hqspi.Init.ClockMode = QSPI_CLOCK_MODE_0;
  hqspi.Init.FlashID = QSPI_FLASH_ID_2;
  hqspi.Init.DualFlash = QSPI_DUALFLASH_DISABLE;
  if (HAL_QSPI_Init(&hqspi) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN QUADSPI_Init 2 */
 
  /* USER CODE END QUADSPI_Init 2 */
 
}
 
static void MX_SPI1_Init(void)
{
 
  /* USER CODE BEGIN SPI1_Init 0 */
 
  /* USER CODE END SPI1_Init 0 */
 
  /* USER CODE BEGIN SPI1_Init 1 */
 
  /* USER CODE END SPI1_Init 1 */
  /* SPI1 parameter configuration*/
  hspi1.Instance = SPI1;
  hspi1.Init.Mode = SPI_MODE_MASTER;
  hspi1.Init.Direction = SPI_DIRECTION_2LINES;
  hspi1.Init.DataSize = SPI_DATASIZE_4BIT;
  hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
  hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
  hspi1.Init.NSS = SPI_NSS_SOFT;
  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
  hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi1.Init.CRCPolynomial = 0x0;
  hspi1.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
  hspi1.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
  hspi1.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
  hspi1.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
  hspi1.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
  hspi1.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
  hspi1.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
  hspi1.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
  hspi1.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
  hspi1.Init.IOSwap = SPI_IO_SWAP_DISABLE;
  if (HAL_SPI_Init(&hspi1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI1_Init 2 */
 
  /* USER CODE END SPI1_Init 2 */
 
}
 
static void MX_SPI2_Init(void)
{
 
  /* USER CODE BEGIN SPI2_Init 0 */
 
  /* USER CODE END SPI2_Init 0 */
 
  /* USER CODE BEGIN SPI2_Init 1 */
 
  /* USER CODE END SPI2_Init 1 */
  /* SPI2 parameter configuration*/
  hspi2.Instance = SPI2;
  hspi2.Init.Mode = SPI_MODE_MASTER;
  hspi2.Init.Direction = SPI_DIRECTION_2LINES;
  hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH;
  hspi2.Init.CLKPhase = SPI_PHASE_2EDGE;
  hspi2.Init.NSS = SPI_NSS_SOFT;
  hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
  hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi2.Init.CRCPolynomial = 0x0;
  hspi2.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
  hspi2.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
  hspi2.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
  hspi2.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
  hspi2.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
  hspi2.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
  hspi2.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
  hspi2.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
  hspi2.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
  hspi2.Init.IOSwap = SPI_IO_SWAP_DISABLE;
  if (HAL_SPI_Init(&hspi2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI2_Init 2 */
 
  /* USER CODE END SPI2_Init 2 */
 
}
 
static void MX_SPI3_Init(void)
{
 
  /* USER CODE BEGIN SPI3_Init 0 */
 
  /* USER CODE END SPI3_Init 0 */
 
  /* USER CODE BEGIN SPI3_Init 1 */
 
  /* USER CODE END SPI3_Init 1 */
  /* SPI3 parameter configuration*/
  hspi3.Instance = SPI3;
  hspi3.Init.Mode = SPI_MODE_MASTER;
  hspi3.Init.Direction = SPI_DIRECTION_2LINES;
  hspi3.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi3.Init.CLKPolarity = SPI_POLARITY_HIGH;
  hspi3.Init.CLKPhase = SPI_PHASE_2EDGE;
  hspi3.Init.NSS = SPI_NSS_SOFT;
  hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
  hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi3.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi3.Init.CRCPolynomial = 0x0;
  hspi3.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
  hspi3.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
  hspi3.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
  hspi3.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
  hspi3.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
  hspi3.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
  hspi3.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
  hspi3.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
  hspi3.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
  hspi3.Init.IOSwap = SPI_IO_SWAP_DISABLE;
  if (HAL_SPI_Init(&hspi3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI3_Init 2 */
 
  /* USER CODE END SPI3_Init 2 */
 
}
 
static void MX_TIM2_Init(void)
{
 
  /* USER CODE BEGIN TIM2_Init 0 */
 
  /* USER CODE END TIM2_Init 0 */
 
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};
 
  /* USER CODE BEGIN TIM2_Init 1 */
 
  /* USER CODE END TIM2_Init 1 */
  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 0;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 4294967295;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM2_Init 2 */
 
  /* USER CODE END TIM2_Init 2 */
  HAL_TIM_MspPostInit(&htim2);
 
}
 
static void MX_TIM6_Init(void)
{
 
  /* USER CODE BEGIN TIM6_Init 0 */
 
  /* USER CODE END TIM6_Init 0 */
 
  TIM_MasterConfigTypeDef sMasterConfig = {0};
 
  /* USER CODE BEGIN TIM6_Init 1 */
 
  /* USER CODE END TIM6_Init 1 */
  htim6.Instance = TIM6;
  htim6.Init.Prescaler = 5999;
  htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim6.Init.Period = 9;
  htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM6_Init 2 */
 
  /* USER CODE END TIM6_Init 2 */
 
}
 
static void MX_TIM7_Init(void)
{
 
  /* USER CODE BEGIN TIM7_Init 0 */
 
  /* USER CODE END TIM7_Init 0 */
 
  TIM_MasterConfigTypeDef sMasterConfig = {0};
 
  /* USER CODE BEGIN TIM7_Init 1 */
 
  /* USER CODE END TIM7_Init 1 */
  htim7.Instance = TIM7;
  htim7.Init.Prescaler = 23999;
  htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim7.Init.Period = 4999;
  htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM7_Init 2 */
 
  /* USER CODE END TIM7_Init 2 */
 
}
 
static void MX_TIM13_Init(void)
{
 
  /* USER CODE BEGIN TIM13_Init 0 */
 
  /* USER CODE END TIM13_Init 0 */
 
  /* USER CODE BEGIN TIM13_Init 1 */
 
  /* USER CODE END TIM13_Init 1 */
  htim13.Instance = TIM13;
  htim13.Init.Prescaler = 239;
  htim13.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim13.Init.Period = 9999;
  htim13.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim13.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim13) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM13_Init 2 */
 
  /* USER CODE END TIM13_Init 2 */
 
}
 
static void MX_TIM15_Init(void)
{
 
  /* USER CODE BEGIN TIM15_Init 0 */
 
  /* USER CODE END TIM15_Init 0 */
 
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};
  TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
 
  /* USER CODE BEGIN TIM15_Init 1 */
 
  /* USER CODE END TIM15_Init 1 */
  htim15.Instance = TIM15;
  htim15.Init.Prescaler = 239;
  htim15.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim15.Init.Period = 999;
  htim15.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim15.Init.RepetitionCounter = 0;
  htim15.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_PWM_Init(&htim15) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim15, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 499;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
  sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
  if (HAL_TIM_PWM_ConfigChannel(&htim15, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  {
    Error_Handler();
  }
  sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
  sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
  sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
  sBreakDeadTimeConfig.DeadTime = 0;
  sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
  sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
  sBreakDeadTimeConfig.BreakFilter = 0;
  sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
  if (HAL_TIMEx_ConfigBreakDeadTime(&htim15, &sBreakDeadTimeConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM15_Init 2 */
 
  /* USER CODE END TIM15_Init 2 */
  HAL_TIM_MspPostInit(&htim15);
 
}
 
static void MX_UART4_Init(void)
{
 
  /* USER CODE BEGIN UART4_Init 0 */
 
  /* USER CODE END UART4_Init 0 */
 
  /* USER CODE BEGIN UART4_Init 1 */
 
  /* USER CODE END UART4_Init 1 */
  huart4.Instance = UART4;
  huart4.Init.BaudRate = 115200;
  huart4.Init.WordLength = UART_WORDLENGTH_8B;
  huart4.Init.StopBits = UART_STOPBITS_1;
  huart4.Init.Parity = UART_PARITY_NONE;
  huart4.Init.Mode = UART_MODE_TX_RX;
  huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart4.Init.OverSampling = UART_OVERSAMPLING_16;
  huart4.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart4.Init.ClockPrescaler = UART_PRESCALER_DIV1;
  huart4.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart4) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetTxFifoThreshold(&huart4, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetRxFifoThreshold(&huart4, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_DisableFifoMode(&huart4) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN UART4_Init 2 */
 
  /* USER CODE END UART4_Init 2 */
 
}
 
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 = 1843200;
  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;
  huart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart1.Init.ClockPrescaler = UART_PRESCALER_DIV1;
  huart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetTxFifoThreshold(&huart1, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetRxFifoThreshold(&huart1, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_EnableFifoMode(&huart1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART1_Init 2 */
 
  /* USER CODE END USART1_Init 2 */
 
}
 
static void MX_USB_OTG_FS_PCD_Init(void)
{
 
  /* USER CODE BEGIN USB_OTG_FS_Init 0 */
 
  /* USER CODE END USB_OTG_FS_Init 0 */
 
  /* USER CODE BEGIN USB_OTG_FS_Init 1 */
 
  /* USER CODE END USB_OTG_FS_Init 1 */
  hpcd_USB_OTG_FS.Instance = USB_OTG_FS;
  hpcd_USB_OTG_FS.Init.dev_endpoints = 9;
  hpcd_USB_OTG_FS.Init.speed = PCD_SPEED_FULL;
  hpcd_USB_OTG_FS.Init.dma_enable = DISABLE;
  hpcd_USB_OTG_FS.Init.phy_itface = PCD_PHY_EMBEDDED;
  hpcd_USB_OTG_FS.Init.Sof_enable = DISABLE;
  hpcd_USB_OTG_FS.Init.low_power_enable = DISABLE;
  hpcd_USB_OTG_FS.Init.lpm_enable = DISABLE;
  hpcd_USB_OTG_FS.Init.battery_charging_enable = DISABLE;
  hpcd_USB_OTG_FS.Init.vbus_sensing_enable = DISABLE;
  hpcd_USB_OTG_FS.Init.use_dedicated_ep1 = DISABLE;
  if (HAL_PCD_Init(&hpcd_USB_OTG_FS) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USB_OTG_FS_Init 2 */
 
  /* USER CODE END USB_OTG_FS_Init 2 */
 
}
 
static void MX_DMA_Init(void)
{
 
  /* DMA controller clock enable */
  __HAL_RCC_DMA1_CLK_ENABLE();
 
  /* DMA interrupt init */
  /* DMA1_Stream0_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn);
  /* DMA1_Stream1_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn);
 
}
 
static void MX_MDMA_Init(void)
{
 
  /* MDMA controller clock enable */
  __HAL_RCC_MDMA_CLK_ENABLE();
  /* Local variables */
 
  /* MDMA interrupt initialization */
  /* MDMA_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(MDMA_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(MDMA_IRQn);
 
}
 
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  /* USER CODE BEGIN MX_GPIO_Init_1 */
 
  /* USER CODE END MX_GPIO_Init_1 */
 
  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOE_CLK_ENABLE();
  __HAL_RCC_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  __HAL_RCC_GPIOD_CLK_ENABLE();
 
  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOE, GPIO_1_Pin|GPIO_LED_FLASH_Pin|GPIO_LED2_Pin|GPIO_LED3_Pin, GPIO_PIN_RESET);
 
  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOA, GPIO_2_Pin|GPIO_3_AIRBRAKES_AUX_Pin|GPIO4_CAMERA_Pin|GIO_SPI1_CS_Pin
                          |GPIO_LED_USB_Pin, GPIO_PIN_RESET);
 
  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOD, BARO_CS_Pin|IMU_CS_Pin|MAG_CS_Pin|GPIO_LED1_Pin, GPIO_PIN_RESET);
 
  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOC, GPIO_Pyro_EN1_Pin|GPIO_Pyro_EN2_Pin|GPIO_DaVinci_Marc_SYNC_Pin, GPIO_PIN_RESET);
 
  /*Configure GPIO pins : GPIO_1_Pin GPIO_LED_FLASH_Pin GPIO_LED2_Pin GPIO_LED3_Pin */
  GPIO_InitStruct.Pin = GPIO_1_Pin|GPIO_LED_FLASH_Pin|GPIO_LED2_Pin|GPIO_LED3_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
 
  /*Configure GPIO pins : GPIO_2_Pin GPIO_3_AIRBRAKES_AUX_Pin GPIO4_CAMERA_Pin GIO_SPI1_CS_Pin
                           GPIO_LED_USB_Pin */
  GPIO_InitStruct.Pin = GPIO_2_Pin|GPIO_3_AIRBRAKES_AUX_Pin|GPIO4_CAMERA_Pin|GIO_SPI1_CS_Pin
                          |GPIO_LED_USB_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
  /*Configure GPIO pins : BARO_CS_Pin IMU_CS_Pin MAG_CS_Pin GPIO_LED1_Pin */
  GPIO_InitStruct.Pin = BARO_CS_Pin|IMU_CS_Pin|MAG_CS_Pin|GPIO_LED1_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
 
  /*Configure GPIO pins : GPIO_Pyro_EN1_Pin GPIO_Pyro_EN2_Pin GPIO_DaVinci_Marc_SYNC_Pin */
  GPIO_InitStruct.Pin = GPIO_Pyro_EN1_Pin|GPIO_Pyro_EN2_Pin|GPIO_DaVinci_Marc_SYNC_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
  /*AnalogSwitch Config */
  HAL_SYSCFG_AnalogSwitchConfig(SYSCFG_SWITCH_PA0, SYSCFG_SWITCH_PA0_CLOSE);
 
  /*AnalogSwitch Config */
  HAL_SYSCFG_AnalogSwitchConfig(SYSCFG_SWITCH_PA1, SYSCFG_SWITCH_PA1_CLOSE);
 
  /* USER CODE BEGIN MX_GPIO_Init_2 */
 
  /* USER CODE END MX_GPIO_Init_2 */
}
 
/* USER CODE BEGIN 4 */
 
/* USER CODE END 4 */
 
/* USER CODE BEGIN Header_StartSensor_Task */
/* USER CODE END Header_StartSensor_Task */
void StartSensor_Task(void *argument)
{
  /* USER CODE BEGIN 5 */
    //xSensorsTaskHandle = xTaskGetCurrentTaskHandle();
    //static uint32_t prescaler = 0;
    uint32_t num_esecuzioni = 0;
    Data_Package_no_Linear_IMU the_rest = {0};
    osDelay(50);
    HAL_TIM_Base_Start_IT(&htim6);
 
  /* Infinite loop */
  for(;;)
  {
      //-----------------------------------------------------------------------------------every 0.25 ms
      osThreadFlagsWait(SENSOR_FLAG, osFlagsWaitAny, osWaitForever);
 
 
      lsm6dso32_acceleration_raw_get(&imu_1, data_raw_acceleration_1);
 
      acceleration_mg_1[0] = lsm6dso32_from_fs16_to_mg(data_raw_acceleration_1[0]);
      acceleration_mg_1[1] = lsm6dso32_from_fs16_to_mg(data_raw_acceleration_1[1]);
      acceleration_mg_1[2] = lsm6dso32_from_fs16_to_mg(data_raw_acceleration_1[2]);
 
 
      //Lettura IMU
      num_esecuzioni +=1;
      num_esecuzioni %= (uint32_t)40;
      if(flag_flash){
          memcpy(data_buffer + num_of_lin_acc_saved*sizeof(Linear_IMU_Package) + num_of_saved_rest_packages*sizeof(Data_Package_no_Linear_IMU),(uint8_t*) acceleration_mg_1,sizeof(Linear_IMU_Package));
          num_of_lin_acc_saved+=1;
      }
 
      if (num_esecuzioni == 0){
 
          lsm6dso32_angular_rate_raw_get(&imu_1, data_raw_angular_rate_1);
 
          bmp3_get_sensor_data(BMP3_PRESS_TEMP, &barometer_data_1, &bmp390_1);
 
          angular_rate_mdps_1[0] = lsm6dso32_from_fs2000_to_mdps(data_raw_angular_rate_1[0]);
          angular_rate_mdps_1[1] = lsm6dso32_from_fs2000_to_mdps(data_raw_angular_rate_1[1]);
          angular_rate_mdps_1[2] = lsm6dso32_from_fs2000_to_mdps(data_raw_angular_rate_1[2]);
 
          the_rest.dps_x = angular_rate_mdps_1[0];
          the_rest.dps_y = angular_rate_mdps_1[1];
          the_rest.dps_z = angular_rate_mdps_1[2];
          the_rest.temperature = barometer_data_1.temperature;
          the_rest.pressure = barometer_data_1.pressure;
 
 
          if(first_measure ){
                Pressure_1 = the_rest.pressure;
                Temperature_1 = the_rest.temperature;
                first_measure = false;
          }
        altitude = readAltitude(Pressure_1,Temperature_1,the_rest.pressure);
        z[0] = (double)altitude;
        the_rest.altitude = altitude;
        the_rest.velocity=velocity;
         z[1] = (double)gps_vel;
 
        switch (flight_state.flight_state){
                    case INVALID:
                        the_rest.phase = 0;
                    break;
                    case CALIBRATING:
                        the_rest.phase = 1;
                        break;
                    case LIFTOFF:
                        the_rest.phase = 2;
                        break;
                    case BURNING:
                        the_rest.phase = 3;
                        break;
                    case ABCSDEPLOYED:
                        the_rest.phase = 4;
                        break;
                    case DROGUE:
                        the_rest.phase = 5;
                        break;
                    case MAIN:
                        the_rest.phase = 6;
                        break;
                    case TOUCHDOWN:
                        the_rest.phase = 7;
                        break;
                }
        if(flag_flash){
            memcpy(data_buffer + num_of_lin_acc_saved*sizeof(Linear_IMU_Package)+num_of_saved_rest_packages*sizeof(Data_Package_no_Linear_IMU),(uint8_t*)& the_rest,sizeof(Data_Package_no_Linear_IMU));
            num_of_saved_rest_packages +=1;
            //-------------------------------------------------------- We enter this portion of code every 10 ms
            if (num_of_saved_rest_packages == 8) {
                num_of_saved_rest_packages_locked = 8;
                num_of_lin_acc_saved = 0;
                num_of_saved_rest_packages = 0;
            } else if (num_of_saved_rest_packages == 4) {
                num_of_saved_rest_packages_locked = 4;
            }
        }
        a_m = acceleration_mg_1[0];
        omega[0] = -angular_rate_mdps_1[2]*M_PI/180000;
        omega[1] = angular_rate_mdps_1[1]*M_PI/180000;
        omega[2] = angular_rate_mdps_1[0]*M_PI/180000;
 
        //++prescaler;
                  //if (prescaler == 100)  // 1 second
                //  {
              //      prescaler = 0;
            //        //toggle_led();
          //          LED_TOGGLE(GPIO_LED1_GPIO_Port,GPIO_LED1_Pin);
        //}
 
        osThreadFlagsSet(Kalman_mkfHandle, KALMAN_FLAG);
        //-----------------------------------------------------------------------------------------------------------------------
        //lis2mdl_magnetic_raw_get(&lis2mdl_dev_ctx, raw_mag_data);
 
        //the_rest.mag_x = lis2mdl_from_lsb_to_nanotesla(raw_mag_data[0]);
        //the_rest.mag_y = lis2mdl_from_lsb_to_nanotesla(raw_mag_data[1]);
        //the_rest.mag_z = lis2mdl_from_lsb_to_nanotesla(raw_mag_data[1]);
        //-----------------------------------------------------------------------------------------------------------------------
      }
 
  }
  /* USER CODE END 5 */
}
 
/* USER CODE BEGIN Header_StartFSM */
/* USER CODE END Header_StartFSM */
void StartFSM(void *argument)
{
  /* USER CODE BEGIN StartFSM */
    osDelay(50);
    HAL_TIM_Base_Start_IT(&htim13);
  /* Infinite loop */
  for(;;)
  {
      osThreadFlagsWait(FSM_FLAG, osFlagsWaitAny, osWaitForever);
 
      linear_acceleration_t acc1;
      acc1.accX = acceleration_mg_1[0];
      acc1.accY = acceleration_mg_1[1];
      acc1.accZ = acceleration_mg_1[2];
 
      estimation_output_t MotionData;
      MotionData.acceleration = sqrt(
              acceleration_mg_1[0]*acceleration_mg_1[0] +
              acceleration_mg_1[1]*acceleration_mg_1[1] +
              acceleration_mg_1[2]*acceleration_mg_1[2]);
      MotionData.velocity = velocity;
      MotionData.height = altitude;
 
      /* Check Flight Phases */
      check_flight_phase(&flight_state,MotionData, acc1);
  }
  /* USER CODE END StartFSM */
}
 
/* USER CODE BEGIN Header_StartKalman_mkf */
/* USER CODE END Header_StartKalman_mkf */
void StartKalman_mkf(void *argument)
{
  /* USER CODE BEGIN StartKalman_mkf */
    static uint32_t prescaler = 0;
 
    /* Infinite loop */
    for(;;) {
        osThreadFlagsWait(KALMAN_FLAG, osFlagsWaitAny, osWaitForever);
 
        if(flag_attitude) {
          attitude_estimation(q,0.01,omega[0],omega[1],omega[2],&alpha,q_new);
          alpha = M_PI/2 - alpha;
          //mekf(q,beta,P,omega, b_B, 0.0, q_new, beta_new, P_new); //mekf tinkered because of the magnetometers question, Height_AGL was replaced with permanent 0
          memcpy(q,q_new,sizeof(q_new));
        }
 
        //memcpy(beta,beta_new,sizeof(beta_new));
        //memcpy(P,P_new,sizeof(P_new));
//--------------------------------------------------------------------------------------------- TODO : to b e reintroduced with the actual mekf
     // z_component = 1 - 2*( q[0]*q[0] + q[1]*q[1] );
 
     // if(z_component < - 1){
    //    z_component = -1.0;
     // }
     // else if(z_component >  1){
    //    z_component = 1.0;
      //}
 
      //alpha = acos(z_component);
//----------------------------------------------------------------------------------------------------
        if(flag_kf) {
            kalmanFilter(x_est, P_2, z, alpha, a_m, Ts, Q, R,gnss_flag); 
            gnss_flag = false;
            altitude = x_est[0];
            velocity = x_est[1];
        } else {
            velocity = gps_vel;
        }
 
        if((num_of_saved_rest_packages_locked == 4 || num_of_saved_rest_packages_locked == 8) && flag_flash == true){
            ++prescaler;
 
            if (prescaler == 25) { // 1 second
                prescaler = 0;
                //toggle_led();
                LED_TOGGLE(GPIO_LED1_GPIO_Port,GPIO_LED1_Pin);
            }
 
 
            if(num_of_saved_rest_packages_locked == 4) {
                memcpy(cpy_buffer,data_buffer,2112);
            } else if(num_of_saved_rest_packages_locked == 8) {
                memcpy(cpy_buffer,data_buffer+2112,2112);
            }
 
            flash_address = (num_of_lin_acc_saved_flash)*sizeof(Linear_IMU_Package) + (num_of_saved_rest_packages_flash)*sizeof(Data_Package_no_Linear_IMU) +4;
            num_of_saved_rest_packages_locked = 0;
 
            if (flash_address >= (67108864 - 211200)) {
                num_of_saved_rest_packages_locked = 0;
                flag_flash = false;
            } else {
                QFlash_Write(flash_address,(uint8_t*) cpy_buffer,sizeof(cpy_buffer));
                num_of_lin_acc_saved_flash += 160;
                num_of_saved_rest_packages_flash += 4;
            }
        }
    }
  /* USER CODE END StartKalman_mkf */
}
 
/* USER CODE BEGIN Header_StartMPC_Task */
/* USER CODE END Header_StartMPC_Task */
void StartMPC_Task(void *argument)
{
  /* USER CODE BEGIN StartMPC_Task */
    static uint32_t prescaler = 0;
    osDelay(50);
    HAL_TIM_Base_Start_IT(&htim7);
 
  /* Infinite loop */
  for(;;)
  {
      osThreadFlagsWait(MPC_FLAG, osFlagsWaitAny, osWaitForever);
      HAL_ADC_Start(&hadc3);
      HAL_ADC_PollForConversion(&hadc3, HAL_MAX_DELAY);
      adcVal =(HAL_ADC_GetValue(&hadc3));
      HAL_ADC_Stop(&hadc3);
      Vin=((adcVal/3180.0f)*3.09f);
      Vr=(Vin*((2700.0f+4700.0f)/2700.0f));
      message.Battery_Voltage = Vr;
 
      message.acc_x = acceleration_mg_1[0];
      message.acc_y = acceleration_mg_1[1];
      message.acc_z = acceleration_mg_1[2];
      message.dps_x = angular_rate_mdps_1[0];
      message.dps_y = angular_rate_mdps_1[1];
      message.dps_z = angular_rate_mdps_1[2];
      switch (flight_state.flight_state){
                case INVALID:
                    message.phase = 0;
                    break;
                case CALIBRATING:
                    message.phase = 1;
                    break;
                case LIFTOFF:
                    message.phase = 2;
                    break;
                case BURNING:
                    message.phase = 3;
                    break;
                case ABCSDEPLOYED:
                    message.phase = 4;
                    break;
                case DROGUE:
                    message.phase = 5;
                    break;
                case MAIN:
                    message.phase = 6;
                    break;
                case TOUCHDOWN:
                    message.phase = 7;
                    break;
        }
      //------------------------------------------------------------------------------------------------- TODO given that magnetometers will not fly we will replace their data with other stuff
      message.mag_x = (float_t)alpha;
 
      if(altitude> max_alt){
          max_alt =altitude;
          message.mag_y = max_alt;
      }
      if(velocity> max_vel){
          max_vel =velocity;
          message.mag_z = max_vel;
 
          }
        message.temperature = barometer_data_1.temperature;
        message.pressure = barometer_data_1.pressure;
        message.altitude =  altitude;
        message.velocity = velocity;
        message.parachute_1 = drogue;
        message.parachute_2 = mainp;
        if(flag_MPC && flight_state.flight_state<DROGUE){
 
            double deg = optimalAngle( sim_time_step,
                                   x_est[0],barometer_data_1.temperature, x_est[1],
                                   barometer_data_1.pressure,message.deg, alpha, target_apogee,
                                   tolerance, mach_states,
                                   deployment_states,
                                   cd_table);
          //------------------------------------------------------------------------------------------------- TODO given that magnetometers will not fly we will replace their data with other stuff
 
            message.deg = (float_t)deg;
 
        }else if(flight_state.flight_state>=DROGUE){
            message.deg = 60.0;
        }
        ++prescaler;
        if (prescaler == 2)  // 1 second
            {
                 prescaler = 0;
            //        //toggle_led();
                 LED_TOGGLE(GPIO_LED3_GPIO_Port,GPIO_LED3_Pin);
            }
        memcpy(array,(uint8_t*)&message,sizeof(message));
        HAL_UART_Transmit_DMA(&huart1, array, sizeof(array));
 
 
  }
  /* USER CODE END StartMPC_Task */
}
 
/* USER CODE BEGIN Header_StartReceiveVel_Task */
/* USER CODE END Header_StartReceiveVel_Task */
void StartReceiveVel_Task(void *argument)
{
  /* USER CODE BEGIN StartReceiveVel_Task */
  /* Infinite loop */
  if (HAL_UART_Receive_DMA(&huart1, vel, 4) != HAL_OK) {
      Error_Handler();
  }
 
  for(;;)
  {
      uint32_t flags = osThreadFlagsWait(RECEIVED_VEL_FLAG , osFlagsWaitAny, osWaitForever);
 
      if (flags & RECEIVED_VEL_FLAG) {
          memcpy(&gps_vel,(uint8_t*)vel,sizeof(vel));
          HAL_UART_Receive_DMA(&huart1, vel, 4);
 
          gnss_flag = gps_vel > 0;
 
          if(gnss_flag) {
              LED_TOGGLE(GPIO_LED_USB_GPIO_Port, GPIO_LED_USB_Pin);
          }
      }
  }
  /* USER CODE END StartReceiveVel_Task */
}
 
 /* MPU Configuration */
 
void MPU_Config(void)
{
  MPU_Region_InitTypeDef MPU_InitStruct = {0};
 
  /* Disables the MPU */
  HAL_MPU_Disable();
 
  MPU_InitStruct.Enable = MPU_REGION_ENABLE;
  MPU_InitStruct.Number = MPU_REGION_NUMBER0;
  MPU_InitStruct.BaseAddress = 0x0;
  MPU_InitStruct.Size = MPU_REGION_SIZE_4GB;
  MPU_InitStruct.SubRegionDisable = 0x87;
  MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
  MPU_InitStruct.AccessPermission = MPU_REGION_NO_ACCESS;
  MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
  MPU_InitStruct.IsShareable = MPU_ACCESS_SHAREABLE;
  MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
  MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
 
  HAL_MPU_ConfigRegion(&MPU_InitStruct);
  /* Enables the MPU */
  HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
 
}
 
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  /* USER CODE BEGIN Callback 0 */
 
  /* USER CODE END Callback 0 */
  if (htim->Instance == TIM4)
  {
    HAL_IncTick();
  }
  /* USER CODE BEGIN Callback 1 */
  if (htim->Instance == TIM7) { //500 ms
      if (HAL_UART_GetState(&huart1) == HAL_UART_STATE_READY || HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_RX) // Ready or only busy receiving
      {
 
          osThreadFlagsSet(MPC_TaskHandle, MPC_FLAG);
      }
    }
 
  if (htim->Instance == TIM6) { // <<< Check for the 4kHz (0.25ms) timer
 
          osThreadFlagsSet(Sensor_TaskHandle, SENSOR_FLAG);
 
 
  }
 
  if(htim->Instance == TIM13){ // 10ms timer for the FSM
      osThreadFlagsSet(FSMHandle, FSM_FLAG);
  }
 
  /* USER CODE END Callback 1 */
}
 
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)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
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 */