reformat with astyle and dos2unix

RemoteIDModule/BLE_TX.cpp

@@ -36,7 +36,7 @@ static esp_ble_gap_ext_adv_params_t legacy_adv_params = {
 
 static esp_ble_gap_ext_adv_params_t ext_adv_params_coded = {
     .type = ESP_BLE_GAP_SET_EXT_ADV_PROP_NONCONN_NONSCANNABLE_UNDIRECTED, //set to unicast advertising
-     .interval_min = 1200, //(unsigned int) 0.75*1000/(OUTPUT_RATE_HZ)/0.625; //allow ble controller to have some room for transmission.
+    .interval_min = 1200, //(unsigned int) 0.75*1000/(OUTPUT_RATE_HZ)/0.625; //allow ble controller to have some room for transmission.
     .interval_max = 1600, //(unsigned int) 1000/(OUTPUT_RATE_HZ)/0.625;
     .channel_map = ADV_CHNL_ALL,
     .own_addr_type = BLE_ADDR_TYPE_RANDOM,
@@ -152,103 +152,103 @@ bool BLE_TX::transmit_longrange(ODID_UAS_Data &UAS_data)
     return true;
 }
 
- bool BLE_TX::transmit_legacy(ODID_UAS_Data &UAS_data)
- {
-	static uint8_t legacy_phase = 0;
-	int legacy_length = 0;
-	// setup ASTM header
-	const uint8_t header[] { 0x1e, 0x16, 0xfa, 0xff, 0x0d }; //exclude the message counter
-	// combine header with payload
-	memset(legacy_payload, 0, sizeof(legacy_payload));
+bool BLE_TX::transmit_legacy(ODID_UAS_Data &UAS_data)
+{
+    static uint8_t legacy_phase = 0;
+    int legacy_length = 0;
+    // setup ASTM header
+    const uint8_t header[] { 0x1e, 0x16, 0xfa, 0xff, 0x0d }; //exclude the message counter
+    // combine header with payload
+    memset(legacy_payload, 0, sizeof(legacy_payload));
     memcpy(legacy_payload, header, sizeof(header));
 
-	switch (legacy_phase)
+    switch (legacy_phase)
     {
-		case  0:
-			ODID_Location_encoded location_encoded;
-			memset(&location_encoded, 0, sizeof(location_encoded));
-			if (encodeLocationMessage(&location_encoded, &UAS_data.Location) != ODID_SUCCESS) {
-				break;
-			}
+    case  0:
+        ODID_Location_encoded location_encoded;
+        memset(&location_encoded, 0, sizeof(location_encoded));
+        if (encodeLocationMessage(&location_encoded, &UAS_data.Location) != ODID_SUCCESS) {
+            break;
+        }
 
-            memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_LOCATION], 1); //set packet counter
-            msg_counters[ODID_MSG_COUNTER_LOCATION]++;
-            //msg_counters[ODID_MSG_COUNTER_LOCATION] %= 256; //likely not be needed as it is defined as unint_8
+        memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_LOCATION], 1); //set packet counter
+        msg_counters[ODID_MSG_COUNTER_LOCATION]++;
+        //msg_counters[ODID_MSG_COUNTER_LOCATION] %= 256; //likely not be needed as it is defined as unint_8
 
-			memcpy(&legacy_payload[sizeof(header) + 1], &location_encoded, sizeof(location_encoded));
-			legacy_length = sizeof(header) + 1 + sizeof(location_encoded);
+        memcpy(&legacy_payload[sizeof(header) + 1], &location_encoded, sizeof(location_encoded));
+        legacy_length = sizeof(header) + 1 + sizeof(location_encoded);
 
-			break;
+        break;
 
-		case  1:
-			ODID_BasicID_encoded basicid_encoded;
-			memset(&basicid_encoded, 0, sizeof(basicid_encoded));
-			if (encodeBasicIDMessage(&basicid_encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
-				break;
-			}
+    case  1:
+        ODID_BasicID_encoded basicid_encoded;
+        memset(&basicid_encoded, 0, sizeof(basicid_encoded));
+        if (encodeBasicIDMessage(&basicid_encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
+            break;
+        }
 
-            memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_BASIC_ID], 1); //set packet counter
-            msg_counters[ODID_MSG_COUNTER_BASIC_ID]++;
-            //msg_counters[ODID_MSG_COUNTER_BASIC_ID] %= 256; //likely not be needed as it is defined as unint_8
+        memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_BASIC_ID], 1); //set packet counter
+        msg_counters[ODID_MSG_COUNTER_BASIC_ID]++;
+        //msg_counters[ODID_MSG_COUNTER_BASIC_ID] %= 256; //likely not be needed as it is defined as unint_8
 
