added DroneCAN message decoding

RemoteIDModule/DroneCAN.cpp

@@ -9,6 +9,11 @@
 #include <uavcan.protocol.GetNodeInfo.h>
 #include <uavcan.protocol.RestartNode.h>
 #include <uavcan.protocol.dynamic_node_id.Allocation.h>
+#include <dronecan.remoteid.BasicID.h>
+#include <dronecan.remoteid.Location.h>
+#include <dronecan.remoteid.SelfID.h>
+#include <dronecan.remoteid.System.h>
+#include <dronecan.remoteid.OperatorID.h>
 
 #define FW_VERSION_MAJOR 1
 #define FW_VERSION_MINOR 0
@@ -25,7 +30,7 @@ static bool shouldAcceptTransfer_trampoline(const CanardInstance* ins, uint64_t*
                                             CanardTransferType transfer_type,
                                             uint8_t source_node_id);
 
-static uavcan_protocol_NodeStatus node_status;
+// decoded messages
 
 void DroneCAN::init(void)
 {
@@ -82,6 +87,8 @@ void DroneCAN::onTransferReceived(CanardInstance* ins,
         return;
     }
 
+    const uint32_t now_ms = millis();
+
     switch (transfer->data_type_id) {
     case UAVCAN_PROTOCOL_GETNODEINFO_ID:
         handle_get_node_info(ins, transfer);
@@ -91,7 +98,32 @@ void DroneCAN::onTransferReceived(CanardInstance* ins,
         delay(20);
         esp_restart();
         break;
-    defauult:
+    case DRONECAN_REMOTEID_BASICID_ID:
+        Serial.printf("Got BasicID\n");
+        dronecan_remoteid_BasicID_decode(transfer, &msg_BasicID);
+        last_basic_id_ms = now_ms;
+        break;
+    case DRONECAN_REMOTEID_LOCATION_ID:
+        Serial.printf("Got Location\n");
+        dronecan_remoteid_Location_decode(transfer, &msg_Location);
+        last_location_ms = now_ms;
+        break;
+    case DRONECAN_REMOTEID_SELFID_ID:
+        Serial.printf("Got SelfID\n");
+        dronecan_remoteid_SelfID_decode(transfer, &msg_SelfID);
+        last_self_id_ms = now_ms;
+        break;
+    case DRONECAN_REMOTEID_SYSTEM_ID:
+        Serial.printf("Got System\n");
+        dronecan_remoteid_System_decode(transfer, &msg_System);
+        last_system_ms = now_ms;
+        break;
+    case DRONECAN_REMOTEID_OPERATORID_ID:
+        Serial.printf("Got OperatorID\n");
+        dronecan_remoteid_OperatorID_decode(transfer, &msg_OperatorID);
+        last_operator_id_ms = now_ms;
+        break;
+    default:
         //Serial.printf("reject %u\n", transfer->data_type_id);
         break;
     }
@@ -109,12 +141,15 @@ bool DroneCAN::shouldAcceptTransfer(const CanardInstance* ins,
         return true;
     }
 
+#define ACCEPT_ID(name) case name ## _ID: *out_data_type_signature = name ## _SIGNATURE; return true
     switch (data_type_id) {
-    case UAVCAN_PROTOCOL_GETNODEINFO_ID:
-        *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE;
-        return true;
-    case UAVCAN_PROTOCOL_RESTARTNODE_ID:
-        *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
+        ACCEPT_ID(UAVCAN_PROTOCOL_GETNODEINFO);
+        ACCEPT_ID(UAVCAN_PROTOCOL_RESTARTNODE);
+        ACCEPT_ID(DRONECAN_REMOTEID_BASICID);
+        ACCEPT_ID(DRONECAN_REMOTEID_LOCATION);
+        ACCEPT_ID(DRONECAN_REMOTEID_SELFID);
+        ACCEPT_ID(DRONECAN_REMOTEID_OPERATORID);
+        ACCEPT_ID(DRONECAN_REMOTEID_SYSTEM);
         return true;
     }
     //Serial.printf("%u: reject ID 0x%x\n", millis(), data_type_id);
@@ -385,6 +420,30 @@ void DroneCAN::readUniqueID(uint8_t id[6])
     esp_efuse_mac_get_default(id);
 }
 
+bool DroneCAN::system_valid(void)
+{
+    uint32_t now_ms = millis();
+    uint32_t max_ms = 15000;
+    if (last_system_ms == 0 ||
+        last_self_id_ms == 0 ||
+        last_basic_id_ms == 0 ||
+        last_system_ms == 0 ||
+        last_operator_id_ms == 0) {
+        return false;
+    }
+    return (now_ms - last_system_ms < max_ms &&
+            now_ms - last_self_id_ms < max_ms &&
+            now_ms - last_basic_id_ms < max_ms &&
+            now_ms - last_operator_id_ms < max_ms);
+}
+
+bool DroneCAN::location_valid(void)
+{
+    uint32_t now_ms = millis();
+    uint32_t max_ms = 2000;
+    return last_location_ms != 0 && now_ms - last_location_ms < max_ms;
+}
+
 #if 0
 // xprintf is useful when debugging in C code such as libcanard
 extern "C" {

RemoteIDModule/DroneCAN.h

@@ -2,6 +2,14 @@
 #include "CANDriver.h"
 #include <canard.h>
 
+#include <canard.h>
+#include <uavcan.protocol.NodeStatus.h>
+#include <dronecan.remoteid.BasicID.h>
+#include <dronecan.remoteid.Location.h>
+#include <dronecan.remoteid.SelfID.h>
+#include <dronecan.remoteid.System.h>
+#include <dronecan.remoteid.OperatorID.h>
+
 #define CAN_POOL_SIZE 4096
 
 class DroneCAN {
@@ -38,6 +46,19 @@ private:
     uint32_t node_id_allocation_unique_id_offset;
     uint32_t last_DNA_start_ms;
 
+    uavcan_protocol_NodeStatus node_status;
+    dronecan_remoteid_BasicID msg_BasicID;
+    dronecan_remoteid_Location msg_Location;
+    dronecan_remoteid_SelfID msg_SelfID;
+    dronecan_remoteid_System msg_System;
+    dronecan_remoteid_OperatorID msg_OperatorID;
+
+    uint32_t last_location_ms;
+    uint32_t last_basic_id_ms;
+    uint32_t last_self_id_ms;
+    uint32_t last_operator_id_ms;
+    uint32_t last_system_ms;
+    
 public:
     void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer);
     bool shouldAcceptTransfer(const CanardInstance* ins,
@@ -45,4 +66,13 @@ public:
                               uint16_t data_type_id,
                               CanardTransferType transfer_type,
                               uint8_t source_node_id);
+
+    const dronecan_remoteid_BasicID &get_basic_id(void) { return msg_BasicID; }
+    const dronecan_remoteid_Location &get_location(void) { return msg_Location; }
+    const dronecan_remoteid_SelfID &get_self_id(void) { return msg_SelfID; }
+    const dronecan_remoteid_System &get_system(void) { return msg_System; }
+    const dronecan_remoteid_OperatorID &get_operator_id(void) { return msg_OperatorID; }
+
+    bool system_valid(void);
+    bool location_valid(void);
 };

RemoteIDModule/RemoteIDModule.ino

@@ -48,7 +48,10 @@ void setup()
     dronecan.init();
 }
 
