flight_control.c

Go to the documentation of this file.

/*
 * FlightControl.c
 *
 *  Created on: Apr 2, 2024
 *      Author: Tommaso & Francesco Abate
 */
#ifndef FLIGHTCONTROL_H
#include "flight_control.h"
#endif
 
#include "z_qflash_W25QXXX.h"
 
float_t previous_altitude = 0;
 
static void clear_fsm_memory(flight_fsm_t *fsm_state);
 
void check_flight_phase(flight_fsm_t *phase, estimation_output_t MotionData,linear_acceleration_t acc_data) {
 
     const flight_fsm_t old_phase = *phase;
 
     switch (phase->flight_state) {
         case CALIBRATING:
              check_Calibrating_phase(phase,MotionData,acc_data);
              break;
 
        case LIFTOFF:
              check_Liftoff_phase(phase, MotionData);
              break;
 
        case BURNING:
              check_Burning_phase(phase, MotionData);
              break;
 
        case ABCSDEPLOYED:
              check_AbcsDeployed_phase(phase,MotionData);
              break;
 
        case DROGUE:
              check_Drogue_phase(phase,MotionData);
              break;
 
        case MAIN:
              check_Main_phase(phase, MotionData);
              break;
 
        case TOUCHDOWN:
              break;
        case INVALID:
              break;
 
      }
 
     phase->state_changed = old_phase.flight_state != phase->flight_state;
}
 
void check_Calibrating_phase(flight_fsm_t *fsm_state,estimation_output_t MotionData,linear_acceleration_t acc_data){
    if(fsm_state->flight_state > CALIBRATING) return;
 
    if (acc_data.accX>LIFTOFF_TRESHOLD_ACC) {
 
            fsm_state->memory[0]++;
        } else {
 
            fsm_state->memory[0] = 0;
        }
 
    if(fsm_state->memory[0] > AIRBRAKES_SAFETY_COUNTER) {
 
            float_t flag = 1.0;
            QFlash_Write(0, (uint8_t*)&flag, 4);
            flag_flash = true;
            flag_attitude = true;
            change_state_to(fsm_state, LIFTOFF);
        }
//----------------------------------------------------------------------------------------- JUST IN CASE WE HAVE AN UMEXPECTED APOGEE
    if (MotionData.height > 20) {
        fsm_state->memory_bis[0]++;
    } else {
        fsm_state->memory_bis[0] = 0;
    }
 
    if(fsm_state->memory_bis[0] > LIFTOFF_SAFETY_COUNTER){
        float_t flag = 1.0;
        QFlash_Write(0, (uint8_t*)&flag, 4);
        flag_flash = true;
        flag_attitude = true;
        change_state_to(fsm_state,LIFTOFF);
    }
//--------------------------------------------------------------------------------------------
 
}
 
void check_Liftoff_phase(flight_fsm_t *fsm_state,estimation_output_t MotionData) {
    // if I'm in a higher state I cannot come back
    if(fsm_state->flight_state > LIFTOFF) return;
 
    if (MotionData.height > 500) {
        fsm_state->memory[0]++;
      } else {
          fsm_state->memory[0] = 0;
      }
 
    if (fsm_state->memory[0] > COASTING_SAFETY_COUNTER) {
        flag_kf = true;
        change_state_to(fsm_state, BURNING);//ABCSDEPLOYED
    }
 
 
//----------------------------------------------------------------------------------------- JUST IN CASE WE HAVE AN UMEXPECTED APOGEE
    if (MotionData.height < previous_altitude) {
        fsm_state->memory_bis[0]++;
    } else {
        fsm_state->memory_bis[0] = 0;
        previous_altitude = MotionData.height;
    }
 
    if(fsm_state->memory_bis[0] > APOGEE_SAFETY_COUNTER) {
        drogue = 1;
        flag_MPC = false;
        flag_attitude = false;
        change_state_to(fsm_state,DROGUE);
    }
//--------------------------------------------------------------------------------------------
}
 
void check_Burning_phase(flight_fsm_t *phase, estimation_output_t MotionData) {
    // if I'm in a higher state I cannot come back
 
    if(phase->flight_state > BURNING) return;
 
    if (MotionData.height > 1000) {
        phase->memory[0]++;
      } else {
          phase->memory[0] = 0;
      }
 
    if (phase->memory[0] > COASTING_SAFETY_COUNTER) {
        flag_MPC = true;
        flag_attitude = false;
        change_state_to(phase, ABCSDEPLOYED);//ABCSDEPLOYED
    }
 
//----------------------------------------------------------------------------------------- JUST IN CASE WE HAVE AN UMEXPECTED APOGEE
    if (MotionData.height < previous_altitude) {
        phase->memory_bis[0]++;
    } else{
        phase->memory_bis[0] = 0;
        previous_altitude = MotionData.height;
    }
 
    if(phase->memory_bis[0] > APOGEE_SAFETY_COUNTER){
        drogue = 1;
        flag_MPC = false;
        flag_attitude = false;
        change_state_to(phase,DROGUE);
    }
//--------------------------------------------------------------------------------------------
 
 
}
 
void check_AbcsDeployed_phase(flight_fsm_t *phase, estimation_output_t MotionData){
    if(phase->flight_state > ABCSDEPLOYED) return;
 
    if (MotionData.height < previous_altitude) {
            phase->memory[0]++;
    } else {
        phase->memory[0] = 0;
        previous_altitude = MotionData.height;
    }
 
    if(phase->memory[0] > APOGEE_SAFETY_COUNTER){
        drogue = 1;
        flag_MPC = false;
        flag_attitude = false;
        change_state_to(phase,DROGUE);
    }
}
 
 
void check_Drogue_phase(flight_fsm_t *phase, estimation_output_t MotionData) {
    // if I'm in a higher state I cannot come back
    if(phase->flight_state > DROGUE) return;
 
    if(MotionData.height < MAIN_DEPLOY_HEIGHT) {
        phase->memory[0]++;
    } else {
        phase->memory[0] = 0;
    }
 
    if(phase->memory[0] > MAIN_SAFETY_COUNTER) {
        mainp = 1;
        change_state_to(phase, MAIN);
    }
}
 
void check_Main_phase(flight_fsm_t *phase, estimation_output_t MotionData) {
    // if I'm in a higher state I cannot come back
    if(phase->flight_state > MAIN) return;
 
    //HAL_GPIO_WritePin(GPIOC, E_Match_Parachute_2_Pin, GPIO_PIN_SET);
 
    /* If the velocity is very small we have touchdown */
    // check the altitude for a specific amount of time
    if (fabsf(MotionData.velocity) < VELOCITY_BOUND_TOUCHDOWN) {
//      /* Touchdown achieved */
        phase->memory[0]++;
    } else {
        /* Touchdown not achieved */
        phase->memory[0] = 0;
    }
 
    if (phase->memory[0] > TOUCHDOWN_SAFETY_COUNTER) {
        flag_flash = false;
        change_state_to(phase, TOUCHDOWN);
    }
 
}
 
 
/* Function that needs to be called every time that a state transition is done */
static void clear_fsm_memory(flight_fsm_t *phase) {
    phase->clock_memory = 0;
    phase->memory[0] = 0;
    phase->memory[1] = 0;
    phase->memory[2] = 0;
}
 
 void change_state_to(flight_fsm_t *phase, flight_phase_t new_phase) {
 
    phase->flight_state=new_phase;
    clear_fsm_memory(phase);
}