Merge pull request 'feat: Auto-detect magnetometer + barometer (#24)' (#27) from sl-firmware/mag-baro-detect into main

This commit is contained in:
seb 2026-02-28 18:45:25 -05:00
commit d37e9ab276
8 changed files with 355 additions and 70 deletions

View File

@ -1,6 +1,19 @@
#ifndef BMP280_H
#define BMP280_H
#include <stdint.h>
/*
* BMP280 / BME280 barometer driver.
*
* Probes I2C1 at 0x76 then 0x77.
* Returns chip_id (0x58=BMP280, 0x60=BME280) on success, negative if not found.
* Requires i2c1_init() to have been called first.
*/
int bmp280_init(void);
void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10);
#endif
/* Convert pressure (Pa) to altitude above sea level (cm), ISA p0=101325 Pa. */
int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa);
#endif /* BMP280_H */

17
include/i2c1.h Normal file
View File

@ -0,0 +1,17 @@
#ifndef I2C1_H
#define I2C1_H
#include "stm32f7xx_hal.h"
/*
* Shared I2C1 bus handle used by baro (BMP280/DPS310) and mag
* (QMC5883L/HMC5883L/IST8310) drivers.
*
* Call i2c1_init() once in main() before any I2C probes.
* PB8 = SCL, PB9 = SDA (AF4_I2C1, open-drain, 100 kHz).
*/
extern I2C_HandleTypeDef hi2c1;
int i2c1_init(void);
#endif /* I2C1_H */

26
include/mag.h Normal file
View File

@ -0,0 +1,26 @@
#ifndef MAG_H
#define MAG_H
#include <stdint.h>
typedef enum {
MAG_NONE = 0,
MAG_QMC5883L, /* I2C 0x0D — most common on external compass modules */
MAG_HMC5883L, /* I2C 0x1E — legacy Honeywell part */
MAG_IST8310, /* I2C 0x0E — iSentek part, common on CUAV boards */
} mag_type_t;
/*
* Auto-detect magnetometer on I2C1.
* Returns detected type (MAG_NONE if nothing found).
* Requires i2c1_init() before calling.
*/
mag_type_t mag_init(void);
/*
* Read compass heading (degrees × 10, 03599).
* Returns -1 if data not ready or no sensor.
*/
int16_t mag_read_heading(void);
#endif /* MAG_H */

View File

