app/rev2/flight.c
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /******************************************************************************* | ||
| 2 | * * | ||
| 3 | * FILE: * | ||
| 4 | * flight.c * | ||
| 5 | * * | ||
| 6 | * DESCRIPTION: * | ||
| 7 | * Flight-qualified partition of APPA. Contains application loop for * | ||
| 8 | * calibration state and beyond. * | ||
| 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 | /*------------------------------------------------------------------------------ | ||
| 28 | Includes | ||
| 29 | ------------------------------------------------------------------------------*/ | ||
| 30 | #include <math.h> | ||
| 31 | #include "main.h" | ||
| 32 | #include "led.h" | ||
| 33 | #include "lora.h" | ||
| 34 | #include "usb.h" | ||
| 35 | #include "math.h" | ||
| 36 | #include "sensor.h" | ||
| 37 | #include "buzzer.h" | ||
| 38 | #include "debug_sdr.h" | ||
| 39 | #include "error_sdr.h" | ||
| 40 | #include "ignition.h" | ||
| 41 | #include "telemetry.h" | ||
| 42 | |||
| 43 | /*------------------------------------------------------------------------------ | ||
| 44 | Global Variables | ||
| 45 | ------------------------------------------------------------------------------*/ | ||
| 46 | extern PID_DATA pid_data; | ||
| 47 | extern SENSOR_DATA sensor_data; | ||
| 48 | extern SERVO_PRESET servo_preset; | ||
| 49 | extern PRESET_DATA preset_data; | ||
| 50 | |||
| 51 | /* Timing (debug) */ | ||
| 52 | #ifdef DEBUG | ||
| 53 | extern volatile uint32_t debug_previous; | ||
| 54 | extern volatile uint32_t debug_delta; | ||
| 55 | #endif | ||
| 56 | |||
| 57 | /*------------------------------------------------------------------------------ | ||
| 58 | Local Variables | ||
| 59 | ------------------------------------------------------------------------------*/ | ||
| 60 | float target; | ||
| 61 | float kP; | ||
| 62 | float kI; | ||
| 63 | float kD; | ||
| 64 | float new_time; | ||
| 65 | float delta_time; | ||
| 66 | float angle; | ||
| 67 | float feedback; | ||
| 68 | float error; | ||
| 69 | float pVal = 0; | ||
| 70 | float iVal = 0; | ||
| 71 | float dVal = 0; | ||
| 72 | float prevErr = 0; | ||
| 73 | uint32_t time_inc = 0; | ||
| 74 | uint32_t pid_start_time = 0; | ||
| 75 | uint32_t pid_previous = 0; | ||
| 76 | uint32_t pid_delta = 0; | ||
| 77 | uint32_t launch_detect_time = 0; | ||
| 78 | uint32_t last_flash_timestamp = 0; | ||
| 79 | |||
| 80 | typedef enum _PID_SETUP_SUBCOM{ | ||
| 81 | PID_READ = 0x10, | ||
| 82 | PID_MODIFY_STATIC = 0x11, | ||
| 83 | PID_MODIFY_DYNAMIC = 0x12, | ||
| 84 | PID_SETUP_EXIT = 0x13 | ||
| 85 | } PID_SETUP_SUBCOM; | ||
| 86 | |||
| 87 | |||
| 88 | /*------------------------------------------------------------------------------ | ||
| 89 | Static Procedures | ||
| 90 | ------------------------------------------------------------------------------*/ | ||
| 91 | |||
| 92 | static void update_state | ||
| 93 | ( | ||
| 94 | void | ||
| 95 | ); | ||
| 96 | |||
| 97 | static void set_state_color | ||
| 98 | ( | ||
| 99 | bool flash_full | ||
| 100 | ); | ||
| 101 | |||
| 102 | /*------------------------------------------------------------------------------ | ||
| 103 | Functions | ||
| 104 | ------------------------------------------------------------------------------*/ | ||
| 105 | |||
| 106 | /******************************************************************************* | ||
| 107 | * * | ||
| 108 | * PROCEDURE: * | ||
| 109 | * should_log_next_frame * | ||
| 110 | * * | ||
| 111 | * DESCRIPTION: * | ||
| 112 | * Check if next flash frame should be logged * | ||
| 113 | * * | ||
| 114 | *******************************************************************************/ | ||
| 115 | 1180 | static inline int should_log_next_frame | |
| 116 | ( | ||
| 117 | uint32_t launch_detect_start_time | ||
| 118 | ) | ||
| 119 | { | ||
| 120 | 1180 | return preset_data.config_settings.flash_rate_limit == 0 | |
| 121 |
4/4✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1178 times.
✓ Branch 3 taken 1 time.
✓ Branch 4 taken 1 time.
|
1180 | || HAL_GetTick() - ( last_flash_timestamp + launch_detect_start_time ) >= ceilf( 1000.0 / preset_data.config_settings.flash_rate_limit ); |
| 122 | |||
| 123 | } /* should_log_next_frame */ | ||
| 124 | |||
| 125 | |||
| 126 | /******************************************************************************* | ||
| 127 | * * | ||
| 128 | * PROCEDURE: * | ||
| 129 | * flight_calib * | ||
| 130 | * * | ||
| 131 | * DESCRIPTION: * | ||
| 132 | * Calibration state of flight loop. * | ||
| 133 | * * | ||
| 134 | *******************************************************************************/ | ||
| 135 | 4 | void flight_calib | |
| 136 | ( | ||
| 137 | uint8_t* gps_mesg_byte, | ||
| 138 | HFLASH_BUFFER* flash_handle, | ||
| 139 | uint32_t* flash_address | ||
| 140 | ) | ||
| 141 | { | ||
| 142 | 4 | LORA_STATUS lora_status = LORA_OK; | |
| 143 | |||
| 144 | 4 | led_set_color( LED_YELLOW ); | |
| 145 | 4 | buzzer_multi_beeps(50, 50, 4); | |
| 146 | |||
| 147 | /* enable GPS if configured */ | ||
| 148 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
|
4 | if ( preset_data.config_settings.enabled_features & GPS_ENABLED ) |
| 149 | { | ||
| 150 | 2 | gps_receive_IT( gps_mesg_byte, 1 ); | |
| 151 | } | ||
| 152 | |||
| 153 | 4 | sensorCalibrationSWCON(); | |
| 154 | 4 | write_preset( flash_handle, flash_address ); | |
| 155 | 4 | flash_erase_preserve_preset( flash_handle, flash_address ); | |
| 156 | |||
| 157 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 time.
|
4 | if ( preset_data.config_settings.enabled_features & WIRELESS_TRANSMISSION_ENABLED ) |
| 158 | { | ||
| 159 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | if( !lora_is_lora_initialized() ) |
| 160 | { | ||
| 161 | 3 | lora_status = lora_configure( &preset_data.lora_preset ); | |
| 162 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 time.
|
3 | debug_assert( lora_status == LORA_OK, ERROR_LORA_INIT_ERROR ); |
| 163 | } | ||
| 164 | |||
| 165 | /* If the modem fails to configure, disable TX in RAM (but do not write back to flash) */ | ||
| 166 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 1 time.
|
2 | if( lora_status != LORA_OK ) |
| 167 | { | ||
| 168 | 1 | preset_data.config_settings.enabled_features &= ~WIRELESS_TRANSMISSION_ENABLED; | |
| 169 | |||
| 170 | /* Give an indication */ | ||
| 171 | 1 | led_set_color(LED_RED); | |
| 172 | 1 | buzzer_multi_beeps(400, 200, 3); | |
| 173 | 1 | led_set_color(LED_YELLOW); | |
| 174 | } | ||
| 175 | |||
| 176 | 3 | lora_fsm_set_mode( LORA_ASYNC_TX ); | |
| 177 | } | ||
| 178 | |||
| 179 | 4 | fc_state_update( FC_STATE_LAUNCH_DETECT ); | |
| 180 | |||
| 181 | 4 | } /* flight_calib */ | |
| 182 | |||
| 183 | |||
| 184 | /******************************************************************************* | ||
| 185 | * * | ||
| 186 | * PROCEDURE: * | ||
| 187 | * flight_loop * | ||
| 188 | * * | ||
| 189 | * DESCRIPTION: * | ||
| 190 | * Executes the flight computer in-flight superloop. * | ||
| 191 | * * | ||
| 192 | *******************************************************************************/ | ||
| 193 | 1189 | void flight_loop | |
| 194 | ( | ||
| 195 | uint32_t* launch_detect_start_time, | ||
| 196 | SENSOR_STATUS* sensor_status, | ||
| 197 | FLASH_STATUS* flash_status, | ||
| 198 | HFLASH_BUFFER* flash_handle, | ||
| 199 | uint32_t* flash_address | ||
| 200 | ) | ||
| 201 | { | ||
| 202 | /*------------------------------------------------------------------------------ | ||
| 203 | Local Variables | ||
| 204 | ------------------------------------------------------------------------------*/ | ||
| 205 | 1189 | uint32_t current_timestamp = HAL_GetTick() - *launch_detect_start_time; | |
| 206 | 1189 | FLIGHT_COMP_STATE_TYPE fc_state; | |
| 207 | |||
| 208 | /*------------------------------------------------------------------------------ | ||
| 209 | Retrieve Sensor Data | ||
| 210 | ------------------------------------------------------------------------------*/ | ||
| 211 | 1189 | *sensor_status = sensor_dump( &sensor_data ); | |
| 212 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1184 times.
|
1188 | if ( *sensor_status != SENSOR_OK ) |
| 213 | { | ||
| 214 | 4 | error_fail_fast( ERROR_SENSOR_CMD_ERROR ); | |
| 215 | } | ||
| 216 | |||
| 217 | /*------------------------------------------------------------------------------ | ||
| 218 | Perform State Updates & Control | ||
| 219 | ------------------------------------------------------------------------------*/ | ||
| 220 | /* store in a local so that future improvements like a coast phase can use it */ | ||
| 221 | 1184 | fc_state = get_fc_state(); | |
| 222 | 1184 | if ( ( fc_state == FC_STATE_ASCENT | |
| 223 |
2/2✓ Branch 0 taken 17 times.
✓ Branch 1 taken 1167 times.
|
1184 | || fc_state == FC_STATE_COAST ) |
| 224 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 7 times.
|
17 | && ( preset_data.config_settings.enabled_features & ACTIVE_ROLL_CONTROL_ENABLED ) ) |
| 225 | { | ||
| 226 | 10 | pid_loop(); | |
| 227 | } | ||
| 228 | |||
| 229 | 1184 | update_state(); | |
| 230 | |||
| 231 | /*------------------------------------------------------------------------------ | ||
| 232 | Update LoRa FSM | ||
| 233 | ------------------------------------------------------------------------------*/ | ||
| 234 |
2/2✓ Branch 0 taken 1161 times.
✓ Branch 1 taken 23 times.
|
1184 | if ( preset_data.config_settings.enabled_features & WIRELESS_TRANSMISSION_ENABLED ) |
| 235 | { | ||
| 236 | 1161 | lora_fsm_update( LORA_FSM_EVENT_SYNCHRONOUS_UPDATE ); | |
| 237 | } | ||
| 238 | |||
| 239 | /*------------------------------------------------------------------------------ | ||
| 240 | Log to Flash | ||
| 241 | ------------------------------------------------------------------------------*/ | ||
| 242 | /* Check if flash memory is full */ | ||
| 243 |
4/4✓ Branch 0 taken 1181 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1180 times.
✓ Branch 3 taken 1 time.
|
1184 | if ( flash_handle->address + sensor_frame_size < FLASH_MAX_ADDR && *flash_status == FLASH_OK ) |
| 244 | { | ||
| 245 | 1180 | set_state_color( false ); | |
| 246 | |||
| 247 | /* Write to flash */ | ||
| 248 |
2/2✓ Branch 2 taken 10 times.
✓ Branch 3 taken 1180 times.
|
1190 | while( flash_is_flash_busy() == FLASH_BUSY ){} |
| 249 |
2/2✓ Branch 1 taken 1179 times.
✓ Branch 2 taken 1 time.
|
1180 | if ( should_log_next_frame( *launch_detect_start_time ) ) |
| 250 | { | ||
| 251 | 1179 | last_flash_timestamp = HAL_GetTick() - *launch_detect_start_time; | |
| 252 | 1179 | *flash_status = store_frame( flash_handle, current_timestamp, flash_address ); | |
| 253 | } | ||
| 254 | |||
| 255 | } | ||
| 256 | else | ||
| 257 | { | ||
| 258 | 4 | set_state_color( true ); | |
| 259 | } | ||
| 260 | |||
| 261 | /*------------------------------------------------------------------------------ | ||
| 262 | Launch Detect: Handle Timeout | ||
| 263 | launch_detect AND (timeout OR flash full) | ||
| 264 | ------------------------------------------------------------------------------*/ | ||
| 265 | 1184 | fc_state = get_fc_state(); | |
| 266 |
2/2✓ Branch 0 taken 1162 times.
✓ Branch 1 taken 22 times.
|
1184 | if ( ( fc_state == FC_STATE_LAUNCH_DETECT ) |
| 267 |
2/2✓ Branch 0 taken 1159 times.
✓ Branch 1 taken 3 times.
|
1162 | && ( ( current_timestamp >= preset_data.config_settings.launch_detect_timeout ) |
| 268 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1159 times.
|
1159 | || ( ( *flash_address + sensor_frame_size ) > FLASH_MAX_ADDR ) ) ) |
| 269 | { | ||
| 270 | 3 | *flash_address = 0; | |
| 271 | 3 | led_set_color( LED_PURPLE ); /* blips to indicate timeout was reached */ | |
| 272 | |||
| 273 | /* Only attempt erase if logging not disabled */ | ||
| 274 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 time.
|
3 | if ( *flash_status == FLASH_OK ) |
| 275 | { | ||
| 276 | 2 | *flash_status = flash_erase_preserve_preset( flash_handle, flash_address ); | |
| 277 | |||
| 278 | /* Reset the timer */ | ||
| 279 | 2 | *launch_detect_start_time = HAL_GetTick(); | |
| 280 | |||
| 281 | /* Reset sensor velos */ | ||
| 282 | 2 | sensor_reset_velo(); | |
| 283 | |||
| 284 | /* Reset memory pointer */ | ||
| 285 | 2 | flash_handle->address = *flash_address; | |
| 286 | } | ||
| 287 | else | ||
| 288 | { | ||
| 289 | /* If logging disabled, just reset timer and continue */ | ||
| 290 | 1 | led_set_color(LED_BLUE); | |
| 291 | 1 | *launch_detect_start_time = HAL_GetTick(); | |
| 292 | } | ||
| 293 | } | ||
| 294 | |||
| 295 | #ifdef DEBUG | ||
| 296 | 1157 | debug_delta = HAL_GetTick() - debug_previous; | |
| 297 | 1157 | debug_previous = HAL_GetTick(); | |
| 298 | #endif | ||
| 299 | |||
| 300 | 1184 | } /* flight_loop */ | |
| 301 | |||
| 302 | |||
| 303 | /******************************************************************************* | ||
| 304 | * * | ||
| 305 | * PROCEDURE: * | ||
| 306 | * flight_deploy * | ||
| 307 | * * | ||
| 308 | * DESCRIPTION: * | ||
| 309 | * Deployment state of flight loop. * | ||
| 310 | * * | ||
| 311 | *******************************************************************************/ | ||
| 312 | 4 | void flight_deploy | |
| 313 | ( | ||
| 314 | void | ||
| 315 | ) | ||
| 316 | { | ||
| 317 | /* Deploy chutes if relevant feature flag enabled. */ | ||
| 318 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 time.
|
4 | if( preset_data.config_settings.enabled_features & DUAL_DEPLOY_ENABLED ) |
| 319 | { | ||
| 320 | /* initialize locals */ | ||
| 321 | 3 | IGN_STATUS drogue_ignition_status = IGN_OK; | |
| 322 | 3 | IGN_STATUS main_ignition_status = IGN_OK; | |
| 323 | 3 | fc_state_update( FC_STATE_APOGEE ); | |
| 324 | 3 | led_set_color( LED_WHITE ); | |
| 325 | |||
| 326 | /* deploy */ | ||
| 327 | 3 | drogue_ignition_status = ign_deploy_drogue(); | |
| 328 | 3 | main_ignition_status = ign_deploy_main(); | |
| 329 | |||
| 330 | /* if it fails, continue attempting deployment. all other systems are secondary. */ | ||
| 331 | 3 | while( main_ignition_status != IGN_SUCCESS | |
| 332 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
|
6 | || drogue_ignition_status != IGN_SUCCESS ) |
| 333 | { | ||
| 334 | /* try the failing chute again */ | ||
| 335 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 time.
|
3 | if( drogue_ignition_status != IGN_SUCCESS ) |
| 336 | { | ||
| 337 | 2 | drogue_ignition_status = ign_deploy_drogue(); | |
| 338 | } | ||
| 339 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 time.
|
3 | if( main_ignition_status != IGN_SUCCESS ) |
| 340 | { | ||
| 341 | 2 | main_ignition_status = ign_deploy_main(); | |
| 342 | } | ||
| 343 | } | ||
| 344 | } | ||
| 345 | |||
| 346 | /* update state */ | ||
| 347 | 4 | fc_state_update( FC_STATE_DESCENT ); | |
| 348 | |||
| 349 | 4 | } /* flight_deploy */ | |
| 350 | |||
| 351 | |||
| 352 | /******************************************************************************* | ||
| 353 | * * | ||
| 354 | * PROCEDURE: * | ||
| 355 | * pid_loop * | ||
| 356 | * * | ||
| 357 | * DESCRIPTION: * | ||
| 358 | * Parent PID control function. * | ||
| 359 | * * | ||
| 360 | *******************************************************************************/ | ||
| 361 | 20 | void pid_loop | |
| 362 | ( | ||
| 363 | void | ||
| 364 | ) | ||
| 365 | { | ||
| 366 | /* Check early exit conditions */ | ||
| 367 | 20 | uint32_t delay_elapsed = HAL_GetTick() - launch_detect_time; | |
| 368 |
2/2✓ Branch 0 taken 19 times.
✓ Branch 1 taken 1 time.
|
20 | if ( delay_elapsed < preset_data.config_settings.control_delay_after_launch ) |
| 369 | { | ||
| 370 | return; | ||
| 371 | } | ||
| 372 | |||
| 373 | /* Compute maximum deflection angles */ | ||
| 374 | 19 | uint8_t max_range_1 = preset_data.servo_preset.rp_servo1 + preset_data.config_settings.control_max_deflection_angle; | |
| 375 | 19 | uint8_t min_range_1 = preset_data.servo_preset.rp_servo1 - preset_data.config_settings.control_max_deflection_angle; | |
| 376 | |||
| 377 | 19 | uint8_t max_range_2 = preset_data.servo_preset.rp_servo2 + preset_data.config_settings.control_max_deflection_angle; | |
| 378 | 19 | uint8_t min_range_2 = preset_data.servo_preset.rp_servo2 - preset_data.config_settings.control_max_deflection_angle; | |
| 379 | |||
| 380 | 19 | uint8_t max_range_3 = preset_data.servo_preset.rp_servo3 + preset_data.config_settings.control_max_deflection_angle; | |
| 381 | 19 | uint8_t min_range_3 = preset_data.servo_preset.rp_servo3 - preset_data.config_settings.control_max_deflection_angle; | |
| 382 | |||
| 383 | 19 | uint8_t max_range_4 = preset_data.servo_preset.rp_servo4 + preset_data.config_settings.control_max_deflection_angle; | |
| 384 | 19 | uint8_t min_range_4 = preset_data.servo_preset.rp_servo4 - preset_data.config_settings.control_max_deflection_angle; | |
| 385 | |||
| 386 | /* Read velocity and body state from sensor */ | ||
| 387 | 19 | float velocity = sensor_data.state_estimate.velocity; | |
| 388 | 19 | float roll_rate = sensor_data.imu_converted.gyro_x; | |
| 389 | |||
| 390 | /* Get timing */ | ||
| 391 | 19 | pid_delta = HAL_GetTick() - pid_previous; | |
| 392 | 19 | pid_previous = HAL_GetTick(); | |
| 393 | |||
| 394 | /* Retrieve PID gains */ | ||
| 395 | 19 | v_pid_function(&pid_data, velocity); | |
| 396 | |||
| 397 | /* Retrieve feedback value */ | ||
| 398 | 19 | feedback = pid_control(roll_rate, 0.0, pid_delta/1000.0); | |
| 399 | |||
| 400 | /* Perform Bounds Checking */ | ||
| 401 | 19 | uint8_t servo_1_turn = preset_data.servo_preset.rp_servo1 + (int8_t) roundf(feedback); | |
| 402 | 19 | uint8_t servo_2_turn = preset_data.servo_preset.rp_servo2 + (int8_t) roundf(feedback); | |
| 403 | 19 | uint8_t servo_3_turn = preset_data.servo_preset.rp_servo3 + (int8_t) roundf(feedback); | |
| 404 | 19 | uint8_t servo_4_turn = preset_data.servo_preset.rp_servo4 + (int8_t) roundf(feedback); | |
| 405 | 19 | servo_1_turn = motor_snap_to_bound(servo_1_turn, max_range_1, min_range_1); | |
| 406 | 19 | servo_2_turn = motor_snap_to_bound(servo_2_turn, max_range_2, min_range_2); | |
| 407 | 19 | servo_3_turn = motor_snap_to_bound(servo_3_turn, max_range_3, min_range_3); | |
| 408 | 19 | servo_4_turn = motor_snap_to_bound(servo_4_turn, max_range_4, min_range_4); | |
| 409 | |||
| 410 | /* Actuate Servos */ | ||
| 411 | 19 | motor_drive( SERVO_1, servo_1_turn ); | |
| 412 | 19 | motor_drive( SERVO_2, servo_2_turn ); | |
| 413 | 19 | motor_drive( SERVO_3, servo_3_turn ); | |
| 414 | 19 | motor_drive( SERVO_4, servo_4_turn ); | |
| 415 | |||
| 416 | } /* pid_loop */ | ||
| 417 | |||
| 418 | |||
| 419 | /******************************************************************************* | ||
| 420 | * * | ||
| 421 | * PROCEDURE: * | ||
| 422 | * pid_control * | ||
| 423 | * * | ||
| 424 | * DESCRIPTION: * | ||
| 425 | * PID control function. * | ||
| 426 | * * | ||
| 427 | *******************************************************************************/ | ||
| 428 | 19 | float pid_control | |
| 429 | ( | ||
| 430 | float current_input, /* roll rate around x axis */ | ||
| 431 | float target, /* target roll rate */ | ||
| 432 | float dtime /* delta time (seconds) */ | ||
| 433 | ) | ||
| 434 | { | ||
| 435 | 19 | error = target - current_input; | |
| 436 | |||
| 437 | 19 | pVal = error; | |
| 438 | 19 | iVal += error * dtime; | |
| 439 | 19 | dVal = (error - prevErr) / dtime; | |
| 440 | |||
| 441 | 19 | float result = pid_data.kP * pVal + pid_data.kI * iVal + pid_data.kD * dVal; | |
| 442 | |||
| 443 | 19 | prevErr = error; | |
| 444 | 19 | return result; | |
| 445 | |||
| 446 | } /* pid_control */ | ||
| 447 | |||
| 448 | |||
| 449 | /******************************************************************************* | ||
| 450 | * * | ||
| 451 | * PROCEDURE: * | ||
| 452 | * v_pid_function * | ||
| 453 | * * | ||
| 454 | * DESCRIPTION: * | ||
| 455 | * Retrieves PID gains. * | ||
| 456 | * * | ||
| 457 | *******************************************************************************/ | ||
| 458 | uint8_t read_samples = 0; | ||
| 459 | bool pid_run_status = false; | ||
| 460 | 19 | void v_pid_function | |
| 461 | ( | ||
| 462 | PID_DATA* pid_data, | ||
| 463 | float velocity | ||
| 464 | ) | ||
| 465 | { | ||
| 466 | /* Eventually, we may wish to tune this based on velocity. Right now, | ||
| 467 | control constants are constant. */ | ||
| 468 | 19 | pid_data->kP = preset_data.config_settings.roll_control_constant_p; | |
| 469 | 19 | pid_data->kI = preset_data.config_settings.roll_control_constant_i; | |
| 470 | 19 | pid_data->kD = preset_data.config_settings.roll_control_constant_d; | |
| 471 | |||
| 472 | 19 | } /* v_pid_function */ | |
| 473 | |||
| 474 | |||
| 475 | /******************************************************************************* | ||
| 476 | * * | ||
| 477 | * PROCEDURE: * | ||
| 478 | * update_state * | ||
| 479 | * * | ||
| 480 | * DESCRIPTION: * | ||
| 481 | * Helper to perform the state transitions for the flight loop. * | ||
| 482 | * * | ||
| 483 | *******************************************************************************/ | ||
| 484 | 1184 | static void update_state | |
| 485 | ( | ||
| 486 | void | ||
| 487 | ) | ||
| 488 | { | ||
| 489 |
4/5✓ Branch 1 taken 1163 times.
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
1184 | switch( get_fc_state() ) |
| 490 | { | ||
| 491 | 1163 | case FC_STATE_LAUNCH_DETECT: | |
| 492 |
2/2✓ Branch 1 taken 1 time.
✓ Branch 2 taken 1162 times.
|
1163 | if( launch_detection( &launch_detect_time ) ) |
| 493 | { | ||
| 494 | 1 | fc_state_update( FC_STATE_ASCENT ); | |
| 495 | } | ||
| 496 | break; | ||
| 497 | 12 | case FC_STATE_ASCENT: | |
| 498 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 10 times.
|
12 | if( coast_detect() ) |
| 499 | { | ||
| 500 | 2 | fc_state_update( FC_STATE_COAST ); | |
| 501 | } | ||
| 502 |
2/2✓ Branch 1 taken 1 time.
✓ Branch 2 taken 9 times.
|
10 | else if( apogee_detect() ) |
| 503 | { | ||
| 504 | 1 | fc_state_update( FC_STATE_APOGEE ); | |
| 505 | // TODO: Log message that coast phase was skipped | ||
| 506 | } | ||
| 507 | break; | ||
| 508 | 5 | case FC_STATE_COAST: | |
| 509 |
2/2✓ Branch 1 taken 1 time.
✓ Branch 2 taken 4 times.
|
5 | if( apogee_detect() ) |
| 510 | { | ||
| 511 | 1 | fc_state_update( FC_STATE_APOGEE ); | |
| 512 | } | ||
| 513 | break; | ||
| 514 | case FC_STATE_APOGEE: | ||
| 515 | case FC_STATE_DESCENT: /* intentional fallthrough */ | ||
| 516 | case FC_STATE_LANDED: /* intentional fallthrough; not yet implemented */ | ||
| 517 | /* no applicable transitions; do nothing */ | ||
| 518 | break; | ||
| 519 | /** | ||
| 520 | * GCOVR_EXCL_START | ||
| 521 | * | ||
| 522 | * This branch is tested, but gcovr doesn't see these statements in | ||
| 523 | * release mode so it reports a coverage hole on the emulator. | ||
| 524 | */ | ||
| 525 | − | default: | |
| 526 | #ifdef DEBUG /* for caution, throw error in debug mode only */ | ||
| 527 | − | error_fail_fast( ERROR_INVALID_STATE_ERROR ); | |
| 528 | #endif | ||
| 529 | − | break; | |
| 530 | /** | ||
| 531 | * GCOVR_EXCL_STOP | ||
| 532 | */ | ||
| 533 | } | ||
| 534 | |||
| 535 | 1184 | } /* update_state */ | |
| 536 | |||
| 537 | |||
| 538 | /******************************************************************************* | ||
| 539 | * * | ||
| 540 | * PROCEDURE: * | ||
| 541 | * set_state_color * | ||
| 542 | * * | ||
| 543 | * DESCRIPTION: * | ||
| 544 | * Helper to identify the correct LED color for each state. * | ||
| 545 | * * | ||
| 546 | *******************************************************************************/ | ||
| 547 | 1184 | static void set_state_color | |
| 548 | ( | ||
| 549 | bool flash_full | ||
| 550 | ) | ||
| 551 | { | ||
| 552 |
3/3✓ Branch 1 taken 1162 times.
✓ Branch 2 taken 21 times.
✓ Branch 3 taken 1 time.
|
1184 | switch( get_fc_state() ) |
| 553 | { | ||
| 554 | 1162 | case FC_STATE_LAUNCH_DETECT: | |
| 555 | 1162 | led_set_color( LED_CYAN ); | |
| 556 | 1162 | break; | |
| 557 | 21 | case FC_STATE_ASCENT: | |
| 558 | case FC_STATE_APOGEE: /* intentional fallthrough */ | ||
| 559 | case FC_STATE_COAST: /* intentional fallthrough */ | ||
| 560 | case FC_STATE_DESCENT: /* intentional fallthrough */ | ||
| 561 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 18 times.
|
21 | if ( flash_full ) |
| 562 | { | ||
| 563 | 3 | led_set_color( LED_BLUE ); | |
| 564 | } | ||
| 565 | else | ||
| 566 | { | ||
| 567 | 18 | led_set_color( LED_PURPLE ); | |
| 568 | } | ||
| 569 | break; | ||
| 570 | /** | ||
| 571 | * GCOVR_EXCL_START | ||
| 572 | * | ||
| 573 | * This branch is tested, but gcovr doesn't see these statements in | ||
| 574 | * release mode so it reports a coverage hole on the emulator. | ||
| 575 | */ | ||
| 576 | − | default: | |
| 577 | #ifdef DEBUG /* for caution, throw error in debug mode only */ | ||
| 578 | − | error_fail_fast( ERROR_INVALID_STATE_ERROR ); | |
| 579 | #endif | ||
| 580 | − | break; | |
| 581 | /** | ||
| 582 | * GCOVR_EXCL_STOP | ||
| 583 | */ | ||
| 584 | } | ||
| 585 | 1184 | } /* set_state_color */ | |
| 586 |