Backport PRX bug fixes and MR72 driver

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -1139,10 +1139,6 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
     }
 
     AP_Proximity &_proximity = *proximity;
-    // check for status of the sensor
-    if (_proximity.get_status() != AP_Proximity::Status::Good) {
-        return;
-    }
     // get total number of obstacles
     const uint8_t obstacle_num = _proximity.get_obstacle_count();
     if (obstacle_num == 0) {
@@ -1434,14 +1430,7 @@ void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_ne
         return;
     }
     AP_Proximity &_proximity = *proximity;
-
-    // exit immediately if proximity sensor is not present
-    if (_proximity.get_status() != AP_Proximity::Status::Good) {
-        return;
-    }
-
     const uint8_t obj_count = _proximity.get_object_count();
-
     // if no objects return
     if (obj_count == 0) {
         return;

libraries/AP_Arming/AP_Arming.cpp

@@ -1242,7 +1242,7 @@ bool AP_Arming::can_checks(bool report)
                 }
                 case AP_CAN::Protocol::USD1:
                 case AP_CAN::Protocol::TOFSenseP:
-                case AP_CAN::Protocol::NanoRadar_NRA24:
+                case AP_CAN::Protocol::NanoRadar:
                 case AP_CAN::Protocol::Benewake:
                 {
                     for (uint8_t j = i; j; j--) {

libraries/AP_CANManager/AP_CAN.h

@@ -28,6 +28,6 @@ class AP_CAN {
         Benewake = 11,
         Scripting2 = 12,
         TOFSenseP = 13,
-        NanoRadar_NRA24 = 14,
+        NanoRadar = 14,
     };
 };

libraries/AP_CANManager/AP_CANDriver.cpp

@@ -30,7 +30,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
     // @Param: PROTOCOL
     // @DisplayName: Enable use of specific protocol over virtual driver
     // @Description: Enabling this option starts selected protocol that will use this virtual driver
-    // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24
+    // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, float(AP_CAN::Protocol::DroneCAN)),
@@ -54,7 +54,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
     // @Param: PROTOCOL2
     // @DisplayName: Secondary protocol with 11 bit CAN addressing
     // @Description: Secondary protocol with 11 bit CAN addressing
-    // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24
+    // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("PROTOCOL2", 6, AP_CANManager::CANDriver_Params, _driver_type_11bit, float(AP_CAN::Protocol::None)),

libraries/AP_CANManager/AP_CANSensor.cpp

@@ -21,6 +21,7 @@
 
 #include <AP_Scheduler/AP_Scheduler.h>
 #include "AP_CANSensor.h"
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -190,5 +191,57 @@ void CANSensor::loop()
     }
 }
 
+MultiCAN::MultiCANLinkedList* MultiCAN::callbacks;
+
+MultiCAN::MultiCAN(ForwardCanFrame cf, AP_CAN::Protocol can_type, const char *driver_name) :
+        CANSensor(driver_name)
+{
+    if (callbacks == nullptr) {
+        callbacks = new MultiCANLinkedList();
+    }
+    if (callbacks == nullptr) {
+        AP_BoardConfig::allocation_error("Failed to create multican callback");
+    }
+
+    // Register new driver
+    register_driver(can_type);
+    callbacks->register_callback(cf);
+}
+
+// handle a received frame from the CAN bus
+void MultiCAN::handle_frame(AP_HAL::CANFrame &frame)
+{
+    if (callbacks != nullptr) {
+        callbacks->handle_frame(frame);
+    }
+
+}
+
+// register a callback for a CAN frame by adding it to the linked list
+void MultiCAN::MultiCANLinkedList::register_callback(ForwardCanFrame callback)
+{
+    CANSensor_Multi* newNode = new CANSensor_Multi();
+    if (newNode == nullptr) {
+        AP_BoardConfig::allocation_error("Failed to create multican node");
+    }
+    WITH_SEMAPHORE(sem);
+    {
+        newNode->_callback = callback;
+        newNode->next = head;
+        head = newNode;
+    }
+}
+
+// distribute the CAN frame to the registered callbacks
+void MultiCAN::MultiCANLinkedList::handle_frame(AP_HAL::CANFrame &frame)
+{
+    WITH_SEMAPHORE(sem);
+    for (CANSensor_Multi* current = head; current != nullptr; current = current->next) {
+        if (current->_callback(frame)) {
+            return;
+        }
+    }
+}
+
 #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS
 

