added DroneCAN parameter system

RemoteIDModule/DroneCAN.cpp

@@ -7,11 +7,14 @@
 #include "version.h"
 #include <time.h>
 #include "DroneCAN.h"
+#include "parameters.h"
+
 #include <canard.h>
 #include <uavcan.protocol.NodeStatus.h>
 #include <uavcan.protocol.GetNodeInfo.h>
 #include <uavcan.protocol.RestartNode.h>
 #include <uavcan.protocol.dynamic_node_id.Allocation.h>
+#include <uavcan.protocol.param.GetSet.h>
 #include <dronecan.remoteid.BasicID.h>
 #include <dronecan.remoteid.Location.h>
 #include <dronecan.remoteid.SelfID.h>
@@ -144,6 +147,9 @@ void DroneCAN::onTransferReceived(CanardInstance* ins,
         Serial.printf("Got OperatorID\n");
         handle_OperatorID(transfer);
         break;
+    case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
+        handle_param_getset(ins, transfer);
+        break;
     default:
         //Serial.printf("reject %u\n", transfer->data_type_id);
         break;
@@ -171,6 +177,7 @@ bool DroneCAN::shouldAcceptTransfer(const CanardInstance* ins,
         ACCEPT_ID(DRONECAN_REMOTEID_SELFID);
         ACCEPT_ID(DRONECAN_REMOTEID_OPERATORID);
         ACCEPT_ID(DRONECAN_REMOTEID_SYSTEM);
+        ACCEPT_ID(UAVCAN_PROTOCOL_PARAM_GETSET);
         return true;
     }
     //Serial.printf("%u: reject ID 0x%x\n", millis(), data_type_id);
@@ -534,6 +541,104 @@ void DroneCAN::handle_Location(CanardRxTransfer* transfer)
     COPY_FIELD(timestamp_accuracy);
 }
 
+/*
+  handle parameter GetSet request
+ */
+void DroneCAN::handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+    uavcan_protocol_param_GetSetRequest req;
+    if (uavcan_protocol_param_GetSetRequest_decode(transfer, &req)) {
+        return;
+    }
+
+    uavcan_protocol_param_GetSetResponse pkt {};
+
+    const Parameters::Param *vp = nullptr;
+
+    if (req.name.len != 0 && req.name.len > PARAM_NAME_MAX_LEN) {
+        vp = nullptr;
+    } else if (req.name.len != 0 && req.name.len <= PARAM_NAME_MAX_LEN) {
+        memcpy((char *)pkt.name.data, (char *)req.name.data, req.name.len);
+        vp = Parameters::find((char *)pkt.name.data);
+    } else {
+        vp = Parameters::find_by_index(req.index);
+    }
+    if (vp != nullptr && req.name.len != 0 && req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY) {
+        // param set
+        switch (vp->ptype) {
+        case Parameters::ParamType::UINT8:
+            if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) {
+                return;
+            }
+            vp->set(uint8_t(req.value.integer_value));
+            break;
+        case Parameters::ParamType::FLOAT:
+            if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE) {
+                return;
+            }
+            vp->set(req.value.real_value);
+            break;
+        case Parameters::ParamType::CHAR20:
+            if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) {
+                return;
+            }
+            vp->set((const char *)&req.value.string_value.data[0]);
+            break;
+        default:
+            return;
+        }
+    }
+    if (vp != nullptr) {
+        switch (vp->ptype) {
+        case Parameters::ParamType::UINT8:
+            pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+            pkt.value.integer_value = vp->get_uint8();
+            pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+            pkt.default_value.integer_value = uint8_t(vp->default_value);
+            pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE;
+            pkt.min_value.integer_value = uint8_t(vp->min_value);
+            pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE;
+            pkt.max_value.integer_value = uint8_t(vp->max_value);
+            break;
+        case Parameters::ParamType::FLOAT:
+            pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
+            pkt.value.real_value = vp->get_float();
+            pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
+            pkt.default_value.real_value = vp->default_value;
+            pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE;
+            pkt.min_value.real_value = vp->min_value;
+            pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE;
+            pkt.max_value.real_value = vp->max_value;
+            break;
+        case Parameters::ParamType::CHAR20: {
+            pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE;
+            const char *s = vp->get_char20();
+            strncpy((char*)pkt.value.string_value.data, s, sizeof(pkt.value.string_value.data));
+            pkt.value.string_value.len = strlen(s);
+            break;
+        }
+        default:
+            return;
+        }
+        pkt.name.len = strlen(vp->name);
+        strncpy((char *)pkt.name.data, vp->name, sizeof(pkt.name.data));
+    }
+
+    uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {};
+    uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer);
+
+    canardRequestOrRespond(ins,
+                           transfer->source_node_id,
+                           UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE,
+                           UAVCAN_PROTOCOL_PARAM_GETSET_ID,
+                           &transfer->transfer_id,
+                           transfer->priority,
+                           CanardResponse,
+                           &buffer[0],
+                           total_size);
+}
+
+
 #if 0
 // xprintf is useful when debugging in C code such as libcanard
 extern "C" {

RemoteIDModule/DroneCAN.h

@@ -55,6 +55,7 @@ private:
     void handle_OperatorID(CanardRxTransfer* transfer);
     void handle_System(CanardRxTransfer* transfer);
     void handle_Location(CanardRxTransfer* transfer);
+    void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer);
 
 public:
     void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer);