@ -1,9 +1,15 @@
/* BMP280 barometer driver — I2C1 */
#include "stm32f7xx_hal.h"
/*
* bmp280.c BMP280/BME280 barometer driver (I2C1, shared bus)
*
* Probes 0x76 first, then 0x77. Requires i2c1_init() before bmp280_init().
* Returns chip_id on success (0x58=BMP280, 0x60=BME280), negative if absent.
*/
#include "bmp280.h"
#include "i2c1.h"
#include <math.h>
static I2C_HandleTypeDef hi2c1;
#define BMP280_ADDR (0x76 << 1) /* SDO to GND = 0x76, to VCC = 0x77 */
/* Shift I2C address for HAL (7-bit left-shifted) */
static uint16_t s_addr;
/* Calibration data */
static uint16_t dig_T1;
@ -14,69 +20,52 @@ static int32_t t_fine;
static uint8_t i2c_read(uint8_t reg) {
uint8_t val = 0;
HAL_I2C_Mem_Read(&hi2c1, BMP280_ADDR, reg, 1, &val, 1, 100);
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &val, 1, 100);
return val;
}
static void i2c_read_burst(uint8_t reg, uint8_t *buf, uint8_t len) {
HAL_I2C_Mem_Read(&hi2c1, BMP280_ADDR, reg, 1, buf, len, 100);
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 100);
}
static void i2c_write(uint8_t reg, uint8_t val) {
HAL_I2C_Mem_Write(&hi2c1, BMP280_ADDR, reg, 1, &val, 1, 100);
HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100);
}
int bmp280_init(void) {
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_I2C1_CLK_ENABLE();
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9; /* PB8=SCL, PB9=SDA */
gpio.Mode = GPIO_MODE_AF_OD;
gpio.Pull = GPIO_PULLUP;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
gpio.Alternate = GPIO_AF4_I2C1;
HAL_GPIO_Init(GPIOB, &gpio);
hi2c1.Instance = I2C1;
hi2c1.Init.Timing = 0x20404768; /* 100kHz @ 54MHz APB1 */
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK) return -1;
HAL_Delay(10);
/* Check chip ID */
static int try_init(uint16_t addr) {
s_addr = addr;
uint8_t id = i2c_read(0xD0);
if (id != 0x58 && id != 0x60) return -(int)id; /* 0x58=BMP280, 0x60=BME280 */
if (id != 0x58 && id != 0x60) return -(int)id;
/* Read calibration */
uint8_t cal[26];
i2c_read_burst(0x88, cal, 26);
dig_T1 = (uint16_t)(cal[1] << 8 | cal[0]);
dig_T2 = (int16_t)(cal[3] << 8 | cal[2]);
dig_T3 = (int16_t)(cal[5] << 8 | cal[4]);
dig_T2 = (int16_t) (cal[3] << 8 | cal[2]);
dig_T3 = (int16_t) (cal[5] << 8 | cal[4]);
dig_P1 = (uint16_t)(cal[7] << 8 | cal[6]);
dig_P2 = (int16_t)(cal[9] << 8 | cal[8]);
dig_P3 = (int16_t)(cal[11] << 8 | cal[10]);
dig_P4 = (int16_t)(cal[13] << 8 | cal[12]);
dig_P5 = (int16_t)(cal[15] << 8 | cal[14]);
dig_P6 = (int16_t)(cal[17] << 8 | cal[16]);
dig_P7 = (int16_t)(cal[19] << 8 | cal[18]);
dig_P8 = (int16_t)(cal[21] << 8 | cal[20]);
dig_P9 = (int16_t)(cal[23] << 8 | cal[22]);
dig_P2 = (int16_t) (cal[9] << 8 | cal[8]);
dig_P3 = (int16_t) (cal[11] << 8 | cal[10]);
dig_P4 = (int16_t) (cal[13] << 8 | cal[12]);
dig_P5 = (int16_t) (cal[15] << 8 | cal[14]);
dig_P6 = (int16_t) (cal[17] << 8 | cal[16]);
dig_P7 = (int16_t) (cal[19] << 8 | cal[18]);
dig_P8 = (int16_t) (cal[21] << 8 | cal[20]);
dig_P9 = (int16_t) (cal[23] << 8 | cal[22]);
/* Config: normal mode, 16x oversampling temp+press, 0.5ms standby */
/* Normal mode, ×16 oversampling temp+press, 0.5 ms standby */
i2c_write(0xF5, 0x00); /* config: standby=0.5ms, filter=off */
i2c_write(0xF4, 0xB7); /* ctrl_meas: osrs_t=x16, osrs_p=x16, normal mode */
i2c_write(0xF4, 0xB7); /* ctrl_meas: osrs_t=×16, osrs_p=×16, normal mode */
HAL_Delay(50);
return (int)id;
}
int bmp280_init(void) {
int ret = try_init(0x76 << 1);
if (ret > 0) return ret;
return try_init(0x77 << 1);
}
void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) {
uint8_t buf[6];
i2c_read_burst(0xF7, buf, 6);
@ -84,24 +73,31 @@ void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) {
int32_t adc_P = (int32_t)((buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4));
int32_t adc_T = (int32_t)((buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4));
/* Temperature compensation (from BMP280 datasheet) */
int32_t var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
int32_t var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1))) >> 12) * ((int32_t)dig_T3)) >> 14;
t_fine = var1 + var2;
*temp_x10 = (int16_t)((t_fine * 5 + 128) >> 8); /* 0.1°C units */
/* Temperature compensation (BMP280 datasheet Section 4.2.3) */
int32_t v1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
int32_t v2 = (((((adc_T >> 4) - (int32_t)dig_T1) *
((adc_T >> 4) - (int32_t)dig_T1)) >> 12) * (int32_t)dig_T3) >> 14;
t_fine = v1 + v2;
*temp_x10 = (int16_t)((t_fine * 5 + 128) >> 8); /* 0.1 °C */
/* Pressure compensation */
int64_t v1 = ((int64_t)t_fine) - 128000;
int64_t v2 = v1 * v1 * (int64_t)dig_P6;
v2 = v2 + ((v1 * (int64_t)dig_P5) << 17);
v2 = v2 + (((int64_t)dig_P4) << 35);
v1 = ((v1 * v1 * (int64_t)dig_P3) >> 8) + ((v1 * (int64_t)dig_P2) << 12);
v1 = (((((int64_t)1) << 47) + v1)) * ((int64_t)dig_P1) >> 33;
if (v1 == 0) { *pressure_pa = 0; return; }
int64_t p1 = ((int64_t)t_fine) - 128000;
int64_t p2 = p1 * p1 * (int64_t)dig_P6;
p2 += (p1 * (int64_t)dig_P5) << 17;
p2 += ((int64_t)dig_P4) << 35;
p1 = ((p1 * p1 * (int64_t)dig_P3) >> 8) + ((p1 * (int64_t)dig_P2) << 12);
p1 = ((((int64_t)1 << 47) + p1)) * ((int64_t)dig_P1) >> 33;
if (p1 == 0) { *pressure_pa = 0; return; }
int64_t p = 1048576 - adc_P;
p = (((p << 31) - v2) * 3125) / v1;
v1 = (((int64_t)dig_P9) * (p >> 13) * (p >> 13)) >> 25;
v2 = (((int64_t)dig_P8) * p) >> 19;
*pressure_pa = (int32_t)((p + v1 + v2) >> 8) + (((int64_t)dig_P7) << 4);
*pressure_pa /= 256; /* Pa */
p = (((p << 31) - p2) * 3125) / p1;
p1 = ((int64_t)dig_P9 * (p >> 13) * (p >> 13)) >> 25;
p2 = ((int64_t)dig_P8 * p) >> 19;
*pressure_pa = (int32_t)(((p + p1 + p2) >> 8) + ((int64_t)dig_P7 << 4)) / 256;
}
int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa) {
/* Barometric formula: h = 44330 * (1 - (p/p0)^(1/5.255)) metres */
float ratio = (float)pressure_pa / 101325.0f;
float alt_m = 44330.0f * (1.0f - powf(ratio, 0.1902949f));
return (int32_t)(alt_m * 100.0f); /* cm */
}

