Compare commits
5 Commits
787a303cf5
...
8ff2542fbc
| Author | SHA1 | Date | |
|---|---|---|---|
| 8ff2542fbc | |||
| df09615d3f | |||
| f58eb0d976 | |||
| d67d9b29d2 | |||
| ef5012b9d4 |
+1
-1
@@ -63,7 +63,7 @@ size_t cel_crsf_build_rc_frame(uint8_t* dst, int16_t const channels[16]) {
|
||||
uint8_t length = 1 + 22 + 1; /* type + payload + crc */
|
||||
dst[0] = 0xC8; /* RC frame address */
|
||||
dst[1] = length;
|
||||
dst[2] = CEL_CRSF_TYPE_RC_CHANNELS_PACKED;
|
||||
dst[2] = CEL_CRSF_TYPE_RC_CHANNELS;
|
||||
memcpy(dst + 3, packed, 22);
|
||||
uint8_t crc = cel_crsf_crc(dst + 2, 1 + 22);
|
||||
dst[2 + length - 1] = crc;
|
||||
|
||||
@@ -18,7 +18,6 @@
|
||||
|
||||
/* CRSF frame types (ELRS) */
|
||||
typedef enum {
|
||||
CEL_CRSF_TYPE_RC_CHANNELS_PACKED = 0x01,
|
||||
CEL_CRSF_TYPE_GPS = 0x02,
|
||||
CEL_CRSF_TYPE_VARIO = 0x07,
|
||||
CEL_CRSF_TYPE_BATTERY = 0x08,
|
||||
|
||||
+20
-8
@@ -2,10 +2,20 @@
|
||||
#include <string.h>
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -27,6 +27,13 @@ cel_serial_platform_handle cel_serial_platform_open(char const* path, int baud_r
|
||||
dcb.Parity = NOPARITY;
|
||||
dcb.StopBits = ONESTOPBIT;
|
||||
|
||||
/* Keep DTR/RTS low so the TX module is not reset on connect. Setting
|
||||
* these in the same SetCommState call (rather than via a later
|
||||
* EscapeCommFunction) avoids a brief DTR/RTS-high pulse if the port's
|
||||
* existing DCB has them enabled. */
|
||||
dcb.fDtrControl = DTR_CONTROL_DISABLE;
|
||||
dcb.fRtsControl = RTS_CONTROL_DISABLE;
|
||||
|
||||
if (!SetCommState(h, &dcb)) {
|
||||
CloseHandle(h);
|
||||
return CEL_SERIAL_PLATFORM_INVALID_HANDLE;
|
||||
|
||||
+1
-1
@@ -193,7 +193,7 @@ void test_build_rc_frame_roundtrip(void) {
|
||||
|
||||
cel_crsf_frame frame;
|
||||
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
|
||||
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_RC_CHANNELS_PACKED, frame.type);
|
||||
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_RC_CHANNELS, frame.type);
|
||||
}
|
||||
|
||||
void test_build_ping_frame_null_dst(void) {
|
||||
|
||||
@@ -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;
|
||||
|
||||
+84
-10
@@ -79,7 +79,7 @@ static void ansi_cursor_up(int n) {
|
||||
/* --------------------------------------------------------------------------- */
|
||||
|
||||
#define LINK_STALE_S 3.0f
|
||||
#define FC_STALE_S 2.0f
|
||||
#define FC_STALE_S 5.0f
|
||||
|
||||
typedef enum {
|
||||
STATUS_LIVE,
|
||||
@@ -88,9 +88,12 @@ typedef enum {
|
||||
STATUS_NO_SIGNAL
|
||||
} status_t;
|
||||
|
||||
static status_t compute_status(double now, double link_t, double fc_t) {
|
||||
static status_t compute_status(double now, double link_t, double fc_t,
|
||||
int has_link, uint8_t up_lq) {
|
||||
if (link_t == 0 || (now - link_t) > LINK_STALE_S)
|
||||
return STATUS_NO_SIGNAL;
|
||||
if (has_link && up_lq == 0)
|
||||
return STATUS_NO_LINK;
|
||||
if (fc_t > 0 && (now - fc_t) > FC_STALE_S)
|
||||
return STATUS_STALE;
|
||||
return STATUS_LIVE;
|
||||
@@ -152,6 +155,7 @@ typedef struct {
|
||||
/* Counts */
|
||||
int rx_frames;
|
||||
int unknown;
|
||||
uint32_t type_counts[256]; /* indexed by raw CRSF frame type byte */
|
||||
} dashboard_t;
|
||||
|
||||
static void dashboard_init(dashboard_t* d) {
|
||||
@@ -167,15 +171,15 @@ static void dashboard_update(dashboard_t* d, cel_telemetry const* telem,
|
||||
d->up_rssi1 = telem->data.link.uplink_rssi1;
|
||||
d->up_rssi2 = telem->data.link.uplink_rssi2;
|
||||
d->up_lq = telem->data.link.uplink_quality;
|
||||
d->up_snr = telem->data.link.uplink_snr - 128;
|
||||
d->up_snr = telem->data.link.uplink_snr;
|
||||
d->power_idx = telem->data.link.uplink_power;
|
||||
d->rf_mode = telem->data.link.rf_mode;
|
||||
d->link_t = now;
|
||||
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;
|
||||
@@ -227,6 +231,34 @@ static void rssi_color(double dbm) {
|
||||
else ansi_red();
|
||||
}
|
||||
|
||||
/* Raw CRSF frame type byte -> name, for diagnostics.
|
||||
Returns NULL for types not in cel_crsf_type. */
|
||||
static char const* crsf_type_name(uint8_t type) {
|
||||
switch (type) {
|
||||
case CEL_CRSF_TYPE_GPS: return "GPS";
|
||||
case CEL_CRSF_TYPE_VARIO: return "VARIO";
|
||||
case CEL_CRSF_TYPE_BATTERY: return "BATTERY";
|
||||
case CEL_CRSF_TYPE_BARO_ALT: return "BARO_ALT";
|
||||
case CEL_CRSF_TYPE_AIRSPEED: return "AIRSPEED";
|
||||
case CEL_CRSF_TYPE_HEARTBEAT: return "HEARTBEAT";
|
||||
case CEL_CRSF_TYPE_RPM: return "RPM";
|
||||
case CEL_CRSF_TYPE_TEMP: return "TEMP";
|
||||
case CEL_CRSF_TYPE_VOLTAGES: return "VOLTAGES";
|
||||
case CEL_CRSF_TYPE_ESC_SENSOR: return "ESC_SENSOR";
|
||||
case CEL_CRSF_TYPE_LINK_STATS: return "LINK_STATS";
|
||||
case CEL_CRSF_TYPE_RC_CHANNELS: return "RC_CHANNELS";
|
||||
case CEL_CRSF_TYPE_ATTITUDE: return "ATTITUDE";
|
||||
case CEL_CRSF_TYPE_FLIGHT_MODE: return "FLIGHT_MODE";
|
||||
case CEL_CRSF_TYPE_DEVICE_PING: return "DEVICE_PING";
|
||||
case CEL_CRSF_TYPE_DEVICE_INFO: return "DEVICE_INFO";
|
||||
case CEL_CRSF_TYPE_PARAM_ENTRY: return "PARAM_ENTRY";
|
||||
case CEL_CRSF_TYPE_PARAM_READ: return "PARAM_READ";
|
||||
case CEL_CRSF_TYPE_PARAM_WRITE: return "PARAM_WRITE";
|
||||
case CEL_CRSF_TYPE_ELRS_STATUS: return "ELRS_STATUS";
|
||||
default: return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
/* --------------------------------------------------------------------------- */
|
||||
/* Dashboard render - tracks line count for in-place redraw */
|
||||
/* --------------------------------------------------------------------------- */
|
||||
@@ -235,7 +267,8 @@ static void render_dashboard(dashboard_t const* d,
|
||||
char const* port, int baud,
|
||||
double elapsed, int* lines) {
|
||||
double now = (double)time(NULL);
|
||||
status_t status = compute_status(now, d->link_t, d->fc_t);
|
||||
status_t status = compute_status(now, d->link_t, d->fc_t,
|
||||
d->has_link, d->up_lq);
|
||||
|
||||
/* Return to top of dashboard if already drawn */
|
||||
if (*lines > 0) {
|
||||
@@ -286,7 +319,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();
|
||||
@@ -326,6 +359,36 @@ static void render_dashboard(dashboard_t const* d,
|
||||
printf("\n");
|
||||
n++;
|
||||
|
||||
/* Frame type breakdown (top 6 by count) */
|
||||
ansi_clear_line();
|
||||
ansi_dim();
|
||||
printf(" types:");
|
||||
uint8_t used[256] = {0};
|
||||
int shown = 0;
|
||||
for (int k = 0; k < 6; k++) {
|
||||
int best = -1;
|
||||
uint32_t best_count = 0;
|
||||
for (int t = 0; t < 256; t++) {
|
||||
if (!used[t] && d->type_counts[t] > best_count) {
|
||||
best_count = d->type_counts[t];
|
||||
best = t;
|
||||
}
|
||||
}
|
||||
if (best < 0) break;
|
||||
used[best] = 1;
|
||||
char const* name = crsf_type_name((uint8_t)best);
|
||||
if (name != NULL) {
|
||||
printf(" %s=%u", name, best_count);
|
||||
} else {
|
||||
printf(" 0x%02X=%u", best, best_count);
|
||||
}
|
||||
shown++;
|
||||
}
|
||||
if (shown == 0) printf(" -");
|
||||
ansi_reset();
|
||||
printf("\n");
|
||||
n++;
|
||||
|
||||
*lines = n;
|
||||
fflush(stdout);
|
||||
}
|
||||
@@ -361,11 +424,13 @@ static int list_ports(void) {
|
||||
|
||||
/* Send a ping and wait for DEVICE_INFO to verify the module responds. */
|
||||
static int verify_connection(cel_serial_port* port) {
|
||||
if (cel_crsf_param_ping(port, 2.0f) != 0) {
|
||||
/* The module may take a few seconds to respond to the first ping,
|
||||
* so retry a few times before giving up. */
|
||||
for (int attempt = 0; attempt < 3; attempt++) {
|
||||
if (cel_crsf_param_ping(port, 2.0f) == 0) return 0;
|
||||
}
|
||||
cel_log_warn("No DEVICE_INFO response - module may not be connected");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char const* argv[]) {
|
||||
@@ -453,6 +518,7 @@ int main(int argc, char const* argv[]) {
|
||||
|
||||
double now = (double)time(NULL);
|
||||
for (int i = 0; i < n; i++) {
|
||||
dash.type_counts[frames[i].type]++;
|
||||
cel_telemetry telem;
|
||||
if (cel_crsf_telemetry_parse(&frames[i], &telem) == 0) {
|
||||
dashboard_update(&dash, &telem, now);
|
||||
@@ -469,6 +535,14 @@ int main(int argc, char const* argv[]) {
|
||||
cel_serial_write(port, rc_buf, rc_len);
|
||||
}
|
||||
|
||||
/* Send DEVICE_PING every 5s so DEVICE_INFO replies keep showing up
|
||||
* in the dashboard's type_counts, like the Python ping_loop. */
|
||||
if (rc_count % 250 == 0) {
|
||||
uint8_t ping_buf[8];
|
||||
size_t ping_len = cel_crsf_build_ping_frame(ping_buf);
|
||||
cel_serial_write(port, ping_buf, ping_len);
|
||||
}
|
||||
|
||||
/* Redraw dashboard every 100 ms */
|
||||
if (rc_count % 5 == 0) {
|
||||
double elapsed = difftime(time(NULL), t_start);
|
||||
|
||||
Reference in New Issue
Block a user