initial version

.gitignore

@@ -0,0 +1 @@
+generated/

.gitmodules

@@ -0,0 +1,9 @@
+[submodule "modules/uav_electronic_ids"]
+	path = modules/uav_electronic_ids
+	url = https://github.com/sxjack/uav_electronic_ids.git
+[submodule "modules/opendroneid-core-c"]
+	path = modules/opendroneid-core-c
+	url = https://github.com/opendroneid/opendroneid-core-c.git
+[submodule "modules/mavlink"]
+	path = modules/mavlink
+	url = https://github.com/ArduPilot/mavlink.git

RemoteIDModule/RemoteIDModule.ino

@@ -0,0 +1,137 @@
+/*
+  implement OpenDroneID MAVLink support, populating squitter for sending bluetooth RemoteID messages
+
+  based on example code from https://github.com/sxjack/uav_electronic_ids
+ */
+/*
+  released under GNU GPL v3 or later
+ */
+
+#include <Arduino.h>
+#include <math.h>
+#include <time.h>
+#include <sys/time.h>
+
+#include <id_open.h>
+#include "mavlink.h"
+
+static ID_OpenDrone          squitter;
+
+static MAVLinkSerial mavlink{Serial1, MAVLINK_COMM_0};
+
+static bool squitter_initialised;
+
+#define OUTPUT_RATE_HZ 5
+
+/*
+  assume ESP32-s3 for now, using pins 17 and 18 for uart
+ */
+#define RX_PIN 17
+#define TX_PIN 18
+#define DEBUG_BAUDRATE 57600
+#define MAVLINK_BAUDRATE 57600
+
+/*
+  setup serial ports
+ */
+void setup()
+{
+    // Serial for debug printf
+    Serial.begin(DEBUG_BAUDRATE);
+
+    // Serial1 for MAVLink
+    Serial1.begin(MAVLINK_BAUDRATE, SERIAL_8N1, RX_PIN, TX_PIN);
+
+    mavlink.init();
+}
+
+static void init_squitter(void)
+{
+    struct UTM_parameters utm_parameters {};
+
+    const auto &operator_id = mavlink.get_operator_id();
+    const auto &basic_id = mavlink.get_basic_id();
+    const auto &system = mavlink.get_system();
+    const auto &self_id = mavlink.get_self_id();
+
+    strncpy((char*)utm_parameters.UAS_operator, (char *)operator_id.operator_id, sizeof(utm_parameters.UAS_operator));
+    strncpy((char*)utm_parameters.UAV_id, (char *)basic_id.uas_id, sizeof(utm_parameters.UAV_id));
+    strncpy((char*)utm_parameters.flight_desc, (char *)self_id.description, sizeof(utm_parameters.flight_desc));
+    utm_parameters.UA_type = basic_id.ua_type;
+    utm_parameters.ID_type = basic_id.id_type;
+    utm_parameters.region = 1; // ??
+    utm_parameters.EU_category = system.category_eu;
+    utm_parameters.EU_class = system.class_eu;
+    // char    UTM_id[ID_SIZE * 2] ??
+
+    squitter.init(&utm_parameters);
+}
+
+void loop()
+{
+    static uint32_t last_update;
+
+    mavlink.update();
+
+    if (!mavlink.initialised()) {
+        return;
+    }
+
+    if (!squitter_initialised) {
+        squitter_initialised = true;
+        init_squitter();
+    }
+
+    const uint32_t msecs = millis();
+
+    if (msecs - last_update < 1000/OUTPUT_RATE_HZ) {
+        // not ready for a new frame yet
+        return;
+    }
+    last_update = msecs;
+
+    const auto &location = mavlink.get_location();
+    const auto &operator_id = mavlink.get_operator_id();
+    const auto &system = mavlink.get_system();
+
+    struct UTM_data utm_data {};
+
+    const float M_PER_SEC_TO_KNOTS = 1.94384449;
+
+    utm_data.heading     = location.direction * 0.01;
+    utm_data.latitude_d  = location.latitude * 1.0e-7;
+    utm_data.longitude_d = location.longitude * 1.0e-7;
+    utm_data.base_latitude = system.operator_latitude * 1.0e-7;
+    utm_data.base_longitude = system.operator_longitude * 1.0e-7;
+    utm_data.base_alt_m     = system.operator_altitude_geo;
+    utm_data.alt_msl_m = location.altitude_geodetic;
+    utm_data.alt_agl_m = location.height;
+    utm_data.speed_kn  = location.speed_horizontal * 0.01 * M_PER_SEC_TO_KNOTS;
+    utm_data.base_valid = (system.operator_latitude != 0 && system.operator_longitude != 0);
+
+    const float groundspeed = location.speed_horizontal * 0.01;
+    const float vel_N = cos(radians(utm_data.heading)) * groundspeed;
+    const float vel_E = sin(radians(utm_data.heading)) * groundspeed;
+
+    utm_data.vel_N_cm = vel_N * 100;
+    utm_data.vel_E_cm = vel_E * 100;
+    utm_data.vel_D_cm = location.speed_vertical * -0.01;
+
+    const uint32_t jan_1_2019_s = 1546261200UL;
+    const time_t unix_s = time_t(system.timestamp) + jan_1_2019_s;
+    const auto *tm = gmtime(&unix_s);
+
+    utm_data.years = tm->tm_year;
+    utm_data.months = tm->tm_mon;
+    utm_data.days = tm->tm_mday;
+    utm_data.hours = tm->tm_hour;
+    utm_data.minutes = tm->tm_min;
+    utm_data.seconds = tm->tm_sec;
+
+    utm_data.satellites = 8;
+
+    //char  *hdop_s;
+    //char  *vdop_s;
+
+    squitter.transmit(&utm_data);
+}