-			memcpy(&legacy_payload[sizeof(header) + 1], &basicid_encoded, sizeof(basicid_encoded));
-            legacy_length = sizeof(header) + 1 + sizeof(basicid_encoded);
+        memcpy(&legacy_payload[sizeof(header) + 1], &basicid_encoded, sizeof(basicid_encoded));
+        legacy_length = sizeof(header) + 1 + sizeof(basicid_encoded);
 
-			break;
+        break;
 
-		case  2:
-			ODID_SelfID_encoded selfid_encoded;
-			memset(&selfid_encoded, 0, sizeof(selfid_encoded));
-			if (encodeSelfIDMessage(&selfid_encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
-				break;
-			}
+    case  2:
+        ODID_SelfID_encoded selfid_encoded;
+        memset(&selfid_encoded, 0, sizeof(selfid_encoded));
+        if (encodeSelfIDMessage(&selfid_encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
+            break;
+        }
 
-            memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_SELF_ID], 1); //set packet counter
-            msg_counters[ODID_MSG_COUNTER_SELF_ID]++;
-            //msg_counters[ODID_MSG_COUNTER_SELF_ID] %= 256; //likely not be needed as it is defined as uint_8
+        memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_SELF_ID], 1); //set packet counter
+        msg_counters[ODID_MSG_COUNTER_SELF_ID]++;
+        //msg_counters[ODID_MSG_COUNTER_SELF_ID] %= 256; //likely not be needed as it is defined as uint_8
 
-			memcpy(&legacy_payload[sizeof(header) + 1], &selfid_encoded, sizeof(selfid_encoded));
-            legacy_length = sizeof(header) + 1 + sizeof(selfid_encoded);
+        memcpy(&legacy_payload[sizeof(header) + 1], &selfid_encoded, sizeof(selfid_encoded));
+        legacy_length = sizeof(header) + 1 + sizeof(selfid_encoded);
 
-			break;
+        break;
 
-		case  3:
-			ODID_System_encoded system_encoded;
-			memset(&system_encoded, 0, sizeof(system_encoded));
-			if (encodeSystemMessage(&system_encoded, &UAS_data.System) != ODID_SUCCESS) {
-				break;
-			}
+    case  3:
+        ODID_System_encoded system_encoded;
+        memset(&system_encoded, 0, sizeof(system_encoded));
+        if (encodeSystemMessage(&system_encoded, &UAS_data.System) != ODID_SUCCESS) {
+            break;
+        }
 
-            memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_SYSTEM], 1); //set packet counter
-            msg_counters[ODID_MSG_COUNTER_SYSTEM]++;
-            //msg_counters[ODID_MSG_COUNTER_SYSTEM] %= 256; //likely not be needed as it is defined as uint_8
+        memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_SYSTEM], 1); //set packet counter
+        msg_counters[ODID_MSG_COUNTER_SYSTEM]++;
+        //msg_counters[ODID_MSG_COUNTER_SYSTEM] %= 256; //likely not be needed as it is defined as uint_8
 
-			memcpy(&legacy_payload[sizeof(header) + 1], &system_encoded, sizeof(system_encoded));
-            legacy_length = sizeof(header) + 1 + sizeof(system_encoded);
+        memcpy(&legacy_payload[sizeof(header) + 1], &system_encoded, sizeof(system_encoded));
+        legacy_length = sizeof(header) + 1 + sizeof(system_encoded);
 
-			break;
+        break;
 
-		case  4:
-			ODID_OperatorID_encoded operatorid_encoded;
-			memset(&operatorid_encoded, 0, sizeof(operatorid_encoded));
-			if (encodeOperatorIDMessage(&operatorid_encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
-				break;
-			}
+    case  4:
+        ODID_OperatorID_encoded operatorid_encoded;
+        memset(&operatorid_encoded, 0, sizeof(operatorid_encoded));
+        if (encodeOperatorIDMessage(&operatorid_encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
+            break;
+        }
 
-            memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_OPERATOR_ID], 1); //set packet counter
-            msg_counters[ODID_MSG_COUNTER_OPERATOR_ID]++;
-            //msg_counters[ODID_MSG_COUNTER_OPERATOR_ID] %= 256; //likely not be needed as it is defined as uint_8
+        memcpy(&legacy_payload[sizeof(header)], &msg_counters[ODID_MSG_COUNTER_OPERATOR_ID], 1); //set packet counter
+        msg_counters[ODID_MSG_COUNTER_OPERATOR_ID]++;
+        //msg_counters[ODID_MSG_COUNTER_OPERATOR_ID] %= 256; //likely not be needed as it is defined as uint_8
 
-			memcpy(&legacy_payload[sizeof(header) + 1], &operatorid_encoded, sizeof(operatorid_encoded));
-            legacy_length = sizeof(header) + 1 + sizeof(operatorid_encoded);
+        memcpy(&legacy_payload[sizeof(header) + 1], &operatorid_encoded, sizeof(operatorid_encoded));
+        legacy_length = sizeof(header) + 1 + sizeof(operatorid_encoded);
 
