From 44c8805bd0312b53ead835712e3d0e66c0e40ff1 Mon Sep 17 00:00:00 2001 From: Christopher Wun Date: Mon, 17 Nov 2025 13:51:15 -0500 Subject: [PATCH 1/4] constructor init of data ptrs, defensive checks on reads --- shared-module/bno080/BNO080.c | 69 ++++++++++++++++++++++++++++++++++- 1 file changed, 68 insertions(+), 1 deletion(-) diff --git a/shared-module/bno080/BNO080.c b/shared-module/bno080/BNO080.c index 962eeb6b1212a..f7648f0955e7b 100644 --- a/shared-module/bno080/BNO080.c +++ b/shared-module/bno080/BNO080.c @@ -768,6 +768,23 @@ void common_hal_bno080_BNO080_construct(bno080_BNO080_obj_t *self, busio_spi_obj common_hal_digitalio_digitalinout_construct(&self->irq, irq); common_hal_digitalio_digitalinout_set_irq(&self->irq, EDGE_FALL, PULL_UP, bno080_isr_recv, self); + // Initialize data arrays to valid float objects (0.0) + for (int i = 0; i < QUAT_DIMENSION; i++) { + self->fquat[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < ACCEL_DIMENSION; i++) { + self->accel[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < GYRO_DIMENSION; i++) { + self->gyro[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < MAG_DIMENSION; i++) { + self->mag[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < GRAV_DIMENSION; i++) { + self->grav[i] = mp_obj_new_float(0.0f); + } + lock_bus(self); common_hal_busio_spi_configure(self->bus, BNO_BAUDRATE, 1, 1, 8); unlock_bus(self); @@ -803,6 +820,23 @@ void common_hal_bno080_BNO080_reset(bno080_BNO080_obj_t *self) { // clear seqnums memset(self->read_seqnums, 0xff, sizeof(self->read_seqnums)); memset(self->write_seqnums, 0x00, sizeof(self->write_seqnums)); + + // Re-initialize data arrays to valid float objects (0.0) + for (int i = 0; i < QUAT_DIMENSION; i++) { + self->fquat[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < ACCEL_DIMENSION; i++) { + self->accel[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < GYRO_DIMENSION; i++) { + self->gyro[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < MAG_DIMENSION; i++) { + self->mag[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < GRAV_DIMENSION; i++) { + self->grav[i] = mp_obj_new_float(0.0f); + } } STATIC void bno080_unary_rotation(bno080_BNO080_obj_t *self, uint8_t feature) { @@ -814,6 +848,7 @@ STATIC void bno080_unary_rotation(bno080_BNO080_obj_t *self, uint8_t feature) { case BNO080_SRID_ROTATION_VECTOR: if (self->selected_rotation != 0 && self->selected_rotation != feature) { uint8_t disable = self->selected_rotation; + mp_printf(&mp_plat_print, "unary_rotation: Disabling feature %d\n", disable); common_hal_bno080_BNO080_set_feature(self, disable, 0, 0, 0, 0, 0); } self->selected_rotation = feature; @@ -825,8 +860,9 @@ STATIC void bno080_unary_rotation(bno080_BNO080_obj_t *self, uint8_t feature) { int common_hal_bno080_BNO080_set_feature(bno080_BNO080_obj_t *self, uint8_t feature, uint32_t refresh_us, uint32_t batch_us, uint8_t flags, uint16_t sns, uint32_t cfg) { int rc = 0; + mp_printf(&mp_plat_print, "checking rotation for feature %d\n", feature); bno080_unary_rotation(self, feature); - + mp_printf(&mp_plat_print, "defining command for feature %d\n", feature); const uint8_t command[17] = { BNO080_SET_FEATURE_COMMAND, @@ -868,14 +904,45 @@ mp_obj_t common_hal_bno080_BNO080_read(bno080_BNO080_obj_t *self, uint8_t report case BNO080_SRID_GEOMAGNETIC_ROTATION_VECTOR: case BNO080_SRID_GAME_ROTATION_VECTOR: case BNO080_SRID_ROTATION_VECTOR: + // Defensive: check for NULLs and re-initialize if needed + for (int i = 0; i < QUAT_DIMENSION; i++) { + if (self->fquat[i] == NULL) { + mp_printf(&mp_plat_print, "Warning: fquat[%d] was NULL, reinitializing to 0.0\n", i); + self->fquat[i] = mp_obj_new_float(0.0f); + } + } return mp_obj_new_list(QUAT_DIMENSION, self->fquat); case BNO080_SRID_ACCELEROMETER: + for (int i = 0; i < ACCEL_DIMENSION; i++) { + if (self->accel[i] == NULL) { + mp_printf(&mp_plat_print, "Warning: accel[%d] was NULL, reinitializing to 0.0\n", i); + self->accel[i] = mp_obj_new_float(0.0f); + } + } return mp_obj_new_list(ACCEL_DIMENSION, self->accel); case BNO080_SRID_GYROSCOPE: + for (int i = 0; i < GYRO_DIMENSION; i++) { + if (self->gyro[i] == NULL) { + mp_printf(&mp_plat_print, "Warning: gyro[%d] was NULL, reinitializing to 0.0\n", i); + self->gyro[i] = mp_obj_new_float(0.0f); + } + } return mp_obj_new_list(GYRO_DIMENSION, self->gyro); case BNO080_SRID_MAGNETIC_FIELD: + for (int i = 0; i < MAG_DIMENSION; i++) { + if (self->mag[i] == NULL) { + mp_printf(&mp_plat_print, "Warning: mag[%d] was NULL, reinitializing to 0.0\n", i); + self->mag[i] = mp_obj_new_float(0.0f); + } + } return mp_obj_new_list(MAG_DIMENSION, self->mag); case BNO080_SRID_GRAVITY: + for (int i = 0; i < GRAV_DIMENSION; i++) { + if (self->grav[i] == NULL) { + mp_printf(&mp_plat_print, "Warning: grav[%d] was NULL, reinitializing to 0.0\n", i); + self->grav[i] = mp_obj_new_float(0.0f); + } + } return mp_obj_new_list(GRAV_DIMENSION, self->grav); } From 31965773de9fa72a6d8a0409a604cd196fb7494d Mon Sep 17 00:00:00 2001 From: Christopher Wun Date: Mon, 17 Nov 2025 13:56:16 -0500 Subject: [PATCH 2/4] remove unnec prints --- shared-module/bno080/BNO080.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/shared-module/bno080/BNO080.c b/shared-module/bno080/BNO080.c index f7648f0955e7b..42f43ecd0a878 100644 --- a/shared-module/bno080/BNO080.c +++ b/shared-module/bno080/BNO080.c @@ -848,7 +848,6 @@ STATIC void bno080_unary_rotation(bno080_BNO080_obj_t *self, uint8_t feature) { case BNO080_SRID_ROTATION_VECTOR: if (self->selected_rotation != 0 && self->selected_rotation != feature) { uint8_t disable = self->selected_rotation; - mp_printf(&mp_plat_print, "unary_rotation: Disabling feature %d\n", disable); common_hal_bno080_BNO080_set_feature(self, disable, 0, 0, 0, 0, 0); } self->selected_rotation = feature; @@ -860,9 +859,7 @@ STATIC void bno080_unary_rotation(bno080_BNO080_obj_t *self, uint8_t feature) { int common_hal_bno080_BNO080_set_feature(bno080_BNO080_obj_t *self, uint8_t feature, uint32_t refresh_us, uint32_t batch_us, uint8_t flags, uint16_t sns, uint32_t cfg) { int rc = 0; - mp_printf(&mp_plat_print, "checking rotation for feature %d\n", feature); bno080_unary_rotation(self, feature); - mp_printf(&mp_plat_print, "defining command for feature %d\n", feature); const uint8_t command[17] = { BNO080_SET_FEATURE_COMMAND, From a8880a3d68f01b9f5fe7124dad211488bf9ad088 Mon Sep 17 00:00:00 2001 From: Christopher Wun Date: Mon, 17 Nov 2025 14:54:16 -0500 Subject: [PATCH 3/4] add helper to be more DRY --- shared-module/bno080/BNO080.c | 50 ++++++++++++++--------------------- 1 file changed, 20 insertions(+), 30 deletions(-) diff --git a/shared-module/bno080/BNO080.c b/shared-module/bno080/BNO080.c index 42f43ecd0a878..627b90f252790 100644 --- a/shared-module/bno080/BNO080.c +++ b/shared-module/bno080/BNO080.c @@ -753,6 +753,24 @@ STATIC void bno080_isr_recv(void *arg) { bno080_spi_sample(self); } +STATIC void bno080_init_data_arrays(bno080_BNO080_obj_t *self) { + for (int i = 0; i < QUAT_DIMENSION; i++) { + self->fquat[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < ACCEL_DIMENSION; i++) { + self->accel[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < GYRO_DIMENSION; i++) { + self->gyro[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < MAG_DIMENSION; i++) { + self->mag[i] = mp_obj_new_float(0.0f); + } + for (int i = 0; i < GRAV_DIMENSION; i++) { + self->grav[i] = mp_obj_new_float(0.0f); + } +} + void common_hal_bno080_BNO080_construct(bno080_BNO080_obj_t *self, busio_spi_obj_t *bus, const mcu_pin_obj_t *cs, const mcu_pin_obj_t *rst, const mcu_pin_obj_t *ps0, const mcu_pin_obj_t *bootn, const mcu_pin_obj_t *irq) { self->bus = bus; self->resp = 0; @@ -769,21 +787,7 @@ void common_hal_bno080_BNO080_construct(bno080_BNO080_obj_t *self, busio_spi_obj common_hal_digitalio_digitalinout_set_irq(&self->irq, EDGE_FALL, PULL_UP, bno080_isr_recv, self); // Initialize data arrays to valid float objects (0.0) - for (int i = 0; i < QUAT_DIMENSION; i++) { - self->fquat[i] = mp_obj_new_float(0.0f); - } - for (int i = 0; i < ACCEL_DIMENSION; i++) { - self->accel[i] = mp_obj_new_float(0.0f); - } - for (int i = 0; i < GYRO_DIMENSION; i++) { - self->gyro[i] = mp_obj_new_float(0.0f); - } - for (int i = 0; i < MAG_DIMENSION; i++) { - self->mag[i] = mp_obj_new_float(0.0f); - } - for (int i = 0; i < GRAV_DIMENSION; i++) { - self->grav[i] = mp_obj_new_float(0.0f); - } + bno080_init_data_arrays(self); lock_bus(self); common_hal_busio_spi_configure(self->bus, BNO_BAUDRATE, 1, 1, 8); @@ -822,21 +826,7 @@ void common_hal_bno080_BNO080_reset(bno080_BNO080_obj_t *self) { memset(self->write_seqnums, 0x00, sizeof(self->write_seqnums)); // Re-initialize data arrays to valid float objects (0.0) - for (int i = 0; i < QUAT_DIMENSION; i++) { - self->fquat[i] = mp_obj_new_float(0.0f); - } - for (int i = 0; i < ACCEL_DIMENSION; i++) { - self->accel[i] = mp_obj_new_float(0.0f); - } - for (int i = 0; i < GYRO_DIMENSION; i++) { - self->gyro[i] = mp_obj_new_float(0.0f); - } - for (int i = 0; i < MAG_DIMENSION; i++) { - self->mag[i] = mp_obj_new_float(0.0f); - } - for (int i = 0; i < GRAV_DIMENSION; i++) { - self->grav[i] = mp_obj_new_float(0.0f); - } + bno080_init_data_arrays(self); } STATIC void bno080_unary_rotation(bno080_BNO080_obj_t *self, uint8_t feature) { From 54af197e8b5e3a0a55db5dc0bd6c211ce8c3f6b5 Mon Sep 17 00:00:00 2001 From: Christopher Wun Date: Thu, 20 Nov 2025 17:02:32 -0500 Subject: [PATCH 4/4] remove data init in reset --- shared-module/bno080/BNO080.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/shared-module/bno080/BNO080.c b/shared-module/bno080/BNO080.c index 627b90f252790..2bfcd73d15cad 100644 --- a/shared-module/bno080/BNO080.c +++ b/shared-module/bno080/BNO080.c @@ -824,9 +824,6 @@ void common_hal_bno080_BNO080_reset(bno080_BNO080_obj_t *self) { // clear seqnums memset(self->read_seqnums, 0xff, sizeof(self->read_seqnums)); memset(self->write_seqnums, 0x00, sizeof(self->write_seqnums)); - - // Re-initialize data arrays to valid float objects (0.0) - bno080_init_data_arrays(self); } STATIC void bno080_unary_rotation(bno080_BNO080_obj_t *self, uint8_t feature) {