RemoteIDModule/mavlink.cpp

@@ -0,0 +1,152 @@
+/*
+  mavlink class for handling OpenDroneID messages
+ */
+#include <Arduino.h>
+#include "mavlink.h"
+
+#define SERIAL_BAUD 115200
+
+static HardwareSerial *serial_ports[MAVLINK_COMM_NUM_BUFFERS];
+
+#include <generated/mavlink_helpers.h>
+
+mavlink_system_t mavlink_system = {2,1};
+
+#define dev_printf(fmt, args ...)  do {Serial.printf(fmt, ## args); } while(0)
+
+/*
+  send a buffer out a MAVLink channel
+ */
+void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
+{
+    if (chan >= MAVLINK_COMM_NUM_BUFFERS || serial_ports[uint8_t(chan)] == nullptr) {
+        return;
+    }
+    auto &serial = *serial_ports[uint8_t(chan)];
+    serial.write(buf, len);
+}
+
+/*
+  abstraction for MAVLink on a serial port
+ */
+MAVLinkSerial::MAVLinkSerial(HardwareSerial &_serial, mavlink_channel_t _chan) :
+    serial(_serial),
+    chan(_chan)
+{
+    serial_ports[uint8_t(_chan - MAVLINK_COMM_0)] = &serial;
+}
+
+void MAVLinkSerial::init(void)
+{
+}
+
+void MAVLinkSerial::update(void)
+{
+    update_send();
+    update_receive();
+}
+
+void MAVLinkSerial::update_send(void)
+{
+    uint32_t now_ms = millis();
+    if (now_ms - last_hb_ms >= 1000) {
+        last_hb_ms = now_ms;
+        mavlink_msg_heartbeat_send(
+            chan,
+            MAV_TYPE_ODID,
+            MAV_AUTOPILOT_ARDUPILOTMEGA,
+            0,
+            0,
+            0);
+    }
+}
+
+void MAVLinkSerial::update_receive(void)
+{
+    // receive new packets
+    mavlink_message_t msg;
+    mavlink_status_t status;
+    uint32_t now_ms = millis();
+
+    status.packet_rx_drop_count = 0;
+
+    const uint16_t nbytes = serial.available();
+    for (uint16_t i=0; i<nbytes; i++) {
+        const uint8_t c = (uint8_t)serial.read();
+        // Try to get a new message
+        if (mavlink_parse_char(chan, c, &msg, &status)) {
+            process_packet(status, msg);
+        }
+    }
+}
+
+/*
+  printf via MAVLink STATUSTEXT for debugging
+ */
+void MAVLinkSerial::mav_printf(uint8_t severity, const char *fmt, ...)
+{
+    va_list arg_list;
+    char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
+    va_start(arg_list, fmt);
+    vsnprintf(text, sizeof(text), fmt, arg_list);
+    va_end(arg_list);
+    mavlink_msg_statustext_send(chan,
+                                severity,
+                                text,
+                                0, 0);
+}
+
+void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &msg)
+{
+    switch (msg.msgid) {
+    case MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION: {
+        dev_printf("Got OPEN_DRONE_ID_LOCATION\n");
+        mavlink_msg_open_drone_id_location_decode(&msg, &location);
+        packets_received_mask |= PKT_LOCATION;
+        break;
+    }
+    case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID: {
+        dev_printf("Got OPEN_DRONE_ID_BASIC_ID\n");
+        mavlink_msg_open_drone_id_basic_id_decode(&msg, &basic_id);
+        packets_received_mask |= PKT_BASIC_ID;
+        break;
+    }
+    case MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION: {
+        dev_printf("Got OPEN_DRONE_ID_AUTHENTICATION\n");
+        mavlink_msg_open_drone_id_authentication_decode(&msg, &authentication);
+        packets_received_mask |= PKT_AUTHENTICATION;
+        break;
+    }
+    case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: {
+        dev_printf("Got OPEN_DRONE_ID_SELF_ID\n");
+        mavlink_msg_open_drone_id_self_id_decode(&msg, &self_id);
+        packets_received_mask |= PKT_SELF_ID;
+        break;
+    }
+    case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM: {
+        dev_printf("Got OPEN_DRONE_ID_SYSTEM\n");
+        mavlink_msg_open_drone_id_system_decode(&msg, &system);
+        packets_received_mask |= PKT_SYSTEM;
+        last_system_msg_ms = millis();
+        break;
+    }
+    case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: {
+        dev_printf("Got OPEN_DRONE_ID_OPERATOR_ID\n");
+        mavlink_msg_open_drone_id_operator_id_decode(&msg, &operator_id);
+        packets_received_mask |= PKT_OPERATOR_ID;
+        break;
+    }
+    default:
+        // we don't care about other packets
+        break;
+    }
+}
+
+/*
+  return true when we have the base set of packets
+ */
+bool MAVLinkSerial::initialised(void)
+{
+    const uint32_t required = PKT_LOCATION | PKT_BASIC_ID | PKT_SELF_ID | PKT_SYSTEM | PKT_OPERATOR_ID;
+    return (packets_received_mask & required) == required;
+}