RemoteIDModule/RemoteIDModule.ino

@@ -16,6 +16,7 @@
 #include "DroneCAN.h"
 #include "WiFi_TX.h"
 #include "BLE_TX.h"
+#include "parameters.h"
 
 #if AP_DRONECAN_ENABLED
 static DroneCAN dronecan;
@@ -46,6 +47,8 @@ static uint32_t last_location_ms;
  */
 void setup()
 {
+    g.load_defaults();
+
     // Serial for debug printf
     Serial.begin(DEBUG_BAUDRATE);
 

RemoteIDModule/parameters.cpp

@@ -0,0 +1,94 @@
+#include "parameters.h"
+#include <string.h>
+
+Parameters g;
+
+const Parameters::Param Parameters::params[] = {
+    { "LOCK_LEVEL",        Parameters::ParamType::UINT8,  (const void*)&g.lock_level,     0, 0, 2 },
+    { "CAN_NODE",          Parameters::ParamType::UINT8,  (const void*)&g.can_node,       0, 0, 127 },
+    { "UAS_ID",            Parameters::ParamType::CHAR20, (const void*)&g.uas_id[0],      0, 0, 0 },
+    { "WIFI_NAN_RATE",     Parameters::ParamType::FLOAT,  (const void*)&g.wifi_nan_rate,  1, 0, 5 },
+    { "BT4_RATE",          Parameters::ParamType::FLOAT,  (const void*)&g.bt4_rate,       1, 0, 5 },
+    { "BT5_RATE",          Parameters::ParamType::FLOAT,  (const void*)&g.bt5_rate,       1, 0, 5 },
+    { "",                  Parameters::ParamType::NONE,   nullptr,  },
+};
+
+/*
+  find by name
+ */
+const Parameters::Param *Parameters::find(const char *name)
+{
+    for (const auto &p : params) {
+        if (strcmp(name, p.name) == 0) {
+            return &p;
+        }
+    }
+    return nullptr;
+}
+
+/*
+  find by index
+ */
+const Parameters::Param *Parameters::find_by_index(uint16_t index)
+{
+    if (index >= (sizeof(params)/sizeof(params[0])-1)) {
+        return nullptr;
+    }
+    return &params[index];
+}
+
+void Parameters::Param::set(uint8_t v) const
+{
+    auto *p = (uint8_t *)ptr;
+    *p = v;
+    g.dirty = true;
+}
+
+void Parameters::Param::set(float v) const
+{
+    auto *p = (float *)ptr;
+    *p = v;
+    g.dirty = true;
+}
+
+void Parameters::Param::set(const char *v) const
+{
+    strncpy((char *)ptr, v, PARAM_NAME_MAX_LEN);
+    g.dirty = true;
+}
+
+uint8_t Parameters::Param::get_uint8() const
+{
+    const auto *p = (const uint8_t *)ptr;
+    return *p;
+}
+
+float Parameters::Param::get_float() const
+{
+    const auto *p = (const float *)ptr;
+    return *p;
+}
+
+const char *Parameters::Param::get_char20() const
+{
+    const char *p = (const char *)ptr;
+    return p;
+}
+
+
+/*
+  load defaults from parameter table
+ */
+void Parameters::load_defaults(void)
+{
+    for (const auto &p : params) {
+        switch (p.ptype) {
+        case ParamType::UINT8:
+            *(uint8_t *)p.ptr = uint8_t(p.default_value);
+            break;
+        case ParamType::FLOAT:
+            *(float *)p.ptr = p.default_value;
+            break;
+        }
+    }
+}

RemoteIDModule/parameters.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <stdint.h>
+
+#define MAX_PUBLIC_KEYS 10
+#define PUBLIC_KEY_LEN 32
+#define PARAM_NAME_MAX_LEN 16
+
+class Parameters {
+public:
+    uint8_t lock_level;
+    uint8_t can_node;
+    char uas_id[21] = "ABCD123456789";
+    float wifi_nan_rate;
+    float bt4_rate;
+    float bt5_rate;
+
+    /*
+      header at the front of storage
+     */
+    struct header {
+        uint32_t format_version;
+        uint32_t flash_counter;
+        uint32_t crc;
+        struct public_key {
+            uint8_t key[PUBLIC_KEY_LEN];
+        } public_keys[MAX_PUBLIC_KEYS];
+        uint32_t reserved[30];
+    };
+
+    enum class ParamType {
+        NONE=0,
+        UINT8=1,
+        FLOAT=2,
+        CHAR20=3,
+    };
+
+    struct Param {
+        char name[PARAM_NAME_MAX_LEN+1];
+        ParamType ptype;
+        const void *ptr;
+        float default_value;
+        float min_value;
+        float max_value;
+        void set(float v) const;
+        void set(uint8_t v) const;
+        void set(const char *v) const;
+        uint8_t get_uint8() const;
+        float get_float() const;
+        const char *get_char20() const;
+    };
+    static const struct Param params[];
+
+    static const Param *find(const char *name);
+    static const Param *find_by_index(uint16_t idx);
+
+    bool dirty;
+
+    void load_defaults(void);
+};
+
+extern Parameters g;

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