From 94d9141aae5c3fcd356126e58d05f72568f47f36 Mon Sep 17 00:00:00 2001 From: dmanslick Date: Wed, 27 May 2026 17:32:59 -0400 Subject: [PATCH 1/2] Updated altitude estimation code to be relative rather than from sea level --- Ganesha_II_V2/Core/Inc/bmp581.h | 7 +++++- Ganesha_II_V2/Core/Src/bmp581.c | 41 ++++++++++++++++++--------------- Ganesha_II_V2/Core/Src/main.c | 14 +++++++---- 3 files changed, 38 insertions(+), 24 deletions(-) diff --git a/Ganesha_II_V2/Core/Inc/bmp581.h b/Ganesha_II_V2/Core/Inc/bmp581.h index b428472..4920c3f 100644 --- a/Ganesha_II_V2/Core/Inc/bmp581.h +++ b/Ganesha_II_V2/Core/Inc/bmp581.h @@ -28,6 +28,10 @@ struct BMP581 { struct bmp5_osr_odr_press_config odr_config; struct bmp5_int_source_select int_config; I2C_HandleTypeDef *hi2c; + float starting_pressure; + float starting_temp; + float tropopause_altitude; + float middle_strat_altitude; }; BMP5_INTF_RET_TYPE read_i2c(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr); @@ -35,6 +39,7 @@ BMP5_INTF_RET_TYPE write_i2c(uint8_t reg_addr, const uint8_t *reg_data, uint32_t int8_t bmp581_init(struct BMP581 *bmp581, I2C_HandleTypeDef *handle); int8_t bmp581_update_data(struct BMP581 *bmp581, struct bmp5_sensor_data *data); int8_t bmp581_get_power_mode(struct BMP581 *bmp581, enum bmp5_powermode *powermode); -float bmp581_estimate_altitude_msl(struct BMP581 *bmp581, struct bmp5_sensor_data *data); +void bmp581_init_altitude_estimator(struct BMP581 *bmp581, struct bmp5_sensor_data *data); +float bmp581_estimate_altitude_relative(struct BMP581 *bmp581, struct bmp5_sensor_data *data); #endif diff --git a/Ganesha_II_V2/Core/Src/bmp581.c b/Ganesha_II_V2/Core/Src/bmp581.c index 3e7afc6..63960d1 100644 --- a/Ganesha_II_V2/Core/Src/bmp581.c +++ b/Ganesha_II_V2/Core/Src/bmp581.c @@ -80,37 +80,40 @@ int8_t bmp581_update_data(struct BMP581 *bmp581, struct bmp5_sensor_data *data) return bmp5_get_sensor_data(data, &(bmp581->odr_config), &(bmp581->device)); } -static inline float calc_altitude_troposphere_msl(float pressure) { +static inline float calc_altitude_troposphere_relative(float starting_pressure, float starting_temp, float current_pressure) { float exponent = (-GAS_CONSTANT * TROPOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); - float pressure_ratio = pressure / STANDARD_SEA_LEVEL_PRESSURE; - float power_term = powf(pressure_ratio, exponent) - 1; - - return (STANDARD_SEA_LEVEL_TEMP / TROPOSPHERE_LAPSE_RATE) * power_term; + return (starting_temp / TROPOSPHERE_LAPSE_RATE) * (powf(current_pressure / starting_pressure, exponent) - 1.0f); } -static inline float calc_altitude_lower_stratosphere_msl(float pressure) { - float log_ratio = logf(pressure / TROPOPAUSE_PRESSURE); - float scale_factor = (GAS_CONSTANT * STRATOSPHERE_BASE_TEMP) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); - - return TROPOPAUSE_BASE_ALTITUDE - (scale_factor * log_ratio); +static inline float calc_altitude_lower_stratosphere_relative(float tropopause_altitude, float current_pressure) { + float fraction = (GAS_CONSTANT * STRATOSPHERE_BASE_TEMP) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); + return tropopause_altitude + (fraction * logf(TROPOPAUSE_PRESSURE / current_pressure)); } -static inline float calc_altitude_upper_stratosphere_msl(float pressure) { - float exponent = (-GAS_CONSTANT * UPPER_STRATOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); - float pressure_ratio = pressure / STRATOSPHERE_MIDDLE_PRESSURE; - float power_term = powf(pressure_ratio, exponent) - 1; +static inline float calc_altitude_upper_stratosphere_relative(float middle_strat_altitude, float current_pressure) { + const float exponent = (-GAS_CONSTANT * UPPER_STRATOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); + return middle_strat_altitude + (STRATOSPHERE_BASE_TEMP / UPPER_STRATOSPHERE_LAPSE_RATE) * (powf(current_pressure / STRATOSPHERE_MIDDLE_PRESSURE, exponent) - 1.0f); +} - return STRATOSPHERE_MIDDLE_BASE_ALTITUDE + (STRATOSPHERE_BASE_TEMP / UPPER_STRATOSPHERE_LAPSE_RATE) * power_term; +void bmp581_init_altitude_estimator(struct BMP581 *bmp581, struct bmp5_sensor_data *data) { + bmp581->starting_pressure = data->pressure; + bmp581->starting_temp = data->temperature; } -float bmp581_estimate_altitude_msl(struct BMP581 *bmp581, struct bmp5_sensor_data *data) { +float bmp581_estimate_altitude_relative(struct BMP581 *bmp581, struct bmp5_sensor_data *data) { + float ret = 0; + if (data->pressure > TROPOPAUSE_PRESSURE) { - return calc_altitude_troposphere_msl(data->pressure); + ret = calc_altitude_troposphere_relative(bmp581->starting_pressure, bmp581->starting_temp + 273.15f, data->pressure); + bmp581->tropopause_altitude = ret; } else if (data->pressure > STRATOSPHERE_MIDDLE_PRESSURE) { - return calc_altitude_lower_stratosphere_msl(data->pressure); + ret = calc_altitude_lower_stratosphere_relative(bmp581->tropopause_altitude, data->pressure); + bmp581->middle_strat_altitude = ret; } else { - return calc_altitude_upper_stratosphere_msl(data->pressure); + return calc_altitude_upper_stratosphere_relative(bmp581->middle_strat_altitude, data->pressure); } + + return ret; } int8_t bmp581_get_power_mode(struct BMP581 *bmp581, enum bmp5_powermode *powermode) { diff --git a/Ganesha_II_V2/Core/Src/main.c b/Ganesha_II_V2/Core/Src/main.c index 7e59e2c..3c5e225 100644 --- a/Ganesha_II_V2/Core/Src/main.c +++ b/Ganesha_II_V2/Core/Src/main.c @@ -175,8 +175,9 @@ void HAL_GPIO_EXTI_Callback(uint16_t pin) { } struct Orientation_Estimator estimator; -uint8_t estimator_init = 0; +uint8_t orientation_estimator_initialized = 0; +uint8_t altitude_estimator_initialized = 0; /* USER CODE END 0 */ /** @@ -318,9 +319,9 @@ int main(void) packet.angular_velocity_y_rads = bmi088_convert_gyro_axis_data(&bmi088, bmi088_gyro_data.y); packet.angular_velocity_z_rads = bmi088_convert_gyro_axis_data(&bmi088, bmi088_gyro_data.z); - if (!estimator_init) { + if (!orientation_estimator_initialized) { orientation_estimator_reset_from_accel(&estimator, packet.acceleration_x_mss, packet.acceleration_y_mss, packet.acceleration_z_mss); - estimator_init = 1; + orientation_estimator_initialized = 1; } orientation_estimator_add_gyro_reading( @@ -347,7 +348,12 @@ int main(void) bmp581_update_data(&bmp581, &bmp_data); } - packet.barometer_hMSL_m = bmp581_estimate_altitude_msl(&bmp581, &bmp_data); + if (!altitude_estimator_initialized && (bmp_data.pressure && bmp_data.temperature)) { + bmp581_init_altitude_estimator(&bmp581, &bmp_data); + altitude_estimator_initialized = 1; + } + + packet.barometer_hMSL_m = bmp581_estimate_altitude_relative(&bmp581, &bmp_data); packet.temperature_c = bmp_data.temperature; if (dumb_timer_done(&camera_timer)) { From 16bb4373cf31d36921568894e256ed4708d24f23 Mon Sep 17 00:00:00 2001 From: dmanslick Date: Fri, 5 Jun 2026 12:14:03 -0700 Subject: [PATCH 2/2] Updatec constants and BMP581 struct --- Ganesha_II_V2/Core/Inc/bmp581.h | 7 ++---- Ganesha_II_V2/Core/Src/bmp581.c | 44 ++++++++++++++++----------------- 2 files changed, 23 insertions(+), 28 deletions(-) diff --git a/Ganesha_II_V2/Core/Inc/bmp581.h b/Ganesha_II_V2/Core/Inc/bmp581.h index 4920c3f..728ed68 100644 --- a/Ganesha_II_V2/Core/Inc/bmp581.h +++ b/Ganesha_II_V2/Core/Inc/bmp581.h @@ -20,7 +20,7 @@ #define TROPOPAUSE_BASE_ALTITUDE 11000.0f // m #define STRATOSPHERE_MIDDLE_BASE_ALTITUDE 20000.0f // m -#define TROPOSPHERE_LAPSE_RATE -0.0065f // K/m +#define TROPOSPHERE_LAPSE_RATE 0.0065f // K/m #define UPPER_STRATOSPHERE_LAPSE_RATE 0.001f // K/m struct BMP581 { @@ -28,10 +28,7 @@ struct BMP581 { struct bmp5_osr_odr_press_config odr_config; struct bmp5_int_source_select int_config; I2C_HandleTypeDef *hi2c; - float starting_pressure; - float starting_temp; - float tropopause_altitude; - float middle_strat_altitude; + float starting_altitude; }; BMP5_INTF_RET_TYPE read_i2c(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr); diff --git a/Ganesha_II_V2/Core/Src/bmp581.c b/Ganesha_II_V2/Core/Src/bmp581.c index 63960d1..f9c1417 100644 --- a/Ganesha_II_V2/Core/Src/bmp581.c +++ b/Ganesha_II_V2/Core/Src/bmp581.c @@ -80,40 +80,38 @@ int8_t bmp581_update_data(struct BMP581 *bmp581, struct bmp5_sensor_data *data) return bmp5_get_sensor_data(data, &(bmp581->odr_config), &(bmp581->device)); } -static inline float calc_altitude_troposphere_relative(float starting_pressure, float starting_temp, float current_pressure) { - float exponent = (-GAS_CONSTANT * TROPOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); - return (starting_temp / TROPOSPHERE_LAPSE_RATE) * (powf(current_pressure / starting_pressure, exponent) - 1.0f); +static inline float troposphere_calc(float pressure) { + const float exponent = (-GAS_CONSTANT * TROPOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); + return (STANDARD_SEA_LEVEL_TEMP / TROPOSPHERE_LAPSE_RATE) * (powf(pressure / STANDARD_SEA_LEVEL_PRESSURE, exponent) - 1.0f); } -static inline float calc_altitude_lower_stratosphere_relative(float tropopause_altitude, float current_pressure) { - float fraction = (GAS_CONSTANT * STRATOSPHERE_BASE_TEMP) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); - return tropopause_altitude + (fraction * logf(TROPOPAUSE_PRESSURE / current_pressure)); +static inline float lower_stratosphere_calc(float pressure) { + const float scale_height = (GAS_CONSTANT * STRATOSPHERE_BASE_TEMP) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); + return TROPOPAUSE_BASE_ALTITUDE + scale_height * logf(TROPOPAUSE_PRESSURE / pressure); } -static inline float calc_altitude_upper_stratosphere_relative(float middle_strat_altitude, float current_pressure) { +static inline float upper_stratosphere_calc(float pressure) { const float exponent = (-GAS_CONSTANT * UPPER_STRATOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS); - return middle_strat_altitude + (STRATOSPHERE_BASE_TEMP / UPPER_STRATOSPHERE_LAPSE_RATE) * (powf(current_pressure / STRATOSPHERE_MIDDLE_PRESSURE, exponent) - 1.0f); + const float term = powf(pressure / STRATOSPHERE_MIDDLE_PRESSURE, exponent) - 1.0f; + return STRATOSPHERE_MIDDLE_BASE_ALTITUDE + (STRATOSPHERE_BASE_TEMP / UPPER_STRATOSPHERE_LAPSE_RATE) * term; +} + +static inline float pressure_to_isa_altitude(float pressure) { + if (pressure > TROPOPAUSE_PRESSURE) { + return troposphere_calc(pressure); + } else if (pressure > STRATOSPHERE_MIDDLE_PRESSURE) { + return lower_stratosphere_calc(pressure); + } else { + return upper_stratosphere_calc(pressure); + } } void bmp581_init_altitude_estimator(struct BMP581 *bmp581, struct bmp5_sensor_data *data) { - bmp581->starting_pressure = data->pressure; - bmp581->starting_temp = data->temperature; + bmp581->starting_altitude = pressure_to_isa_altitude(data->pressure); } float bmp581_estimate_altitude_relative(struct BMP581 *bmp581, struct bmp5_sensor_data *data) { - float ret = 0; - - if (data->pressure > TROPOPAUSE_PRESSURE) { - ret = calc_altitude_troposphere_relative(bmp581->starting_pressure, bmp581->starting_temp + 273.15f, data->pressure); - bmp581->tropopause_altitude = ret; - } else if (data->pressure > STRATOSPHERE_MIDDLE_PRESSURE) { - ret = calc_altitude_lower_stratosphere_relative(bmp581->tropopause_altitude, data->pressure); - bmp581->middle_strat_altitude = ret; - } else { - return calc_altitude_upper_stratosphere_relative(bmp581->middle_strat_altitude, data->pressure); - } - - return ret; + return pressure_to_isa_altitude(data->pressure) - bmp581->starting_altitude; } int8_t bmp581_get_power_mode(struct BMP581 *bmp581, enum bmp5_powermode *powermode) {