make for easy config of firmware behaviour

RemoteIDModule/BLE_TX.cpp

@@ -129,6 +129,9 @@ bool BLE_TX::transmit(ODID_UAS_Data &UAS_data)
 {
     // create a packed UAS data message
     int length = odid_message_build_pack(&UAS_data, payload, 255);
+    if (length <= 0) {
+        return false;
+    }
 
     // setup ASTM header
     const uint8_t header[] { uint8_t(length+5), 0x16, 0xfa, 0xff, 0x0d, uint8_t(msg_counter++) };

RemoteIDModule/DroneCAN.cpp

@@ -471,23 +471,6 @@ 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();

RemoteIDModule/DroneCAN.h

@@ -80,6 +80,5 @@ public:
     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

@@ -12,16 +12,28 @@
 #include <sys/time.h>
 #include <opendroneid.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_ENABLED
 static BLE_TX ble;
+#endif
 
 #define OUTPUT_RATE_HZ 5
 
@@ -50,11 +62,22 @@ void setup()
     // Serial1 for MAVLink
     Serial1.begin(MAVLINK_BAUDRATE, SERIAL_8N1, RX_PIN, TX_PIN);
 
+    // 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_ENABLED
     ble.init();
+#endif
 }
 
 #define MIN(x,y) ((x)<(y)?(x):(y))
@@ -124,38 +147,42 @@ static void set_data_mavlink(MAVLinkSerial &m)
     UAS_data.SelfIDValid = 1;
 
     // System
-    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;
+    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
-    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;
+    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;
+    }
 
     m.set_parse_fail(check_parse());
 }
@@ -163,6 +190,7 @@ static void set_data_mavlink(MAVLinkSerial &m)
 #undef ODID_COPY_STR
 #define ODID_COPY_STR(to, from) memcpy(to, from.data, MIN(from.len, sizeof(to)))
 
+#if AP_DRONECAN_ENABLED
 static void set_data_dronecan(void)
 {
     const auto &operator_id = dronecan.get_operator_id();
@@ -188,43 +216,46 @@ static void set_data_dronecan(void)
     UAS_data.SelfIDValid = 1;
 
     // System
-    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;
+    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
-    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;
+    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;
+    }
 
     dronecan.set_parse_fail(check_parse());
 }
-
-static uint8_t beacon_payload[256];
+#endif // AP_DRONECAN_ENABLED
 
 void loop()
 {
@@ -232,7 +263,9 @@ void loop()
 
     mavlink1.update();
     mavlink2.update();
+#if AP_DRONECAN_ENABLED
     dronecan.update();
+#endif
 
     const uint32_t now_ms = millis();
 
@@ -241,27 +274,46 @@ void loop()
         return;
     }
 
-    const bool mavlink1_ok = mavlink1.location_valid() && mavlink1.system_valid();
-    const bool mavlink2_ok = mavlink2.location_valid() && mavlink2.system_valid();
-    const bool dronecan_ok = dronecan.location_valid() && dronecan.system_valid();
-    if (!mavlink1_ok && !mavlink2_ok && !dronecan_ok) {
+    bool data_ok = false;
+
+#if AP_MAVLINK_ENABLED
+    const bool mavlink1_ok = mavlink1.location_valid();
+    const bool mavlink2_ok = mavlink2.location_valid();
+    data_ok |= mavlink1_ok || mavlink2_ok;
+#endif
+
+#if AP_DRONECAN_ENABLED
+    const bool dronecan_ok = dronecan.location_valid();
+    data_ok |= dronecan_ok;
+#endif
+
+#if !AP_BROADCAST_ON_POWER_UP
+    if (!data_ok) {
         return;
     }
+#endif
 
+#if AP_MAVLINK_ENABLED
     if (mavlink1_ok) {
         set_data_mavlink(mavlink1);
     }
     if (mavlink2_ok) {
         set_data_mavlink(mavlink2);
     }
+#endif
+
+#if AP_DRONECAN_ENABLED
     if (dronecan_ok) {
         set_data_dronecan();
     }
+#endif
 
     last_update = now_ms;
 
-    const auto length = odid_message_build_pack(&UAS_data,beacon_payload,255);
-
+#if AP_WIFI_NAN_ENABLED
     wifi.transmit(UAS_data);
+#endif
+#if AP_BLE_ENABLED
     ble.transmit(UAS_data);
+#endif
 }

RemoteIDModule/mavlink.cpp

@@ -117,34 +117,29 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
     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: {
         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;
         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_ms = now_ms;
         break;
     }
@@ -166,7 +161,6 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
     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;
     }
@@ -210,31 +204,6 @@ void MAVLinkSerial::arm_status_send(void)
         reason);
 }
 
-/*
-  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;
-}
-
-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_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();

RemoteIDModule/mavlink.h

@@ -64,10 +64,6 @@ public:
     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);
 
     void set_parse_fail(const char *msg) {
@@ -79,15 +75,6 @@ private:
     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);
@@ -101,6 +88,5 @@ private:
     mavlink_open_drone_id_system_t system;
     mavlink_open_drone_id_operator_id_t operator_id;
 
-    uint32_t packets_received_mask;
     const char *parse_fail;
 };

RemoteIDModule/options.h

@@ -0,0 +1,20 @@
+/*
+  control optional behaviour in the firmware
+ */
+
+// enable WiFi NAN support
+#define AP_WIFI_NAN_ENABLED 1
+
+// enable bluetooth 4 and 5 support
+#define AP_BLE_ENABLED 1
+
+// start sending packets as soon we we power up,
+// not waiting for location data from flight controller
+#define AP_BROADCAST_ON_POWER_UP 1
+
+// do we support DroneCAN connnection to flight controller?
+#define AP_DRONECAN_ENABLED 1
+
+// do we support MAVLink connnection to flight controller?
+#define AP_MAVLINK_ENABLED 1
+