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,45 @@
|
||||
; =============================================================================
|
||||
; AR BNO085 Compact Node v1 -- firmware build configuration
|
||||
; =============================================================================
|
||||
;
|
||||
; Target hardware: AR-BNO085-NODE v1.0 (ESP32-DOWD, compact board)
|
||||
; Role: NMEA 2000 IMU node -- publishes heading (PGN 127250)
|
||||
; and rate-of-turn (PGN 127251) from BNO085 sensor.
|
||||
;
|
||||
; Pinout:
|
||||
; GPIO21 -- I2C SDA (BNO085)
|
||||
; GPIO22 -- I2C SCL (BNO085)
|
||||
; GPIO34 -- BNO085 INT (data-ready, active low, input-only)
|
||||
; GPIO13 -- BNO085 NRST (output, active low, drive LOW to reset)
|
||||
; GPIO23 -- CAN TX (MCP2562T)
|
||||
; GPIO4 -- CAN RX (MCP2562T)
|
||||
; =============================================================================
|
||||
|
||||
[platformio]
|
||||
src_dir = src
|
||||
default_envs = esp32-dev
|
||||
|
||||
[env]
|
||||
platform = espressif32@^6.7.0
|
||||
framework = arduino
|
||||
monitor_speed = 115200
|
||||
monitor_filters = esp32_exception_decoder, time
|
||||
build_flags =
|
||||
-std=gnu++17
|
||||
-DCORE_DEBUG_LEVEL=3
|
||||
-DAR_FW_VERSION=\"1.0.0\"
|
||||
-Wall
|
||||
-Wno-unused-parameter
|
||||
build_unflags =
|
||||
-std=gnu++11
|
||||
lib_deps =
|
||||
ttlappalainen/NMEA2000-library@^4.22.0
|
||||
ttlappalainen/NMEA2000_esp32@^1.0.3
|
||||
sparkfun/SparkFun BNO08x Cortex Based IMU@^1.0.3
|
||||
|
||||
[env:esp32-dev]
|
||||
board = esp32dev
|
||||
build_type = release
|
||||
build_flags =
|
||||
${env.build_flags}
|
||||
-Os
|
||||
@@ -0,0 +1,182 @@
|
||||
// =============================================================================
|
||||
// AR BNO085 Compact Node v1 — main.cpp
|
||||
// =============================================================================
|
||||
//
|
||||
// Reads heading (ARVR Stabilized Rotation Vector) and yaw rate (Gyroscope)
|
||||
// from the BNO085 via I2C and publishes them on the NMEA 2000 backbone:
|
||||
// PGN 127250 — Vessel Heading (100 Hz)
|
||||
// PGN 127251 — Rate of Turn (100 Hz)
|
||||
//
|
||||
// The main autopilot board (ar_autopilot_v1) receives these PGNs from the
|
||||
// backbone via its nmea2000_consumer and feeds them into the PID outer loop.
|
||||
// =============================================================================
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
|
||||
// --- NMEA 2000 ---
|
||||
#define ESP32_CAN_TX_PIN GPIO_NUM_23
|
||||
#define ESP32_CAN_RX_PIN GPIO_NUM_4
|
||||
#include <NMEA2000_CAN.h>
|
||||
#include <N2kMessages.h>
|
||||
|
||||
// --- BNO085 ---
|
||||
#include <SparkFun_BNO08x_Arduino_Library.h>
|
||||
|
||||
// =============================================================================
|
||||
// Pin definitions
|
||||
// =============================================================================
|
||||
static constexpr int PIN_I2C_SDA = 21;
|
||||
static constexpr int PIN_I2C_SCL = 22;
|
||||
static constexpr int PIN_BNO_INT = 34; // data-ready, active low
|
||||
static constexpr int PIN_BNO_NRST = 13; // reset, active low
|
||||
|
||||
// =============================================================================
|
||||
// BNO085 instance
|
||||
// =============================================================================
|
||||
static BNO08x imu;
|
||||
|
||||
// Latest measurements (updated from BNO085 reports)
|
||||
static volatile float g_heading_rad = 0.0f; // true heading, radians
|
||||
static volatile float g_rot_rad_s = 0.0f; // yaw rate, rad/s (+ve = stbd)
|
||||
static volatile bool g_heading_valid = false;
|
||||
static volatile bool g_rot_valid = false;
|
||||
|
||||
// =============================================================================
|
||||
// NMEA 2000 setup
|
||||
// =============================================================================
|
||||
static void nmea2000_init() {
|
||||
NMEA2000.SetProductInformation(
|
||||
"AR-BNO-1",
|
||||
200,
|
||||
"AR-BNO085-NODE v1",
|
||||
"1.0.0",
|
||||
"AR-BNO085-NODE v1.0"
|
||||
);
|
||||
NMEA2000.SetDeviceInformation(
|
||||
2, // Unique number (different from autopilot node = 1)
|
||||
140, // Device function: Attitude sensor
|
||||
60, // Device class: Navigation
|
||||
2046 // Manufacturer code (test)
|
||||
);
|
||||
NMEA2000.SetMode(tNMEA2000::N2km_NodeOnly, 26);
|
||||
NMEA2000.EnableForward(false);
|
||||
NMEA2000.Open();
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// BNO085 initialisation
|
||||
// =============================================================================
|
||||
static bool bno085_init() {
|
||||
// Hard reset — hold NRST low for 10 ms, then release.
|
||||
pinMode(PIN_BNO_NRST, OUTPUT);
|
||||
digitalWrite(PIN_BNO_NRST, LOW);
|
||||
delay(15);
|
||||
digitalWrite(PIN_BNO_NRST, HIGH);
|
||||
delay(100); // wait for BNO085 startup (~50 ms typical)
|
||||
|
||||
Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL);
|
||||
Wire.setClock(400000); // 400 kHz Fast Mode
|
||||
|
||||
if (!imu.begin(0x4A, Wire, PIN_BNO_INT)) {
|
||||
Serial.println("[BNO085] begin() failed — check wiring / I2C address");
|
||||
return false;
|
||||
}
|
||||
|
||||
// ARVR Stabilized Rotation Vector → heading (tilt-compensated), 100 Hz
|
||||
if (!imu.enableARVRStabilizedRotationVector(10000)) { // 10 ms = 100 Hz
|
||||
Serial.println("[BNO085] enableARVRStabilizedRotationVector() failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Calibrated Gyroscope → yaw rate, 100 Hz
|
||||
if (!imu.enableGyro(10000)) {
|
||||
Serial.println("[BNO085] enableGyro() failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
Serial.println("[BNO085] init OK — heading + yaw rate @ 100 Hz");
|
||||
return true;
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Read BNO085 reports (call frequently)
|
||||
// =============================================================================
|
||||
static void bno085_read() {
|
||||
if (!imu.dataAvailable()) return;
|
||||
|
||||
// Rotation Vector → heading (yaw around Z)
|
||||
if (imu.getSensorEventID() == SENSOR_REPORTID_ARVR_STABILIZED_ROTATION_VECTOR) {
|
||||
// SparkFun library: getYaw() returns yaw in radians, -pi..+pi
|
||||
const float yaw = imu.getYaw();
|
||||
// Convert to 0..2pi (nautical convention, 0 = North, + = clockwise)
|
||||
float hdg = -yaw; // BNO085: +yaw = CCW (math convention); nautical = CW
|
||||
if (hdg < 0.0f) hdg += 2.0f * (float)M_PI;
|
||||
if (hdg >= 2.0f * (float)M_PI) hdg -= 2.0f * (float)M_PI;
|
||||
g_heading_rad = hdg;
|
||||
g_heading_valid = true;
|
||||
}
|
||||
|
||||
// Gyroscope Z → yaw rate (+ = clockwise = starboard turn)
|
||||
if (imu.getSensorEventID() == SENSOR_REPORTID_GYROSCOPE_CALIBRATED) {
|
||||
// getGyroZ(): rad/s, +Z = looking down = CW from above = starboard
|
||||
g_rot_rad_s = imu.getGyroZ();
|
||||
g_rot_valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Publish NMEA 2000 PGNs
|
||||
// =============================================================================
|
||||
static void publish_pgns() {
|
||||
if (g_heading_valid) {
|
||||
tN2kMsg msg;
|
||||
SetN2kHeading(msg,
|
||||
0, // SID
|
||||
(double)g_heading_rad,
|
||||
N2kDoubleNA, // Deviation
|
||||
N2kDoubleNA, // Variation
|
||||
N2khr_true); // Reference: true north (BNO085 is absolute)
|
||||
NMEA2000.SendMsg(msg);
|
||||
}
|
||||
|
||||
if (g_rot_valid) {
|
||||
tN2kMsg msg;
|
||||
// PGN 127251: rate in rad/s
|
||||
SetN2kRateOfTurn(msg, 0, (double)g_rot_rad_s);
|
||||
NMEA2000.SendMsg(msg);
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Arduino entry points
|
||||
// =============================================================================
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
Serial.println("[AR-BNO085-NODE] booting...");
|
||||
|
||||
nmea2000_init();
|
||||
|
||||
if (!bno085_init()) {
|
||||
Serial.println("[AR-BNO085-NODE] FATAL: IMU init failed. Halting.");
|
||||
for (;;) delay(1000);
|
||||
}
|
||||
|
||||
Serial.println("[AR-BNO085-NODE] ready.");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Read sensor at ~100 Hz (non-blocking, interrupt-driven via INT pin)
|
||||
bno085_read();
|
||||
|
||||
// Publish to NMEA 2000 backbone at 10 Hz
|
||||
static uint32_t last_pub = 0;
|
||||
const uint32_t now = millis();
|
||||
if (now - last_pub >= 100) {
|
||||
last_pub = now;
|
||||
publish_pgns();
|
||||
}
|
||||
|
||||
// Keep the NMEA2000 stack alive
|
||||
NMEA2000.ParseMessages();
|
||||
}
|
||||
Reference in New Issue
Block a user