RemoteIDModule/mavlink.h

@@ -0,0 +1,93 @@
+/*
+  mavlink class for handling OpenDroneID messages
+ */
+#pragma once
+
+// we have separate helpers disabled to make it possible
+// to select MAVLink 1.0 in the arduino GUI build
+#define MAVLINK_SEPARATE_HELPERS
+#define MAVLINK_NO_CONVERSION_HELPERS
+
+#define MAVLINK_SEND_UART_BYTES(chan, buf, len) comm_send_buffer(chan, buf, len)
+
+// two buffers, one for USB, one for UART. This makes for easier testing with SITL
+#define MAVLINK_COMM_NUM_BUFFERS 2
+
+#define MAVLINK_MAX_PAYLOAD_LEN 255
+
+#include <mavlink2.h>
+
+/// MAVLink system definition
+extern mavlink_system_t mavlink_system;
+
+void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);
+
+#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+#include <generated/all/mavlink.h>
+
+/*
+  abstraction for MAVLink on a serial port
+ */
+class MAVLinkSerial {
+public:
+    MAVLinkSerial(HardwareSerial &serial, mavlink_channel_t chan);
+    void init(void);
+    void update(void);
+
+    const mavlink_open_drone_id_location_t &get_location(void) const {
+        return location;
+    }
+
+    const mavlink_open_drone_id_basic_id_t &get_basic_id(void) const {
+        return basic_id;
+    }
+
+    const mavlink_open_drone_id_authentication_t &get_authentication(void) const {
+        return authentication;
+    }
+
+    const mavlink_open_drone_id_self_id_t &get_self_id(void) const {
+        return self_id;
+    }
+
+    const mavlink_open_drone_id_system_t &get_system(void) const {
+        return system;
+    }
+
+    const mavlink_open_drone_id_operator_id_t &get_operator_id(void) const {
+        return operator_id;
+    }
+
+    // return true when we have the key messages available
+    bool initialised(void);
+    
+private:
+    HardwareSerial &serial;
+    mavlink_channel_t chan;
+    uint32_t last_hb_ms;
+
+    enum {
+        PKT_LOCATION       = (1U<<0),
+        PKT_BASIC_ID       = (1U<<1),
+        PKT_AUTHENTICATION = (1U<<2),
+        PKT_SELF_ID        = (1U<<3),
+        PKT_SYSTEM         = (1U<<4),
+        PKT_OPERATOR_ID    = (1U<<5),
+    };
+
+    void update_receive(void);
+    void update_send(void);
+    void process_packet(mavlink_status_t &status, mavlink_message_t &msg);
+    void mav_printf(uint8_t severity, const char *fmt, ...);
+
+    mavlink_open_drone_id_location_t location;
+    mavlink_open_drone_id_basic_id_t basic_id;
+    mavlink_open_drone_id_authentication_t authentication;
+    mavlink_open_drone_id_self_id_t self_id;
+    mavlink_open_drone_id_system_t system;
+    mavlink_open_drone_id_operator_id_t operator_id;
+
+    uint32_t last_system_msg_ms;
+
+    uint32_t packets_received_mask;
+};

