flight_control.c File Reference

#include "flight_control.h"
#include "z_qflash_W25QXXX.h"

Include dependency graph for flight_control.c:

Go to the source code of this file.

Functions
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)
	Main FSM router function that delegates to the current state's handler.
void	check_Calibrating_phase (flight_fsm_t *fsm_state, estimation_output_t MotionData, linear_acceleration_t acc_data)
	Handler for the CALIBRATING state.
void	check_Liftoff_phase (flight_fsm_t *fsm_state, estimation_output_t MotionData)
	Handler for the LIFTOFF state.
void	check_Burning_phase (flight_fsm_t *phase, estimation_output_t MotionData)
	Handler for the BURNING state (powered ascent).
void	check_AbcsDeployed_phase (flight_fsm_t *phase, estimation_output_t MotionData)
	Handler for the ABCSDEPLOYED state (apogee control).
void	check_Drogue_phase (flight_fsm_t *phase, estimation_output_t MotionData)
	Handler for the DROGUE state (descent under drogue parachute).
void	check_Main_phase (flight_fsm_t *phase, estimation_output_t MotionData)
	Handler for the MAIN state (descent under main parachute).
void	change_state_to (flight_fsm_t *phase, flight_phase_t new_phase)
	Handles the mechanics of a state transition.

Variables
float_t	previous_altitude = 0

Function Documentation

change_state_to()

void change_state_to	(	flight_fsm_t *	fsm_state,
		flight_phase_t	new_state
	)

Handles the mechanics of a state transition.

This utility function updates the FSM's state to the new phase, sets the state_changed flag, and records the transition timestamp. It ensures state changes are handled consistently.

Parameters

[in,out]	fsm_state	Pointer to the FSM state object to be updated.
[in]	new_state	The new flight_phase_t to transition into.

Returns

None

Definition at line 229 of file flight_control.c.

                                                                     {
 
    phase->flight_state=new_phase;
    clear_fsm_memory(phase);
}

References clear_fsm_memory(), and flight_fsm_t::flight_state.

Referenced by check_AbcsDeployed_phase(), check_Burning_phase(), check_Calibrating_phase(), check_Drogue_phase(), check_Liftoff_phase(), and check_Main_phase().

Here is the call graph for this function:

Here is the caller graph for this function:

check_AbcsDeployed_phase()

void check_AbcsDeployed_phase	(	flight_fsm_t *	fsm_state,
		estimation_output_t	MotionData
	)

Handler for the ABCSDEPLOYED state (apogee control).

Monitors for apogee detection (velocity changing from positive to negative) to trigger drogue parachute deployment.

Parameters

[in,out]	fsm_state	Pointer to the FSM state object.
[in]	MotionData	Filtered estimation data (altitude, velocity).

Definition at line 162 of file flight_control.c.

                                                                                  {
    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);
    }
}

References ABCSDEPLOYED, APOGEE_SAFETY_COUNTER, change_state_to(), DROGUE, drogue, flag_attitude, flag_MPC, flight_fsm_t::flight_state, estimation_output_t::height, flight_fsm_t::memory, and previous_altitude.

Referenced by check_flight_phase().

Here is the call graph for this function:

Here is the caller graph for this function:

check_Burning_phase()

void check_Burning_phase	(	flight_fsm_t *	fsm_state,
		estimation_output_t	MotionData
	)

Handler for the BURNING state (powered ascent).

Monitors for motor burnout and transitions to the coasting/apogee control phase.

Parameters

[in,out]	fsm_state	Pointer to the FSM state object.
[in]	MotionData	Filtered estimation data (altitude, velocity).

Definition at line 126 of file flight_control.c.

                                                                              {
    // 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);
    }
//--------------------------------------------------------------------------------------------
 
 
}

References ABCSDEPLOYED, APOGEE_SAFETY_COUNTER, BURNING, change_state_to(), COASTING_SAFETY_COUNTER, DROGUE, drogue, flag_attitude, flag_MPC, flight_fsm_t::flight_state, estimation_output_t::height, flight_fsm_t::memory, flight_fsm_t::memory_bis, and previous_altitude.

Referenced by check_flight_phase().

Here is the call graph for this function:

Here is the caller graph for this function:

check_Calibrating_phase()

void check_Calibrating_phase	(	flight_fsm_t *	fsm_state,
		estimation_output_t	MotionData,
		linear_acceleration_t	acc_data
	)

Handler for the CALIBRATING state.

Monitors for a sustained high vertical acceleration that indicates liftoff.

Parameters

[in,out]	fsm_state	Pointer to the FSM state object.
[in]	MotionData	Filtered estimation data (altitude, velocity).
[in]	acc_data	Raw linear acceleration data from the IMU.

Definition at line 56 of file flight_control.c.

                                                                                                                   {
    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);
    }
//--------------------------------------------------------------------------------------------
 
}

