diff --git a/include/baro.h b/include/baro.h new file mode 100644 index 0000000..be8c01f --- /dev/null +++ b/include/baro.h @@ -0,0 +1,66 @@ +#ifndef BARO_H +#define BARO_H + +#include +#include + +/* + * baro — BME280/BMP280 barometric pressure & ambient temperature module + * (Issue #672). + * + * Reads pressure and temperature from the BME280 at BARO_READ_HZ (1 Hz), + * computes pressure altitude using the ISA barometric formula, and publishes + * JLINK_TLM_BARO (0x8D) telemetry to the Orin at BARO_TLM_HZ (1 Hz). + * + * Runs entirely on the Mamba F722S — no Orin dependency. + * Altitude is exposed via baro_get_alt_cm() for use by slope compensation + * in the balance PID (Issue #672 requirement). + * + * Usage: + * 1. Call i2c1_init() then bmp280_init() and pass the chip_id result. + * 2. Call baro_tick(now_ms) every ms from the main loop. + * 3. Call baro_get_alt_cm() to read the latest altitude. + */ + +/* ---- Configuration ---- */ +#define BARO_READ_HZ 1u /* sensor poll rate (Hz) */ +#define BARO_TLM_HZ 1u /* JLink telemetry rate (Hz) */ + +/* ---- Data ---- */ +typedef struct { + int32_t pressure_pa; /* barometric pressure (Pa) */ + int16_t temp_x10; /* ambient temperature (°C × 10; e.g. 235 = 23.5 °C) */ + int32_t alt_cm; /* pressure altitude above ISA sea level (cm) */ + int16_t humidity_pct_x10; /* %RH × 10 (BME280 only); -1 if BMP280/absent */ + bool valid; /* true once at least one reading has been obtained */ +} baro_data_t; + +/* ---- API ---- */ + +/* + * baro_init(chip_id) — register chip type from bmp280_init() result. + * chip_id : 0x58 = BMP280, 0x60 = BME280, 0 = absent/not found. + * Call after i2c1_init() and bmp280_init(); no-op if chip_id == 0. + */ +void baro_init(int chip_id); + +/* + * baro_tick(now_ms) — rate-limited sensor read + JLink telemetry publish. + * Call every ms from the main loop. No-op if chip absent. + * Reads at BARO_READ_HZ; sends JLINK_TLM_BARO at BARO_TLM_HZ. + */ +void baro_tick(uint32_t now_ms); + +/* + * baro_get(out) — copy latest baro data into *out. + * Returns true on success; false if no valid reading yet. + */ +bool baro_get(baro_data_t *out); + +/* + * baro_get_alt_cm() — latest pressure altitude (cm above ISA sea level). + * Returns 0 if no valid reading. Used by slope compensation in balance PID. + */ +int32_t baro_get_alt_cm(void); + +#endif /* BARO_H */ diff --git a/include/jlink.h b/include/jlink.h index 5e55ba6..bf8a882 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -99,6 +99,7 @@ #define JLINK_TLM_STEERING 0x8Au /* jlink_tlm_steering_t (8 bytes, Issue #616) */ #define JLINK_TLM_LVC 0x8Bu /* jlink_tlm_lvc_t (4 bytes, Issue #613) */ #define JLINK_TLM_ODOM 0x8Cu /* jlink_tlm_odom_t (16 bytes, Issue #632) */ +#define JLINK_TLM_BARO 0x8Du /* jlink_tlm_baro_t (12 bytes, Issue #672) */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ typedef struct __attribute__((packed)) { @@ -239,6 +240,15 @@ typedef struct __attribute__((packed)) { int16_t speed_mmps; /* linear speed of centre point (mm/s) */ } jlink_tlm_odom_t; /* 16 bytes */ +/* ---- Telemetry BARO payload (12 bytes, packed) Issue #672 ---- */ +/* Sent at BARO_TLM_HZ (1 Hz); reports ambient pressure, temperature, altitude. */ +typedef struct __attribute__((packed)) { + int32_t pressure_pa; /* barometric pressure (Pa) */ + int16_t temp_x10; /* ambient temperature (°C × 10; e.g. 235 = 23.5 °C) */ + int32_t alt_cm; /* pressure altitude above ISA sea level (cm) */ + int16_t humidity_pct_x10; /* %RH × 10 (BME280 only); -1 = BMP280/absent */ +} jlink_tlm_baro_t; /* 12 bytes */ + /* ---- Volatile state (read from main loop) ---- */ typedef struct { /* Drive command - updated on JLINK_CMD_DRIVE */ @@ -377,4 +387,11 @@ void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm); */ void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm); +/* + * jlink_send_baro_tlm(tlm) - transmit JLINK_TLM_BARO (0x8D) frame + * (18 bytes total) at BARO_TLM_HZ (1 Hz). Issue #672. + * Rate-limiting handled by baro_tick(); call from there only. + */ +void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm); + #endif /* JLINK_H */ diff --git a/src/baro.c b/src/baro.c new file mode 100644 index 0000000..3a401a6 --- /dev/null +++ b/src/baro.c @@ -0,0 +1,90 @@ +/* + * baro.c — BME280/BMP280 barometric pressure & ambient temperature module + * (Issue #672). + * + * Reads pressure, temperature, and (on BME280) humidity from the sensor at + * BARO_READ_HZ (1 Hz). Computes pressure altitude using bmp280_pressure_to_alt_cm() + * (ISA barometric formula, p0 = 101325 Pa). Publishes JLINK_TLM_BARO (0x8D) + * telemetry to the Orin at BARO_TLM_HZ (1 Hz). + * + * Runs entirely on the Mamba F722S. No Orin dependency. + * baro_get_alt_cm() exposes altitude for slope compensation in the balance PID. + */ + +#include "baro.h" +#include "bmp280.h" +#include "jlink.h" + +static int s_chip_id = 0; /* 0x58=BMP280, 0x60=BME280, 0=absent */ +static baro_data_t s_data; /* latest reading */ +static uint32_t s_last_read_ms; /* timestamp of last I2C read */ +static uint32_t s_last_tlm_ms; /* timestamp of last telemetry TX */ + +/* ---- baro_init() ---- */ +void baro_init(int chip_id) +{ + s_chip_id = chip_id; + s_data.pressure_pa = 0; + s_data.temp_x10 = 0; + s_data.alt_cm = 0; + s_data.humidity_pct_x10 = -1; + s_data.valid = false; + + /* + * Initialise timestamps so the first baro_tick() call fires immediately + * (same convention as slope_estimator_init and steering_pid_init). + */ + const uint32_t interval_ms = 1000u / BARO_READ_HZ; + s_last_read_ms = (uint32_t)(-(uint32_t)interval_ms); + s_last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / BARO_TLM_HZ)); +} + +/* ---- baro_tick() ---- */ +void baro_tick(uint32_t now_ms) +{ + if (s_chip_id == 0) return; + + const uint32_t read_interval_ms = 1000u / BARO_READ_HZ; + if ((now_ms - s_last_read_ms) < read_interval_ms) return; + s_last_read_ms = now_ms; + + /* Read pressure (Pa) and temperature (°C × 10) */ + bmp280_read(&s_data.pressure_pa, &s_data.temp_x10); + + /* Compute pressure altitude: ISA formula, p0 = 101325 Pa */ + s_data.alt_cm = bmp280_pressure_to_alt_cm(s_data.pressure_pa); + + /* Humidity: BME280 (0x60) only; BMP280 returns -1 */ + s_data.humidity_pct_x10 = (s_chip_id == 0x60) + ? bmp280_read_humidity() + : (int16_t)-1; + + s_data.valid = true; + + /* Publish telemetry to Orin via JLink (JLINK_TLM_BARO = 0x8D) */ + const uint32_t tlm_interval_ms = 1000u / BARO_TLM_HZ; + if ((now_ms - s_last_tlm_ms) >= tlm_interval_ms) { + s_last_tlm_ms = now_ms; + + jlink_tlm_baro_t tlm; + tlm.pressure_pa = s_data.pressure_pa; + tlm.temp_x10 = s_data.temp_x10; + tlm.alt_cm = s_data.alt_cm; + tlm.humidity_pct_x10 = s_data.humidity_pct_x10; + jlink_send_baro_tlm(&tlm); + } +} + +/* ---- baro_get() ---- */ +bool baro_get(baro_data_t *out) +{ + if (!s_data.valid) return false; + *out = s_data; + return true; +} + +/* ---- baro_get_alt_cm() ---- */ +int32_t baro_get_alt_cm(void) +{ + return s_data.valid ? s_data.alt_cm : 0; +} diff --git a/src/jlink.c b/src/jlink.c index 3ccee4b..14b7ac2 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -662,3 +662,28 @@ void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm) jlink_tx_locked(frame, sizeof(frame)); } + +/* ---- jlink_send_baro_tlm() -- Issue #672 ---- */ +void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm) +{ + /* + * Frame: [STX][LEN][0x8D][12 bytes BARO][CRC_hi][CRC_lo][ETX] + * LEN = 1 + 12 = 13; total = 18 bytes + * At 921600 baud: 18×10/921600 ≈ 0.20 ms — safe to block. + */ + static uint8_t frame[18]; + const uint8_t plen = (uint8_t)sizeof(jlink_tlm_baro_t); /* 12 */ + const uint8_t len = 1u + plen; /* 13 */ + + frame[0] = JLINK_STX; + frame[1] = len; + frame[2] = JLINK_TLM_BARO; + memcpy(&frame[3], tlm, plen); + + uint16_t crc = crc16_xmodem(&frame[2], len); + frame[3 + plen] = (uint8_t)(crc >> 8); + frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); + frame[3 + plen + 2] = JLINK_ETX; + + jlink_tx_locked(frame, sizeof(frame)); +} diff --git a/src/main.c b/src/main.c index 28e8ff6..e52aea1 100644 --- a/src/main.c +++ b/src/main.c @@ -14,6 +14,7 @@ #include "crsf.h" #include "i2c1.h" #include "bmp280.h" +#include "baro.h" #include "mag.h" #include "bno055.h" #include "jetson_cmd.h" @@ -271,6 +272,7 @@ int main(void) { if (i2c1_init() == 0) { int chip = bmp280_init(); baro_chip = (chip > 0) ? chip : 0; + baro_init(baro_chip); /* Issue #672: 1 Hz baro read + JLink telemetry */ mag_type = mag_init(); ina219_init(); /* Init INA219 dual motor current monitoring (Issue #214) */ } @@ -333,6 +335,9 @@ int main(void) { /* LVC: update low-voltage protection state machine (Issue #613) */ lvc_tick(now, battery_read_mv()); + /* Baro: 1 Hz BME280 read + JLink telemetry (Issue #672) */ + baro_tick(now); + /* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness. * pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */ pm_pwm_phase++; diff --git a/test/test_baro.c b/test/test_baro.c new file mode 100644 index 0000000..e59b134 --- /dev/null +++ b/test/test_baro.c @@ -0,0 +1,315 @@ +/* + * test_baro.c — Unit tests for the baro module (Issue #672). + * + * Build (host, no hardware): + * gcc -I include -I test/stubs -DTEST_HOST -lm \ + * -o /tmp/test_baro src/baro.c test/test_baro.c + * + * All tests are self-contained; no HAL, no I2C, no UART required. + * bmp280_init/read/humidity/alt and jlink_send_baro_tlm are stubbed below. + */ + +/* ---- Stubs ---- */ + +/* Prevent jlink.h from pulling in HAL / pid_flash types */ +#define JLINK_H +#include +#include + +/* Minimal jlink_tlm_baro_t matching the real definition */ +typedef struct __attribute__((packed)) { + int32_t pressure_pa; + int16_t temp_x10; + int32_t alt_cm; + int16_t humidity_pct_x10; +} jlink_tlm_baro_t; + +/* Capture last transmitted baro tlm for inspection */ +static jlink_tlm_baro_t g_last_baro_tlm; +static int g_baro_tlm_count = 0; + +void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm) +{ + g_last_baro_tlm = *tlm; + g_baro_tlm_count++; +} + +/* Stub bmp280_read() — returns configurable values */ +static int32_t g_stub_pressure_pa = 101325; +static int16_t g_stub_temp_x10 = 230; /* 23.0 °C */ + +void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) +{ + *pressure_pa = g_stub_pressure_pa; + *temp_x10 = g_stub_temp_x10; +} + +/* Stub bmp280_read_humidity() */ +static int16_t g_stub_humidity = 500; /* 50.0 %RH */ + +int16_t bmp280_read_humidity(void) +{ + return g_stub_humidity; +} + +/* Stub bmp280_pressure_to_alt_cm() — proportional approx for testing */ +int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa) +{ + /* Simplified: sea level = 101325 Pa = 0 cm; decrease of 12 Pa ≈ 100 cm */ + return (int32_t)((101325 - pressure_pa) * 100 / 12); +} + +/* ---- Include implementation directly ---- */ +#include "../src/baro.c" + +/* ---- Test framework ---- */ +#include +#include +#include + +static int g_pass = 0; +static int g_fail = 0; + +#define ASSERT(cond, msg) do { \ + if (cond) { g_pass++; } \ + else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \ +} while(0) + +/* ---- Helper ---- */ +static void reset_stubs(void) +{ + g_stub_pressure_pa = 101325; + g_stub_temp_x10 = 230; + g_stub_humidity = 500; + g_baro_tlm_count = 0; + memset(&g_last_baro_tlm, 0, sizeof(g_last_baro_tlm)); +} + +/* ---- Tests ---- */ + +static void test_init_absent_chip_no_read(void) +{ + reset_stubs(); + baro_init(0); /* chip absent */ + + /* baro_tick must be a no-op */ + baro_tick(1000u); + ASSERT(g_baro_tlm_count == 0, "absent chip: no telemetry sent"); + + baro_data_t d; + ASSERT(!baro_get(&d), "absent chip: baro_get returns false"); + ASSERT(baro_get_alt_cm() == 0, "absent chip: baro_get_alt_cm returns 0"); +} + +static void test_first_tick_fires_immediately(void) +{ + /* + * After baro_init() timestamps are pre-wound so that the very first + * baro_tick() triggers a read and a telemetry send. + */ + reset_stubs(); + baro_init(0x60); /* BME280 */ + + baro_tick(1000u); + ASSERT(g_baro_tlm_count == 1, "first tick sends telemetry immediately"); + + baro_data_t d; + ASSERT(baro_get(&d), "first tick: data valid"); +} + +static void test_data_populated_after_tick(void) +{ + reset_stubs(); + g_stub_pressure_pa = 100000; + g_stub_temp_x10 = 150; /* 15.0 °C */ + g_stub_humidity = 400; /* 40.0 %RH */ + + baro_init(0x60); + baro_tick(0u); + + baro_data_t d; + ASSERT(baro_get(&d), "data valid after tick"); + ASSERT(d.pressure_pa == 100000, "pressure_pa correct"); + ASSERT(d.temp_x10 == 150, "temp_x10 correct"); + ASSERT(d.humidity_pct_x10 == 400, "humidity correct (BME280)"); + ASSERT(d.valid, "valid flag set"); +} + +static void test_altitude_computed(void) +{ + reset_stubs(); + g_stub_pressure_pa = 101325 - 120; /* 120 Pa below sea level → ~1000 cm */ + baro_init(0x58); /* BMP280 */ + baro_tick(0u); + + baro_data_t d; + baro_get(&d); + /* stub: alt_cm = (101325 - 101205)*100/12 = 12000/12 = 1000 cm */ + ASSERT(d.alt_cm == 1000, "altitude computed correctly"); + ASSERT(baro_get_alt_cm() == 1000, "baro_get_alt_cm matches"); +} + +static void test_bmp280_no_humidity(void) +{ + reset_stubs(); + baro_init(0x58); /* BMP280 — no humidity */ + baro_tick(0u); + + baro_data_t d; + baro_get(&d); + ASSERT(d.humidity_pct_x10 == (int16_t)-1, + "BMP280: humidity_pct_x10 == -1"); +} + +static void test_bme280_has_humidity(void) +{ + reset_stubs(); + g_stub_humidity = 650; + baro_init(0x60); /* BME280 */ + baro_tick(0u); + + baro_data_t d; + baro_get(&d); + ASSERT(d.humidity_pct_x10 == 650, + "BME280: humidity_pct_x10 populated"); +} + +static void test_rate_limited_1hz(void) +{ + /* + * At BARO_READ_HZ = 1, only one read per second. + * Call baro_tick() every ms for 3000 ms → expect 3 sends. + */ + reset_stubs(); + baro_init(0x60); + + for (uint32_t ms = 0; ms < 3000u; ms++) { + baro_tick(ms); + } + ASSERT(g_baro_tlm_count == 3, + "1 Hz rate limit: 3 sends in 3000 ms"); +} + +static void test_no_tlm_before_interval(void) +{ + reset_stubs(); + baro_init(0x60); + + /* First tick at ms=0 fires (pre-wound). Next must not fire until ms=1000. */ + baro_tick(0u); + ASSERT(g_baro_tlm_count == 1, "first tick sends"); + + baro_tick(500u); /* only 500 ms later — must not send again */ + ASSERT(g_baro_tlm_count == 1, "second tick before interval: no extra send"); +} + +static void test_tlm_payload_encoding(void) +{ + reset_stubs(); + g_stub_pressure_pa = 95000; + g_stub_temp_x10 = -50; /* -5.0 °C */ + g_stub_humidity = 900; + baro_init(0x60); + baro_tick(0u); + + ASSERT(g_baro_tlm_count == 1, "TLM sent"); + ASSERT(g_last_baro_tlm.pressure_pa == 95000, + "TLM: pressure_pa encoded"); + ASSERT(g_last_baro_tlm.temp_x10 == (int16_t)-50, + "TLM: negative temp encoded"); + ASSERT(g_last_baro_tlm.humidity_pct_x10 == 900, + "TLM: humidity encoded"); + /* alt_cm via stub: (101325-95000)*100/12 = 632500/12 ≈ 52708 */ + ASSERT(g_last_baro_tlm.alt_cm == g_last_baro_tlm.alt_cm, /* sanity */ + "TLM: alt_cm field present"); + /* verify alt matches baro_get */ + baro_data_t d; + baro_get(&d); + ASSERT(g_last_baro_tlm.alt_cm == d.alt_cm, + "TLM: alt_cm matches baro_get"); +} + +static void test_get_returns_false_before_first_tick(void) +{ + reset_stubs(); + baro_init(0x60); + + /* Trick: call with a time that won't fire yet by advancing only 1 ms + * BUT init pre-winds timestamps so tick at 0 fires. Test the initial + * state by checking valid flag before any tick. */ + /* We can observe this by checking the internal s_data.valid = false + * by attempting a get on a freshly reinitialised module. + * We simulate by re-init'ing with chip=0 so tick is no-op. */ + baro_init(0); /* chip absent — tick will never fire */ + baro_data_t d; + ASSERT(!baro_get(&d), "no tick yet: baro_get returns false"); +} + +static void test_get_alt_zero_before_valid(void) +{ + reset_stubs(); + baro_init(0); /* no chip → never valid */ + ASSERT(baro_get_alt_cm() == 0, "alt_cm == 0 when no valid data"); +} + +static void test_data_updates_each_interval(void) +{ + /* + * Change stub pressure between ticks. + * Verify baro_get() reflects the latest reading. + */ + reset_stubs(); + baro_init(0x58); + + g_stub_pressure_pa = 101325; + baro_tick(0u); + baro_data_t d1; + baro_get(&d1); + + g_stub_pressure_pa = 100000; + baro_tick(1000u); /* one interval later */ + baro_data_t d2; + baro_get(&d2); + + ASSERT(d1.pressure_pa == 101325, "first reading correct"); + ASSERT(d2.pressure_pa == 100000, "second reading updated"); + ASSERT(d1.alt_cm != d2.alt_cm, "alt_cm updates between reads"); +} + +static void test_reinit_clears_valid(void) +{ + reset_stubs(); + baro_init(0x60); + baro_tick(0u); + + baro_data_t d; + ASSERT(baro_get(&d), "data valid after tick"); + + /* Re-init with absent chip — valid flag cleared */ + baro_init(0); + ASSERT(!baro_get(&d), "after re-init (absent): valid cleared"); + ASSERT(baro_get_alt_cm() == 0, "after re-init (absent): alt == 0"); +} + +/* ---- main ---- */ +int main(void) +{ + printf("=== test_baro ===\n"); + + test_init_absent_chip_no_read(); + test_first_tick_fires_immediately(); + test_data_populated_after_tick(); + test_altitude_computed(); + test_bmp280_no_humidity(); + test_bme280_has_humidity(); + test_rate_limited_1hz(); + test_no_tlm_before_interval(); + test_tlm_payload_encoding(); + test_get_returns_false_before_first_tick(); + test_get_alt_zero_before_valid(); + test_data_updates_each_interval(); + test_reinit_clears_valid(); + + printf("\nResults: %d passed, %d failed\n", g_pass, g_fail); + return g_fail ? 1 : 0; +}