-			break;
-	}
+        break;
+    }
 
     legacy_phase++;
     legacy_phase %= 5;
 
-	advert.setAdvertisingData(0, legacy_length, legacy_payload);
+    advert.setAdvertisingData(0, legacy_length, legacy_payload);
 
     if (!started) {
         advert.start();

RemoteIDModule/CANDriver.cpp

@@ -42,7 +42,8 @@ static const twai_general_config_t g_config =                      {.mode = TWAI
                                                                     .clkout_io = TWAI_IO_UNUSED, .bus_off_io = TWAI_IO_UNUSED,      \
                                                                     .tx_queue_len = 5, .rx_queue_len = 5,                           \
                                                                     .alerts_enabled = TWAI_ALERT_NONE,  .clkout_divider = 0,        \
-                                                                    .intr_flags = ESP_INTR_FLAG_LEVEL2};
+                                                                    .intr_flags = ESP_INTR_FLAG_LEVEL2
+                                                                   };
 
 static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();
 static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
@@ -66,8 +67,8 @@ void CANDriver::init_once(bool enable_irq)
     } else {
         Serial.printf("Failed to reconfigure CAN/TWAI alerts");
     }
-    
-     //Start TWAI driver
+
+    //Start TWAI driver
     if (twai_start() == ESP_OK)
     {
         Serial.printf("CAN/TWAI Driver started\n");

RemoteIDModule/CANDriver.h

@@ -8,7 +8,7 @@ public:
 
     bool send(const CANFrame &frame);
     bool receive(CANFrame &out_frame);
-    
+
 private:
     struct Timings {
         uint16_t prescaler;
@@ -50,12 +50,12 @@ struct CANFrame {
     };
     uint8_t dlc;                ///< Data Length Code
 
-CANFrame() :
-    id(0),
+    CANFrame() :
+        id(0),
         dlc(0)
-        {
-            memset(data,0, MaxDataLen);
-        }
+    {
+        memset(data,0, MaxDataLen);
+    }
 
     CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false);
 

RemoteIDModule/DroneCAN.cpp

@@ -27,8 +27,8 @@
 
 static void onTransferReceived_trampoline(CanardInstance* ins, CanardRxTransfer* transfer);
 static bool shouldAcceptTransfer_trampoline(const CanardInstance* ins, uint64_t* out_data_type_signature, uint16_t data_type_id,
-                                            CanardTransferType transfer_type,
-                                            uint8_t source_node_id);
+        CanardTransferType transfer_type,
+        uint8_t source_node_id);
 
 // decoded messages
 
@@ -104,7 +104,7 @@ void DroneCAN::onTransferReceived(CanardInstance* ins,
 {
     if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) {
         if (transfer->transfer_type == CanardTransferTypeBroadcast &&
-            transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
+                transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
             handle_allocation_response(ins, transfer);
         }
         return;
@@ -154,7 +154,7 @@ bool DroneCAN::shouldAcceptTransfer(const CanardInstance* ins,
                                     uint8_t source_node_id)
 {
     if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID &&
-        data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
+            data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
         *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE;
         return true;
     }
@@ -175,7 +175,7 @@ bool DroneCAN::shouldAcceptTransfer(const CanardInstance* ins,
 }
 
 static void onTransferReceived_trampoline(CanardInstance* ins,
-                                          CanardRxTransfer* transfer)
+        CanardRxTransfer* transfer)
 {
     DroneCAN *dc = (DroneCAN *)ins->user_reference;
     dc->onTransferReceived(ins, transfer);
@@ -185,10 +185,10 @@ static void onTransferReceived_trampoline(CanardInstance* ins,
   see if we want to process this packet
  */
 static bool shouldAcceptTransfer_trampoline(const CanardInstance* ins,
-                                            uint64_t* out_data_type_signature,
-                                            uint16_t data_type_id,
-                                            CanardTransferType transfer_type,
-                                            uint8_t source_node_id)
+        uint64_t* out_data_type_signature,
+        uint16_t data_type_id,
+        CanardTransferType transfer_type,
+        uint8_t source_node_id)
 {
     DroneCAN *dc = (DroneCAN *)ins->user_reference;
     return dc->shouldAcceptTransfer(ins, out_data_type_signature,
@@ -239,13 +239,13 @@ void DroneCAN::processRx(void)
                       rx_frame.data_len,
                       err);
 #else
-        UNUSED(err);                      
+        UNUSED(err);
 #endif
     }
 }
 
 CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) :
-        id(can_id)
+    id(can_id)
 {
     if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) {
         return;
@@ -414,7 +414,7 @@ bool DroneCAN::do_DNA(void)
 
     static const uint8_t MaxLenOfUniqueIDInRequest = 6;
     uint8_t uid_size = (uint8_t)(sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data) - node_id_allocation_unique_id_offset);
-    
+
     if (uid_size > MaxLenOfUniqueIDInRequest) {
         uid_size = MaxLenOfUniqueIDInRequest;
     }
