From 635540b4d6e624d2840fd7fa722d98416d235bf6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 22 Nov 2018 10:15:34 +1100
Subject: [PATCH] SITL: simulate Solo gimbal parameter handling

this allows us to get into running state
---
 libraries/SITL/SIM_Gimbal.cpp | 110 ++++++++++++++++++++++++++++------
 libraries/SITL/SIM_Gimbal.h   |   5 ++
 2 files changed, 96 insertions(+), 19 deletions(-)

diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp
index ff5fd694ed..989c4a713f 100644
--- a/libraries/SITL/SIM_Gimbal.cpp
+++ b/libraries/SITL/SIM_Gimbal.cpp
@@ -174,12 +174,78 @@ void Gimbal::update(void)
     send_report();
 }
 
+static struct gimbal_param {
+    const char *name;
+    float value;
+} gimbal_params[] = {
+    {"GMB_OFF_ACC_X", 0},
+    {"GMB_OFF_ACC_Y", 0},
+    {"GMB_OFF_ACC_Z", 0},
+    {"GMB_GN_ACC_X", 0},
+    {"GMB_GN_ACC_Y", 0},
+    {"GMB_GN_ACC_Z", 0},
+    {"GMB_OFF_GYRO_X", 0},
+    {"GMB_OFF_GYRO_Y", 0},
+    {"GMB_OFF_GYRO_Z", 0},
+    {"GMB_OFF_JNT_X", 0},
+    {"GMB_OFF_JNT_Y", 0},
+    {"GMB_OFF_JNT_Z", 0},
+    {"GMB_K_RATE", 0},
+    {"GMB_POS_HOLD", 0},
+    {"GMB_MAX_TORQUE", 0},
+    {"GMB_SND_TORQUE", 0},
+    {"GMB_SYSID", 0},
+    {"GMB_FLASH", 0},
+};
+
+/*
+  find a parameter structure
+ */
+struct gimbal_param *Gimbal::param_find(const char *name)
+{
+    for (uint8_t i=0; i<ARRAY_SIZE(gimbal_params); i++) {
+        if (strncmp(name, gimbal_params[i].name, 16) == 0) {
+            return &gimbal_params[i];
+        }
+    }
+    return nullptr;
+}
+    
+/*
+  send a parameter to flight board
+ */
+void Gimbal::param_send(const struct gimbal_param *p)
+{
+    mavlink_message_t msg;
+    mavlink_param_value_t param_value;
+    strncpy(param_value.param_id, p->name, sizeof(param_value.param_id));
+    param_value.param_value = p->value;
+    param_value.param_count = 0;
+    param_value.param_index = 0;
+
+    mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
+    uint8_t saved_seq = chan0_status->current_tx_seq;
+    chan0_status->current_tx_seq = mavlink.seq;
+    uint16_t len = mavlink_msg_param_value_encode(vehicle_system_id,
+                                                  vehicle_component_id,
+                                                  &msg, &param_value);
+    chan0_status->current_tx_seq = saved_seq;
+
+    uint8_t msgbuf[len];
+    len = mavlink_msg_to_send_buffer(msgbuf, &msg);
+    if (len > 0) {
+        mav_socket.send(msgbuf, len);
+    }
+}
+
+    
 /*
   send a report to the vehicle control code over MAVLink
 */
 void Gimbal::send_report(void)
 {
-    if (AP_HAL::millis() < 10000) {
+    uint32_t now = AP_HAL::millis();
+    if (now < 10000) {
         // don't send gimbal reports until 10s after startup. This
         // avoids a windows threading issue with non-blocking sockets
         // and the initial wait on uartA
@@ -193,6 +259,14 @@ void Gimbal::send_report(void)
         return;
     }
 
+    if (param_send_last_ms && now - param_send_last_ms > 100) {
+        param_send(&gimbal_params[param_send_idx]);
+        if (++param_send_idx == ARRAY_SIZE(gimbal_params)) {
+            printf("Finished sending parameters\n");
+            param_send_last_ms = 0;
+        }
+    }
+
     // check for incoming MAVLink messages
     uint8_t buf[100];
     ssize_t ret;
@@ -237,24 +311,23 @@ void Gimbal::send_report(void)
                     mavlink_msg_param_set_decode(&msg, &pkt);
                     printf("Gimbal got PARAM_SET %.16s %f\n", pkt.param_id, pkt.param_value);
 
-                    mavlink_param_value_t param_value;
-                    memcpy(param_value.param_id, pkt.param_id, sizeof(pkt.param_id));
-                    param_value.param_value = pkt.param_value;
-                    param_value.param_count = 0;
-                    param_value.param_index = 0;
-                    mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
-                    uint8_t saved_seq = chan0_status->current_tx_seq;
-                    chan0_status->current_tx_seq = mavlink.seq;
-                    uint16_t len = mavlink_msg_param_value_encode(vehicle_system_id,
-                                                                  vehicle_component_id,
-                                                                  &msg, &param_value);
-                    chan0_status->current_tx_seq = saved_seq;
-
-                    uint8_t msgbuf[len];
-                    len = mavlink_msg_to_send_buffer(msgbuf, &msg);
-                    if (len > 0) {
-                        mav_socket.send(msgbuf, len);
+                    struct gimbal_param *p = param_find(pkt.param_id);
+                    if (p) {
+                        p->value = pkt.param_value;
+                        param_send(p);
                     }
+
+                    break;
+                }
+                case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+                    mavlink_param_request_list_t pkt;
+                    mavlink_msg_param_request_list_decode(&msg, &pkt);
+                    if (pkt.target_system == 0 && pkt.target_component == MAV_COMP_ID_GIMBAL) {
+                        // start param send
+                        param_send_idx = 0;
+                        param_send_last_ms = AP_HAL::millis();
+                    }
+                    printf("Gimbal sending %u parameters\n", (unsigned)ARRAY_SIZE(gimbal_params));
                     break;
                 }
                 default:
@@ -268,7 +341,6 @@ void Gimbal::send_report(void)
     if (!seen_heartbeat) {
         return;
     }
-    uint32_t now = AP_HAL::millis();
     mavlink_message_t msg;
     uint16_t len;
 
diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_Gimbal.h
index efb8fb2075..59375d3677 100644
--- a/libraries/SITL/SIM_Gimbal.h
+++ b/libraries/SITL/SIM_Gimbal.h
@@ -102,7 +102,12 @@ private:
         uint8_t seq;
     } mavlink;
 
+    uint32_t param_send_last_ms;
+    uint8_t param_send_idx;
+
     void send_report(void);
+    void param_send(const struct gimbal_param *p);
+    struct gimbal_param *param_find(const char *name);
 };
 
 }  // namespace SITL