add_libraries.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+# setup libraries links in $HOME/Arduino
+
+DEST=~/Arduino/libraries/
+
+mkdir -p "$DEST"
+ln -sf $PWD/modules/uav_electronic_ids/id_open "$DEST"
+ln -sf $PWD/modules/uav_electronic_ids/utm "$DEST"
+ln -sf $PWD/modules/opendroneid-core-c/libopendroneid "$DEST"
+ln -sf $PWD/libraries/mavlink2 ~/Arduino/libraries/ "$DEST"

libraries/mavlink2/library.properties

@@ -0,0 +1,10 @@
+name=mavlink2
+version=1.0
+author=Andrew Tridgell
+maintainer=Andrew Tridgell
+sentence=Arduino/MAVLink2
+paragraph=Arduino/MAVLink2
+category=Uncategorized
+url=https://github.com/ArduPilot/mavlink
+architectures=esp32
+includes=mavlink2.h

libraries/mavlink2/mavlink2.h

@@ -0,0 +1,2 @@
+#include "generated/all/version.h"
+#include "generated/mavlink_types.h"

libraries/mavlink2/regen_mavlink.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+# re-generate mavlink headers, assumes pymavlink is installed
+
+mavgen.py --wire-protocol 2.0 --lang C ../../modules/mavlink/message_definitions/v1.0/all.xml -o generated
+
+

modules/mavlink

@@ -0,0 +1 @@
+Subproject commit 60c48d29c63875362cd3c4ee2ff7f08271c5a43f

modules/opendroneid-core-c

@@ -0,0 +1 @@
+Subproject commit aa2fefcc91f459569fcefe08b0e6056fd24cecf5

modules/uav_electronic_ids

@@ -0,0 +1 @@
+Subproject commit e7788194eacf6e23f85e91d163b60f7032088df5