backup

APMrover2/Rover.cpp

@@ -307,17 +307,27 @@ void Rover::one_second_loop(void)
     g2.wp_nav.set_turn_params(g.turn_max_g, g2.turn_radius, g2.motors.have_skid_steering());
 
     // gcs().send_text(MAV_SEVERITY_CRITICAL, "test");
-
+    // rover.g2.proximity.re_init();
+	static uint32_t prx_init_time = AP_HAL::millis();
+    static bool init_prx = false;
+    if (init_prx == false ) {
+        uint32_t tnow = AP_HAL::millis();
+        if(tnow - prx_init_time > 30000){
+            init_prx = true;
+            init_proximity();
+            gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%dms -> %dms ---",prx_init_time,tnow);
+        }
+    }
 
 #if DUAL_ANT_HEADING
-    static uint8_t prt_cnt;
-    prt_cnt++;
-    if(prt_cnt > 5 && gps.gps_heading_enable()){
-        prt_cnt = 0;
-        float heading = gps.gps_heading()*0.01f;
-        float gps_heading_rad = ToRad(heading);
-        gcs().send_text(MAV_SEVERITY_INFO, "[GNSS]:heading:%3.2f , heading_rad:%3.2f  ", heading, gps_heading_rad);
-    }
+    // static uint8_t prt_cnt;
+    // prt_cnt++;
+    // if(prt_cnt > 5 && gps.gps_heading_enable()){
+    //     prt_cnt = 0;
+    //     float heading = gps.gps_heading()*0.01f;
+    //     float gps_heading_rad = ToRad(heading);
+    //     gcs().send_text(MAV_SEVERITY_INFO, "[GNSS]:heading:%3.2f , heading_rad:%3.2f  ", heading, gps_heading_rad);
+    // }
 #endif
 
 #if DUAL_ANT_HEADING

APMrover2/system.cpp

@@ -99,7 +99,7 @@ void Rover::init_ardupilot()
     rangefinder.init(ROTATION_NONE);
 
     // init proximity sensor
-    init_proximity();
+    // init_proximity();
 
     // init beacons used for non-gps position estimation
     init_beacon();

APMrover2/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZR_Boat V4.0.3"
+#define THISFIRMWARE "ZR_Boat V4.0.3 RC2"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,3,FIRMWARE_VERSION_TYPE_DEV
@@ -20,5 +20,6 @@
  * @brief 更新说明
  * 
  * 4.0.3  去除声呐串口读取，使用uavcan，增加8、9通道控制relay 1、2
+ * 4.0.3 rc2 uavcan避障调整
  * 
  */

libraries/AP_Proximity/AP_Proximity.cpp

@@ -215,13 +215,15 @@ void AP_Proximity::init(void)
 // update Proximity state for all instances. This should be called at a high rate by the main loop
 void AP_Proximity::update(void)
 {
+    uint8_t cnt = 0;
     for (uint8_t i=0; i<num_instances; i++) {
         if (!valid_instance(i)) {
             continue;
         }
+        cnt += 1;
         drivers[i]->update();
     }
-
+    prx_init_cnt = cnt;
     // work out primary instance - first sensor returning good data
     for (int8_t i=num_instances-1; i>=0; i--) {
         if (drivers[i] != nullptr && (state[i].status == Status::Good)) {
@@ -230,6 +232,29 @@ void AP_Proximity::update(void)
     }
 }
 
+#include <GCS_MAVLink/GCS.h>
+bool AP_Proximity::re_init()
+{
+    bool ret = (prx_init_cnt != 0);
+    static uint32_t last_5s;
+    uint32_t tnow = AP_HAL::millis();
+    
+    if(!ret){
+        if (tnow - last_5s > 5000) {
+            last_5s = tnow;
+            init();
+            gcs().send_text(MAV_SEVERITY_NOTICE, "init [%d] prx: %ld",num_instances,tnow);
+        }
+        
+    }
+    if (tnow - last_5s > 5000) {
+            last_5s = tnow;
+            // init();
+            gcs().send_text(MAV_SEVERITY_NOTICE, " prx[%d] : %ld, %d",num_instances,tnow,ret);
+        }
+    return ret;
+}
+
 // return sensor orientation
 uint8_t AP_Proximity::get_orientation(uint8_t instance) const
 {

libraries/AP_Proximity/AP_Proximity.h

@@ -136,6 +136,7 @@ public:
     bool sensor_enabled() const;
     bool sensor_failed() const;
 
+    bool re_init();
 private:
     static AP_Proximity *_singleton;
     Proximity_State state[PROXIMITY_MAX_INSTANCES];
@@ -158,6 +159,8 @@ private:
     AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE];    // width of beam (in degrees) that should be ignored
 
     void detect_instance(uint8_t instance);
+    // bool prx_init;
+    uint8_t prx_init_cnt;
 };
 
 namespace AP {

libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp

@@ -8,6 +8,13 @@
 #include <AP_UAVCAN/AP_UAVCAN.h>
 
 #include <zrzk/equipment/range_sensor/Proximity.hpp>
+#define PRX_DEBUG 1
+#if PRX_DEBUG
+#include <GCS_MAVLink/GCS.h>
+    # define Prx_Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "prx-%d: " fmt "", __LINE__, ## args)
+#else
+    # define Prx_Debug(fmt, args ...)
+#endif
 
 extern const AP_HAL::HAL &hal;
 
@@ -16,18 +23,24 @@ extern const AP_HAL::HAL &hal;
     {                                                                      \
         if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) \
         {                                                                  \
-            printf(fmt, ##args);                                           \
+            hal.console->printf(fmt, ##args);                              \
         }                                                                  \
     } while (0)
 
 HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry;
 UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity);
+
+uint8_t AP_Proximity_UAVCAN::_node_id = 0;
+AP_Proximity_UAVCAN* AP_Proximity_UAVCAN::_driver = nullptr;
+AP_UAVCAN* AP_Proximity_UAVCAN::_ap_uavcan = nullptr;
+
 AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0};
 
 AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend,
                                          AP_Proximity::Proximity_State &_state) : 
                                          AP_Proximity_Backend(_frontend, _state)
 {
+    _driver = this;
 }
 
 void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