33
src/i2c1.c Normal file
View File

@ -0,0 +1,33 @@
/*
* i2c1.c Shared I2C1 bus (PB8=SCL, PB9=SDA, 100 kHz)
*
* Used by barometer and magnetometer drivers.
* Call i2c1_init() once before any I2C probes.
*/
#include "i2c1.h"
#include "stm32f7xx_hal.h"
I2C_HandleTypeDef hi2c1;
int i2c1_init(void) {
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_I2C1_CLK_ENABLE();
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9; /* PB8=SCL, PB9=SDA */
gpio.Mode = GPIO_MODE_AF_OD;
gpio.Pull = GPIO_PULLUP;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
gpio.Alternate = GPIO_AF4_I2C1;
HAL_GPIO_Init(GPIOB, &gpio);
hi2c1.Instance = I2C1;
hi2c1.Init.Timing = 0x20404768; /* 100 kHz @ 54 MHz APB1 */
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
return (HAL_I2C_Init(&hi2c1) == HAL_OK) ? 0 : -1;
}

155
src/mag.c Normal file
View File

@ -0,0 +1,155 @@
/*
* mag.c Auto-detect magnetometer on I2C1
*
* Probes for QMC5883L (0x0D), HMC5883L (0x1E), IST8310 (0x0E) in that order.
* Returns detected type; mag_read_heading() returns heading in degrees×10.
*
* Requires i2c1_init() called before mag_init().
*/
#include "mag.h"
#include "i2c1.h"
#include <math.h>
/* ------------------------------------------------------------------ */
/* I2C helpers (7-bit addr left-shifted for HAL) */
/* ------------------------------------------------------------------ */
static uint16_t s_addr;
static mag_type_t s_type = MAG_NONE;
static uint8_t mag_rreg(uint8_t reg) {
uint8_t v = 0;
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &v, 1, 50);
return v;
}
static void mag_wreg(uint8_t reg, uint8_t val) {
HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 50);
}
static void mag_rregs(uint8_t reg, uint8_t *buf, uint8_t len) {
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 50);
}
/* ------------------------------------------------------------------ */
/* QMC5883L — QUST QMC5883L (0x0D) */
/* chip_id reg 0x0D = 0xFF */
/* Data regs 0x00-0x05: XL XH YL YH ZL ZH */
/* ------------------------------------------------------------------ */
static int qmc_init(void) {
s_addr = 0x0D << 1;
uint8_t id = mag_rreg(0x0D);
if (id != 0xFF) return 0;
/* Reset */
mag_wreg(0x0B, 0x01); /* SET/RESET period = 0x01 */
HAL_Delay(10);
/* Control 1: continuous mode (0x01), 200 Hz ODR (0x0C), 512 OSR (0x00), 2G (0x00) */
mag_wreg(0x09, 0x0D); /* MODE=cont, ODR=200Hz, RNG=2G, OSR=512 */
HAL_Delay(10);
return 1;
}
static int16_t qmc_heading(void) {
/* Status reg 0x06, bit0=DRDY */
if (!(mag_rreg(0x06) & 0x01)) return -1;
uint8_t buf[6];
mag_rregs(0x00, buf, 6);
int16_t x = (int16_t)((buf[1] << 8) | buf[0]);
int16_t y = (int16_t)((buf[3] << 8) | buf[2]);
float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f);
if (heading_deg < 0.0f) heading_deg += 360.0f;
return (int16_t)(heading_deg * 10.0f);
}
/* ------------------------------------------------------------------ */
/* HMC5883L — Honeywell HMC5883L (0x1E) */
/* ID regs 0x0A='H', 0x0B='4', 0x0C='3' */
/* Data regs 0x03-0x08: XH XL ZH ZL YH YL (note XZY order!) */
/* ------------------------------------------------------------------ */
static int hmc_init(void) {
s_addr = 0x1E << 1;
uint8_t a = mag_rreg(0x0A);
uint8_t b = mag_rreg(0x0B);
uint8_t c = mag_rreg(0x0C);
if (a != 'H' || b != '4' || c != '3') return 0;
mag_wreg(0x00, 0x70); /* Config A: 8 samples/output, 15 Hz, normal */
mag_wreg(0x01, 0x20); /* Config B: gain=1090 LSB/Ga */
mag_wreg(0x02, 0x00); /* Mode: continuous measurement */
HAL_Delay(10);
return 1;
}
static int16_t hmc_heading(void) {
/* Status reg 0x09, bit0=RDY */
if (!(mag_rreg(0x09) & 0x01)) return -1;
uint8_t buf[6];
mag_rregs(0x03, buf, 6);
int16_t x = (int16_t)((buf[0] << 8) | buf[1]);
int16_t y = (int16_t)((buf[4] << 8) | buf[5]); /* HMC order: XZY */
float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f);
if (heading_deg < 0.0f) heading_deg += 360.0f;
return (int16_t)(heading_deg * 10.0f);
}
/* ------------------------------------------------------------------ */
/* IST8310 — iSentek IST8310 (0x0E) */
/* WHO_AM_I reg 0x00 = 0x10 */
/* Data regs 0x03-0x08: XL XH YL YH ZL ZH */
/* ------------------------------------------------------------------ */
static int ist_init(void) {
s_addr = 0x0E << 1;
uint8_t id = mag_rreg(0x00);
if (id != 0x10) return 0;
/* Soft reset via CNTL2 reg 0x0B bit[0] */
mag_wreg(0x0B, 0x01);
HAL_Delay(20);
/* CNTL1 reg 0x0A: single measurement mode, trigger on write */
/* Use continuous mode bit[2]=1 if supported — trigger manually instead */
mag_wreg(0x0A, 0x01); /* single measurement */
HAL_Delay(10);
return 1;
}
static int16_t ist_heading(void) {
/* Trigger a measurement */
mag_wreg(0x0A, 0x01);
HAL_Delay(10); /* IST8310 needs ~6.6ms for single measurement */
/* STAT1 reg 0x02 bit0=DRDY */
if (!(mag_rreg(0x02) & 0x01)) return -1;
uint8_t buf[6];
mag_rregs(0x03, buf, 6);
int16_t x = (int16_t)((buf[1] << 8) | buf[0]);
int16_t y = (int16_t)((buf[3] << 8) | buf[2]);
float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f);
if (heading_deg < 0.0f) heading_deg += 360.0f;
return (int16_t)(heading_deg * 10.0f);
}
/* ------------------------------------------------------------------ */
/* Public API */
/* ------------------------------------------------------------------ */
mag_type_t mag_init(void) {
if (qmc_init()) { s_type = MAG_QMC5883L; return s_type; }
if (hmc_init()) { s_type = MAG_HMC5883L; return s_type; }
if (ist_init()) { s_type = MAG_IST8310; return s_type; }
s_type = MAG_NONE;
return MAG_NONE;
}
int16_t mag_read_heading(void) {
switch (s_type) {
case MAG_QMC5883L: return qmc_heading();
case MAG_HMC5883L: return hmc_heading();
case MAG_IST8310: return ist_heading();
default: return -1;
}
}

