replay-rangefinder.patch

From 4f3240878abf7e5bbd5750e5d204fb598589cc05 Mon Sep 17 00:00:00 2001
From: Reinis Veips <reinis.veips@XXX>
Date: Tue, 29 May 2018 16:30:21 +0300
Subject: [PATCH 1/1] Use rangefinder in EKF Replay

---
 Tools/Replay/LR_MsgHandler.cpp                  |  6 +++
 Tools/Replay/LR_MsgHandler.h                    | 14 +++++++
 Tools/Replay/LogReader.cpp                      |  9 ++++-
 Tools/Replay/LogReader.h                        |  3 +-
 Tools/Replay/Parameters.h                       |  1 +
 Tools/Replay/Replay.cpp                         |  7 ++++
 Tools/Replay/Replay.h                           |  4 +-
 libraries/AP_RangeFinder/AP_RangeFinder_HIL.cpp | 53 +++++++++++++++++++++++++
 libraries/AP_RangeFinder/AP_RangeFinder_HIL.h   | 19 +++++++++
 libraries/AP_RangeFinder/RangeFinder.cpp        | 26 +++++++++++-
 libraries/AP_RangeFinder/RangeFinder.h          |  8 ++++
 11 files changed, 144 insertions(+), 6 deletions(-)
 create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_HIL.cpp
 create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_HIL.h

diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp
index ac27f39..1bb097e 100644
--- a/Tools/Replay/LR_MsgHandler.cpp
+++ b/Tools/Replay/LR_MsgHandler.cpp
@@ -113,6 +113,12 @@ void LR_MsgHandler_BARO::process_message(uint8_t *msg)
                 last_update_ms);
 }
 
+void LR_MsgHandler_RFND::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+    _rng.set_hil_distance(0, require_field_uint16_t(msg, "Dist1"));
+    _rng.set_hil_distance(1, require_field_uint16_t(msg, "Dist2"));
+}
 
 #define DATA_ARMED                          10
 #define DATA_DISARMED                       11
diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h
index 64d7e3d..4295036 100644
--- a/Tools/Replay/LR_MsgHandler.h
+++ b/Tools/Replay/LR_MsgHandler.h
@@ -121,6 +121,20 @@ private:
     AP_Baro &baro;
 };
 
+class LR_MsgHandler_RFND : public LR_MsgHandler
+{
+public:
+    LR_MsgHandler_RFND(log_Format &_f, DataFlash_Class &_dataflash,
+                    uint64_t &_last_timestamp_usec, RangeFinder &rng)
+        : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), _rng(rng) { };
+
+    virtual void process_message(uint8_t *msg);
+
+private:
+    RangeFinder &_rng;
+};
+
+
 
 class LR_MsgHandler_Event : public LR_MsgHandler
 {
diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp
index 3918df0..53ed62f 100644
--- a/Tools/Replay/LogReader.cpp
+++ b/Tools/Replay/LogReader.cpp
@@ -35,12 +35,13 @@ const struct LogStructure log_structure[] = {
       "CHEK", "QccCLLffff",  "TimeUS,Roll,Pitch,Yaw,Lat,Lng,Alt,VN,VE,VD" }
 };
 
-LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, 
+LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, RangeFinder& _rng, Compass &_compass, AP_GPS &_gps, 
                      AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&_nottypes):
     vehicle(VehicleType::VEHICLE_UNKNOWN),
     ahrs(_ahrs),
     ins(_ins),
     baro(_baro),
+    rng(_rng),
     compass(_compass),
     gps(_gps),
     airspeed(_airspeed),
@@ -226,7 +227,11 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
 	} else if (streq(name, "BARO")) {
 	  msgparser[f.type] = new LR_MsgHandler_BARO(formats[f.type], dataflash,
                                                   last_timestamp_usec, baro);
-	} else if (streq(name, "ARM")) {
+                                                  
+	}else if (streq(name, "RFND")) {
+	  msgparser[f.type] = new LR_MsgHandler_RFND(formats[f.type], dataflash,
+                                                  last_timestamp_usec, rng);
+    } else if (streq(name, "ARM")) {
 	  msgparser[f.type] = new LR_MsgHandler_ARM(formats[f.type], dataflash,
                                                   last_timestamp_usec);
 	} else if (streq(name, "EV")) {
diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h
index 9fabe73..1c840a7 100644
--- a/Tools/Replay/LogReader.h
+++ b/Tools/Replay/LogReader.h
@@ -6,7 +6,7 @@
 class LogReader : public DataFlashFileReader
 {
 public:
-    LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&nottypes);
+    LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, RangeFinder &_rng, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&nottypes);
     bool wait_type(const char *type);
 
     const Vector3f &get_attitude(void) const { return attitude; }
@@ -38,6 +38,7 @@ private:
     AP_AHRS &ahrs;
     AP_InertialSensor &ins;
     AP_Baro &baro;
+    RangeFinder &rng;
     Compass &compass;
     AP_GPS &gps;
     AP_Airspeed &airspeed;