@@ -534,7 +534,7 @@ void DroneCAN::handle_Location(CanardRxTransfer* transfer)
 #if 0
 // xprintf is useful when debugging in C code such as libcanard
 extern "C" {
-void xprintf(const char *fmt, ...);
+    void xprintf(const char *fmt, ...);
 }
 
 void xprintf(const char *fmt, ...)

RemoteIDModule/RemoteIDModule.ino

@@ -1,289 +1,289 @@
-/*
-  implement OpenDroneID MAVLink and DroneCAN support
- */
-/*
-  released under GNU GPL v3 or later
- */
-
-#include <Arduino.h>
-#include "version.h"
-#include <math.h>
-#include <time.h>
-#include <sys/time.h>
-#include <opendroneid.h>
-#include "board_config.h"
-#include "options.h"
-#include "mavlink.h"
-#include "DroneCAN.h"
-#include "WiFi_TX.h"
-#include "BLE_TX.h"
-
-#if AP_DRONECAN_ENABLED
-static DroneCAN dronecan;
-#endif
-
-#if AP_MAVLINK_ENABLED
-static MAVLinkSerial mavlink1{Serial1, MAVLINK_COMM_0};
-static MAVLinkSerial mavlink2{Serial, MAVLINK_COMM_1};
-#endif
-
-#if AP_WIFI_NAN_ENABLED
-static WiFi_NAN wifi;
-#endif
-
-#if AP_BLE_LEGACY_ENABLED || AP_BLE_LONGRANGE_ENABLED
-static BLE_TX ble;
-#endif
-
-#define DEBUG_BAUDRATE 57600
-#define MAVLINK_BAUDRATE 57600
-
-// OpenDroneID output data structure
-static ODID_UAS_Data UAS_data;
-static uint32_t last_location_ms;
-
-/*
-  setup serial ports
- */
-void setup()
-{
-    // Serial for debug printf
-    Serial.begin(DEBUG_BAUDRATE);
-
-    Serial.printf("ArduRemoteID version %u.%u %08x\n",
-                  FW_VERSION_MAJOR, FW_VERSION_MINOR, GIT_VERSION);
-
-    // Serial1 for MAVLink
-    Serial1.begin(MAVLINK_BAUDRATE, SERIAL_8N1, PIN_UART_RX, PIN_UART_TX);
-
-    // set all fields to invalid/initial values
-    odid_initUasData(&UAS_data);
-
-#if AP_MAVLINK_ENABLED
-    mavlink1.init();
-    mavlink2.init();
-#endif
-#if AP_DRONECAN_ENABLED
-    dronecan.init();
-#endif
-#if AP_WIFI_NAN_ENABLED
-    wifi.init();
-#endif
-#if AP_BLE_LEGACY_ENABLED || AP_BLE_LONGRANGE_ENABLED
-    ble.init();
-#endif
-
-#if defined(PIN_CAN_EN)
-    // optional CAN enable pin
-    pinMode(PIN_CAN_EN, OUTPUT);
-    digitalWrite(PIN_CAN_EN, HIGH);
-#endif
-
-#if defined(PIN_CAN_nSILENT)
-    // disable silent pin
-    pinMode(PIN_CAN_nSILENT, OUTPUT);
-    digitalWrite(PIN_CAN_nSILENT, HIGH);
-#endif
-
-#if defined(PIN_CAN_TERM)
-    // optional CAN termination control
-    pinMode(PIN_CAN_TERM, OUTPUT);
-    digitalWrite(PIN_CAN_TERM, HIGH);
-#endif
-}
-
-#define IMIN(x,y) ((x)<(y)?(x):(y))
-#define ODID_COPY_STR(to, from) strncpy(to, (const char*)from, IMIN(sizeof(to), sizeof(from)))
-
-/*
-  check parsing of UAS_data, this checks ranges of values to ensure we
-  will produce a valid pack
- */
-static const char *check_parse(void)
-{
-    {
-        ODID_Location_encoded encoded {};
-        if (encodeLocationMessage(&encoded, &UAS_data.Location) != ODID_SUCCESS) {
-            return "bad LOCATION data";
-        }
-    }
-    {
-        ODID_System_encoded encoded {};
-        if (encodeSystemMessage(&encoded, &UAS_data.System) != ODID_SUCCESS) {
-            return "bad SYSTEM data";
-        }
-    }
-    {
-        ODID_BasicID_encoded encoded {};
-        if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
-            return "bad BASIC_ID data";
-        }
-    }
-    {
-        ODID_SelfID_encoded encoded {};
-        if (encodeSelfIDMessage(&encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
-            return "bad SELF_ID data";
-        }
-    }
-    {
-        ODID_OperatorID_encoded encoded {};
-        if (encodeOperatorIDMessage(&encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
-            return "bad OPERATOR_ID data";
-        }
-    }
-    return nullptr;
-}
-
-/*
-  fill in UAS_data from MAVLink packets
- */
-static void set_data(Transport &t)
-{
-    const auto &operator_id = t.get_operator_id();
-    const auto &basic_id = t.get_basic_id();
-    const auto &system = t.get_system();
-    const auto &self_id = t.get_self_id();
-    const auto &location = t.get_location();
-
-    // BasicID
-    UAS_data.BasicID[0].UAType = (ODID_uatype_t)basic_id.ua_type;
-    UAS_data.BasicID[0].IDType = (ODID_idtype_t)basic_id.id_type;
-    ODID_COPY_STR(UAS_data.BasicID[0].UASID, basic_id.uas_id);
-    UAS_data.BasicIDValid[0] = 1;
-
-    // OperatorID
-    UAS_data.OperatorID.OperatorIdType = (ODID_operatorIdType_t)operator_id.operator_id_type;
-    ODID_COPY_STR(UAS_data.OperatorID.OperatorId, operator_id.operator_id);
-    UAS_data.OperatorIDValid = 1;
-
-    // SelfID
-    UAS_data.SelfID.DescType = (ODID_desctype_t)self_id.description_type;
-    ODID_COPY_STR(UAS_data.SelfID.Desc, self_id.description);
-    UAS_data.SelfIDValid = 1;
-
-    // System
-    if (system.timestamp != 0) {
-        UAS_data.System.OperatorLocationType = (ODID_operator_location_type_t)system.operator_location_type;
-        UAS_data.System.ClassificationType = (ODID_classification_type_t)system.classification_type;
-        UAS_data.System.OperatorLatitude = system.operator_latitude * 1.0e-7;
-        UAS_data.System.OperatorLongitude = system.operator_longitude * 1.0e-7;
-        UAS_data.System.AreaCount = system.area_count;
-        UAS_data.System.AreaRadius = system.area_radius;
-        UAS_data.System.AreaCeiling = system.area_ceiling;
-        UAS_data.System.AreaFloor = system.area_floor;
-        UAS_data.System.CategoryEU = (ODID_category_EU_t)system.category_eu;
-        UAS_data.System.ClassEU = (ODID_class_EU_t)system.class_eu;
-        UAS_data.System.OperatorAltitudeGeo = system.operator_altitude_geo;
-        UAS_data.System.Timestamp = system.timestamp;
-        UAS_data.SystemValid = 1;
-    }
-
-    // Location
-    if (location.timestamp != 0) {
-        UAS_data.Location.Status = (ODID_status_t)location.status;
-        UAS_data.Location.Direction = location.direction*0.01;
-        UAS_data.Location.SpeedHorizontal = location.speed_horizontal*0.01;
-        UAS_data.Location.SpeedVertical = location.speed_vertical*0.01;
-        UAS_data.Location.Latitude = location.latitude*1.0e-7;
-        UAS_data.Location.Longitude = location.longitude*1.0e-7;
-        UAS_data.Location.AltitudeBaro = location.altitude_barometric;
-        UAS_data.Location.AltitudeGeo = location.altitude_geodetic;
-        UAS_data.Location.HeightType = (ODID_Height_reference_t)location.height_reference;
-        UAS_data.Location.Height = location.height;
-        UAS_data.Location.HorizAccuracy = (ODID_Horizontal_accuracy_t)location.horizontal_accuracy;
-        UAS_data.Location.VertAccuracy = (ODID_Vertical_accuracy_t)location.vertical_accuracy;
-        UAS_data.Location.BaroAccuracy = (ODID_Vertical_accuracy_t)location.barometer_accuracy;
-        UAS_data.Location.SpeedAccuracy = (ODID_Speed_accuracy_t)location.speed_accuracy;
-        UAS_data.Location.TSAccuracy = (ODID_Timestamp_accuracy_t)location.timestamp_accuracy;
-        UAS_data.Location.TimeStamp = location.timestamp;
-        UAS_data.LocationValid = 1;
-    }
-
-    const char *reason = check_parse();
-    if (reason == nullptr) {
-        t.arm_status_check(reason);
-    }
-    t.set_parse_fail(reason);
-
-#ifdef PIN_STATUS_LED
-    // LED off if good to arm
-    pinMode(PIN_STATUS_LED, OUTPUT);
-    digitalWrite(PIN_STATUS_LED, reason==nullptr?!STATUS_LED_ON:STATUS_LED_ON);
-#endif
-
-    uint32_t now_ms = millis();
-    uint32_t location_age_ms = now_ms - t.get_last_location_ms();
-    uint32_t last_location_age_ms = now_ms - last_location_ms;
-    if (location_age_ms < last_location_age_ms) {
-        last_location_ms = t.get_last_location_ms();
-    }
-}
-
-uint8_t loop_counter = 0;
-void loop()
-{
-    static uint32_t last_update;
-
-    mavlink1.update();
-    mavlink2.update();
-#if AP_DRONECAN_ENABLED
-    dronecan.update();
-#endif
-
-    const uint32_t now_ms = millis();
-
-    if (now_ms - last_update < 1000/(OUTPUT_RATE_HZ*6)) {  //Bluetooth 4 needs to run at a 6 times higher update rate as other protocols. F3586 requires a minimum broadcast refresh rate of 1 Hz for static information. (This value overwrites the default value of 3Hz of F3411)
-        // not ready for a new frame yet
-        return;
-    }
-    loop_counter++;
-    loop_counter %= 6;
-
-    // the transports have common static data, so we can just use the
-    // first for status
-    auto &transport = mavlink1;
-
-    bool have_location = false;
-    const uint32_t last_location_ms = transport.get_last_location_ms();
-
-#if AP_BROADCAST_ON_POWER_UP
-    // if we are broadcasting on powerup we always mark location valid
-    // so the location with default data is sent
-    if (!UAS_data.LocationValid) {
-        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
-        UAS_data.LocationValid = 1;
-    }
-#else
-    // only broadcast if we have received a location at least once
-    if (last_location_ms == 0) {
-        return;
-    }
-#endif
-
-    if (last_location_ms == 0 ||
-        now_ms - last_location_ms > 5000) {
-        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
-    }
-
-    set_data(transport);
-    last_update = now_ms;
-#if AP_WIFI_NAN_ENABLED
-    if (loop_counter == 0) { //only run on the original update rate
-        wifi.transmit(UAS_data);
-    }
-#endif
-
-#if AP_BLE_LONGRANGE_ENABLED
-    if (loop_counter == 0) { //only run on the original update rate
-        ble.transmit_longrange(UAS_data);
-    }
-#endif
-
-#if AP_BLE_LEGACY_ENABLED
-    ble.transmit_legacy(UAS_data);
-#endif
-
-#if AP_BLE_LEGACY_ENABLED || AP_BLE_LONGRANGE_ENABLED
-    ble.transmit_legacy_name(UAS_data);
-#endif
-}
+/*
+  implement OpenDroneID MAVLink and DroneCAN support
+ */
+/*
+  released under GNU GPL v3 or later
+ */
+
+#include <Arduino.h>
+#include "version.h"
+#include <math.h>
+#include <time.h>
+#include <sys/time.h>
+#include <opendroneid.h>
+#include "board_config.h"
+#include "options.h"
+#include "mavlink.h"
+#include "DroneCAN.h"
+#include "WiFi_TX.h"
+#include "BLE_TX.h"
+
+#if AP_DRONECAN_ENABLED
+static DroneCAN dronecan;
+#endif
+
+#if AP_MAVLINK_ENABLED
+static MAVLinkSerial mavlink1 {Serial1, MAVLINK_COMM_0};
+static MAVLinkSerial mavlink2{Serial, MAVLINK_COMM_1};
+#endif
+
+#if AP_WIFI_NAN_ENABLED
+static WiFi_NAN wifi;
+#endif
+
+#if AP_BLE_LEGACY_ENABLED || AP_BLE_LONGRANGE_ENABLED
+static BLE_TX ble;
+#endif
+
+#define DEBUG_BAUDRATE 57600
+#define MAVLINK_BAUDRATE 57600
+
+// OpenDroneID output data structure
+static ODID_UAS_Data UAS_data;
+static uint32_t last_location_ms;
+
+/*
+  setup serial ports
+ */
+void setup()
+{
+    // Serial for debug printf
+    Serial.begin(DEBUG_BAUDRATE);
+
+    Serial.printf("ArduRemoteID version %u.%u %08x\n",
+                  FW_VERSION_MAJOR, FW_VERSION_MINOR, GIT_VERSION);
+
+    // Serial1 for MAVLink
+    Serial1.begin(MAVLINK_BAUDRATE, SERIAL_8N1, PIN_UART_RX, PIN_UART_TX);
+
+    // set all fields to invalid/initial values
+    odid_initUasData(&UAS_data);
+
+#if AP_MAVLINK_ENABLED
+    mavlink1.init();
+    mavlink2.init();
+#endif
+#if AP_DRONECAN_ENABLED
+    dronecan.init();
+#endif
+#if AP_WIFI_NAN_ENABLED
+    wifi.init();
+#endif
+#if AP_BLE_LEGACY_ENABLED || AP_BLE_LONGRANGE_ENABLED
+    ble.init();
+#endif
+
+#if defined(PIN_CAN_EN)
+    // optional CAN enable pin
+    pinMode(PIN_CAN_EN, OUTPUT);
+    digitalWrite(PIN_CAN_EN, HIGH);
+#endif
+
+#if defined(PIN_CAN_nSILENT)
+    // disable silent pin
+    pinMode(PIN_CAN_nSILENT, OUTPUT);
+    digitalWrite(PIN_CAN_nSILENT, HIGH);
+#endif
+
+#if defined(PIN_CAN_TERM)
+    // optional CAN termination control
+    pinMode(PIN_CAN_TERM, OUTPUT);
+    digitalWrite(PIN_CAN_TERM, HIGH);
+#endif
+}
+
+#define IMIN(x,y) ((x)<(y)?(x):(y))
+#define ODID_COPY_STR(to, from) strncpy(to, (const char*)from, IMIN(sizeof(to), sizeof(from)))
+
+/*
+  check parsing of UAS_data, this checks ranges of values to ensure we
+  will produce a valid pack
+ */
+static const char *check_parse(void)
+{
+    {
+        ODID_Location_encoded encoded {};
+        if (encodeLocationMessage(&encoded, &UAS_data.Location) != ODID_SUCCESS) {
+            return "bad LOCATION data";
+        }
+    }
+    {
+        ODID_System_encoded encoded {};
+        if (encodeSystemMessage(&encoded, &UAS_data.System) != ODID_SUCCESS) {
+            return "bad SYSTEM data";
+        }
+    }
+    {
+        ODID_BasicID_encoded encoded {};
+        if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
+            return "bad BASIC_ID data";
+        }
+    }
+    {
+        ODID_SelfID_encoded encoded {};
+        if (encodeSelfIDMessage(&encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
+            return "bad SELF_ID data";
+        }
+    }
+    {
+        ODID_OperatorID_encoded encoded {};
+        if (encodeOperatorIDMessage(&encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
+            return "bad OPERATOR_ID data";
+        }
+    }
+    return nullptr;
+}
+
+/*
+  fill in UAS_data from MAVLink packets
+ */
+static void set_data(Transport &t)
+{
+    const auto &operator_id = t.get_operator_id();
+    const auto &basic_id = t.get_basic_id();
+    const auto &system = t.get_system();
+    const auto &self_id = t.get_self_id();
+    const auto &location = t.get_location();
+
+    // BasicID
+    UAS_data.BasicID[0].UAType = (ODID_uatype_t)basic_id.ua_type;
+    UAS_data.BasicID[0].IDType = (ODID_idtype_t)basic_id.id_type;
+    ODID_COPY_STR(UAS_data.BasicID[0].UASID, basic_id.uas_id);
+    UAS_data.BasicIDValid[0] = 1;
+
+    // OperatorID
+    UAS_data.OperatorID.OperatorIdType = (ODID_operatorIdType_t)operator_id.operator_id_type;
+    ODID_COPY_STR(UAS_data.OperatorID.OperatorId, operator_id.operator_id);
+    UAS_data.OperatorIDValid = 1;
+
+    // SelfID
+    UAS_data.SelfID.DescType = (ODID_desctype_t)self_id.description_type;
+    ODID_COPY_STR(UAS_data.SelfID.Desc, self_id.description);
+    UAS_data.SelfIDValid = 1;
+
+    // System
+    if (system.timestamp != 0) {
+        UAS_data.System.OperatorLocationType = (ODID_operator_location_type_t)system.operator_location_type;
+        UAS_data.System.ClassificationType = (ODID_classification_type_t)system.classification_type;
+        UAS_data.System.OperatorLatitude = system.operator_latitude * 1.0e-7;
+        UAS_data.System.OperatorLongitude = system.operator_longitude * 1.0e-7;
+        UAS_data.System.AreaCount = system.area_count;
+        UAS_data.System.AreaRadius = system.area_radius;
+        UAS_data.System.AreaCeiling = system.area_ceiling;
+        UAS_data.System.AreaFloor = system.area_floor;
+        UAS_data.System.CategoryEU = (ODID_category_EU_t)system.category_eu;
+        UAS_data.System.ClassEU = (ODID_class_EU_t)system.class_eu;
+        UAS_data.System.OperatorAltitudeGeo = system.operator_altitude_geo;
+        UAS_data.System.Timestamp = system.timestamp;
+        UAS_data.SystemValid = 1;
+    }
+
+    // Location
+    if (location.timestamp != 0) {
+        UAS_data.Location.Status = (ODID_status_t)location.status;
+        UAS_data.Location.Direction = location.direction*0.01;
+        UAS_data.Location.SpeedHorizontal = location.speed_horizontal*0.01;
+        UAS_data.Location.SpeedVertical = location.speed_vertical*0.01;
+        UAS_data.Location.Latitude = location.latitude*1.0e-7;
+        UAS_data.Location.Longitude = location.longitude*1.0e-7;
+        UAS_data.Location.AltitudeBaro = location.altitude_barometric;
+        UAS_data.Location.AltitudeGeo = location.altitude_geodetic;
+        UAS_data.Location.HeightType = (ODID_Height_reference_t)location.height_reference;
+        UAS_data.Location.Height = location.height;
+        UAS_data.Location.HorizAccuracy = (ODID_Horizontal_accuracy_t)location.horizontal_accuracy;
+        UAS_data.Location.VertAccuracy = (ODID_Vertical_accuracy_t)location.vertical_accuracy;
+        UAS_data.Location.BaroAccuracy = (ODID_Vertical_accuracy_t)location.barometer_accuracy;
+        UAS_data.Location.SpeedAccuracy = (ODID_Speed_accuracy_t)location.speed_accuracy;
+        UAS_data.Location.TSAccuracy = (ODID_Timestamp_accuracy_t)location.timestamp_accuracy;
+        UAS_data.Location.TimeStamp = location.timestamp;
+        UAS_data.LocationValid = 1;
+    }
+
+    const char *reason = check_parse();
+    if (reason == nullptr) {
+        t.arm_status_check(reason);
+    }
+    t.set_parse_fail(reason);
+
+#ifdef PIN_STATUS_LED
+    // LED off if good to arm
+    pinMode(PIN_STATUS_LED, OUTPUT);
+    digitalWrite(PIN_STATUS_LED, reason==nullptr?!STATUS_LED_ON:STATUS_LED_ON);
+#endif
+
+    uint32_t now_ms = millis();
+    uint32_t location_age_ms = now_ms - t.get_last_location_ms();
+    uint32_t last_location_age_ms = now_ms - last_location_ms;
+    if (location_age_ms < last_location_age_ms) {
+        last_location_ms = t.get_last_location_ms();
+    }
+}
+
+uint8_t loop_counter = 0;
+void loop()
+{
+    static uint32_t last_update;
+
+    mavlink1.update();
+    mavlink2.update();
+#if AP_DRONECAN_ENABLED
+    dronecan.update();
+#endif
+
+    const uint32_t now_ms = millis();
+
+    if (now_ms - last_update < 1000/(OUTPUT_RATE_HZ*6)) {  //Bluetooth 4 needs to run at a 6 times higher update rate as other protocols. F3586 requires a minimum broadcast refresh rate of 1 Hz for static information. (This value overwrites the default value of 3Hz of F3411)
+        // not ready for a new frame yet
+        return;
+    }
+    loop_counter++;
+    loop_counter %= 6;
+
+    // the transports have common static data, so we can just use the
+    // first for status
+    auto &transport = mavlink1;
+
+    bool have_location = false;
+    const uint32_t last_location_ms = transport.get_last_location_ms();
+
+#if AP_BROADCAST_ON_POWER_UP
+    // if we are broadcasting on powerup we always mark location valid
+    // so the location with default data is sent
+    if (!UAS_data.LocationValid) {
+        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
+        UAS_data.LocationValid = 1;
+    }
+#else
+    // only broadcast if we have received a location at least once
+    if (last_location_ms == 0) {
+        return;
+    }
+#endif
+
+    if (last_location_ms == 0 ||
+            now_ms - last_location_ms > 5000) {
+        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
+    }
+
+    set_data(transport);
+    last_update = now_ms;
+#if AP_WIFI_NAN_ENABLED
+    if (loop_counter == 0) { //only run on the original update rate
+        wifi.transmit(UAS_data);
+    }
+#endif
+
+#if AP_BLE_LONGRANGE_ENABLED
+    if (loop_counter == 0) { //only run on the original update rate
+        ble.transmit_longrange(UAS_data);
+    }
+#endif
+
+#if AP_BLE_LEGACY_ENABLED
+    ble.transmit_legacy(UAS_data);
+#endif
+
+#if AP_BLE_LEGACY_ENABLED || AP_BLE_LONGRANGE_ENABLED
+    ble.transmit_legacy_name(UAS_data);
+#endif
+}

RemoteIDModule/WiFi_TX.cpp

@@ -16,7 +16,7 @@ bool WiFi_NAN::init(void)
     WiFi.softAP("", NULL, wifi_channel);
 
     esp_wifi_get_config(WIFI_IF_AP, &wifi_config);
-  
+
     wifi_config.ap.ssid_hidden = 1;
 
     if (esp_wifi_set_config(WIFI_IF_AP, &wifi_config) != ESP_OK) {
@@ -38,15 +38,15 @@ bool WiFi_NAN::transmit(ODID_UAS_Data &UAS_data)
 
     int length;
     if ((length = odid_wifi_build_nan_sync_beacon_frame((char *)WiFi_mac_addr,
-                                                        buffer,sizeof(buffer))) > 0) {
+                  buffer,sizeof(buffer))) > 0) {
         if (esp_wifi_80211_tx(WIFI_IF_AP,buffer,length,true) != ESP_OK) {
             return false;
         }
     }
 
     if ((length = odid_wifi_build_message_pack_nan_action_frame(&UAS_data,(char *)WiFi_mac_addr,
-                                                                ++send_counter,
-                                                                buffer,sizeof(buffer))) > 0) {
+                  ++send_counter,
+                  buffer,sizeof(buffer))) > 0) {
         if (esp_wifi_80211_tx(WIFI_IF_AP,buffer,length,true) != ESP_OK) {
             return false;
         }