support power levels

RemoteIDModule/BLE_TX.cpp

@@ -13,9 +13,9 @@
 
 #include <BLEDevice.h>
 #include <BLEAdvertising.h>
+#include "parameters.h"
+
 
-// set max power
-static const int8_t tx_power = ESP_PWR_LVL_P18;
 
 //interval min/max are configured for 1 Hz update rate. Somehow dynamic setting of these fields fails
 //shorter intervals lead to more BLE transmissions. This would result in increased power consumption and can lead to more interference to other radio systems.
@@ -26,7 +26,7 @@ static esp_ble_gap_ext_adv_params_t legacy_adv_params = {
     .channel_map = ADV_CHNL_ALL,
     .own_addr_type = BLE_ADDR_TYPE_RANDOM,
     .filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_WLST, // we want unicast non-connectable transmission
-    .tx_power = tx_power,
+    .tx_power = 0,
     .primary_phy = ESP_BLE_GAP_PHY_1M,
     .max_skip = 0,
     .secondary_phy = ESP_BLE_GAP_PHY_1M,
@@ -41,7 +41,7 @@ static esp_ble_gap_ext_adv_params_t ext_adv_params_coded = {
     .channel_map = ADV_CHNL_ALL,
     .own_addr_type = BLE_ADDR_TYPE_RANDOM,
     .filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_WLST, // we want unicast non-connectable transmission
-    .tx_power = tx_power,
+    .tx_power = 0,
     .primary_phy = ESP_BLE_GAP_PHY_CODED,
     .max_skip = 0,
     .secondary_phy = ESP_BLE_GAP_PHY_CODED,
@@ -56,7 +56,7 @@ static esp_ble_gap_ext_adv_params_t blename_adv_params = {
     .channel_map = ADV_CHNL_ALL,
     .own_addr_type = BLE_ADDR_TYPE_RANDOM,
     .filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_WLST, // we want unicast non-connectable transmission
-    .tx_power = tx_power,
+    .tx_power = 0,
     .primary_phy = ESP_BLE_GAP_PHY_1M,
     .max_skip = 0,
     .secondary_phy = ESP_BLE_GAP_PHY_1M,
@@ -64,6 +64,41 @@ static esp_ble_gap_ext_adv_params_t blename_adv_params = {
     .scan_req_notif = false,
 };
 
+/*
+  map dBm to a TX power
+ */
+uint8_t BLE_TX::dBm_to_tx_power(float dBm) const
+{
+    static const struct {
+        uint8_t level;
+        float dBm;
+    } dBm_table[] = {
+        { ESP_PWR_LVL_N27,-27 },
+        { ESP_PWR_LVL_N24,-24 },
+        { ESP_PWR_LVL_N21,-21 },
+        { ESP_PWR_LVL_N18,-18 },
+        { ESP_PWR_LVL_N15,-15 },
+        { ESP_PWR_LVL_N12,-12 },
+        { ESP_PWR_LVL_N9,  -9 },
+        { ESP_PWR_LVL_N6,  -6 },
+        { ESP_PWR_LVL_N3,  -3 },
+        { ESP_PWR_LVL_N0,   0 },
+        { ESP_PWR_LVL_P3,   3 },
+        { ESP_PWR_LVL_P6,   6 },
+        { ESP_PWR_LVL_P9,   9 },
+        { ESP_PWR_LVL_P12, 12 },
+        { ESP_PWR_LVL_P15, 15 },
+        { ESP_PWR_LVL_P18, 18 },
+    };
+    for (const auto &t : dBm_table) {
+        if (dBm <= t.dBm) {
+            return t.level;
+        }
+    }
+    return ESP_PWR_LVL_P18;
+}
+
+
 static BLEMultiAdvertising advert(3);
 
 bool BLE_TX::init(void)
@@ -74,6 +109,11 @@ bool BLE_TX::init(void)
     initialised = true;
     BLEDevice::init("");
 
+    // setup power levels
+    legacy_adv_params.tx_power = dBm_to_tx_power(g.bt4_power);
+    ext_adv_params_coded.tx_power = dBm_to_tx_power(g.bt5_power);
+    blename_adv_params.tx_power = dBm_to_tx_power(g.bt4_power);
+
     // generate random mac address
     uint8_t mac_addr[6];
     generate_random_mac(mac_addr);

RemoteIDModule/BLE_TX.h

@@ -18,4 +18,6 @@ private:
     uint8_t legacy_payload[36];
     uint8_t longrange_payload[250];
     bool started;
+
+    uint8_t dBm_to_tx_power(float dBm) const;
 };

RemoteIDModule/DroneCAN.cpp

@@ -8,6 +8,7 @@
 #include <time.h>
 #include "DroneCAN.h"
 #include "parameters.h"
+#include <stdarg.h>
 
 #include <canard.h>
 #include <uavcan.protocol.NodeStatus.h>
@@ -15,6 +16,7 @@
 #include <uavcan.protocol.RestartNode.h>
 #include <uavcan.protocol.dynamic_node_id.Allocation.h>
 #include <uavcan.protocol.param.GetSet.h>
+#include <uavcan.protocol.debug.LogMessage.h>
 #include <dronecan.remoteid.BasicID.h>
 #include <dronecan.remoteid.Location.h>
 #include <dronecan.remoteid.SelfID.h>
@@ -592,7 +594,11 @@ void DroneCAN::handle_param_getset(CanardInstance* ins, CanardRxTransfer* transf
             }
             char v[21] {};
             strncpy(v, (const char *)&req.value.string_value.data[0], req.value.string_value.len);
-            vp->set_char20(v);
+            if (vp->min_len > 0 && strlen(v) < vp->min_len) {
+                can_printf("%s too short - min %u", vp->name, vp->min_len);
+            } else {
+                vp->set_char20(v);
+            }
             break;
         }
         case Parameters::ParamType::CHAR64: {
@@ -678,6 +684,33 @@ void DroneCAN::handle_param_getset(CanardInstance* ins, CanardRxTransfer* transf
                            total_size);
 }
 
+// printf to CAN LogMessage for debugging
+void DroneCAN::can_printf(const char *fmt, ...)
+{
+    uavcan_protocol_debug_LogMessage pkt {};
+    uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {};
+    va_list ap;
+    va_start(ap, fmt);
+    uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap);
+    va_end(ap);
+    pkt.text.len = n;
+    if (sizeof(pkt.text.data) < n) {
+        pkt.text.len = sizeof(pkt.text.data);
+    }
+
+    uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer);
+    static uint8_t tx_id;
+
+    canardBroadcast(&canard,
+                    UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
+                    UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,
+                    &tx_id,
+                    CANARD_TRANSFER_PRIORITY_LOW,
+                    buffer,
+                    len);
+}
+
+
 
 #if 0
 // xprintf is useful when debugging in C code such as libcanard

