uavcan proximity 调整

ArduCopter/Parameters.cpp

@@ -1677,7 +1677,7 @@ const char* Copter::get_sysid_board_id(void)
      int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd;
     //  int8_t type = g.sysid_type;
      int8_t type;
-    if(ZR_FRAME_TYPE != 0){
+    if(g.sysid_type < 6){
         type = ZR_FRAME_TYPE;
     }else{
         type = g.sysid_type;
@@ -1712,6 +1712,9 @@ const char* Copter::get_sysid_board_id(void)
         case 8:
             snprintf(buf, sizeof(buf),  "Version: D100H-v%u.%u.%u ,ID: D100%04d%05d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)nameValue1,(int)nameValue2);
             break;
+        case 10:
+            snprintf(buf, sizeof(buf),  "Version: zr-v%u.%u.%u ,ID: ZRZK.19QT2.%d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)nameValue2);
+            break;
 
     }
     

ArduCopter/zr_flight.cpp

@@ -93,7 +93,7 @@ void Copter::zr_SuperSlowLoop(){
             if(tnow - prx_init_time > 5000){
                 init_prx = true;
                 init_proximity();
-                gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%dms -> %dms ---",prx_init_time,tnow);
+                // gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%dms -> %dms ---",prx_init_time,tnow);
             }
         }
     }
@@ -166,7 +166,7 @@ void Copter::do_avoid_action(){
     AP_Proximity *proximity = AP::proximity();
     if(proximity == nullptr)
         return;
-    bool get_new_dist = proximity->get_horizontal_distance(315,proxy_dist);
+    bool get_new_dist = proximity->get_horizontal_distance(0,proxy_dist);
     bool data_failed = proximity->sensor_failed();
     if(get_new_dist == false || data_failed){
         proxy_dist = avoid.get_zr_avd_max() + 1.0;

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)
@@ -130,7 +130,7 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
 
 float AP_Proximity_UAVCAN::distance_max() const
 {
-    return 40.0f;
+    return 60.0f;
 }
 
 float AP_Proximity_UAVCAN::distance_min() const
@@ -142,14 +142,18 @@ void AP_Proximity_UAVCAN::update(void)
 {
     WITH_SEMAPHORE(_sem_proximity);
 
+    // update_sector_data(0, _interim_data.d1_cm);   // d1
+    // update_sector_data(45, _interim_data.d2_cm);  // d8
+    // update_sector_data(90, _interim_data.d3_cm);  // d7
+    // update_sector_data(135, _interim_data.d4_cm); // d6
+    // update_sector_data(180, _interim_data.d5_cm); // d5
+    // update_sector_data(225, _interim_data.d6_cm); // d4
+    // update_sector_data(270, _interim_data.d7_cm); // d3
+    // update_sector_data(315, _interim_data.d8_cm); // d2
+
     update_sector_data(0, _interim_data.d1_cm);   // d1
     update_sector_data(45, _interim_data.d2_cm);  // d8
-    update_sector_data(90, _interim_data.d3_cm);  // d7
-    update_sector_data(135, _interim_data.d4_cm); // d6
-    update_sector_data(180, _interim_data.d5_cm); // d5
-    update_sector_data(225, _interim_data.d6_cm); // d4
-    update_sector_data(270, _interim_data.d7_cm); // d3
-    update_sector_data(315, _interim_data.d8_cm); // d2
+    update_sector_data(315, _interim_data.d3_cm);  // d7
 
     if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS))
     {