libraries/AP_CANManager/AP_CANSensor.h

@@ -90,5 +90,41 @@ class CANSensor : public AP_CANDriver {
 #endif
 };
 
+// a class to allow for multiple CAN backends with one
+// CANSensor driver. This can be shared among different libraries like rangefinder and proximity
+class MultiCAN : public CANSensor {
+public:
+    // callback functor def for forwarding frames
+    FUNCTOR_TYPEDEF(ForwardCanFrame, bool, AP_HAL::CANFrame &);
+
+    MultiCAN(ForwardCanFrame cf, AP_CAN::Protocol can_type, const char *driver_name);
+
+    // handle a received frame from the CAN bus
+    void handle_frame(AP_HAL::CANFrame &frame) override;
+
+private:
+    // class to allow for multiple callbacks implemented as a linked list
+    class MultiCANLinkedList {
+    public:
+        struct CANSensor_Multi {
+            ForwardCanFrame _callback;
+            CANSensor_Multi* next = nullptr;
+        };
+
+        // register a callback for a CAN frame by adding it to the linked list
+        void register_callback(ForwardCanFrame callback);
+
+        // distribute the CAN frame to the registered callbacks
+        void handle_frame(AP_HAL::CANFrame &frame);
+        HAL_Semaphore sem;
+
+    private:
+        CANSensor_Multi* head = nullptr;
+    };
+
+    // Pointer to static instance of the linked list for persistence across instances
+    static MultiCANLinkedList* callbacks;
+};
+
 #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS
 

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]);
+        }
     }
 }
 
@@ -276,17 +308,19 @@ AP_Proximity::Status AP_Proximity::get_instance_status(uint8_t instance) const
     return state[instance].status;
 }
 
+// Returns status of first good sensor. If no good sensor found, returns status of last instance sensor
 AP_Proximity::Status AP_Proximity::get_status() const
 {
+    Status sensors_status = Status::NotConnected;
     for (uint8_t i=0; i<num_instances; i++) {
-        const Status sensors_status = get_instance_status(i);
-        if (sensors_status != Status::Good) {
-            // return first bad status
+        sensors_status = get_instance_status(i);
+        if (sensors_status == Status::Good) {
+            // return first good status
             return sensors_status;
         }
     }
-    // All valid sensors seem to be working
-    return Status::Good;
+    // no good sensor found
+    return sensors_status;
 }
 
 // return proximity backend for Lua scripting
@@ -427,7 +461,13 @@ bool AP_Proximity::sensor_enabled() const
 
 bool AP_Proximity::sensor_failed() const
 {
-    return get_status() != Status::Good;
+    for (uint8_t i=0; i<num_instances; i++) {
+        if (get_instance_status(i) != Status::Good) {
+            // return first bad status
+            return true;
+        }
+    }
+    return false;
 }
 
 // get distance in meters upwards, returns true on success

libraries/AP_Proximity/AP_Proximity.h

@@ -86,6 +86,9 @@ class AP_Proximity
 #endif
 #if AP_PROXIMITY_LD06_ENABLED
         LD06 = 16,
+#endif
+#if AP_PROXIMITY_MR72_ENABLED
+        MR72 = 17,
 #endif
     };
 
@@ -112,6 +115,8 @@ class AP_Proximity
 
     // return sensor health
     Status get_instance_status(uint8_t instance) const;
+
+    // Returns status of first good sensor. If no good sensor found, returns status of last instance sensor 
     Status get_status() const;
 
     // prearm checks
@@ -179,8 +184,12 @@ class AP_Proximity
     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[];