diff --git a/Tools/Replay/Parameters.h b/Tools/Replay/Parameters.h
index af4c263..598e402 100644
--- a/Tools/Replay/Parameters.h
+++ b/Tools/Replay/Parameters.h
@@ -9,6 +9,7 @@ public:
     enum {
         k_param_dummy,
         k_param_barometer,
+        k_param_rng,
         k_param_ins,
         k_param_ahrs,
         k_param_airspeed,
diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp
index f16c265..1a43500 100644
--- a/Tools/Replay/Replay.cpp
+++ b/Tools/Replay/Replay.cpp
@@ -53,6 +53,10 @@ const AP_Param::Info ReplayVehicle::var_info[] = {
     // @Path: ../libraries/AP_Baro/AP_Baro.cpp
     GOBJECT(barometer, "GND_", AP_Baro),
 
+    // @Group: RNGFND
+    // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
+    GOBJECT(rng, "RNGFND", RangeFinder),
+
     // @Group: INS_
     // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
     GOBJECT(ins,                    "INS_", AP_InertialSensor),
@@ -81,6 +85,7 @@ const AP_Param::Info ReplayVehicle::var_info[] = {
     // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
     GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3),
 
+
     AP_VAREND
 };
 
@@ -132,6 +137,8 @@ void ReplayVehicle::setup(void)
     printf("Starting disarmed\n");
     hal.util->set_soft_armed(false);
 
+    rng.set_hil_mode(true);
+    rng.init();
     barometer.init();
     barometer.setHIL(0);
     barometer.update();
diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h
index 03ec16c..243ae70 100644
--- a/Tools/Replay/Replay.h
+++ b/Tools/Replay/Replay.h
@@ -63,7 +63,7 @@ public:
     Compass compass;
     AP_SerialManager serial_manager;
     RangeFinder rng {serial_manager, ROTATION_PITCH_270};
-    NavEKF2 EKF2{&ahrs, rng};
+    NavEKF2 EKF2{&ahrs, barometer, rng};
     NavEKF3 EKF3{&ahrs, barometer, rng};
     AP_AHRS_NavEKF ahrs {ins, barometer, gps, rng, EKF2, EKF3};
     AP_InertialNav_NavEKF inertial_nav{ahrs};
@@ -118,7 +118,7 @@ private:
     SITL::SITL sitl;
 #endif
 
-    LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.barometer, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, nottypes};
+    LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.barometer, _vehicle.rng, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, nottypes};
 
     FILE *ekf1f;
     FILE *ekf2f;
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_HIL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_HIL.cpp
new file mode 100644
index 0000000..6dacc5e
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_HIL.cpp
@@ -0,0 +1,53 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *   AP_RangeFinder_HIL.cpp - rangefinder for HIL mode
+ *
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include "RangeFinder.h"
+#include "AP_RangeFinder_HIL.h"
+
+extern const AP_HAL::HAL& hal;
+
+/* 
+   HIL rangefinder
+*/
+AP_RangeFinder_HIL::AP_RangeFinder_HIL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
+    AP_RangeFinder_Backend(_ranger, instance, _state)
+{
+    set_status(RangeFinder::RangeFinder_NoData);
+}
+
+/* 
+   HIL rangefinder is always connected
+*/
+bool AP_RangeFinder_HIL::detect(RangeFinder &_ranger, uint8_t instance)
+{
+    return true;
+}
+
+/*
+  update distance_cm (does nothing as it's already being done in frontend)
+ */
+void AP_RangeFinder_HIL::update(void)
+{
+    update_status();
+}
+
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_HIL.h b/libraries/AP_RangeFinder/AP_RangeFinder_HIL.h
new file mode 100644
index 0000000..ffe5b59
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_HIL.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "RangeFinder.h"
+#include "RangeFinder_Backend.h"
+
+class AP_RangeFinder_HIL : public AP_RangeFinder_Backend
+{
+public:
+    // constructor
+    AP_RangeFinder_HIL(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
+
+    // static detection function
+    static bool detect(RangeFinder &ranger, uint8_t instance);
+
+    // update state
+    void update(void);
+
+private:
+};
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 1f02a0c..85c8029 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -29,7 +29,9 @@
 #include "AP_RangeFinder_trone.h"
 #include "AP_RangeFinder_L10.h"
 #include "AP_RangeFinder_LRM512.h"
+#include "AP_RangeFinder_HIL.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <stdio.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -529,7 +531,8 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
 RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default) :
     num_instances(0),
     estimated_terrain_height(0),
-    serial_manager(_serial_manager)
+    serial_manager(_serial_manager),
+    _hil_mode(false)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -610,6 +613,15 @@ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
  */
 void RangeFinder::detect_instance(uint8_t instance)
 {
+
+    if (_hil_mode) {
+        if (AP_RangeFinder_HIL::detect(*this, instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_RangeFinder_HIL(*this, instance, state[instance]);
+        }
+        return;
+    }
+
     enum RangeFinder_Type type = (enum RangeFinder_Type)_type[instance].get();
     switch (type) {
     case RangeFinder_TYPE_PLI2C:
@@ -889,3 +901,15 @@ const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) co
     }
     return pos_offset_zero;
 }
+
+void RangeFinder::set_hil_distance(uint8_t instance, float dist_cm)
+{
+
+    if (instance >= num_instances || drivers[instance] == nullptr) {
+        ::printf("rangefinder instance out of range\n");
+        return;
+    }
+
+    state[instance].distance_cm = dist_cm;
+    drivers[instance]->update();
+}
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index 43f65d2..d2205ae 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -55,6 +55,7 @@ public:
         RangeFinder_TYPE_PLI2CV3= 15,
 		RangeFinder_TYPE_L10= 16,
 		RangeFinder_TYPE_LRM512= 17,
+        RangeFinder_TYPE_HIL= 99,
     };
 
     enum RangeFinder_Function {
@@ -191,6 +192,12 @@ public:
     }
     const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
 
+    void set_hil_mode(bool hil_mode) {
+        _hil_mode = hil_mode;
+    }
+    
+    void set_hil_distance(uint8_t instance, float distance_cm);
+
 private:
     RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
     AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
@@ -204,4 +211,5 @@ private:
 
     void update_pre_arm_check(uint8_t instance);
     bool _add_backend(AP_RangeFinder_Backend *driver);
+    bool _hil_mode;
 };
-- 
2.7.4