state_transition.c
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /******************************************************************************* | ||
| 2 | * * | ||
| 3 | * FILE: * | ||
| 4 | * launch_detect.c * | ||
| 5 | * * | ||
| 6 | * DESCRIPTION: * | ||
| 7 | * Includes functions that handle launch detection based on sensor * | ||
| 8 | * readouts. * | ||
| 9 | * * | ||
| 10 | * CRITICALITY: * | ||
| 11 | * FQ - Flight Qualified * | ||
| 12 | * * | ||
| 13 | * COPYRIGHT: * | ||
| 14 | * Copyright (c) 2025 Sun Devil Rocketry. * | ||
| 15 | * All rights reserved. * | ||
| 16 | * * | ||
| 17 | * This software is licensed under terms that can be found in the LICENSE * | ||
| 18 | * file in the root directory of this software component. * | ||
| 19 | * If no LICENSE file comes with this software, it is covered under the * | ||
| 20 | * BSD-3-Clause. * | ||
| 21 | * * | ||
| 22 | * https://opensource.org/license/bsd-3-clause * | ||
| 23 | * * | ||
| 24 | *******************************************************************************/ | ||
| 25 | |||
| 26 | /*------------------------------------------------------------------------------ | ||
| 27 | Standard Includes | ||
| 28 | ------------------------------------------------------------------------------*/ | ||
| 29 | #include <stdint.h> | ||
| 30 | #include <stdbool.h> | ||
| 31 | |||
| 32 | /*------------------------------------------------------------------------------ | ||
| 33 | Project Includes | ||
| 34 | ------------------------------------------------------------------------------*/ | ||
| 35 | |||
| 36 | /* Application Layer */ | ||
| 37 | #include "main.h" | ||
| 38 | |||
| 39 | /* Error Handling */ | ||
| 40 | #include "error_sdr.h" | ||
| 41 | |||
| 42 | #include "math_sdr.h" | ||
| 43 | |||
| 44 | /*------------------------------------------------------------------------------ | ||
| 45 | Global Variables | ||
| 46 | ------------------------------------------------------------------------------*/ | ||
| 47 | |||
| 48 | /* DAQ */ | ||
| 49 | extern PRESET_DATA preset_data; /* Struct with preset data */ | ||
| 50 | extern SENSOR_DATA sensor_data; /* Struct with all sensor */ | ||
| 51 | |||
| 52 | /*------------------------------------------------------------------------------ | ||
| 53 | Procedures | ||
| 54 | ------------------------------------------------------------------------------*/ | ||
| 55 | |||
| 56 | /********************************************************************************* | ||
| 57 | * * | ||
| 58 | * FUNCTION: * | ||
| 59 | * launch_detection * | ||
| 60 | * * | ||
| 61 | * DESCRIPTION: * | ||
| 62 | * Launch detection using acceleration or baro readout. Sets * | ||
| 63 | * launch_detect_flag to true if detected. * | ||
| 64 | * * | ||
| 65 | * NOTE: * | ||
| 66 | * Only use in the main application loop * | ||
| 67 | * * | ||
| 68 | *********************************************************************************/ | ||
| 69 | 72 | bool launch_detection | |
| 70 | ( | ||
| 71 | uint32_t* launch_detect_time | ||
| 72 | ) | ||
| 73 | { | ||
| 74 | 72 | static uint8_t acc_detect_cnts = 0; | |
| 75 | 72 | static uint8_t baro_detect_cnts = 0; | |
| 76 | 72 | float accX = sensor_data.imu_converted.accel_x; | |
| 77 | 72 | float pressure = sensor_data.baro_pressure; | |
| 78 | 72 | float acc_scalar = fabsf(accX); | |
| 79 | |||
| 80 |
2/2✓ Branch 0 taken 48 times.
✓ Branch 1 taken 24 times.
|
72 | if ( preset_data.config_settings.enabled_features & LAUNCH_DETECT_ACCEL_ENABLED ) |
| 81 | { | ||
| 82 |
2/2✓ Branch 0 taken 41 times.
✓ Branch 1 taken 7 times.
|
48 | if ( acc_scalar > (float)preset_data.config_settings.launch_detect_accel_threshold * 9.81f ) |
| 83 | { | ||
| 84 | // Count detection counts | ||
| 85 | 41 | acc_detect_cnts++; | |
| 86 | } | ||
| 87 | else | ||
| 88 | { | ||
| 89 | 7 | acc_detect_cnts = 0; | |
| 90 | } | ||
| 91 | } | ||
| 92 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 48 times.
|
72 | if ( preset_data.config_settings.enabled_features & LAUNCH_DETECT_BARO_ENABLED ) |
| 93 | { | ||
| 94 |
2/2✓ Branch 0 taken 11 times.
✓ Branch 1 taken 13 times.
|
24 | if ( pressure < ( preset_data.baro_preset.baro_pres - preset_data.config_settings.launch_detect_baro_threshold ) ) |
| 95 | { | ||
| 96 | 11 | baro_detect_cnts++; | |
| 97 | } | ||
| 98 | else | ||
| 99 | { | ||
| 100 | 13 | baro_detect_cnts = 0; | |
| 101 | } | ||
| 102 | } | ||
| 103 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 48 times.
|
72 | if ( !( preset_data.config_settings.enabled_features & ( LAUNCH_DETECT_BARO_ENABLED | LAUNCH_DETECT_ACCEL_ENABLED ) ) ) |
| 104 | { | ||
| 105 | /* neither case is hit. for now, throw an error. */ | ||
| 106 | 24 | error_fail_fast( ERROR_UNSUPPORTED_OP_ERROR ); /* DOES NOT MEET FQ STANDARD. */ | |
| 107 | } | ||
| 108 | |||
| 109 | |||
| 110 | // Trigger the flag once pass the threshold for number of times | ||
| 111 |
2/2✓ Branch 0 taken 71 times.
✓ Branch 1 taken 1 time.
|
72 | if ( acc_detect_cnts > preset_data.config_settings.launch_detect_accel_samples |
| 112 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 70 times.
|
71 | || baro_detect_cnts > preset_data.config_settings.launch_detect_baro_samples ) |
| 113 | { | ||
| 114 | 2 | *launch_detect_time = HAL_GetTick(); | |
| 115 | 2 | return true; | |
| 116 | } | ||
| 117 | else | ||
| 118 | { | ||
| 119 | return false; | ||
| 120 | } | ||
| 121 | |||
| 122 | } /* launch_detection */ | ||
| 123 | |||
| 124 | |||
| 125 | /********************************************************************************* | ||
| 126 | * * | ||
| 127 | * FUNCTION: * | ||
| 128 | * apogee_detect * | ||
| 129 | * * | ||
| 130 | * DESCRIPTION: * | ||
| 131 | * Detects apogee based on a series of decreasing altitude values. * | ||
| 132 | * If the barometric altitude decreases for 'apogee_detect_window' * | ||
| 133 | * consecutive samples, apogee is detected. * | ||
| 134 | * * | ||
| 135 | *********************************************************************************/ | ||
| 136 | 33 | bool apogee_detect | |
| 137 | ( | ||
| 138 | void | ||
| 139 | ) | ||
| 140 | { | ||
| 141 | 33 | static float prev_alt = 0.0f; | |
| 142 | 33 | static uint8_t decreasing_count = 0; | |
| 143 | 33 | float curr_alt = sensor_data.baro_alt; | |
| 144 | |||
| 145 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 time.
|
33 | if( prev_alt != 0.0f ) |
| 146 | { | ||
| 147 |
2/2✓ Branch 0 taken 19 times.
✓ Branch 1 taken 13 times.
|
32 | if( curr_alt < prev_alt ) |
| 148 | { | ||
| 149 | 19 | decreasing_count++; | |
| 150 | } | ||
| 151 | else | ||
| 152 | { | ||
| 153 | 13 | decreasing_count = 0; | |
| 154 | } | ||
| 155 | } | ||
| 156 | |||
| 157 | 33 | prev_alt = curr_alt; | |
| 158 | |||
| 159 |
2/2✓ Branch 0 taken 21 times.
✓ Branch 1 taken 12 times.
|
33 | if( decreasing_count >= preset_data.config_settings.apogee_detect_samples ) |
| 160 | { | ||
| 161 | return true; /* Apogee detected */ | ||
| 162 | } | ||
| 163 | else | ||
| 164 | { | ||
| 165 | 21 | return false; /* Apogee not detected */ | |
| 166 | } | ||
| 167 | |||
| 168 | } /* apogee_detect */ | ||
| 169 | |||
| 170 | |||
| 171 | /********************************************************************************* | ||
| 172 | * * | ||
| 173 | * FUNCTION: * | ||
| 174 | * coast_detect * | ||
| 175 | * * | ||
| 176 | * DESCRIPTION: * | ||
| 177 | * Detects initial motor burnout by measuring a sharp drop in acceleration * | ||
| 178 | * on the axis of thrust. * | ||
| 179 | * * | ||
| 180 | *********************************************************************************/ | ||
| 181 | 32 | bool coast_detect | |
| 182 | ( | ||
| 183 | void | ||
| 184 | ) | ||
| 185 | { | ||
| 186 | /* local variables */ | ||
| 187 | 32 | static uint8_t positive_readings = 0; | |
| 188 | |||
| 189 | /* check if accel is sufficiently low */ | ||
| 190 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 12 times.
|
32 | if ( sensor_data.imu_converted.accel_x < COAST_DETECT_THRESHOLD * GRAVITY ) |
| 191 | { | ||
| 192 | 20 | positive_readings++; | |
| 193 | } | ||
| 194 | else | ||
| 195 | { | ||
| 196 | 12 | positive_readings = 0; | |
| 197 | } | ||
| 198 | |||
| 199 | /* check if we've hit our sample threshold */ | ||
| 200 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 2 times.
|
32 | if( positive_readings >= COAST_DETECT_SAMPLES ) |
| 201 | { | ||
| 202 | return true; | ||
| 203 | } | ||
| 204 | else | ||
| 205 | { | ||
| 206 | 30 | return false; | |
| 207 | } | ||
| 208 | |||
| 209 | } /* coast_detect */ | ||
| 210 |