fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor(bmp388): refactor variable declaration and initialization

Browse Source 
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
murata,katsutoshi 2026-04-10 08:38:17 +09:00 committed by GitHub
parent a49cffb09f
commit fc53da51fa
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 39 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
src/drivers/barometer/bmp388/bmp388.cpp
View File

@ -210,16 +210,15 @@ BMP388::collect()
bool
BMP388::soft_reset()
{
bool result = false;
uint8_t status;
int ret;
ret = _interface->get_reg(BMP3_SENS_STATUS_REG_ADDR, &status);
int ret = _interface->get_reg(BMP3_SENS_STATUS_REG_ADDR, &status);
if (ret != OK) {
return false;
}
bool result = false;
if (status & BMP3_CMD_RDY) {
ret = _interface->set_reg(BPM3_CMD_SOFT_RESET, BMP3_CMD_ADDR);
@ -248,16 +247,9 @@ BMP388::soft_reset()
static int8_t cal_crc(uint8_t seed, uint8_t data)
{
int8_t poly = 0x1D;
int8_t var2;
uint8_t i;
for (i = 0; i < 8; i++) {
if ((seed & 0x80) ^ (data & 0x80)) {
var2 = 1;
} else {
var2 = 0;
}
for (uint8_t i = 0; i < 8; i++) {
int8_t var2 = ((seed & 0x80) ^ (data & 0x80)) ? 1 : 0;
seed = (seed & 0x7F) << 1;
data = (data & 0x7F) << 1;
@ -276,7 +268,6 @@ bool
BMP388::validate_trimming_param()
{
uint8_t crc = 0xFF;
uint8_t stored_crc;
uint8_t *trim_param = (uint8_t *)_cal;
static_assert(BMP3_CALIB_DATA_LEN <= sizeof(*_cal), "unexpected struct size");
@ -287,6 +278,8 @@ BMP388::validate_trimming_param()
crc = (crc ^ 0xFF);
uint8_t stored_crc;
if (_interface->get_reg(BMP3_TRIM_CRC_DATA_ADDR, &stored_crc) != OK) {
return false;
}
@ -425,18 +418,14 @@ BMP388::set_sensor_settings()
bool
BMP388::set_op_mode(uint8_t op_mode)
{
bool result = false;
uint8_t last_set_mode;
uint8_t op_mode_reg_val;
int ret = OK;
ret = _interface->get_reg(BMP3_PWR_CTRL_ADDR, &op_mode_reg_val);
int ret = _interface->get_reg(BMP3_PWR_CTRL_ADDR, &op_mode_reg_val);
if (ret != OK) {
return false;
}
last_set_mode = BMP3_GET_BITS(op_mode_reg_val, BMP3_OP_MODE);
uint8_t last_set_mode = BMP3_GET_BITS(op_mode_reg_val, BMP3_OP_MODE);
/* Device needs to be put in sleep mode to transition */
if (last_set_mode != BMP3_SLEEP_MODE) {
@ -450,6 +439,8 @@ BMP388::set_op_mode(uint8_t op_mode)
px4_usleep(BMP3_POST_SLEEP_WAIT_TIME);
}
bool result = false;
if (ret == OK) {
ret = _interface->get_reg(BMP3_PWR_CTRL_ADDR, &op_mode_reg_val);
@ -478,13 +469,9 @@ BMP388::set_op_mode(uint8_t op_mode)
*/
static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data *uncomp_data)
{
uint32_t data_xlsb;
uint32_t data_lsb;
uint32_t data_msb;
data_xlsb = (uint32_t)reg_data[0];
data_lsb = (uint32_t)reg_data[1] << 8;
data_msb = (uint32_t)reg_data[2] << 16;
uint32_t data_xlsb = (uint32_t)reg_data[0];
uint32_t data_lsb = (uint32_t)reg_data[1] << 8;
uint32_t data_msb = (uint32_t)reg_data[2] << 16;
uncomp_data->pressure = data_msb | data_lsb | data_xlsb;
data_xlsb = (uint32_t)reg_data[3];
@ -503,24 +490,16 @@ static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data *
*/
static int64_t compensate_temperature(const struct bmp3_uncomp_data *uncomp_data, struct bmp3_calib_data *calib_data)
{
int64_t partial_data1;
int64_t partial_data2;
int64_t partial_data3;
int64_t partial_data4;
int64_t partial_data5;
int64_t partial_data6;
int64_t comp_temp;
partial_data1 = ((int64_t)uncomp_data->temperature - (256 * calib_data->reg_calib_data.par_t1));
partial_data2 = calib_data->reg_calib_data.par_t2 * partial_data1;
partial_data3 = (partial_data1 * partial_data1);
partial_data4 = (int64_t)partial_data3 * calib_data->reg_calib_data.par_t3;
partial_data5 = ((int64_t)(partial_data2 * 262144) + partial_data4);
partial_data6 = partial_data5 / 4294967296;
int64_t partial_data1 = ((int64_t)uncomp_data->temperature - (256 * calib_data->reg_calib_data.par_t1));
int64_t partial_data2 = calib_data->reg_calib_data.par_t2 * partial_data1;
int64_t partial_data3 = (partial_data1 * partial_data1);
int64_t partial_data4 = (int64_t)partial_data3 * calib_data->reg_calib_data.par_t3;
int64_t partial_data5 = ((int64_t)(partial_data2 * 262144) + partial_data4);
int64_t partial_data6 = partial_data5 / 4294967296;
/* Store t_lin in dev. structure for pressure calculation */
calib_data->reg_calib_data.t_lin = partial_data6;
comp_temp = (int64_t)((partial_data6 * 25) / 16384);
int64_t comp_temp = (int64_t)((partial_data6 * 25) / 16384);
return comp_temp;
}
@ -536,27 +515,19 @@ static uint64_t compensate_pressure(const struct bmp3_uncomp_data *uncomp_data,
const struct bmp3_calib_data *calib_data)
{
const struct bmp3_reg_calib_data *reg_calib_data = &calib_data->reg_calib_data;
int64_t partial_data1;
int64_t partial_data2;
int64_t partial_data3;
int64_t partial_data4;
int64_t partial_data5;
int64_t partial_data6;
int64_t offset;
int64_t sensitivity;
uint64_t comp_press;
partial_data1 = reg_calib_data->t_lin * reg_calib_data->t_lin;
partial_data2 = partial_data1 / 64;
partial_data3 = (partial_data2 * reg_calib_data->t_lin) / 256;
partial_data4 = (reg_calib_data->par_p8 * partial_data3) / 32;
partial_data5 = (reg_calib_data->par_p7 * partial_data1) * 16;
partial_data6 = (reg_calib_data->par_p6 * reg_calib_data->t_lin) * 4194304;
offset = (reg_calib_data->par_p5 * 140737488355328) + partial_data4 + partial_data5 + partial_data6;
int64_t partial_data1 = reg_calib_data->t_lin * reg_calib_data->t_lin;
int64_t partial_data2 = partial_data1 / 64;
int64_t partial_data3 = (partial_data2 * reg_calib_data->t_lin) / 256;
int64_t partial_data4 = (reg_calib_data->par_p8 * partial_data3) / 32;
int64_t partial_data5 = (reg_calib_data->par_p7 * partial_data1) * 16;
int64_t partial_data6 = (reg_calib_data->par_p6 * reg_calib_data->t_lin) * 4194304;
int64_t offset = (reg_calib_data->par_p5 * 140737488355328) + partial_data4 + partial_data5 + partial_data6;
partial_data2 = (reg_calib_data->par_p4 * partial_data3) / 32;
partial_data4 = (reg_calib_data->par_p3 * partial_data1) * 4;
partial_data5 = (reg_calib_data->par_p2 - 16384) * reg_calib_data->t_lin * 2097152;
sensitivity = ((reg_calib_data->par_p1 - 16384) * 70368744177664) + partial_data2 + partial_data4 + partial_data5;
int64_t sensitivity = ((reg_calib_data->par_p1 - 16384) * 70368744177664) + partial_data2 + partial_data4 +
partial_data5;
partial_data1 = (sensitivity / 16777216) * uncomp_data->pressure;
partial_data2 = reg_calib_data->par_p10 * reg_calib_data->t_lin;
partial_data3 = partial_data2 + (65536 * reg_calib_data->par_p9);
@ -568,7 +539,7 @@ static uint64_t compensate_pressure(const struct bmp3_uncomp_data *uncomp_data,
partial_data2 = (reg_calib_data->par_p11 * partial_data6) / 65536;
partial_data3 = (partial_data2 * uncomp_data->pressure) / 128;
partial_data4 = (offset / 4) + partial_data1 + partial_data5 + partial_data3;
comp_press = (((uint64_t)partial_data4 * 25) / (uint64_t)1099511627776);
uint64_t comp_press = (((uint64_t)partial_data4 * 25) / (uint64_t)1099511627776);
return comp_press;
}
@ -589,7 +560,7 @@ BMP388::compensate_data(uint8_t sensor_comp,
struct bmp3_reg_calib_data *reg_calib_data = &calib_data.reg_calib_data;
memcpy(reg_calib_data, _cal, 21);
if ((uncomp_data != NULL) && (comp_data != NULL)) {
if ((uncomp_data != nullptr) && (comp_data != nullptr)) {
if (sensor_comp & (BMP3_PRESS | BMP3_TEMP)) {
comp_data->temperature = compensate_temperature(uncomp_data, &calib_data);
}
@ -599,10 +570,10 @@ BMP388::compensate_data(uint8_t sensor_comp,
}
} else {
rslt = -1;
rslt = ERROR;
}
return (rslt == 0);
return (rslt == OK);
}
/*!
@ -613,13 +584,12 @@ BMP388::compensate_data(uint8_t sensor_comp,
bool
BMP388::get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data)
{
uint8_t reg_data[BMP3_P_T_DATA_LEN] {};
int8_t rslt = _interface->get_reg_buf(BMP3_SENS_STATUS_REG_ADDR, reg_data, BMP3_P_T_DATA_LEN);
bool result = false;
int8_t rslt;
uint8_t reg_data[BMP3_P_T_DATA_LEN];
struct bmp3_uncomp_data uncomp_data;
rslt = _interface->get_reg_buf(BMP3_SENS_STATUS_REG_ADDR, reg_data, BMP3_P_T_DATA_LEN);
struct bmp3_uncomp_data uncomp_data {};
if (rslt == OK) {
uint8_t status = reg_data[0];