Tools: Replay: pass a lambda function to LR_MsgHandler_PARM to set parameters

This avoid duplication of parameter setting logic.

Tools/Replay/LR_MsgHandler.cpp

@@ -379,7 +379,7 @@ void LR_MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
 }
 
 
-bool LR_MsgHandler::set_parameter(const char *name, float value)
+bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
 {
     const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_TYPE", "EK2_ENABLE", "EK3_ENABLE"
                                    "COMPASS_ORIENT", "COMPASS_ORIENT2",
@@ -390,32 +390,8 @@ bool LR_MsgHandler::set_parameter(const char *name, float value)
             return true;
         }
     }
-    enum ap_var_type var_type;
+
-    AP_Param *vp = AP_Param::find(name, &var_type);
+    return _set_parameter_callback(name, value);
-    if (vp == NULL) {
-        return false;
-    }
-    float old_value = 0;
-    if (var_type == AP_PARAM_FLOAT) {
-        old_value = ((AP_Float *)vp)->cast_to_float();
-        ((AP_Float *)vp)->set(value);
-    } else if (var_type == AP_PARAM_INT32) {
-        old_value = ((AP_Int32 *)vp)->cast_to_float();
-        ((AP_Int32 *)vp)->set(value);
-    } else if (var_type == AP_PARAM_INT16) {
-        old_value = ((AP_Int16 *)vp)->cast_to_float();
-        ((AP_Int16 *)vp)->set(value);
-    } else if (var_type == AP_PARAM_INT8) {
-        old_value = ((AP_Int8 *)vp)->cast_to_float();
-        ((AP_Int8 *)vp)->set(value);
-    } else {
-        // we don't support mavlink set on this parameter
-        return false;
-    }
-    if (fabsf(old_value - value) > 1.0e-12) {
-        ::printf("Changed %s to %.8f from %.8f\n", name, value, old_value);
-    }
-    return true;
 }
 
 void LR_MsgHandler_PARM::process_message(uint8_t *msg)

Tools/Replay/LR_MsgHandler.h

@@ -2,13 +2,14 @@
 
 #include "MsgHandler.h"
 
+#include <functional>
+
 class LR_MsgHandler : public MsgHandler {
 public:
     LR_MsgHandler(struct log_Format &f,
                   DataFlash_Class &_dataflash,
                   uint64_t &last_timestamp_usec);
     virtual void process_message(uint8_t *msg) = 0;
-    bool set_parameter(const char *name, float value);
 
     // state for CHEK message
     struct CheckState {
@@ -430,9 +431,18 @@ private:
 class LR_MsgHandler_PARM : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t &_last_timestamp_usec) : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) {};
+    LR_MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash,
+                       uint64_t &_last_timestamp_usec,
+                       const std::function<bool(const char *name, const float)>&set_parameter_callback) :
+        LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
+        _set_parameter_callback(set_parameter_callback)
+        {};
 
     virtual void process_message(uint8_t *msg);
+
+private:
+    bool set_parameter(const char *name, const float value);
+    const std::function<bool(const char *name, const float)>_set_parameter_callback;
 };
 
 class LR_MsgHandler_PM : public LR_MsgHandler

Tools/Replay/LogReader.cpp

@@ -85,8 +85,6 @@ void LogReader::maybe_install_vehicle_specific_parsers() {
     }
 }
 
-LR_MsgHandler_PARM *parameter_handler;
-
 /*
   messages which we will be generating, so should be discarded
  */
@@ -171,9 +169,12 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
 
 	// map from format name to a parser subclass:
 	if (streq(name, "PARM")) {
-            parameter_handler = new LR_MsgHandler_PARM(formats[f.type], dataflash,
+            msgparser[f.type] = new LR_MsgHandler_PARM
-                                                    last_timestamp_usec);
+                (formats[f.type], dataflash,
-	    msgparser[f.type] = parameter_handler;
+                 last_timestamp_usec,
+                 [this](const char *xname, const float xvalue) {
+                    return set_parameter(xname, xvalue);
+                 });
 	} else if (streq(name, "GPS")) {
 	    msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type],
                                                       dataflash,
@@ -323,14 +324,34 @@ bool LogReader::wait_type(const char *wtype)
     return true;
 }
 
-
 bool LogReader::set_parameter(const char *name, float value)
 {
-    if (parameter_handler == NULL) {
+    enum ap_var_type var_type;
-        ::printf("No parameter format message found");
+    AP_Param *vp = AP_Param::find(name, &var_type);
+    if (vp == NULL) {
         return false;
     }
-    return parameter_handler->set_parameter(name, value);
+    float old_value = 0;
+    if (var_type == AP_PARAM_FLOAT) {
+        old_value = ((AP_Float *)vp)->cast_to_float();
+        ((AP_Float *)vp)->set(value);
+    } else if (var_type == AP_PARAM_INT32) {
+        old_value = ((AP_Int32 *)vp)->cast_to_float();
+        ((AP_Int32 *)vp)->set(value);
+    } else if (var_type == AP_PARAM_INT16) {
+        old_value = ((AP_Int16 *)vp)->cast_to_float();
+        ((AP_Int16 *)vp)->set(value);
+    } else if (var_type == AP_PARAM_INT8) {
+        old_value = ((AP_Int8 *)vp)->cast_to_float();
+        ((AP_Int8 *)vp)->set(value);
+    } else {
+        // we don't support mavlink set on this parameter
+        return false;
+    }
+    if (fabsf(old_value - value) > 1.0e-12) {
+        ::printf("Changed %s to %.8f from %.8f\n", name, value, old_value);
+    }
+    return true;
 }
 
 /*

Tools/Replay/Replay.cpp

@@ -558,17 +558,10 @@ void Replay::set_ins_update_rate(uint16_t _update_rate) {
 }
 
 void Replay::inhibit_gyro_cal() {
-    // swiped from LR_MsgHandler.cpp; until we see PARM messages, we
+    if (!logreader.set_parameter("INS_GYR_CAL", AP_InertialSensor::GYRO_CAL_NEVER)) {
-    // don't have a PARM handler available to set parameters.
+        ::fprintf(stderr, "Failed to set GYR_CAL parameter\n");
-    enum ap_var_type var_type;
-    AP_Param *vp = AP_Param::find("INS_GYR_CAL", &var_type);
-    if (vp == NULL) {
-        ::fprintf(stderr, "No GYR_CAL parameter found\n");
         abort();
     }
-    ((AP_Float *)vp)->set(AP_InertialSensor::GYRO_CAL_NEVER);
-
-    // logreader.set_parameter("GYR_CAL", AP_InertialSensor::GYRO_CAL_NEVER);
 }
 
 /*