AP_Proximity: Add support for CAN MR72

libraries/AP_Proximity/AP_Proximity.cpp

@@ -30,6 +30,8 @@
 #include "AP_Proximity_DroneCAN.h"
 #include "AP_Proximity_Scripting.h"
 #include "AP_Proximity_LD06.h"
+#include "AP_Proximity_MR72_CAN.h"
+
 
 #include <AP_Logger/AP_Logger.h>
 
@@ -81,27 +83,45 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
     // @Path: AP_Proximity_Params.cpp
     AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params),
 
+    // @Group: 1_
+    // @Path: AP_Proximity_MR72_CAN.cpp
+    AP_SUBGROUPVARPTR(drivers[0], "1_",  26, AP_Proximity, backend_var_info[0]),
+
 #if PROXIMITY_MAX_INSTANCES > 1
     // @Group: 2
     // @Path: AP_Proximity_Params.cpp
     AP_SUBGROUPINFO(params[1], "2", 22, AP_Proximity, AP_Proximity_Params),
+
+    // @Group: 2_
+    // @Path: AP_Proximity_MR72_CAN.cpp
+    AP_SUBGROUPVARPTR(drivers[1], "2_",  27, AP_Proximity, backend_var_info[1]),
 #endif
 
 #if PROXIMITY_MAX_INSTANCES > 2
     // @Group: 3
     // @Path: AP_Proximity_Params.cpp
     AP_SUBGROUPINFO(params[2], "3", 23, AP_Proximity, AP_Proximity_Params),
+
+    // @Group: 3_
+    // @Path: AP_Proximity_MR72_CAN.cpp
+    AP_SUBGROUPVARPTR(drivers[2], "3_",  28, AP_Proximity, backend_var_info[2]),
 #endif
 
 #if PROXIMITY_MAX_INSTANCES > 3
     // @Group: 4
     // @Path: AP_Proximity_Params.cpp
     AP_SUBGROUPINFO(params[3], "4", 24, AP_Proximity, AP_Proximity_Params),
+
+    // @Group: 4_
+    // @Path: AP_Proximity_MR72_CAN.cpp
+    AP_SUBGROUPVARPTR(drivers[3], "4_",  29, AP_Proximity, backend_var_info[3]),
 #endif
 
     AP_GROUPEND
 };
 
+const AP_Param::GroupInfo *AP_Proximity::backend_var_info[PROXIMITY_MAX_INSTANCES];
+
 AP_Proximity::AP_Proximity()
 {
     AP_Param::setup_object_defaults(this, var_info);
@@ -206,6 +226,12 @@ void AP_Proximity::init()
             drivers[instance] = new AP_Proximity_Scripting(*this, state[instance], params[instance]);
         break;
 #endif
+#if AP_PROXIMITY_MR72_ENABLED
+        case Type::MR72:
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_MR72_CAN(*this, state[instance], params[instance]);
+            break;
+# endif
 #if AP_PROXIMITY_SITL_ENABLED
         case Type::SITL:
             state[instance].instance = instance;
@@ -237,6 +263,12 @@ void AP_Proximity::init()
 
         // initialise status
         state[instance].status = Status::NotConnected;
+
+        // if the backend has some local parameters then make those available in the tree
+        if (drivers[instance] && state[instance].var_info) {
+            backend_var_info[instance] = state[instance].var_info;
+            AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]);
+        }
     }
 }
 

libraries/AP_Proximity/AP_Proximity.h

@@ -86,6 +86,9 @@ public:
 #endif
 #if AP_PROXIMITY_LD06_ENABLED
         LD06 = 16,
+#endif
+#if AP_PROXIMITY_MR72_ENABLED
+        MR72 = 17,
 #endif
     };
 
@@ -179,8 +182,12 @@ public:
     struct Proximity_State {
         uint8_t instance;   // the instance number of this proximity sensor
         Status status;      // sensor status
+
+        const struct AP_Param::GroupInfo *var_info; // stores extra parameter information for the sensor (if it exists)
     };
 
+    static const struct AP_Param::GroupInfo *backend_var_info[PROXIMITY_MAX_INSTANCES];
+
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 

libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp

@@ -0,0 +1,146 @@
+/*
+   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_Proximity_config.h"
+
+#if AP_PROXIMITY_MR72_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include "AP_Proximity_MR72_CAN.h"
+
+const AP_Param::GroupInfo AP_Proximity_MR72_CAN::var_info[] = {
+
+    // @Param: RECV_ID
+    // @DisplayName: CAN receive ID
+    // @Description: The receive ID of the CAN frames. A value of zero means all IDs are accepted.
+    // @Range: 0 65535
+    // @User: Advanced
+    AP_GROUPINFO("RECV_ID", 1, AP_Proximity_MR72_CAN, receive_id, 0),
+
+    AP_GROUPEND
+};
+
+MR72_MultiCAN *AP_Proximity_MR72_CAN::multican;
+
+AP_Proximity_MR72_CAN::AP_Proximity_MR72_CAN(AP_Proximity &_frontend,
+                                     AP_Proximity::Proximity_State &_state,
+                                     AP_Proximity_Params& _params):
+    AP_Proximity_Backend(_frontend, _state, _params)
+{
+    if (multican == nullptr) {
+        multican = new MR72_MultiCAN();
+        if (multican == nullptr) {
+            AP_BoardConfig::allocation_error("MR72_CAN");
+        }
+    }
+
+    {
+        // add to linked list of drivers
+        WITH_SEMAPHORE(multican->sem);
+        auto *prev = multican->drivers;
+        next = prev;
+        multican->drivers = this;
+    }
+
+    AP_Param::setup_object_defaults(this, var_info);
+    state.var_info = var_info;
+}
+
+// update state
+void AP_Proximity_MR72_CAN::update(void)
+{
+    WITH_SEMAPHORE(_sem);
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_update_ms > 500) {
+        // no new data.
+        set_status(AP_Proximity::Status::NoData);
+    } else {
+        set_status(AP_Proximity::Status::Good);
+    }
+}
+
+// handler for incoming frames. These come in at 100Hz
+bool AP_Proximity_MR72_CAN::handle_frame(AP_HAL::CANFrame &frame)
+{
+    WITH_SEMAPHORE(_sem);
+
+
+    // check if message is coming from the right sensor ID
+    const uint16_t id = frame.id;
+
+    if (receive_id > 0 && (get_radar_id(frame.id) != uint32_t(receive_id))) {
+        return false;
+    }
+
+    switch (id & 0xFU) {
+    case 0xAU:
+        // number of objects
+        _object_count = frame.data[0];
+        _current_object_index = 0;
+        _temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
+        _temp_boundary.reset();
+        last_update_ms = AP_HAL::millis();
+        break;
+    case 0xBU:
+        // obstacle data
+        parse_distance_message(frame);
+        break;
+    default:
+        break;
+    }
+
+    return true;
+
+}
+
+// parse a distance message from CAN frame
+bool AP_Proximity_MR72_CAN::parse_distance_message(AP_HAL::CANFrame &frame)
+{
+    if (_current_object_index >= _object_count) {
+        // should never happen
+        return false;
+    }
+    _current_object_index++;
+
+    Vector2f obstacle_fr;
+    // This parsing comes from the NanoRadar MR72 datasheet
+    obstacle_fr.x = ((frame.data[2] & 0x07U) * 256 + frame.data[3]) * 0.2 - 204.6;
+    obstacle_fr.y = (frame.data[1] * 32 + (frame.data[2] >> 3)) * 0.2 - 500;
+    const float yaw = correct_angle_for_orientation(wrap_360(degrees(atan2f(obstacle_fr.x, obstacle_fr.y))));
+
+    const float objects_dist = obstacle_fr.length();
+
+    if (ignore_reading(yaw, objects_dist)) {
+        // obstacle is probably near ground or out of range
+        return false;
+    }
+
+    const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw);
+    _temp_boundary.add_distance(face, yaw, objects_dist);
+    return true;
+}
+
+// handle frames from CANSensor, passing to the drivers
+void MR72_MultiCAN::handle_frame(AP_HAL::CANFrame &frame)
+{
+    WITH_SEMAPHORE(sem);
+    for (auto *d = drivers; d; d=d->next) {
+        if (d->handle_frame(frame)) {
+            break;
+        }
+    }
+}
+
+#endif // HAL_PROXIMITY_ENABLED

libraries/AP_Proximity/AP_Proximity_MR72_CAN.h

@@ -0,0 +1,68 @@
+#pragma once
+#include "AP_Proximity_config.h"
+
+#if AP_PROXIMITY_MR72_ENABLED
+
+#include "AP_Proximity.h"
+#include "AP_Proximity_Backend.h"
+#include <AP_HAL/AP_HAL.h>
+#include <AP_CANManager/AP_CANSensor.h>
+
+#define MR72_MAX_RANGE_M             50.0f   // max range of the sensor in meters
+#define MR72_MIN_RANGE_M             0.2f   // min range of the sensor in meters
+
+class MR72_MultiCAN;
+
+class AP_Proximity_MR72_CAN : public AP_Proximity_Backend {
+public:
+    friend class MR72_MultiCAN;
+
+    AP_Proximity_MR72_CAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params);
+
+    void update() override;
+
+    // handler for incoming frames. Return true if consumed
+    bool handle_frame(AP_HAL::CANFrame &frame);
+
+    // parse a distance message from CAN frame
+    bool parse_distance_message(AP_HAL::CANFrame &frame);
+
+    // get maximum and minimum distances (in meters) of sensor
+    float distance_max() const override { return MR72_MAX_RANGE_M; }
+    float distance_min() const override { return MR72_MIN_RANGE_M; }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+    AP_Proximity_Temp_Boundary _temp_boundary;
+
+private:
+
+    uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); }
+
+    uint32_t _object_count;             // total number of objects to read
+    uint32_t _current_object_index;     // current object index
+    uint32_t last_update_ms;            // last update time in ms
+
+    AP_Int32 receive_id;                // ID of the sensor
+
+    static MR72_MultiCAN *multican;     // linked list
+    AP_Proximity_MR72_CAN *next;
+};
+
+// a class to allow for multiple MR_72 backends with one
+// CANSensor driver
+class MR72_MultiCAN : public CANSensor {
+public:
+    MR72_MultiCAN() : CANSensor("MR72") {
+        register_driver(AP_CAN::Protocol::MR72);
+    }
+
+    // handler for incoming frames
+    void handle_frame(AP_HAL::CANFrame &frame) override;
+
+    HAL_Semaphore sem;
+    AP_Proximity_MR72_CAN *drivers;
+
+};
+
+#endif // HAL_PROXIMITY_ENABLED

libraries/AP_Proximity/AP_Proximity_Params.cpp

@@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Proximity type
     // @Description: What type of proximity sensor is connected
-    // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06
+    // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06, 17: MR72_CAN
     // @RebootRequired: True
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE",   1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE),

libraries/AP_Proximity/AP_Proximity_config.h

@@ -37,6 +37,11 @@
 #define AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
 #endif
 
+
+#ifndef AP_PROXIMITY_MR72_ENABLED
+#define AP_PROXIMITY_MR72_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_MAX_CAN_PROTOCOL_DRIVERS
+#endif
+
 #ifndef AP_PROXIMITY_SCRIPTING_ENABLED
 #define AP_PROXIMITY_SCRIPTING_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && AP_SCRIPTING_ENABLED
 #endif