-static void init_squitter(void)
+#define MIN(x,y) ((x)<(y)?(x):(y))
+#define ODID_COPY_MAVSTR(to, from) strncpy(to, (const char*)from, MIN(sizeof(to), sizeof(from)))
+
+static void init_squitter_mavlink(void)
 {
     struct UTM_parameters utm_parameters {};
 
@@ -57,9 +60,9 @@ static void init_squitter(void)
     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));
+    ODID_COPY_MAVSTR(utm_parameters.UAS_operator, operator_id.operator_id);
+    ODID_COPY_MAVSTR(utm_parameters.UAV_id, basic_id.uas_id);
+    ODID_COPY_MAVSTR(utm_parameters.flight_desc, self_id.description);
     utm_parameters.UA_type = basic_id.ua_type;
     utm_parameters.ID_type = basic_id.id_type;
     utm_parameters.region = 1; // ??
@@ -70,6 +73,43 @@ static void init_squitter(void)
     squitter.init(&utm_parameters);
 }
 
+#define MIN(x,y) ((x)<(y)?(x):(y))
+#define ODID_COPY_STR(to, from) memcpy(to, from.data, MIN(from.len, sizeof(to)))
+
+static void init_squitter_dronecan(void)
+{
+    struct UTM_parameters utm_parameters {};
+
+    const auto &operator_id = dronecan.get_operator_id();
+    const auto &basic_id = dronecan.get_basic_id();
+    const auto &system = dronecan.get_system();
+    const auto &self_id = dronecan.get_self_id();
+
+    ODID_COPY_STR(utm_parameters.UAS_operator, operator_id.operator_id);
+    ODID_COPY_STR(utm_parameters.UAV_id, basic_id.uas_id);
+    ODID_COPY_STR(utm_parameters.flight_desc, self_id.description);
+    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;
+    squitter.init(&utm_parameters);
+}
+
+static void timestamp_to_utm_time(struct UTM_data &utm_data, uint32_t timestamp)
+{
+    const uint32_t jan_1_2019_s = 1546261200UL;
+    const time_t unix_s = time_t(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;
+}
+
 void loop()
 {
     static uint32_t last_update;
@@ -77,65 +117,94 @@ void loop()
     mavlink.update();
     dronecan.update();
 
-    if (!mavlink.initialised()) {
-        return;
+    if (!squitter_initialised && mavlink.system_valid()) {
+        squitter_initialised = true;
+        init_squitter_mavlink();
     }
 
-    if (!squitter_initialised) {
+    if (!squitter_initialised && dronecan.system_valid()) {
         squitter_initialised = true;
-        init_squitter();
+        init_squitter_dronecan();
     }
 
-    const uint32_t msecs = millis();
+    if (!squitter_initialised) {
+        return;
+    }
+    
+    const uint32_t now_ms = millis();
 
-    if (msecs - last_update < 1000/OUTPUT_RATE_HZ) {
+    if (now_ms - 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();
+    if (!mavlink.location_valid() &&
+        !dronecan.location_valid()) {
+        return;
+    }
 
-    struct UTM_data utm_data {};
+    last_update = now_ms;
 
+    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;
+    if (mavlink.location_valid()) {
+        const auto &location = mavlink.get_location();
+        const auto &operator_id = mavlink.get_operator_id();
+        const auto &system = mavlink.get_system();
+
+        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;
+
+        timestamp_to_utm_time(utm_data, system.timestamp);
+
+        utm_data.satellites = 8;
+    }
 
+    if (dronecan.location_valid()) {
+        const auto &location = dronecan.get_location();
+        const auto &operator_id = dronecan.get_operator_id();
+        const auto &system = dronecan.get_system();
+
+        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;
+
+        timestamp_to_utm_time(utm_data, system.timestamp);
+
+        utm_data.satellites = 8;
+    }
+    
     squitter.transmit(&utm_data);
 }

RemoteIDModule/mavlink.cpp

@@ -98,17 +98,20 @@ void MAVLinkSerial::mav_printf(uint8_t severity, const char *fmt, ...)
 
 void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &msg)
 {
+    const uint32_t now_ms = millis();
     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;
+        last_location_ms = now_ms;
         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;
+        last_basic_id_ms = now_ms;
         break;
     }
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION: {
@@ -121,19 +124,21 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
         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;
+        last_self_id_ms = now_ms;
         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();
+        last_system_msg_ms = now_ms;
         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;
+        last_operator_id_ms = now_ms;
         break;
     }
     default:
@@ -150,3 +155,27 @@ 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;
 }
+
+bool MAVLinkSerial::system_valid(void)
+{
+    uint32_t now_ms = millis();
+    uint32_t max_ms = 15000;
+    if (last_system_ms == 0 ||
+        last_self_id_ms == 0 ||
+        last_basic_id_ms == 0 ||
+        last_system_ms == 0 ||
+        last_operator_id_ms == 0) {
+        return false;
+    }
+    return (now_ms - last_system_ms < max_ms &&
+            now_ms - last_self_id_ms < max_ms &&
+            now_ms - last_basic_id_ms < max_ms &&
+            now_ms - last_operator_id_ms < max_ms);
+}
+
+bool MAVLinkSerial::location_valid(void)
+{
+    uint32_t now_ms = millis();
+    uint32_t max_ms = 2000;
+    return last_location_ms != 0 && now_ms - last_location_ms < max_ms;
+}

RemoteIDModule/mavlink.h

@@ -58,9 +58,18 @@ public:
         return operator_id;
     }
 
+    uint32_t last_location_ms;
+    uint32_t last_basic_id_ms;
+    uint32_t last_self_id_ms;
+    uint32_t last_operator_id_ms;
+    uint32_t last_system_ms;
+
     // return true when we have the key messages available
     bool initialised(void);
-    
+
+    bool system_valid(void);
+    bool location_valid(void);
+
 private:
     HardwareSerial &serial;
     mavlink_channel_t chan;

modules/DSDL

@@ -1 +1 @@
-Subproject commit 6070f45c922184c6a84e7d659529721cd2e0c27d
+Subproject commit 71622221cfccf3dd109989f82e83ccc7bda02568

regen_headers.sh

@@ -10,7 +10,7 @@ rm -rf libraries/DroneCAN_generated
 python3 modules/dronecan_dsdlc/dronecan_dsdlc.py -O libraries/DroneCAN_generated modules/DSDL/uavcan modules/DSDL/dronecan modules/DSDL/com
 
 # cope with horrible Arduino library handling
-PACKETS="NodeStatus GetNodeInfo HardwareVersion SoftwareVersion RestartNode dynamic_node_id"
+PACKETS="NodeStatus GetNodeInfo HardwareVersion SoftwareVersion RestartNode dynamic_node_id remoteid"
 for p in $PACKETS; do
     (
         cd libraries/DroneCAN_generated