AP_RangeFinder: added support for MSP based range finders

libraries/AP_BLHeli/AP_BLHeli.cpp

@@ -448,8 +448,8 @@ void AP_BLHeli::msp_process_command(void)
         break;
     }
 
-    case MSP_SET_4WAY_IF: {
-        debug("MSP_SET_4WAY_IF");
+    case MSP_SET_PASSTHROUGH: {
+        debug("MSP_SET_PASSTHROUGH");
         if (msp.dataSize == 0) {
             msp.escMode = PROTOCOL_4WAY;
         } else if (msp.dataSize == 2) {

libraries/AP_BLHeli/AP_BLHeli.h

@@ -30,7 +30,7 @@
 
 #include <AP_Param/AP_Param.h>
 #include <Filter/LowPassFilter.h>
-#include "msp_protocol.h"
+#include <AP_MSP/msp_protocol.h>
 #include "blheli_4way_protocol.h"
 
 #define AP_BLHELI_MAX_ESCS 8

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -46,6 +46,7 @@
 #include "AP_RangeFinder_Lanbao.h"
 #include "AP_RangeFinder_LeddarVu8.h"
 #include "AP_RangeFinder_SITL.h"
+#include "AP_RangeFinder_MSP.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -541,6 +542,14 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         break;
 #endif
 
+#if HAL_MSP_ENABLED
+    case Type::MSP:
+        if (AP_RangeFinder_MSP::detect()) {
+            drivers[instance] = new AP_RangeFinder_MSP(state[instance], params[instance]);
+        }
+        break;
+#endif //HAL_MSP_ENABLED
+
     default:
         break;
     }
@@ -587,6 +596,18 @@ void RangeFinder::handle_msg(const mavlink_message_t &msg)
     }
 }
 
+#if HAL_MSP_ENABLED
+void RangeFinder::handle_msp(const MSP::msp_rangefinder_sensor_t &pkt)
+{
+    uint8_t i;
+    for (i=0; i<num_instances; i++) {
+        if ((drivers[i] != nullptr) && ((Type)params[i].type.get() == Type::MSP)) {
+          drivers[i]->handle_msp(pkt);
+        }
+    }
+}
+#endif //HAL_MSP_ENABLED
+
 // return true if we have a range finder with the specified orientation
 bool RangeFinder::has_orientation(enum Rotation orientation) const
 {

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -18,6 +18,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_MSP/msp.h>
 #include "AP_RangeFinder_Params.h"
 
 // Maximum number of range finder instances available on this platform
@@ -81,6 +82,7 @@ public:
         LeddarVu8_Serial = 29,
         HC_SR04 = 30,
         GYUS42v2 = 31,
+        MSP = 32,
         SITL = 100,
     };
 
@@ -134,6 +136,10 @@ public:
     // Handle an incoming DISTANCE_SENSOR message (from a MAVLink enabled range finder)
     void handle_msg(const mavlink_message_t &msg);
 
+#if HAL_MSP_ENABLED
+    // Handle an incoming DISTANCE_SENSOR message (from a MSP enabled range finder)
+    void handle_msp(const MSP::msp_rangefinder_sensor_t &pkt);
+#endif //HAL_MSP_ENABLED
     // return true if we have a range finder with the specified orientation
     bool has_orientation(enum Rotation orientation) const;
 

libraries/AP_RangeFinder/AP_RangeFinder_Backend.h

@@ -32,6 +32,7 @@ public:
     virtual void update() = 0;
 
     virtual void handle_msg(const mavlink_message_t &msg) { return; }
+    virtual void handle_msp(const MSP::msp_rangefinder_sensor_t &pkt) { return; }
 
     enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
     uint16_t distance_cm() const { return state.distance_cm; }

libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp

@@ -0,0 +1,71 @@
+/*
+   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/>.
+ */
+
+#include "AP_RangeFinder_MSP.h"
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_MSP_ENABLED
+
+extern const AP_HAL::HAL& hal;
+
+/*
+   The constructor also initialises the rangefinder. Note that this
+   constructor is not called until detect() returns true, so we
+   already know that we should setup the rangefinder
+*/
+AP_RangeFinder_MSP::AP_RangeFinder_MSP(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
+    AP_RangeFinder_Backend(_state, _params)
+{
+    state.last_reading_ms = AP_HAL::millis();
+    distance_cm = 0;
+}
+
+/*
+   detect if a MSP rangefinder is connected. We'll detect by
+   checking a parameter.
+*/
+bool AP_RangeFinder_MSP::detect()
+{
+    // Assume that if the user set the RANGEFINDER_TYPE parameter to MSP,
+    // there is an attached MSP rangefinder
+    return true;
+}
+
+/*
+   Set the distance based on a MSP message
+*/
+void AP_RangeFinder_MSP::handle_msp(const MSP::msp_rangefinder_sensor_t &pkt)
+{
+    state.last_reading_ms = AP_HAL::millis();
+    distance_cm = pkt.distance_mm / 10;
+}
+
+/*
+   update the state of the sensor
+*/
+void AP_RangeFinder_MSP::update(void)
+{
+    //Time out on incoming data; if we don't get new
+    //data in 500ms, dump it
+    if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MSP_TIMEOUT_MS) {
+        set_status(RangeFinder::Status::NoData);
+        state.distance_cm = 0;
+    } else {
+        state.distance_cm = distance_cm;
+        update_status();
+    }
+}
+
+#endif //HAL_MSP_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_MSP.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend.h"
+
+#if HAL_MSP_ENABLED
+
+// Data timeout
+#define AP_RANGEFINDER_MSP_TIMEOUT_MS 500
+
+class AP_RangeFinder_MSP : public AP_RangeFinder_Backend
+{
+
+public:
+    // constructor
+    AP_RangeFinder_MSP(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
+
+    // static detection function
+    static bool detect();
+
+    // update state
+    void update(void) override;
+
+    // Get update from msp
+    void handle_msp(const MSP::msp_rangefinder_sensor_t &pkt) override;
+
+protected:
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_UNKNOWN;
+    }
+
+private:
+    uint16_t distance_cm;
+
+    // start a reading
+    static bool start_reading(void);
+    static bool get_reading(uint16_t &reading_cm);
+};
+
+#endif  //HAL_MSP_ENABLED