RemoteIDModule/DroneCAN.h

@@ -57,6 +57,8 @@ private:
     void handle_Location(CanardRxTransfer* transfer);
     void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer);
 
+    void can_printf(const char *fmt, ...);
+
 public:
     void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer);
     bool shouldAcceptTransfer(const CanardInstance* ins,

RemoteIDModule/WiFi_TX.cpp

@@ -8,6 +8,7 @@
 #include <esp_wifi.h>
 #include <WiFi.h>
 #include <esp_system.h>
+#include "parameters.h"
 
 bool WiFi_NAN::init(void)
 {
@@ -39,6 +40,7 @@ bool WiFi_NAN::init(void)
     if (esp_read_mac(WiFi_mac_addr, ESP_MAC_WIFI_STA) != ESP_OK) {
         return false;
     }
+    esp_wifi_set_max_tx_power(dBm_to_tx_power(g.wifi_power));
 
     return true;
 }
@@ -69,3 +71,17 @@ bool WiFi_NAN::transmit(ODID_UAS_Data &UAS_data)
 
     return true;
 }
+
+/*
+  map dBm to a TX power
+ */
+uint8_t WiFi_NAN::dBm_to_tx_power(float dBm) const
+{
+    if (dBm < 2) {
+        dBm = 2;
+    }
+    if (dBm > 20) {
+        dBm = 20;
+    }
+    return uint8_t((dBm+1.125) * 4);
+}

RemoteIDModule/WiFi_TX.h

@@ -17,4 +17,6 @@ private:
     uint8_t wifi_channel = 6;
     size_t ssid_length;
     uint8_t send_counter;
+
+    uint8_t dBm_to_tx_power(float dBm) const;
 };

RemoteIDModule/options.h

@@ -5,17 +5,6 @@
 
 #include "board_config.h"
 
-// enable WiFi NAN support
-#define AP_WIFI_NAN_ENABLED 0
-
-// allow enabling legacy or long range only, or both
-#define AP_BLE_LEGACY_ENABLED 1 // bluetooth 4 legacy
-#define AP_BLE_LONGRANGE_ENABLED 1 // bluetooth 5 long range
-
-// 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 defined(PIN_CAN_TX) && defined(PIN_CAN_RX)
 

