From d67d9b29d21e3031d4c2cd0d1e22abf82974823c Mon Sep 17 00:00:00 2001 From: portersky <24420859+portersky@users.noreply.github.com> Date: Sun, 14 Jun 2026 23:34:12 +0200 Subject: [PATCH] fix: parse CRSF battery as big-endian per protocol spec CRSF battery frame is big-endian: voltage(u16 BE 0.1V), current(u16 BE 0.1A), capacity(u24 BE mAh), remaining(u8 %). Previous code read little-endian with wrong byte count (7 vs 8) and wrong scaling (/1000 vs /10), producing 9.98V for a 1S battery. --- celrs/crsf_telemetry.c | 28 ++++++++++++++++++++-------- celrs/crsf_telemetry.h | 8 ++++---- tests/test_crsf_telemetry.c | 20 ++++++++++++-------- tools/telemetry.c | 6 +++--- 4 files changed, 39 insertions(+), 23 deletions(-) diff --git a/celrs/crsf_telemetry.c b/celrs/crsf_telemetry.c index 7e33eea..8074b69 100644 --- a/celrs/crsf_telemetry.c +++ b/celrs/crsf_telemetry.c @@ -2,10 +2,20 @@ #include /* Helper: read uint16_t little-endian from buffer */ -static uint16_t read_u16(uint8_t const* buf) { +static uint16_t read_u16_le(uint8_t const* buf) { return (uint16_t)buf[0] | ((uint16_t)buf[1] << 8); } +/* Helper: read uint16_t big-endian from buffer */ +static uint16_t read_u16_be(uint8_t const* buf) { + return ((uint16_t)buf[0] << 8) | (uint16_t)buf[1]; +} + +/* Helper: read uint32_t big-endian from 3-byte buffer */ +static uint32_t read_u24_be(uint8_t const* buf) { + return ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | (uint32_t)buf[2]; +} + int cel_crsf_telemetry_parse(cel_crsf_frame const* frame, cel_telemetry* out) { if (frame == NULL || out == NULL) return -1; @@ -32,26 +42,28 @@ int cel_crsf_telemetry_parse(cel_crsf_frame const* frame, } case CEL_CRSF_TYPE_BATTERY: { - if (len < 7) return -1; + if (len < 8) return -1; out->type = CEL_TELEM_BATTERY; - out->data.battery.voltage_mv = read_u16(p); - out->data.battery.current_ma = read_u16(p + 2); - out->data.battery.capacity_mah = read_u16(p + 4); - out->data.battery.remaining_pct = p[6]; + /* CRSF battery: voltage(u16 BE 0.1V), current(u16 BE 0.1A), + capacity(u24 BE mAh), remaining(u8 %) */ + out->data.battery.voltage_x10 = read_u16_be(p); + out->data.battery.current_x10 = read_u16_be(p + 2); + out->data.battery.capacity_mah = read_u24_be(p + 4); + out->data.battery.remaining_pct = p[7]; return 0; } case CEL_CRSF_TYPE_HEARTBEAT: { if (len < 2) return -1; out->type = CEL_TELEM_HEARTBEAT; - out->data.heartbeat.origin_addr = read_u16(p); + out->data.heartbeat.origin_addr = read_u16_le(p); return 0; } case CEL_CRSF_TYPE_AIRSPEED: { if (len < 2) return -1; out->type = CEL_TELEM_AIRSPEED; - out->data.airspeed.speed_kmh = read_u16(p); + out->data.airspeed.speed_kmh = read_u16_le(p); return 0; } diff --git a/celrs/crsf_telemetry.h b/celrs/crsf_telemetry.h index 46c6466..f78a756 100644 --- a/celrs/crsf_telemetry.h +++ b/celrs/crsf_telemetry.h @@ -36,11 +36,11 @@ typedef struct { int16_t vertical_speed_cms; } cel_telem_vario; -/* Battery sensor */ +/* Battery sensor (CRSF: u16 BE 0.1V, u16 BE 0.1A, u24 BE mAh, u8 %) */ typedef struct { - uint16_t voltage_mv; /* millivolts */ - uint16_t current_ma; /* milliamps */ - uint16_t capacity_mah; /* mAh consumed */ + uint16_t voltage_x10; /* x 0.1V */ + uint16_t current_x10; /* x 0.1A */ + uint32_t capacity_mah; /* mAh consumed */ uint8_t remaining_pct; /* percentage remaining */ } cel_telem_battery; diff --git a/tests/test_crsf_telemetry.c b/tests/test_crsf_telemetry.c index 29b9c32..554a6d1 100644 --- a/tests/test_crsf_telemetry.c +++ b/tests/test_crsf_telemetry.c @@ -60,13 +60,15 @@ void test_parse_link_stats_short(void) { void test_parse_battery(void) { uint8_t buf[32]; - uint8_t payload[7] = { - 0x40, 0x03, /* voltage: 0x0340 = 832 -> 0.832V */ - 0x00, 0x00, /* current: 0 */ - 0x00, 0x00, /* capacity: 0 */ + /* CRSF battery: voltage(u16 BE 0.1V), current(u16 BE 0.1A), + capacity(u24 BE mAh), remaining(u8 %) = 8 bytes */ + uint8_t payload[8] = { + 0x03, 0xE8, /* voltage: 0x03E8 = 1000 -> 100.0V (10S LiPo) */ + 0x00, 0x64, /* current: 0x0064 = 100 -> 10.0A */ + 0x00, 0x03, 0xE8, /* capacity: 0x0003E8 = 1000mAh */ 0x64 /* remaining: 100% */ }; - build_frame(buf, 0x08, CEL_CRSF_TYPE_BATTERY, payload, 7); + build_frame(buf, 0x08, CEL_CRSF_TYPE_BATTERY, payload, 8); cel_crsf_frame frame; TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf))); @@ -74,13 +76,15 @@ void test_parse_battery(void) { cel_telemetry telem; TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem)); TEST_ASSERT_EQUAL_UINT(CEL_TELEM_BATTERY, telem.type); - TEST_ASSERT_EQUAL_UINT16(0x0340, telem.data.battery.voltage_mv); + TEST_ASSERT_EQUAL_UINT16(0x03E8, telem.data.battery.voltage_x10); + TEST_ASSERT_EQUAL_UINT16(0x0064, telem.data.battery.current_x10); + TEST_ASSERT_EQUAL_UINT32(0x0003E8, telem.data.battery.capacity_mah); TEST_ASSERT_EQUAL_UINT8(0x64, telem.data.battery.remaining_pct); } void test_parse_heartbeat(void) { uint8_t buf[32]; - uint8_t payload[2] = {0x10, 0x80}; /* origin_addr = 0x8010 */ + uint8_t payload[2] = {0x10, 0x80}; /* origin_addr LE = 0x8010 */ build_frame(buf, 0x10, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); cel_crsf_frame frame; @@ -94,7 +98,7 @@ void test_parse_heartbeat(void) { void test_parse_airspeed(void) { uint8_t buf[32]; - uint8_t payload[2] = {0x00, 0x01}; /* speed = 0x0100 = 256 km/h */ + uint8_t payload[2] = {0x00, 0x01}; /* speed LE = 0x0100 = 256 km/h */ build_frame(buf, 0x08, CEL_CRSF_TYPE_AIRSPEED, payload, 2); cel_crsf_frame frame; diff --git a/tools/telemetry.c b/tools/telemetry.c index af1bd48..55a1f74 100644 --- a/tools/telemetry.c +++ b/tools/telemetry.c @@ -174,8 +174,8 @@ static void dashboard_update(dashboard_t* d, cel_telemetry const* telem, break; case CEL_TELEM_BATTERY: d->has_batt = 1; - d->batt_v = telem->data.battery.voltage_mv / 1000.0f; - d->batt_a = telem->data.battery.current_ma / 1000.0f; + d->batt_v = telem->data.battery.voltage_x10 / 10.0f; + d->batt_a = telem->data.battery.current_x10 / 10.0f; d->batt_mah = telem->data.battery.capacity_mah; d->batt_pct = telem->data.battery.remaining_pct; d->batt_t = now; @@ -286,7 +286,7 @@ static void render_dashboard(dashboard_t const* d, if (d->batt_v > 3.0f) ansi_green(); else ansi_red(); printf("%.2fV ", d->batt_v); ansi_reset(); - printf("%.1fA %dmah %d%%", d->batt_a, d->batt_mah, d->batt_pct); + printf("%.1fA %umah %d%%", d->batt_a, d->batt_mah, d->batt_pct); print_age(now, d->batt_t); } else { ansi_dim(); printf("waiting..."); ansi_reset();