View File

@ -11,6 +11,9 @@
#include "status.h"
#include "safety.h"
#include "crsf.h"
#include "i2c1.h"
#include "bmp280.h"
#include "mag.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
@ -126,6 +129,14 @@ int main(void) {
motor_driver_t motors;
motor_driver_init(&motors);
/* Probe I2C1 for optional sensors — skip gracefully if not found */
int baro_ok = 0;
mag_type_t mag_type = MAG_NONE;
if (i2c1_init() == 0) {
baro_ok = (bmp280_init() > 0) ? 1 : 0;
mag_type = mag_init();
}
/*
* IWDG starts AFTER all peripheral inits avoids reset during mpu6000_init()
* which takes ~510ms (well above the 50ms WATCHDOG_TIMEOUT_MS).
@ -217,15 +228,37 @@ int main(void) {
send_tick = now;
if (imu_ret == 0) {
float err = bal.setpoint - bal.pitch_deg;
len = snprintf(buf, sizeof(buf),
"{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d,\"y\":%d}\n",
/* Build JSON incrementally; append optional sensor fields */
char *p = buf;
int rem = (int)sizeof(buf);
int n;
n = snprintf(p, rem,
"{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d,\"y\":%d",
(int)(bal.pitch_deg * 10), /* pitch degrees x10 */
(int)(imu.roll * 10), /* roll degrees x10 */
(int)(err * 10), /* PID error x10 */
(int)(bal.integral * 10), /* integral x10 (windup monitor) */
(int)bal.motor_cmd, /* ESC command -1000..+1000 */
(int)bal.state,
(int)(imu.yaw * 10)); /* yaw degrees x10 (gyro-integrated, drifts) */
(int)(imu.yaw * 10)); /* yaw degrees x10 (gyro-integrated) */
p += n; rem -= n;
if (mag_type != MAG_NONE) {
int16_t hd = mag_read_heading();
if (hd >= 0)
n = snprintf(p, rem, ",\"hd\":%d", hd); /* heading deg×10 */
else
n = snprintf(p, rem, ",\"hd\":-1"); /* not ready */
p += n; rem -= n;
}
if (baro_ok) {
int32_t pres_pa; int16_t temp_x10;
bmp280_read(&pres_pa, &temp_x10);
int32_t alt_cm = bmp280_pressure_to_alt_cm(pres_pa);
n = snprintf(p, rem, ",\"alt\":%ld", (long)alt_cm); /* cm */
p += n; rem -= n;
}
n = snprintf(p, rem, "}\n");
len = (int)(p + n - buf);
} else {
len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret);
}