@@ -56,7 +69,16 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
     WITH_SEMAPHORE(_sem_registry);
 
     AP_Proximity_UAVCAN *backend = nullptr;
+    Prx_Debug("---------- AP_Proximity_UAVCAN ----------");
+    backend = new AP_Proximity_UAVCAN(_frontend, _state);
+    if (backend == nullptr)
+    {
+        Prx_Debug("backend:%d",(int)backend);
+    }else{
         
+        Prx_Debug("faild,backend");
+    }
+    /*
     for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
     {
         if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr)
@@ -64,6 +86,11 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
             backend = new AP_Proximity_UAVCAN(_frontend, _state);
             if (backend == nullptr)
             {
+                Prx_Debug("------index:%d, Failed register Node %d on Bus %d\n",
+                                        _detected_modules[i].ap_uavcan->get_driver_index(),
+                                        _detected_modules[i].node_id,
+                                       _detected_modules[i].ap_uavcan->get_driver_index());
+
                 debug_proximity_uavcan(2,
                                        _detected_modules[i].ap_uavcan->get_driver_index(),
                                        "Failed register UAVCAN Proximity Node %d on Bus %d\n",
@@ -72,6 +99,11 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
             }
             else
             {
+                Prx_Debug("-----index:%d, Registered Node %d on Bus %d\n",
+                                        _detected_modules[i].ap_uavcan->get_driver_index(),
+                                        _detected_modules[i].node_id,
+                                       _detected_modules[i].ap_uavcan->get_driver_index());
+
                 _detected_modules[i].driver = backend;
                 debug_proximity_uavcan(2,
                                        _detected_modules[i].ap_uavcan->get_driver_index(),
@@ -82,6 +114,8 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
             break;
         }
     }
+    */
+    Prx_Debug("---------- AP_Proximity_UAVCAN ----------\n");
     return backend;
 }
 
@@ -166,14 +200,37 @@ void AP_Proximity_UAVCAN::update(void)
 
 void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb)
 {
-    WITH_SEMAPHORE(_sem_registry);
     
+    if (_driver == nullptr) {
+        return;
+    }
+    
+    if (_ap_uavcan == nullptr) {
+        _ap_uavcan = ap_uavcan;
+        _node_id = node_id;
+        Prx_Debug("---- uavcan: ap:%d,id:%d ----",(int)_ap_uavcan,_node_id);
+    }
+    
+    if (_ap_uavcan == ap_uavcan && _node_id == node_id) {
+        WITH_SEMAPHORE(_sem_registry);
+            _driver->_interim_data.d1_cm = cb.msg->d0;
+            _driver->_interim_data.d2_cm = cb.msg->d45;
+            _driver->_interim_data.d3_cm = cb.msg->d90;
+            _driver->_interim_data.d4_cm = cb.msg->d135;
+            _driver->_interim_data.d5_cm = cb.msg->d180;
+            _driver->_interim_data.d6_cm = cb.msg->d225;
+            _driver->_interim_data.d7_cm = cb.msg->d270;
+            _driver->_interim_data.d8_cm = cb.msg->d315;
+            _driver->_last_distance_received_ms = AP_HAL::millis();
+    }
+    /*
     AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan,node_id);
     if (driver == nullptr)
     {
         return;
     }
     driver->handle_proximity_data(cb);
+    */
 }
 
 void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb)
@@ -207,13 +264,20 @@ void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb)
 
 void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
 {
+    
+    
     const uint8_t sector = convert_angle_to_sector(angle_deg);
+    // if(convert_angle_to_sector(angle_deg,sector)){
         _angle[sector] = angle_deg;
         _distance[sector] = ((float)distance_cm) / 1000;
-    _distance_valid[sector] = distance_cm != 0xffff;
+        // _distance_valid[sector] = distance_cm != 0xffff;
         //_last_distance_received_ms = AP_HAL::millis();
         // update boundary used for avoidance
+
+        _distance_valid[sector] = (_distance[sector] >= distance_min()) && (_distance[sector] <= distance_max());
+
         update_boundary_for_sector(sector, true);
+    // }
 }
 
 #endif

libraries/AP_Proximity/AP_Proximity_UAVCAN.h

@@ -30,6 +30,8 @@ public:
 
 private:
   static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id);
+  
+  static AP_Proximity_UAVCAN* _driver;
   // inline void register_sensor()
   // {
   //   //  instance = frontend.register_sensor();
@@ -61,5 +63,10 @@ private:
 
   static HAL_Semaphore _sem_registry;
   HAL_Semaphore _sem_proximity;
+  
+    static AP_UAVCAN* _ap_uavcan;
+    static uint8_t _node_id;
+
+
 };
 #endif //HAL_WITH_UAVCAN

zr-rover.sh

@@ -1,3 +1,3 @@
 ./waf configure --board zr-v4
 ./waf rover
-cp build/zr-v4/bin/ardurover.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/zrv4-rover.px4
+cp build/zr-v4/bin/ardurover.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/zrrover-4.0.3-rc2.px4