AP_Proximity: add MAV type

This accepts DISTANCE_SENSOR messages from eight sectors around the vehicle

libraries/AP_Proximity/AP_Proximity.cpp

@@ -15,6 +15,7 @@
 
 #include "AP_Proximity.h"
 #include "AP_Proximity_LightWareSF40C.h"
+#include "AP_Proximity_MAV.h"
 #include "AP_Proximity_SITL.h"
 
 extern const AP_HAL::HAL &hal;
@@ -252,6 +253,11 @@ void AP_Proximity::detect_instance(uint8_t instance)
             return;
         }
     }
+    if (type == Proximity_Type_MAV) {
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_MAV(*this, state[instance]);
+        return;
+    }
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     if (type == Proximity_Type_SITL) {
         state[instance].instance = instance;

libraries/AP_Proximity/AP_Proximity.h

@@ -38,6 +38,7 @@ public:
     enum Proximity_Type {
         Proximity_Type_None  = 0,
         Proximity_Type_SF40C = 1,
+        Proximity_Type_MAV   = 2,
         Proximity_Type_SITL  = 10,
     };
 

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -0,0 +1,62 @@
+/*
+   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_HAL/AP_HAL.h>
+#include "AP_Proximity_MAV.h"
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <ctype.h>
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+/* 
+   The constructor also initialises the proximity sensor. Note that this
+   constructor is not called until detect() returns true, so we
+   already know that we should setup the proximity sensor
+*/
+AP_Proximity_MAV::AP_Proximity_MAV(AP_Proximity &_frontend,
+                                   AP_Proximity::Proximity_State &_state) :
+    AP_Proximity_Backend(_frontend, _state)
+{
+}
+
+// update the state of the sensor
+void AP_Proximity_MAV::update(void)
+{
+    // check for timeout and set health status
+    if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_MAV_TIMEOUT_MS)) {
+        set_status(AP_Proximity::Proximity_NoData);
+    } else {
+        set_status(AP_Proximity::Proximity_Good);
+    }
+}
+
+// handle mavlink DISTANCE_SENSOR messages
+void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
+{
+    mavlink_distance_sensor_t packet;
+    mavlink_msg_distance_sensor_decode(msg, &packet);
+
+    // store distance to appropriate sector based on orientation field
+    if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
+        uint8_t sector = packet.orientation;
+        _angle[sector] = sector * 45;
+        _distance[sector] = packet.current_distance / 100.0f;
+        _distance_valid[sector] = true;
+        _distance_min = packet.min_distance / 100.0f;
+        _distance_max = packet.max_distance / 100.0f;
+        _last_update_ms = AP_HAL::millis();
+    }
+}

libraries/AP_Proximity/AP_Proximity_MAV.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "AP_Proximity.h"
+#include "AP_Proximity_Backend.h"
+
+#define PROXIMITY_MAV_TIMEOUT_MS    200 // requests timeout after 0.2 seconds
+
+class AP_Proximity_MAV : public AP_Proximity_Backend
+{
+
+public:
+    // constructor
+    AP_Proximity_MAV(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
+
+    // update state
+    void update(void);
+
+    // get maximum and minimum distances (in meters) of sensor
+    float distance_max() const { return _distance_max; }
+    float distance_min() const { return _distance_min; };
+
+    // handle mavlink DISTANCE_SENSOR messages
+    void handle_msg(mavlink_message_t *msg);
+
+private:
+
+    // initialise sensor (returns true if sensor is succesfully initialised)
+    bool initialise();
+
+    uint32_t _last_update_ms;   // system time of last DISTANCE_SENSOR message received
+    float _distance_max;        // max range of sensor in meters
+    float _distance_min;        // min range of sensor in meters
+};