RemoteIDModule/parameters.cpp

@@ -15,11 +15,14 @@ const Parameters::Param Parameters::params[] = {
     { "UAS_ID",            Parameters::ParamType::CHAR20, (const void*)&g.uas_id[0],        0, 0, 0 },
     { "BAUDRATE",          Parameters::ParamType::UINT32, (const void*)&g.baudrate,         57600, 9600, 921600 },
     { "WIFI_NAN_RATE",     Parameters::ParamType::FLOAT,  (const void*)&g.wifi_nan_rate,    0, 0, 5 },
+    { "WIFI_POWER",        Parameters::ParamType::FLOAT,  (const void*)&g.wifi_power,       20, 2, 20 },
     { "BT4_RATE",          Parameters::ParamType::FLOAT,  (const void*)&g.bt4_rate,         1, 0, 5 },
+    { "BT4_POWER",         Parameters::ParamType::FLOAT,  (const void*)&g.bt4_power,        18, -27, 18 },
     { "BT5_RATE",          Parameters::ParamType::FLOAT,  (const void*)&g.bt5_rate,         1, 0, 5 },
+    { "BT5_POWER",         Parameters::ParamType::FLOAT,  (const void*)&g.bt5_power,        18, -27, 18 },
     { "WEBSERVER_ENABLE",  Parameters::ParamType::UINT8,  (const void*)&g.webserver_enable, 1, 0, 1 },
     { "WIFI_SSID",         Parameters::ParamType::CHAR20, (const void*)&g.wifi_ssid, },
-    { "WIFI_PASSWORD",     Parameters::ParamType::CHAR20, (const void*)&g.wifi_password,    0, 0, 0, PARAM_FLAG_HIDDEN },
+    { "WIFI_PASSWORD",     Parameters::ParamType::CHAR20, (const void*)&g.wifi_password,    0, 0, 0, PARAM_FLAG_HIDDEN, 8 },
     { "BCAST_POWERUP",     Parameters::ParamType::UINT8,  (const void*)&g.bcast_powerup,    1, 0, 1 },
     { "PUBLIC_KEY1",       Parameters::ParamType::CHAR64, (const void*)&g.public_keys[0], },
     { "PUBLIC_KEY2",       Parameters::ParamType::CHAR64, (const void*)&g.public_keys[1], },
@@ -81,6 +84,9 @@ void Parameters::Param::set_float(float v) const
 
 void Parameters::Param::set_char20(const char *v) const
 {
+    if (min_len > 0 && strlen(v) < min_len) {
+        return;
+    }
     memset((void*)ptr, 0, 21);
     strncpy((char *)ptr, v, 20);
     nvs_set_str(handle, name, v);
@@ -88,6 +94,9 @@ void Parameters::Param::set_char20(const char *v) const
 
 void Parameters::Param::set_char64(const char *v) const
 {
+    if (min_len > 0 && strlen(v) < min_len) {
+        return;
+    }
     memset((void*)ptr, 0, 65);
     strncpy((char *)ptr, v, 64);
     nvs_set_str(handle, name, v);

RemoteIDModule/parameters.h

@@ -6,6 +6,7 @@
 #define PUBLIC_KEY_LEN 32
 #define PARAM_NAME_MAX_LEN 16
 
+#define PARAM_FLAG_NONE 0
 #define PARAM_FLAG_HIDDEN (1U<<0)
 
 class Parameters {
@@ -18,8 +19,11 @@ public:
     uint8_t id_type;
     char uas_id[21] = "ABCD123456789";
     float wifi_nan_rate;
+    float wifi_power;
     float bt4_rate;
+    float bt4_power;
     float bt5_rate;
+    float bt5_power;
     uint8_t webserver_enable;
     char wifi_ssid[21] = "RID_123456";
     char wifi_password[21] = "penguin1234";
@@ -44,6 +48,7 @@ public:
         float min_value;
         float max_value;
         uint16_t flags;
+        uint8_t min_len;
         void set_float(float v) const;
         void set_uint8(uint8_t v) const;
         void set_uint32(uint32_t v) const;

scripts/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 remoteid param"
+PACKETS="NodeStatus GetNodeInfo HardwareVersion SoftwareVersion RestartNode dynamic_node_id remoteid param Log"
 for p in $PACKETS; do
     (
         cd libraries/DroneCAN_generated