RTOS Tasks

Implementations of the main application threads (RTOS tasks). More...

Collaboration diagram for RTOS Tasks:

Functions
void	StartSensor_Task (void *argument)
	Function implementing the Sensor_Task thread.
void	StartFSM (void *argument)
	Function implementing the FSM thread.
void	StartKalman_mkf (void *argument)
	Function implementing the Kalman_mkf thread.
void	StartMPC_Task (void *argument)
	Function implementing the MPC_Task thread.

Detailed Description

Implementations of the main application threads (RTOS tasks).

Function Documentation

StartFSM()

void StartFSM

(

void *

argument

)

Function implementing the FSM thread.

Parameters

argument

Not used

Return values

None

Definition at line 1721 of file main.c.

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

References estimation_output_t::acceleration, acceleration_mg_1, linear_acceleration_t::accX, linear_acceleration_t::accY, linear_acceleration_t::accZ, altitude, check_flight_phase(), flight_state, FSM_FLAG, estimation_output_t::height, htim13, estimation_output_t::velocity, and velocity.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

StartKalman_mkf()

void StartKalman_mkf

(

void *

argument

)

Function implementing the Kalman_mkf thread.

Parameters

argument

Not used

Return values

None

gnss_flag 0: no fix dal GNSS, 1:fix dal GNSS

Definition at line 1758 of file main.c.

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

References a_m, alpha, altitude, attitude_estimation(), cpy_buffer, data_buffer, flag_attitude, flag_flash, flag_kf, flash_address, gnss_flag, gps_vel, KALMAN_FLAG, kalmanFilter(), num_of_lin_acc_saved_flash, num_of_saved_rest_packages_flash, num_of_saved_rest_packages_locked, omega, P_2, q, Q, q_new, QFlash_Write(), R, Ts, velocity, x_est, and z.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

StartMPC_Task()

void StartMPC_Task

(

void *

argument

)

Function implementing the MPC_Task thread.

Parameters

argument

Not used

Return values

None

Definition at line 1837 of file main.c.

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

References ABCSDEPLOYED, Data_Package_For_Marconi::acc_x, Data_Package_For_Marconi::acc_y, Data_Package_For_Marconi::acc_z, acceleration_mg_1, adcVal, alpha, Data_Package_For_Marconi::altitude, altitude, angular_rate_mdps_1, array, barometer_data_1, Data_Package_For_Marconi::Battery_Voltage, BURNING, CALIBRATING, cd_table, Data_Package_For_Marconi::deg, deployment_states, Data_Package_For_Marconi::dps_x, Data_Package_For_Marconi::dps_y, Data_Package_For_Marconi::dps_z, DROGUE, drogue, flag_MPC, flight_fsm_t::flight_state, flight_state, hadc3, htim7, huart1, INVALID, LIFTOFF, mach_states, Data_Package_For_Marconi::mag_x, Data_Package_For_Marconi::mag_y, Data_Package_For_Marconi::mag_z, MAIN, mainp, max_alt, max_vel, message, MPC_FLAG, optimalAngle(), Data_Package_For_Marconi::parachute_1, Data_Package_For_Marconi::parachute_2, Data_Package_For_Marconi::phase, bmp3_data::pressure, Data_Package_For_Marconi::pressure, sim_time_step, target_apogee, bmp3_data::temperature, Data_Package_For_Marconi::temperature, tolerance, TOUCHDOWN, Data_Package_For_Marconi::velocity, velocity, Vin, Vr, and x_est.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

StartSensor_Task()

void StartSensor_Task

(

void *

argument

)

Function implementing the Sensor_Task thread.

Parameters

argument

Not used

Return values

None

Definition at line 1588 of file main.c.

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

References a_m, ABCSDEPLOYED, acceleration_mg_1, Data_Package_no_Linear_IMU::altitude, altitude, angular_rate_mdps_1, barometer_data_1, bmp390_1, bmp3_get_sensor_data(), BMP3_PRESS_TEMP, BURNING, CALIBRATING, data_buffer, data_raw_acceleration_1, data_raw_angular_rate_1, Data_Package_no_Linear_IMU::dps_x, Data_Package_no_Linear_IMU::dps_y, Data_Package_no_Linear_IMU::dps_z, DROGUE, first_measure, flag_flash, flight_fsm_t::flight_state, flight_state, gps_vel, htim6, imu_1, INVALID, KALMAN_FLAG, Kalman_mkfHandle, LIFTOFF, lsm6dso32_acceleration_raw_get(), lsm6dso32_angular_rate_raw_get(), lsm6dso32_from_fs16_to_mg(), lsm6dso32_from_fs2000_to_mdps(), MAIN, num_of_lin_acc_saved, num_of_saved_rest_packages, num_of_saved_rest_packages_locked, omega, Data_Package_no_Linear_IMU::phase, bmp3_data::pressure, Data_Package_no_Linear_IMU::pressure, Pressure_1, readAltitude(), SENSOR_FLAG, bmp3_data::temperature, Data_Package_no_Linear_IMU::temperature, Temperature_1, TOUCHDOWN, Data_Package_no_Linear_IMU::velocity, velocity, and z.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function: