[mavlink] parse PARAM_SET

sw/airborne/modules/datalink/mavlink.c

@@ -44,7 +44,10 @@
 mavlink_system_t mavlink_system;
 
 static uint8_t mavlink_params_idx = NB_SETTING; /**< Transmitting parameters index */
-static char mavlink_params[NB_SETTING][16] = SETTINGS; /**< Transmitting parameter names */
+/** mavlink parameter names.
+ * 16 chars + 1 NULL termination.
+ */
+static char mavlink_params[NB_SETTING][16+1] = SETTINGS;
 static uint8_t custom_version[8]; /**< first 8 bytes (16 chars) of GIT SHA1 */
 
 static inline void mavlink_send_heartbeat(void);
@@ -93,12 +96,36 @@ void mavlink_periodic(void)
   RunOnceEvery(32, mavlink_send_autopilot_version());
 }
 
+static int16_t settings_idx_from_param_id(char *param_id)
+{
+  int i, j;
+  int16_t settings_idx = -1;
+
+  // Go trough all the settings to search the ID
+  for (i = 0; i < NB_SETTING; i++) {
+    for (j = 0; j < 16; j++) {
+      if (mavlink_params[i][j] != param_id[j]) {
+        break;
+      }
+
+      if (mavlink_params[i][j] == '\0') {
+        settings_idx = i;
+        return settings_idx;
+      }
+    }
+
+    if (mavlink_params[i][j] == '\0') {
+      break;
+    }
+  }
+  return settings_idx;
+}
+
 /**
  * Event MAVLink calls
  */
 void mavlink_event(void)
 {
-  int i, j;
   mavlink_message_t msg;
   mavlink_status_t status;
 
@@ -151,24 +178,7 @@ void mavlink_event(void)
 
           // First check param_index and search for the ID if needed
           if (cmd.param_index == -1) {
-
-            // Go trough all the settings to search the ID
-            for (i = 0; i < NB_SETTING; i++) {
-              for (j = 0; j < 16; j++) {
-                if (mavlink_params[i][j] != cmd.param_id[j]) {
-                  break;
-                }
-
-                if (mavlink_params[i][j] == '\0') {
-                  cmd.param_index = i;
-                  break;
-                }
-              }
-
-              if (mavlink_params[i][j] == '\0') {
-                break;
-              }
-            }
+            cmd.param_index = settings_idx_from_param_id(cmd.param_id);
           }
 
           mavlink_msg_param_value_send(MAVLINK_COMM_0,
@@ -182,6 +192,35 @@ void mavlink_event(void)
           break;
         }
 
+        case MAVLINK_MSG_ID_PARAM_SET: {
+          mavlink_param_set_t set;
+          mavlink_msg_param_set_decode(&msg, &set);
+
+          // Check if this message is for this system
+          if ((uint8_t) set.target_system == AC_ID) {
+            int16_t idx = settings_idx_from_param_id(set.param_id);
+
+            // setting found
+            if (idx >= 0) {
+              // Only write if new value is NOT "not-a-number"
+              // AND is NOT infinity
+              if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
+                  !isnan(set.param_value) && !isinf(set.param_value)) {
+                DlSetting(idx, set.param_value);
+                // Report back new value
+                mavlink_msg_param_value_send(MAVLINK_COMM_0,
+                                             mavlink_params[idx],
+                                             settings_get_value(idx),
+                                             MAV_PARAM_TYPE_REAL32,
+                                             NB_SETTING,
+                                             idx);
+                MAVLink(SendMessage());
+              }
+            }
+          }
+        }
+          break;
+
         default:
           //Do nothing
           //printf("Received message with id: %d\r\n", msg.msgid);