implement mavlink parameters

RemoteIDModule/mavlink.cpp

@@ -5,6 +5,7 @@
 #include "mavlink.h"
 #include "board_config.h"
 #include "version.h"
+#include "parameters.h"
 
 #define SERIAL_BAUD 115200
 
@@ -12,7 +13,7 @@ static HardwareSerial *serial_ports[MAVLINK_COMM_NUM_BUFFERS];
 
 #include <generated/mavlink_helpers.h>
 
-mavlink_system_t mavlink_system = {0,MAV_COMP_ID_ODID_TXRX_1};
+mavlink_system_t mavlink_system = {0, MAV_COMP_ID_ODID_TXRX_1};
 
 /*
   send a buffer out a MAVLink channel
@@ -45,13 +46,32 @@ void MAVLinkSerial::init(void)
 
 void MAVLinkSerial::update(void)
 {
+    const uint32_t now_ms = millis();
+
     if (mavlink_system.sysid != 0) {
         update_send();
-    } else if (millis() - last_hb_warn_ms >= 2000) {
+    } else if (now_ms - last_hb_warn_ms >= 2000) {
         last_hb_warn_ms = millis();
         serial.printf("Waiting for heartbeat\n");
     }
     update_receive();
+
+    if (param_request_last_ms != 0 && now_ms - param_request_last_ms > 50) {
+        param_request_last_ms = now_ms;
+        float value;
+        if (param_next->get_as_float(value)) {
+            mavlink_msg_param_value_send(chan,
+                                         param_next->name, value,
+                                         MAV_PARAM_TYPE_REAL32,
+                                         g.param_count_float(),
+                                         g.param_index_float(param_next));
+        }
+        param_next++;
+        if (param_next->ptype == Parameters::ParamType::NONE) {
+            param_next = nullptr;
+            param_request_last_ms = 0;
+        }
+    }
 }
 
 void MAVLinkSerial::update_send(void)
@@ -162,6 +182,57 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
         last_operator_id_ms = now_ms;
         break;
     }
+    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+        param_next = g.find_by_index_float(0);
+        param_request_last_ms = millis();
+        break;
+    };
+    case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+        mavlink_param_request_read_t pkt;
+        mavlink_msg_param_request_read_decode(&msg, &pkt);
+        const Parameters::Param *p;
+        if (pkt.param_index < 0) {
+            p = g.find(pkt.param_id);
+        } else {
+            p = g.find_by_index_float(pkt.param_index);
+        }
+        float value;
+        if (!p || !p->get_as_float(value)) {
+            return;
+        }
+        mavlink_msg_param_value_send(chan,
+                                     p->name, value,
+                                     MAV_PARAM_TYPE_REAL32,
+                                     g.param_count_float(),
+                                     g.param_index_float(p));
+        break;
+    }
+    case MAVLINK_MSG_ID_PARAM_SET: {
+        mavlink_param_set_t pkt;
+        mavlink_msg_param_set_decode(&msg, &pkt);
+        auto *p = g.find(pkt.param_id);
+        float value;
+        if (pkt.param_type != MAV_PARAM_TYPE_REAL32 ||
+            !p || !p->get_as_float(value)) {
+            return;
+        }
+        p->get_as_float(value);
+        if (g.lock_level != 0 &&
+            (strcmp(p->name, "LOCK_LEVEL") != 0 ||
+             uint8_t(pkt.param_value) <= uint8_t(value))) {
+            // only param set allowed is to increase lock level
+            mav_printf(MAV_SEVERITY_ERROR, "Parameters locked");
+        } else {
+            p->set_as_float(pkt.param_value);
+        }
+        p->get_as_float(value);
+        mavlink_msg_param_value_send(chan,
+                                     p->name, value,
+                                     MAV_PARAM_TYPE_REAL32,
+                                     g.param_count_float(),
+                                     g.param_index_float(p));
+        break;
+    }
     default:
         // we don't care about other packets
         break;

RemoteIDModule/mavlink.h

@@ -3,6 +3,7 @@
  */
 #pragma once
 #include "transport.h"
