RangeFinder: add MAVLink rangefinder

libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.cpp

@@ -0,0 +1,74 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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_CompanionComputer.h"
+#include <AP_HAL/AP_HAL.h>
+
+
+
+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_CompanionComputer::AP_RangeFinder_CompanionComputer(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
+    AP_RangeFinder_Backend(_ranger, instance, _state)
+{
+    last_update_ms = AP_HAL::millis();
+    cc_distance_cm = 0;
+}
+
+/* 
+   detect if a CompanionComputer rangefinder is connected. We'll detect by
+   checking a paramter.
+*/
+bool AP_RangeFinder_CompanionComputer::detect(RangeFinder &_ranger, uint8_t instance)
+{
+    //Assume that if the user set the RANGEFINDER_TYPE parameter to CompanionComputer,
+    //there is an attached companion computer rangefinder
+    return true;
+}
+
+/*
+   Set the distance based on a MAVLINK message
+*/
+void AP_RangeFinder_CompanionComputer::handle_msg(mavlink_message_t *msg)
+{
+    mavlink_distance_sensor_t packet;
+    mavlink_msg_distance_sensor_decode(msg, &packet);
+
+    last_update_ms = AP_HAL::millis();
+    cc_distance_cm = packet.current_distance;
+}
+
+/* 
+   update the state of the sensor
+*/
+void AP_RangeFinder_CompanionComputer::update(void)
+{
+    //Time out on incoming data; if we don't get new
+    //data in 500ms, dump it
+    if(AP_HAL::millis() - last_update_ms > AP_RANGEFINDER_COMPANIONCOMPUTER_TIMEOUT_MS)
+    {
+        set_status(RangeFinder::RangeFinder_NoData);
+        state.distance_cm = 0;
+    } else {
+        set_status(RangeFinder::RangeFinder_Good);
+        state.distance_cm = cc_distance_cm;
+    }
+}

libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.h

@@ -0,0 +1,37 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#pragma once
+
+#include "RangeFinder.h"
+#include "RangeFinder_Backend.h"
+
+// min and max distances
+#define AP_RANGEFINDER_COMPANIONCOMPUTER_MIN_DISTANCE 0
+#define AP_RANGEFINDER_COMPANIONCOMPUTER_MAX_DISTANCE 1400
+
+// Data timeout
+#define AP_RANGEFINDER_COMPANIONCOMPUTER_TIMEOUT_MS 500
+
+class AP_RangeFinder_CompanionComputer : public AP_RangeFinder_Backend
+{
+
+public:
+    // constructor
+    AP_RangeFinder_CompanionComputer(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);
+
+    // Get update from mavlink
+    void handle_msg(mavlink_message_t *msg);
+
+private:
+    uint16_t cc_distance_cm;
+    uint32_t last_update_ms;
+
+    // start a reading
+    static bool start_reading(void);
+    static bool get_reading(uint16_t &reading_cm);
+};

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -24,13 +24,14 @@
 #include "AP_RangeFinder_LightWareI2C.h"
 #include "AP_RangeFinder_LightWareSerial.h"
 #include "AP_RangeFinder_Bebop.h"
+#include "AP_RangeFinder_CompanionComputer.h"
 
 // table of user settable parameters
 const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial
+    // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:CompanionComputer
     // @User: Standard
     AP_GROUPINFO("_TYPE",    0, RangeFinder, _type[0], 0),
 
@@ -538,6 +539,13 @@ void RangeFinder::detect_instance(uint8_t instance)
         }
     }
 #endif
+    if (type == RangeFinder_TYPE_CC) {
+        if (AP_RangeFinder_CompanionComputer::detect(*this, instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_RangeFinder_CompanionComputer(*this, instance, state[instance]);
+            return;
+        }
+    }
     if (type == RangeFinder_TYPE_ANALOG) {
         // note that analog must be the last to be checked, as it will
         // always come back as present if the pin is valid
@@ -564,6 +572,13 @@ RangeFinder::RangeFinder_Status RangeFinder::status(uint8_t instance) const
     return state[instance].status;
 }
 
+void RangeFinder::handle_msg(mavlink_message_t *msg) {
+  uint8_t i;
+  for (i=0; i<num_instances; i++)
+    if(_type[i] == RangeFinder_TYPE_CC)
+      drivers[i]->handle_msg(msg);
+}
+
 // true if sensor is returning data
 bool RangeFinder::has_data(uint8_t instance) const
 {

libraries/AP_RangeFinder/RangeFinder.h

@@ -47,7 +47,8 @@ public:
         RangeFinder_TYPE_BBB_PRU= 6,
         RangeFinder_TYPE_LWI2C  = 7,
         RangeFinder_TYPE_LWSER  = 8,
-        RangeFinder_TYPE_BEBOP = 9
+        RangeFinder_TYPE_BEBOP  = 9,
+        RangeFinder_TYPE_CC     = 10
     };
 
     enum RangeFinder_Function {
@@ -106,6 +107,9 @@ public:
     // 10Hz from main loop
     void update(void);
 
+    // Handle an incoming message (for companion computer)
+    void handle_msg(mavlink_message_t *msg);
+
 #define _RangeFinder_STATE(instance) state[instance]
 
     uint16_t distance_cm(uint8_t instance) const {

libraries/AP_RangeFinder/RangeFinder_Backend.h

@@ -37,6 +37,8 @@ public:
         return ranger._powersave_range > 0 && ranger.estimated_terrain_height > ranger._powersave_range;
     }
 
+    virtual void handle_msg(mavlink_message_t *msg) { return; };
+
 protected:
 
     // update status based on distance measurement