避障雷达uavcan ok

ArduCopter/APM_Config.h

@@ -58,11 +58,11 @@
 // Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below).
 //#define USERHOOK_VARIABLES "UserVariables.h"
 // Put your custom code into the UserCode.cpp with function names matching those listed below and ensure the appropriate #define below is uncommented below
-//#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
-//#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
-//#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
-//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
-//#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
-//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
-//#define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
+#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
+#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
+#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
+#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
+#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
+#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
+// #define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
 //#define USER_PARAMS_ENABLED ENABLED                       // to enable user parameters

ArduCopter/UserCode.cpp

@@ -40,6 +40,17 @@ void Copter::userhook_SlowLoop()
 void Copter::userhook_SuperSlowLoop()
 {
     // put your 1Hz code here
+    
+        // static bool init_prx = false;
+        // if (init_prx == false ) {
+        //     static uint32_t prx_init_time = AP_HAL::millis();
+        //     uint32_t tnow = AP_HAL::millis();
+        //     if(tnow - prx_init_time > 5000){
+        //         init_prx = true;
+        //         init_proximity();
+        //         gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%ldms -> %ldms ---",prx_init_time,tnow);
+        //     }
+        // }
 }
 #endif
 

libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp

@@ -8,7 +8,7 @@
 #include <AP_UAVCAN/AP_UAVCAN.h>
 
 #include <zrzk/equipment/range_sensor/Proximity.hpp>
-#define PRX_DEBUG 1
+#define PRX_DEBUG 0
 #if PRX_DEBUG
 #include <GCS_MAVLink/GCS.h>
     # define Prx_Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "prx-%d: " fmt "", __LINE__, ## args)
@@ -47,9 +47,11 @@ void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
 {
     if (ap_uavcan == nullptr)
     {
+        Prx_Debug("ap_uavcan:nullptr" );
         return;
     }
 
+        Prx_Debug("ap_uavcan:subscribe_msgs");
     auto *node = ap_uavcan->get_node();
 
     uavcan::Subscriber<zrzk::equipment::range_sensor::Proximity, ProximityCb> *proximity_listener;
@@ -71,7 +73,7 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
     AP_Proximity_UAVCAN *backend = nullptr;
     Prx_Debug("---------- AP_Proximity_UAVCAN ----------");
     backend = new AP_Proximity_UAVCAN(_frontend, _state);
-    if (backend == nullptr)
+    if (backend != nullptr)
     {
         Prx_Debug("backend:%d",(int)backend);
     }else{
@@ -89,7 +91,7 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
     {
         return nullptr;
     }
-
+    Prx_Debug("get_uavcan:%d",node_id);
     for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
     {
         if (_detected_modules[i].driver != nullptr &&
@@ -170,6 +172,8 @@ void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan,
 {
     
     if (_driver == nullptr) {
+        Prx_Debug("----nullptr : ap:%d,id:%d ----",(int)_ap_uavcan,_node_id);
+
         return;
     }
     
@@ -178,7 +182,7 @@ void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan,
         _node_id = node_id;
         Prx_Debug("---- uavcan: ap:%d,id:%d ----",(int)_ap_uavcan,_node_id);
     }
-    // Prx_Debug("handle_proximity: ap:%d,id:%d ,%d",(int)_ap_uavcan,_node_id,_driver->_interim_data.d1_cm);
+    Prx_Debug("handle_proximity: ap:%d,id:%d ,%d,addr:%d",(int)_ap_uavcan,_node_id,cb.msg->d0,cb.msg->sensor_id);
     if (_ap_uavcan == ap_uavcan && _node_id == node_id) {
         WITH_SEMAPHORE(_sem_registry);
             _driver->_interim_data.d1_cm = cb.msg->d0;

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -58,6 +58,7 @@
 #include <AP_Logger/AP_Logger.h>
 
 #include <zrzk/equipment/indication/ZrRGB.hpp>
+#include <AP_Proximity/AP_Proximity_UAVCAN.h>
 
 #define LED_DELAY_US 50000
 
@@ -276,6 +277,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     AP_Airspeed_UAVCAN::subscribe_msgs(this);
     AP_OpticalFlow_HereFlow::subscribe_msgs(this);
     AP_RangeFinder_UAVCAN::subscribe_msgs(this);
+    AP_Proximity_UAVCAN::subscribe_msgs(this);
 
     act_out_array[driver_index] = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*_node);
     act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));