+#include "parameters.h"
 
 /*
   abstraction for MAVLink on a serial port
@@ -19,6 +20,8 @@ private:
     mavlink_channel_t chan;
     uint32_t last_hb_ms;
     uint32_t last_hb_warn_ms;
+    uint32_t param_request_last_ms;
+    const Parameters::Param *param_next;
 
     void update_receive(void);
     void update_send(void);

RemoteIDModule/parameters.cpp

@@ -35,6 +35,52 @@ const Parameters::Param Parameters::params[] = {
     { "",                  Parameters::ParamType::NONE,   nullptr,  },
 };
 
+/*
+  get count of parameters capable of being converted to load
+ */
+uint16_t Parameters::param_count_float(void)
+{
+    uint16_t count = 0;
+    for (const auto &p : params) {
+        if (p.flags & PARAM_FLAG_HIDDEN) {
+            continue;
+        }
+        switch (p.ptype) {
+        case ParamType::UINT8:
+        case ParamType::UINT32:
+        case ParamType::FLOAT:
+            count++;
+            break;
+        }
+    }
+    // remove 1 for DONE_INIT
+    return count-1;
+}
+
+/*
+  get index of parameter counting only those capable of representation as float
+ */
+int16_t Parameters::param_index_float(const Parameters::Param *f)
+{
+    uint16_t count = 0;
+    for (const auto &p : params) {
+    switch (p.ptype) {
+        if (p.flags & PARAM_FLAG_HIDDEN) {
+            continue;
+        }
+        case ParamType::UINT8:
+        case ParamType::UINT32:
+        case ParamType::FLOAT:
+            if (&p == f) {
+                return count;
+            }
+            count++;
+            break;
+    }
+    }
+    return -1;
+}
+
 /*
   find by name
  */
@@ -53,12 +99,36 @@ const Parameters::Param *Parameters::find(const char *name)
  */
 const Parameters::Param *Parameters::find_by_index(uint16_t index)
 {
-    if (index >= (sizeof(params)/sizeof(params[0])-1)) {
+    if (index >= ARRAY_SIZE(params)-2) {
         return nullptr;
     }
     return &params[index];
 }
 
+/*
+  find by index
+ */
+const Parameters::Param *Parameters::find_by_index_float(uint16_t index)
+{
+    uint16_t count = 0;
+    for (const auto &p : params) {
+    switch (p.ptype) {
+        if (p.flags & PARAM_FLAG_HIDDEN) {
+            continue;
+        }
+        case ParamType::UINT8:
+        case ParamType::UINT32:
+        case ParamType::FLOAT:
+            if (index == count) {
+                return &p;
+            }
+            count++;
+            break;
+    }
+    }
+    return nullptr;
+}
+
 void Parameters::Param::set_uint8(uint8_t v) const
 {
     auto *p = (uint8_t *)ptr;
@@ -135,6 +205,45 @@ const char *Parameters::Param::get_char64() const
     return p;
 }
 
+/*
+  get parameter as a float
+ */
+bool Parameters::Param::get_as_float(float &v) const
+{
+    switch (ptype) {
+        case ParamType::UINT8:
+            v = float(get_uint8());
+            break;
+        case ParamType::UINT32:
+            v = float(get_uint32());
+            break;
+        case ParamType::FLOAT:
+            v = get_float();
+            break;
+    default:
+        return false;
+    }
+    return true;
+}
+
+/*
+  set parameter from a float
+ */
+void Parameters::Param::set_as_float(float v) const
+{
+    switch (ptype) {
+        case ParamType::UINT8:
+            set_uint8(uint8_t(v));
+            break;
+        case ParamType::UINT32:
+            set_uint32(uint32_t(v));
+            break;
+        case ParamType::FLOAT:
+            set_float(v);
+            break;
+    }
+}
+
 
 /*
   load defaults from parameter table

RemoteIDModule/parameters.h

@@ -61,11 +61,14 @@ public:
         float get_float() const;
         const char *get_char20() const;
         const char *get_char64() const;
+        bool get_as_float(float &v) const;
+        void set_as_float(float v) const;
     };
     static const struct Param params[];
 
     static const Param *find(const char *name);
     static const Param *find_by_index(uint16_t idx);
+    static const Param *find_by_index_float(uint16_t idx);
 
     void init(void);
 
@@ -81,6 +84,9 @@ public:
     bool get_public_key(uint8_t i, uint8_t key[32]) const;
     bool no_public_keys(void) const;
 
+    static uint16_t param_count_float(void);
+    static int16_t param_index_float(const Param *p);
+
 private:
     void load_defaults(void);
 };

RemoteIDModule/status.cpp

@@ -6,6 +6,7 @@
 #include "version.h"
 #include <opendroneid.h>
 #include "status.h"
+#include "util.h"
 
 extern ODID_UAS_Data UAS_data;
 
@@ -171,8 +172,6 @@ static const enum_map_t enum_tsacc[] = {
     { ODID_TIME_ACC_1_5_SECOND , "1.5 s" },
 };
 
-#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
-
 static String enum_string(const enum_map_t *m, uint8_t n, int v)
 {
     for (uint8_t i=0; i<n; i++) {

RemoteIDModule/util.h

@@ -4,6 +4,8 @@
 #define MIN(a,b) ((a)<(b)?(a):(b))
 #endif
 
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+
 #include <stdint.h>
 
 /*