nicoboy
ba3221ae4f
- Heartbeat MAVLink v1 toutes les 1s (broadcast 192.168.4.255:14553) - Relais WiFi <-> UART (Jetson <-> Pi Zero) sur ports 14550-14553 - LED RGB WS2812 GPIO48 : rouge pulsé (attente), vert dim (idle), flash vert (heartbeat) - MODE_AP=true : hotspot WireClaw-CAM / false : connexion Jetson - Fix: retrait ARDUINO_USB_CDC_ON_BOOT (carte Freenove utilise CH343/UART0)
74 lines
2.9 KiB
C
74 lines
2.9 KiB
C
#pragma once
|
|
#include <stdint.h>
|
|
|
|
// ─────────────────────────────────────────────────────────────
|
|
// MAVLink v1 — HEARTBEAT (msg_id = 0)
|
|
// Trame complète : 17 octets
|
|
//
|
|
// Byte Field
|
|
// 0 STX 0xFE (MAVLink v1 magic)
|
|
// 1 LEN 9 (payload = 9 octets)
|
|
// 2 SEQ séquence incrémentale
|
|
// 3 SYS_ID system id (on se déclare comme GCS = 255)
|
|
// 4 COMP_ID component id (GCS = 0)
|
|
// 5 MSG_ID 0 (HEARTBEAT)
|
|
// 6-14 PAYLOAD 9 octets (voir ci-dessous)
|
|
// 15-16 CRC CRC-16/MCRF4XX avec CRC_EXTRA = 50
|
|
//
|
|
// Payload HEARTBEAT (9 octets, little-endian) :
|
|
// [0-3] custom_mode uint32 0x00000000
|
|
// [4] type uint8 MAV_TYPE_GCS = 6
|
|
// [5] autopilot uint8 MAV_AUTOPILOT_INVALID = 8
|
|
// [6] base_mode uint8 0
|
|
// [7] system_status uint8 MAV_STATE_ACTIVE = 4
|
|
// [8] mavlink_version uint8 3
|
|
// ─────────────────────────────────────────────────────────────
|
|
|
|
// CRC-16/MCRF4XX (algorithme MAVLink)
|
|
static inline void mavCrcAccumulate(uint8_t data, uint16_t &crc) {
|
|
uint8_t tmp = data ^ (uint8_t)(crc & 0xFF);
|
|
tmp ^= (tmp << 4);
|
|
crc = (crc >> 8) ^ ((uint16_t)tmp << 8) ^ ((uint16_t)tmp << 3) ^ ((uint16_t)tmp >> 4);
|
|
}
|
|
|
|
// Construit un paquet HEARTBEAT MAVLink v1 dans buf (doit faire >= 17 octets)
|
|
// Retourne la taille du paquet (toujours 17)
|
|
static inline uint8_t mavlinkBuildHeartbeat(uint8_t *buf, uint8_t &seq) {
|
|
const uint8_t PAYLOAD_LEN = 9;
|
|
const uint8_t SYS_ID = 255; // GCS
|
|
const uint8_t COMP_ID = 0; // GCS component
|
|
const uint8_t MSG_ID = 0; // HEARTBEAT
|
|
const uint8_t CRC_EXTRA = 50; // valeur fixe MAVLink pour msg #0
|
|
|
|
// Header
|
|
buf[0] = 0xFE; // STX
|
|
buf[1] = PAYLOAD_LEN;
|
|
buf[2] = seq++; // numéro de séquence (overflow géré par uint8_t)
|
|
buf[3] = SYS_ID;
|
|
buf[4] = COMP_ID;
|
|
buf[5] = MSG_ID;
|
|
|
|
// Payload (little-endian)
|
|
buf[6] = 0x00; // custom_mode [0]
|
|
buf[7] = 0x00; // custom_mode [1]
|
|
buf[8] = 0x00; // custom_mode [2]
|
|
buf[9] = 0x00; // custom_mode [3]
|
|
buf[10] = 6; // type = MAV_TYPE_GCS
|
|
buf[11] = 8; // autopilot = MAV_AUTOPILOT_INVALID
|
|
buf[12] = 0; // base_mode
|
|
buf[13] = 4; // system_status = MAV_STATE_ACTIVE
|
|
buf[14] = 3; // mavlink_version
|
|
|
|
// CRC sur header (sans STX) + payload + CRC_EXTRA
|
|
uint16_t crc = 0xFFFF;
|
|
for (uint8_t i = 1; i <= 5 + PAYLOAD_LEN; i++) {
|
|
mavCrcAccumulate(buf[i], crc);
|
|
}
|
|
mavCrcAccumulate(CRC_EXTRA, crc);
|
|
|
|
buf[15] = crc & 0xFF; // CRC low byte
|
|
buf[16] = (crc >> 8) & 0xFF; // CRC high byte
|
|
|
|
return 17;
|
|
}
|