View File

@ -51,6 +51,8 @@
<div class="stat"><span class="label">ROLL</span> <span class="val" id="v-roll">--</span>°</div>
<div class="stat"><span class="label">YAW</span> <span class="val" id="v-yaw">--</span>°</div>
<div class="stat"><span class="label">MOTOR</span> <span class="val" id="v-motor">--</span></div>
<div class="stat" id="row-hdg" style="display:none"><span class="label">HEADING</span> <span class="val" id="v-hdg">--</span>°</div>
<div class="stat" id="row-alt" style="display:none"><span class="label">ALT</span> <span class="val" id="v-alt">--</span> m</div>
<div class="stat"><span class="label">HZ</span> <span class="val" id="v-hz">--</span></div>
<div>
<button class="btn" id="connect-btn" onclick="toggleSerial()">CONNECT USB</button>
@ -201,6 +203,16 @@ window.updateIMU = function(data) {
document.getElementById('v-yaw').textContent = yaw.toFixed(1);
document.getElementById('v-motor').textContent = motorCmd;
// Optional sensors — show row only when data is present
if (data.hd !== undefined && data.hd >= 0) {
document.getElementById('row-hdg').style.display = '';
document.getElementById('v-hdg').textContent = (data.hd / 10.0).toFixed(1);
}
if (data.alt !== undefined) {
document.getElementById('row-alt').style.display = '';
document.getElementById('v-alt').textContent = (data.alt / 100.0).toFixed(1);
}
// Pitch bar: center at 50%, ±90°
document.getElementById('bar-pitch').style.width = ((pitch + 90) / 180 * 100) + '%';
// Motor bar: center at 50%, ±1000