References linear_acceleration_t::accX, AIRBRAKES_SAFETY_COUNTER, CALIBRATING, change_state_to(), flag_attitude, flag_flash, flight_fsm_t::flight_state, estimation_output_t::height, LIFTOFF, LIFTOFF_SAFETY_COUNTER, LIFTOFF_TRESHOLD_ACC, flight_fsm_t::memory, flight_fsm_t::memory_bis, and QFlash_Write().

Referenced by check_flight_phase().

Here is the call graph for this function:

Here is the caller graph for this function:

check_Drogue_phase()

void check_Drogue_phase	(	flight_fsm_t *	fsm_state,
		estimation_output_t	MotionData
	)

Handler for the DROGUE state (descent under drogue parachute).

Monitors altitude to trigger main parachute deployment at a predefined height.

Parameters

[in,out]	fsm_state	Pointer to the FSM state object.
[in]	MotionData	Filtered estimation data (altitude, velocity).

Definition at line 181 of file flight_control.c.

                                                                             {
    // 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);
    }
}

References change_state_to(), DROGUE, flight_fsm_t::flight_state, estimation_output_t::height, MAIN, MAIN_DEPLOY_HEIGHT, MAIN_SAFETY_COUNTER, mainp, and flight_fsm_t::memory.

Referenced by check_flight_phase().

Here is the call graph for this function:

Here is the caller graph for this function:

check_flight_phase()

void check_flight_phase	(	flight_fsm_t *	fsm_state,
		estimation_output_t	MotionData,
		linear_acceleration_t	acc_data
	)

Main FSM router function that delegates to the current state's handler.

This function acts as a switchboard. Based on the flight_state member of the FSM object, it calls the appropriate handler function for the current phase (e.g., check_Calibrating_phase, check_Liftoff_phase, etc.).

Parameters

[in,out]	fsm_state	Pointer to the FSM state object.
[in]	MotionData	The latest output from the state estimation filter.
[in]	acc_data	The latest raw linear acceleration data.

Returns

None

Definition at line 17 of file flight_control.c.

                                                                                                            {
 
     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;
}

References ABCSDEPLOYED, BURNING, CALIBRATING, check_AbcsDeployed_phase(), check_Burning_phase(), check_Calibrating_phase(), check_Drogue_phase(), check_Liftoff_phase(), check_Main_phase(), DROGUE, flight_fsm_t::flight_state, INVALID, LIFTOFF, MAIN, flight_fsm_t::state_changed, and TOUCHDOWN.

Referenced by StartFSM().

Here is the call graph for this function:

Here is the caller graph for this function:

check_Liftoff_phase()

void check_Liftoff_phase	(	flight_fsm_t *	fsm_state,
		estimation_output_t	MotionData
	)

Handler for the LIFTOFF state.

Monitors for the end of the motor burn phase based on acceleration drop.

Parameters

[in,out]	fsm_state	Pointer to the FSM state object.
[in]	MotionData	Filtered estimation data (altitude, velocity).

Definition at line 93 of file flight_control.c.

                                                                                 {
    // 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);
    }
//--------------------------------------------------------------------------------------------
}

References APOGEE_SAFETY_COUNTER, BURNING, change_state_to(), COASTING_SAFETY_COUNTER, DROGUE, drogue, flag_attitude, flag_kf, flag_MPC, flight_fsm_t::flight_state, estimation_output_t::height, LIFTOFF, flight_fsm_t::memory, flight_fsm_t::memory_bis, and previous_altitude.

Referenced by check_flight_phase().

Here is the call graph for this function:

Here is the caller graph for this function:

check_Main_phase()

void check_Main_phase	(	flight_fsm_t *	fsm_state,
		estimation_output_t	MotionData
	)

Handler for the MAIN state (descent under main parachute).

Monitors for a stable, low altitude and a spike in acceleration to detect landing.

Parameters

[in,out]	fsm_state	Pointer to the FSM state object.
[in]	MotionData	Filtered estimation data (altitude, velocity).

Definition at line 197 of file flight_control.c.

                                                                           {
    // 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);
    }
 
}

References change_state_to(), flag_flash, flight_fsm_t::flight_state, MAIN, flight_fsm_t::memory, TOUCHDOWN, TOUCHDOWN_SAFETY_COUNTER, estimation_output_t::velocity, and VELOCITY_BOUND_TOUCHDOWN.

Referenced by check_flight_phase().

Here is the call graph for this function:

Here is the caller graph for this function:

clear_fsm_memory()

static void clear_fsm_memory ( flight_fsm_t *  fsm_state )

static

Definition at line 222 of file flight_control.c.

                                                  {
    phase->clock_memory = 0;
    phase->memory[0] = 0;
    phase->memory[1] = 0;
    phase->memory[2] = 0;
}

References flight_fsm_t::clock_memory, and flight_fsm_t::memory.

Referenced by change_state_to().

Here is the caller graph for this function:

Variable Documentation

previous_altitude

float_t previous_altitude = 0

Definition at line 13 of file flight_control.c.

Referenced by check_AbcsDeployed_phase(), check_Burning_phase(), and check_Liftoff_phase().