alro65
080e47efc0
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.
94 lines
2.9 KiB
C++
94 lines
2.9 KiB
C++
// =============================================================================
|
|
// 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
|