Skip to content

Changed Touchdown transition to IMU and not velocity. #376

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jbinz wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Changed Touchdown transition to IMU and not velocity. #376

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 22 additions & 12 deletions flight_computer/src/control/flight_phases.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ static void check_ready_phase(flight_fsm_t *fsm_state, vf32_t acc_data, const co
static void check_thrusting_phase(flight_fsm_t *fsm_state, estimation_output_t state_data);
static void check_coasting_phase(flight_fsm_t *fsm_state, estimation_output_t state_data);
static void check_drogue_phase(flight_fsm_t *fsm_state, estimation_output_t state_data);
static void check_main_phase(flight_fsm_t *fsm_state, estimation_output_t state_data);
static void check_main_phase(flight_fsm_t *fsm_state, vf32_t acc_data, vf32_t gyro_data);

static void clear_fsm_memory(flight_fsm_t *fsm_state);
static void change_state_to(flight_fsm_e new_state, cats_event_e event_to_trigger, flight_fsm_t *fsm_state);
Expand Down Expand Up @@ -39,7 +39,7 @@ void check_flight_phase(flight_fsm_t *fsm_state, vf32_t acc_data, vf32_t gyro_da
check_drogue_phase(fsm_state, state_data);
break;
case MAIN:
check_main_phase(fsm_state, state_data);
check_main_phase(fsm_state, acc_data, gyro_data);
break;
case TOUCHDOWN:
default:
Expand All @@ -52,12 +52,12 @@ void check_flight_phase(flight_fsm_t *fsm_state, vf32_t acc_data, vf32_t gyro_da
static void check_calibrating_phase(flight_fsm_t *fsm_state, vf32_t acc_data, vf32_t gyro_data) {
/* Check if the IMU moved between two timesteps */
/* Add an error bound as the IMU is noisy which is accepted */
if ((fabsf(fsm_state->old_acc_data.x - acc_data.x) < ALLOWED_ACC_ERROR) &&
(fabsf(fsm_state->old_acc_data.y - acc_data.y) < ALLOWED_ACC_ERROR) &&
(fabsf(fsm_state->old_acc_data.z - acc_data.z) < ALLOWED_ACC_ERROR) &&
(fabsf(fsm_state->old_gyro_data.x - gyro_data.x) < ALLOWED_GYRO_ERROR) &&
(fabsf(fsm_state->old_gyro_data.y - gyro_data.y) < ALLOWED_GYRO_ERROR) &&
(fabsf(fsm_state->old_gyro_data.z - gyro_data.z) < ALLOWED_GYRO_ERROR)) {
if ((fabsf(fsm_state->old_acc_data.x - acc_data.x) < ALLOWED_ACC_ERROR_CALIB) &&
(fabsf(fsm_state->old_acc_data.y - acc_data.y) < ALLOWED_ACC_ERROR_CALIB) &&
(fabsf(fsm_state->old_acc_data.z - acc_data.z) < ALLOWED_ACC_ERROR_CALIB) &&
(fabsf(fsm_state->old_gyro_data.x - gyro_data.x) < ALLOWED_GYRO_ERROR_CALIB) &&
(fabsf(fsm_state->old_gyro_data.y - gyro_data.y) < ALLOWED_GYRO_ERROR_CALIB) &&
(fabsf(fsm_state->old_gyro_data.z - gyro_data.z) < ALLOWED_GYRO_ERROR_CALIB)) {
fsm_state->memory[0]++;
} else {
fsm_state->memory[0] = 0;
Expand Down Expand Up @@ -137,16 +137,26 @@ static void check_drogue_phase(flight_fsm_t *fsm_state, estimation_output_t stat
}
}

static void check_main_phase(flight_fsm_t *fsm_state, estimation_output_t state_data) {
/* If the velocity is very small we have touchdown */
if (fabsf(state_data.velocity) < VELOCITY_BOUND_TOUCHDOWN) {
/* Touchdown achieved */
static void check_main_phase(flight_fsm_t *fsm_state, vf32_t acc_data, vf32_t gyro_data) {
/* Check if the IMU moved between two timesteps */
/* Add an error bound as the IMU is noisy which is accepted */
if ((fabsf(fsm_state->old_acc_data.x - acc_data.x) < ALLOWED_ACC_ERROR_TD) &&
(fabsf(fsm_state->old_acc_data.y - acc_data.y) < ALLOWED_ACC_ERROR_TD) &&
(fabsf(fsm_state->old_acc_data.z - acc_data.z) < ALLOWED_ACC_ERROR_TD) &&
(fabsf(fsm_state->old_gyro_data.x - gyro_data.x) < ALLOWED_GYRO_ERROR_TD) &&
(fabsf(fsm_state->old_gyro_data.y - gyro_data.y) < ALLOWED_GYRO_ERROR_TD) &&
(fabsf(fsm_state->old_gyro_data.z - gyro_data.z) < ALLOWED_GYRO_ERROR_TD)) {
fsm_state->memory[0]++;
} else {
/* Touchdown not achieved */
fsm_state->memory[0] = 0;
}

/* Update old IMU value */
fsm_state->old_acc_data = acc_data;
fsm_state->old_gyro_data = gyro_data;

/* Check if we reached the threshold */
if (fsm_state->memory[0] > TOUCHDOWN_SAFETY_COUNTER) {
change_state_to(TOUCHDOWN, EV_TOUCHDOWN, fsm_state);
}
Expand Down
18 changes: 11 additions & 7 deletions flight_computer/src/control/flight_phases.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,11 @@ inline constexpr uint16_t TIME_THRESHOLD_CALIB_TO_READY = 1000;

// m/s^2, if the IMU measurement is smaller than 0.6 m/s^2 it is not considered as movement for the transition
// CALIBRATING -> READY
inline constexpr float ALLOWED_ACC_ERROR = 0.6F;
inline constexpr float ALLOWED_ACC_ERROR_CALIB = 0.6F;

// dps, if the GYRO measurement is smaller than 10 dps it is not considered as movement for the transition CALIBRATING
// -> READY
inline constexpr float ALLOWED_GYRO_ERROR = 10.0F;
inline constexpr float ALLOWED_GYRO_ERROR_CALIB = 10.0F;

/* READY */

Expand All @@ -40,11 +40,15 @@ inline constexpr uint16_t MAIN_SAFETY_COUNTER = 30;
inline constexpr uint16_t MIN_TICK_COUNTS_BETWEEN_THRUSTING_APOGEE = 1500;

/* MAIN */
// m/s, velocity needs to be smaller than this to detect touchdown
inline constexpr float VELOCITY_BOUND_TOUCHDOWN = 3.0F;

// num iterations, for at least 1s it needs to be smaller
inline constexpr uint16_t TOUCHDOWN_SAFETY_COUNTER = 100;
// m/s^2, if the IMU measurement is smaller than 0.6 m/s^2 it is not considered as movement for the transition
// MAIN -> TOUCHDWON
inline constexpr float ALLOWED_ACC_ERROR_TD = 0.6F;
// dps, if the GYRO measurement is smaller than 10 dps it is not considered as movement for the transition MAIN
// -> TOUCHDWON
inline constexpr float ALLOWED_GYRO_ERROR_TD = 10.0F;

// num iterations, for at least 3s there can't be imu movement
inline constexpr uint16_t TOUCHDOWN_SAFETY_COUNTER = 300;

/* Function which implements the FSM */
void check_flight_phase(flight_fsm_t *fsm_state, vf32_t acc_data, vf32_t gyro_data, estimation_output_t state_data,
Expand Down