diff --git a/NOTICE b/NOTICE index 7a24331..5c966b2 100644 --- a/NOTICE +++ b/NOTICE @@ -1 +1,3 @@ -This larger work, the Sun Devil Rocketry mod Submodule, uses the BSD-3-Clause. This software project does not use any dependencies, and all files in this project are covered under this license. \ No newline at end of file +This larger work, the Sun Devil Rocketry mod Submodule, uses the BSD-3-Clause. This software project does not have any official dependencies, however some derivative code is used. This is attributed below. + +sensor.c:sensor_conv_mag - Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. BSD-3-Clause license. https://opensource.org/license/bsd-3-clause \ No newline at end of file diff --git a/baro/baro.c b/baro/baro.c index a009dd3..5581ca5 100644 --- a/baro/baro.c +++ b/baro/baro.c @@ -49,7 +49,7 @@ static BARO_CAL_DATA baro_cal_data; uint8_t baro_raw_buffer[6]; float baro_pres_proc = NAN; float baro_temp_proc = NAN; -bool baro_data_ready = false; +static bool baro_data_ready = false; /*------------------------------------------------------------------------------ @@ -863,6 +863,25 @@ return write_reg( BARO_REG_CMD, BARO_CMD_FIFO_FLUSH ); } /* baro_flush_fifo */ +/******************************************************************************* +* * +* PROCEDURE: * +* baro_get_baro_data_ready * +* * +* DESCRIPTION: * +* Returns the baro_data_ready flag * +* * +*******************************************************************************/ +bool baro_get_baro_data_ready + ( + void + ) +{ +return baro_data_ready; + +} /* baro_get_baro_data_ready */ + + /******************************************************************************* * * * PROCEDURE: * diff --git a/baro/baro.h b/baro/baro.h index 996d0cf..2a4847b 100644 --- a/baro/baro.h +++ b/baro/baro.h @@ -282,6 +282,12 @@ BARO_STATUS baro_get_altitude void ); +/* returns the baro_data_ready flag */ +bool baro_get_baro_data_ready + ( + void + ); + BARO_STATUS start_baro_read_IT(); BARO_STATUS baro_IT_handler(); BARO_STATUS get_baro_it(float* pres_ptr, float* temp_ptr); diff --git a/common/common.c b/common/common.c index 0a6e41c..4cc9844 100644 --- a/common/common.c +++ b/common/common.c @@ -47,6 +47,41 @@ API Functions ------------------------------------------------------------------------------*/ +/******************************************************************************* +* * +* PROCEDURE: * +* disable_irq * +* * +* DESCRIPTION: * +* Disables IRQ interrupts; wrapper for __disable_irq() * +* * +*******************************************************************************/ +void disable_irq + ( + void + ) +{ +__disable_irq(); +} /* disable_irq */ + + +/******************************************************************************* +* * +* PROCEDURE: * +* enable_irq * +* * +* DESCRIPTION: * +* Enables IRQ interrupts; wrapper for __enable_irq() * +* * +*******************************************************************************/ +void enable_irq + ( + void + ) +{ +__enable_irq(); +} /* enable_irq */ + /******************************************************************************* * * @@ -66,6 +101,7 @@ HAL_Delay(delay); } /* delay_ms */ + /******************************************************************************* * END OF FILE * -*******************************************************************************/ \ No newline at end of file +*******************************************************************************/ diff --git a/common/common.h b/common/common.h index c2819d2..af73bf2 100644 --- a/common/common.h +++ b/common/common.h @@ -36,7 +36,7 @@ extern "C" { /*------------------------------------------------------------------------------ - Typdefs + Typedefs ------------------------------------------------------------------------------*/ /* UID serial number (packed struct inhibits padding) */ @@ -107,7 +107,15 @@ memcpy( uid_buffer, uid, sizeof( ST_UID_TYPE ) ); Function Prototypes ------------------------------------------------------------------------------*/ -/* common */ +void disable_irq + ( + void + ); + +void enable_irq + ( + void + ); void delay_ms ( @@ -121,4 +129,4 @@ void delay_ms /******************************************************************************* * END OF FILE * -*******************************************************************************/ \ No newline at end of file +*******************************************************************************/ diff --git a/imu/imu.c b/imu/imu.c index c80c6b0..e659ca6 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -52,10 +52,10 @@ #if defined( A0002_REV2 ) uint8_t imu_raw_buffer[12]; IMU_RAW imu_raw_processed; -bool imu_data_ready; +static bool imu_data_ready; uint8_t mag_raw_buffer[8]; -bool mag_data_ready; +static bool mag_data_ready; #endif MAG_TRIM mag_trim; @@ -871,6 +871,44 @@ else } /* read_imu_regs */ +/******************************************************************************* +* * +* PROCEDURE: * +* imu_get_imu_data_ready * +* * +* DESCRIPTION: * +* Returns the imu_data_ready flag * +* * +*******************************************************************************/ +bool imu_get_imu_data_ready + ( + void + ) +{ +return imu_data_ready; + +} /* imu_get_imu_data_ready */ + + +/******************************************************************************* +* * +* PROCEDURE: * +* imu_get_mag_data_ready * +* * +* DESCRIPTION: * +* Returns the mag_data_ready flag * +* * +*******************************************************************************/ +bool imu_get_mag_data_ready + ( + void + ) +{ +return mag_data_ready; + +} /* imu_get_mag_data_ready */ + + #if defined( A0002_REV2 ) /******************************************************************************* * * diff --git a/imu/imu.h b/imu/imu.h index 0bc6bcb..f962ee1 100644 --- a/imu/imu.h +++ b/imu/imu.h @@ -487,6 +487,18 @@ void IMU_config uint16_t mag_setting ); +/* Get the static variable imu_data_ready */ +bool imu_get_imu_data_ready + ( + void + ); + +/* Get the static variable mag_data_ready */ +bool imu_get_mag_data_ready + ( + void + ); + #ifdef A0002_REV2 IMU_STATUS start_imu_read_IT(void); IMU_STATUS imu_it_handler(); diff --git a/sensor/sensor.c b/sensor/sensor.c index 13ac322..8bc4226 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -48,6 +48,7 @@ ------------------------------------------------------------------------------*/ #include "main.h" #if defined( FLIGHT_COMPUTER ) + #include "common.h" #include "imu.h" #include "baro.h" #elif defined( FLIGHT_COMPUTER_LITE ) @@ -156,11 +157,9 @@ static void pt6_adc_channel_select #endif /* #ifdef L0002_REV5 */ #ifdef A0002_REV2 -static SENSOR_STATUS sensor_it_imu_baro +static SENSOR_STATUS sensor_get_it_ready ( - uint32_t timeout, - SENSOR_DATA* sensor_data_ptr, - IMU_RAW* imu_raw + uint32_t timeout ); #endif @@ -711,10 +710,8 @@ SENSOR_STATUS sensor_dump ------------------------------------------------------------------------------*/ #if defined( FLIGHT_COMPUTER ) SENSOR_STATUS parallel_status; - IMU_STATUS accel_status; - IMU_STATUS gyro_status; - BARO_STATUS press_status; - BARO_STATUS temp_status; + IMU_STATUS imu_status; + BARO_STATUS baro_status; IMU_RAW imu_raw; #elif defined( ENGINE_CONTROLLER ) #if defined(L0002_REV4 ) @@ -732,10 +729,8 @@ SENSOR_STATUS sensor_dump ------------------------------------------------------------------------------*/ #if defined( FLIGHT_COMPUTER ) parallel_status = SENSOR_OK; - accel_status = IMU_OK; - gyro_status = IMU_OK; - press_status = BARO_OK; - temp_status = BARO_OK; + imu_status = IMU_OK; + baro_status = BARO_OK; #elif defined( ENGINE_CONTROLLER ) #ifdef L0002_REV4 pt_status = PRESSURE_OK; @@ -748,37 +743,58 @@ SENSOR_STATUS sensor_dump /* Poll Sensors */ #if defined( FLIGHT_COMPUTER ) - /*Call sensor API functions*/ + /*Call sensor API functions*/ + + /* check that IMU & BARO are ready to be read */ + parallel_status = sensor_get_it_ready( HAL_DEFAULT_TIMEOUT ); + + if( parallel_status != SENSOR_OK ) + { + return parallel_status; + } - memset( &(imu_raw), 0, sizeof( IMU_RAW ) ); + /* Disabling interrupts to avoid race conditions */ + disable_irq(); - /* GPS sensor */ - sensor_data_ptr->gps_altitude_ft = gps_data.altitude_ft; - sensor_data_ptr->gps_speed_kmh = gps_data.speed_km; - sensor_data_ptr->gps_utc_time = gps_data.utc_time; - sensor_data_ptr->gps_dec_longitude = gps_data.dec_longitude; - sensor_data_ptr->gps_dec_latitude = gps_data.dec_latitude; - sensor_data_ptr->gps_ns = gps_data.ns; - sensor_data_ptr->gps_ew = gps_data.ew; - sensor_data_ptr->gps_gll_status = gps_data.gll_status; - sensor_data_ptr->gps_rmc_status = gps_data.rmc_status; + /* CRITICAL SECTION BEGIN */ - parallel_status = sensor_it_imu_baro( HAL_DEFAULT_TIMEOUT, sensor_data_ptr, &imu_raw ); + memset( &(imu_raw), 0, sizeof( IMU_RAW ) ); - /*Compute State Estimations*/ + /* GPS sensor */ + sensor_data_ptr->gps_altitude_ft = gps_data.altitude_ft; + sensor_data_ptr->gps_speed_kmh = gps_data.speed_km; + sensor_data_ptr->gps_utc_time = gps_data.utc_time; + sensor_data_ptr->gps_dec_longitude = gps_data.dec_longitude; + sensor_data_ptr->gps_dec_latitude = gps_data.dec_latitude; + sensor_data_ptr->gps_ns = gps_data.ns; + sensor_data_ptr->gps_ew = gps_data.ew; + sensor_data_ptr->gps_gll_status = gps_data.gll_status; + sensor_data_ptr->gps_rmc_status = gps_data.rmc_status; - /* Calculated and retrieve converted IMU data */ - sensor_conv_imu( &(sensor_data_ptr->imu_data), &imu_raw ); + /* IMU Read */ + imu_status = get_imu_it( &imu_raw ); - /* Calculated to get body state */ - sensor_body_state( &(sensor_data_ptr->imu_data) ); + /* Baro Read */ + baro_status = get_baro_it( &(sensor_data_ptr->baro_pressure), &(sensor_data_ptr->baro_temp) ); - /* Calculated velocity and position */ - sensor_imu_velo( &(sensor_data_ptr->imu_data) ); + /*Compute State Estimations*/ - /* Calculated velocity from barometer */ - sensor_baro_velo( sensor_data_ptr ); + /* Calculated and retrieve converted IMU data */ + sensor_conv_imu( &(sensor_data_ptr->imu_data), &imu_raw ); + /* Calculated to get body state */ + sensor_body_state( &(sensor_data_ptr->imu_data) ); + + /* Calculated velocity and position */ + sensor_imu_velo( &(sensor_data_ptr->imu_data) ); + + /* Calculated velocity from barometer */ + sensor_baro_velo( sensor_data_ptr ); + + /* CRITICAL SECTION END */ + + /* Re-enabling interrupts after potentially dangerous reads/writes occur */ + enable_irq(); #elif defined( ENGINE_CONTROLLER ) #ifndef L0002_REV5 @@ -809,27 +825,22 @@ SENSOR_STATUS sensor_dump /* Start next measurement and return status */ parallel_status |= sensor_start_IT( sensor_data_ptr ); - if( accel_status != IMU_OK ) - { - return SENSOR_ACCEL_ERROR; - } - else if ( gyro_status != IMU_OK ) - { - return SENSOR_GYRO_ERROR; - } - else if ( press_status != BARO_OK || - temp_status != BARO_OK ) - { - return SENSOR_BARO_ERROR; - } + if( imu_status != IMU_OK ) + { + return SENSOR_IMU_FAIL; + } + else if ( baro_status != BARO_OK) + { + return SENSOR_BARO_ERROR; + } else if ( parallel_status != SENSOR_OK ) - { - return SENSOR_IT_TIMEOUT; - } + { + return SENSOR_IT_TIMEOUT; + } else - { - return SENSOR_OK; - } + { + return SENSOR_OK; + } #elif defined( ENGINE_CONTROLLER ) #ifdef L0002_REV4 if ( pt_status != PRESSURE_OK ) @@ -2155,41 +2166,28 @@ HAL_ADC_ConfigChannel( &hadc3, &sConfig ); /******************************************************************************* * * * PROCEDURE: * -* sensor_it_imu_baro * +* sensor_get_it_ready * * * * DESCRIPTION: * -* Collect data from the double buffers and put it in sensor_data. * +* Ensures baro & mag & imu are ready to be read from. * * * *******************************************************************************/ -static SENSOR_STATUS sensor_it_imu_baro +static SENSOR_STATUS sensor_get_it_ready ( - uint32_t timeout, - SENSOR_DATA* sensor_data_ptr, - IMU_RAW* imu_raw + uint32_t timeout ) { /* set up timeout */ uint32_t starting_time = HAL_GetTick(); uint32_t curr_time = HAL_GetTick(); -IMU_STATUS imu_ready = IMU_BUSY; -BARO_STATUS baro_ready = BARO_BUSY; while( curr_time <= starting_time + timeout ) { - /* determine if ready */ - if( imu_ready == IMU_BUSY ) - { - imu_ready = get_imu_it( imu_raw ); - } - if( baro_ready == BARO_BUSY ) + /* Ensure both the IMU and barometer are ready to be read */ + if ( imu_get_imu_data_ready() + && imu_get_mag_data_ready() + && baro_get_baro_data_ready() ) { - /* doing IMU first. return ready. */ - baro_ready = get_baro_it( &(sensor_data_ptr->baro_pressure), &(sensor_data_ptr->baro_temp) ); - } - - /* compute return if ready */ - if( baro_ready != BARO_BUSY && imu_ready != IMU_BUSY ) - { - return baro_ready | imu_ready; + return SENSOR_OK; } /* update timeout poll */ @@ -2197,11 +2195,10 @@ while( curr_time <= starting_time + timeout ) } return SENSOR_IT_TIMEOUT; + } -#endif /* #ifdef A0002_REV2 */ -#ifdef A0002_REV2 /******************************************************************************* * * * PROCEDURE: *