feat(firmware): BNO085 node, concentrador y listener PGN 127237
AR_electronics — AR-Autopilot Project Tarea #3 — firmware/ar_bno085_node_v1/ Nodo compacto ESP32+CAN lee heading y yaw rate del BNO085 via I2C y los publica en el backbone NMEA 2000 como PGN 127250+127251 a 10 Hz. Tarea #4 — firmware/ar_concentrador_v1/ Gateway NMEA2000<->NMEA0183 con 4 puertos OUT (UART1 TX broadcast) y 4 puertos IN (UART2 RX comandos). Parsea sentencias $PARP, gestiona autoridad de mando entre estaciones con timeout 10s y override del puente. Reenvía comandos autopilot como PGN 127237 al backbone. Tarea #5 — nmea2000_consumer (ar_autopilot_v1) Listener PGN 127237 entrante desde concentrador: HeadingControlSnapshot, HandleHeadingControl() con filtro source address, nmea2000_htc() publico, ventana stale 3s para comandos externos.
This commit is contained in:
@@ -0,0 +1,93 @@
|
||||
// =============================================================================
|
||||
// protocols/nmea0183_parser.cpp -- $PARP sentence parser
|
||||
// =============================================================================
|
||||
|
||||
#include "nmea0183_parser.h"
|
||||
|
||||
#include <cstring>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <Arduino.h>
|
||||
|
||||
namespace arconcentrador::protocols {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
uint8_t nmea_checksum(const char* sentence) {
|
||||
// Find content between '$' and '*'
|
||||
const char* start = strchr(sentence, '$');
|
||||
if (!start) return 0xFF;
|
||||
start++; // skip '$'
|
||||
const char* end = strchr(start, '*');
|
||||
if (!end) return 0xFF;
|
||||
uint8_t crc = 0;
|
||||
for (const char* p = start; p < end; ++p) crc ^= (uint8_t)*p;
|
||||
return crc;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
ParpCommand parp_parse(const char* sentence) {
|
||||
ParpCommand result{};
|
||||
result.valid = false;
|
||||
|
||||
// Must start with $PARP,
|
||||
if (strncmp(sentence, "$PARP,", 6) != 0) return result;
|
||||
|
||||
// Validate checksum
|
||||
const char* star = strchr(sentence, '*');
|
||||
if (!star || strlen(star) < 3) return result;
|
||||
const uint8_t recv_crc = (uint8_t)strtoul(star + 1, nullptr, 16);
|
||||
const uint8_t calc_crc = nmea_checksum(sentence);
|
||||
if (recv_crc != calc_crc) {
|
||||
Serial.printf("[PARSER] checksum error: recv=%02X calc=%02X\n",
|
||||
recv_crc, calc_crc);
|
||||
return result;
|
||||
}
|
||||
|
||||
// Copy body between "$PARP," and '*'
|
||||
char body[128];
|
||||
const char* body_start = sentence + 6;
|
||||
const size_t body_len = (size_t)(star - body_start);
|
||||
if (body_len == 0 || body_len >= sizeof(body)) return result;
|
||||
memcpy(body, body_start, body_len);
|
||||
body[body_len] = '\0';
|
||||
|
||||
// Tokenize: CMD,VALUE,STATION_ID
|
||||
char* saveptr = nullptr;
|
||||
const char* tok_cmd = strtok_r(body, ",", &saveptr);
|
||||
const char* tok_val = strtok_r(nullptr, ",", &saveptr);
|
||||
const char* tok_sta = strtok_r(nullptr, ",", &saveptr);
|
||||
|
||||
if (!tok_cmd || !tok_val || !tok_sta) return result;
|
||||
|
||||
strncpy(result.cmd, tok_cmd, sizeof(result.cmd) - 1);
|
||||
result.value = (float)atof(tok_val);
|
||||
result.station_id = (uint8_t)atoi(tok_sta);
|
||||
|
||||
if (result.station_id < 1 || result.station_id > 4) return result;
|
||||
|
||||
result.valid = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
static char s_line_buf[256];
|
||||
static size_t s_line_len = 0;
|
||||
|
||||
bool parp_feed(char c, ParpCommand& out) {
|
||||
if (c == '$') {
|
||||
// Start of new sentence — reset buffer.
|
||||
s_line_len = 0;
|
||||
}
|
||||
if (s_line_len < sizeof(s_line_buf) - 1) {
|
||||
s_line_buf[s_line_len++] = c;
|
||||
}
|
||||
if (c == '\n') {
|
||||
s_line_buf[s_line_len] = '\0';
|
||||
s_line_len = 0;
|
||||
out = parp_parse(s_line_buf);
|
||||
return out.valid;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace arconcentrador::protocols
|
||||
Reference in New Issue
Block a user