一些打印消息调整，避障增加超范围无效

ArduCopter/GCS_Mavlink.cpp

@@ -584,7 +584,7 @@ void GCS_MAVLINK_Copter::send_banner()
     send_text(MAV_SEVERITY_INFO, "%s", frame_and_type_string);
     
     static char  buf[50];
-    snprintf(buf, sizeof(buf),  "Version: zr-v%u.%u.%u ,ID: %lu",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,copter.zr_app.get_zr_sysid());
+    snprintf(buf, sizeof(buf),  "Version: zr-v%u.%u.%u ,ID: %08lu",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,copter.zr_app.get_zr_sysid());
 
     uint32_t deadline = 0;
     copter.zr_app.get_deadline_params(deadline);

ArduCopter/UserCode.cpp

@@ -42,6 +42,11 @@ void Copter::userhook_SlowLoop()
 void Copter::userhook_SuperSlowLoop()
 {
     // put your 1Hz code here
+    if(motors->armed()){    //  电子开关锁
+        relay.on(3);
+    }else{
+        relay.off(3);
+    }
 
 }
 #endif

ArduCopter/version.h

@@ -6,14 +6,16 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ArduCopter V4.1.5"
-
+#define THISFIRMWARE "ZRUAV V5.0.0-dev"
+#define FIRMWARE_TYPE 1
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,1,5,FIRMWARE_VERSION_TYPE_OFFICIAL
+// #define FIRMWARE_VERSION 4,1,5,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 5,0,0,FIRMWARE_TYPE
 
-#define FW_MAJOR 4
-#define FW_MINOR 1
-#define FW_PATCH 5
-#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FW_MAJOR 5
+#define FW_MINOR 0
+#define FW_PATCH 0
+// #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FW_TYPE FIRMWARE_TYPE
 
 #include <AP_Common/AP_FWVersionDefine.h>

ArduCopter/zr_app.cpp

@@ -25,7 +25,7 @@ void Copter::zr_app_50hz(){
             gcs().send_text(MAV_SEVERITY_INFO, "data_trans init");
         }
     }
-    
+
     if(zr_serial_api.data_trans_init){
         Vector3l pos_neu_cm;
         
@@ -139,4 +139,6 @@ void Copter::zr_app_50hz(){
         }
     }
     
-}
+}
+
+

libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp

@@ -50,8 +50,7 @@ void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
         Prx_Debug("ap_uavcan:nullptr" );
         return;
     }
-
-        Prx_Debug("ap_uavcan:subscribe_msgs");
+    Prx_Debug("ap_uavcan:subscribe_msgs");
     auto *node = ap_uavcan->get_node();
 
     uavcan::Subscriber<zrzk::equipment::range_sensor::Proximity, ProximityCb> *proximity_listener;
@@ -77,10 +76,8 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
     {
         Prx_Debug("backend:%d",(int)backend);
     }else{
-        
         Prx_Debug("faild,backend");
     }
-
     Prx_Debug("---------- AP_Proximity_UAVCAN ----------\n");
     return backend;
 }
@@ -137,33 +134,23 @@ float AP_Proximity_UAVCAN::distance_max() const
 
 float AP_Proximity_UAVCAN::distance_min() const
 {
-    return 0.20f;
+    return 1.00f;
 }
 
 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(315, _interim_data.d3_cm);  // d7
 
-    if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS))
-    {
+    if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS)){
         set_status(AP_Proximity::Status::NoData);
         // gcs().send_text(MAV_SEVERITY_INFO, "NoData\r\n");
-    }
-    else
-    {
+    }else if( _interim_data.d1_cm > 4000 ||  _interim_data.d1_cm < 100){
+        set_status(AP_Proximity::Status::NoData);
+    }else{
         set_status(AP_Proximity::Status::Good);
     }
 }
@@ -173,10 +160,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;
     }
-    
     if (_ap_uavcan == nullptr) {
         _ap_uavcan = ap_uavcan;
         _node_id = node_id;
@@ -195,14 +180,6 @@ void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan,
             _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)
@@ -235,17 +212,6 @@ void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distanc
         boundary.reset_face(face);
     }
     _last_distance_received_ms = AP_HAL::millis();
-
-
-    /*
-    uint8_t sector;
-    if(convert_angle_to_sector(angle_deg,sector)){
-        _angle[sector] = angle_deg;
-        _distance[sector] = ((float)distance_cm) / 100;
-        _distance_valid[sector] = (_distance[sector] > distance_min() && _distance[sector] < distance_max());
-        update_boundary_for_sector(sector, true);
-    }
-    */
 }
 
 #endif

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -281,7 +281,7 @@ void AP_Vehicle::scheduler_delay_callback()
         if (AP_BoardConfig::in_config_error()) {
             gcs().send_text(MAV_SEVERITY_CRITICAL, "Config Error: fix problem then reboot");
         } else {
-            gcs().send_text(MAV_SEVERITY_INFO, "Initialising ArduPilot");
+            gcs().send_text(MAV_SEVERITY_INFO, "Initialising ZRUAV");
         }
     }
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -3612,8 +3612,8 @@ void GCS_MAVLINK::send_banner()
     // mark the firmware version in the tlog
     const AP_FWVersion &fwver = AP::fwversion();
 
-    send_text(MAV_SEVERITY_INFO, "{%d}%s",__LINE__, fwver.fw_string);
-
+    send_text(MAV_SEVERITY_INFO, "%s", fwver.fw_string);
+#if 0
     if (fwver.middleware_name && fwver.os_name) {
         send_text(MAV_SEVERITY_INFO, "{%d}%s: %s %s: %s",__LINE__,
                   fwver.middleware_name, fwver.middleware_hash_str,
@@ -3641,6 +3641,7 @@ void GCS_MAVLINK::send_banner()
             send_text(MAV_SEVERITY_INFO, "{%d}%s",__LINE__, banner_msg);
         }
     }
+#endif
 }