合并六轴，备份

ArduCopter/Parameters.cpp

@@ -1629,11 +1629,11 @@ const char* Copter::get_sysid_board_id(void)
     static char  buf[50];
     // snprintf(buf, sizeof(buf),  "Version: v3.5.6 ,Board ID: ZRZK.19QT3.%d",(int)g.sysid_board_id);
 
-    //  int32_t nameValue1 =(int32_t) g.sysid_board_name_1st;
+     int32_t nameValue1 =(int32_t) g.sysid_board_name_1st;
      int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd;   
 
-    snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
+    // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
-    // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
+    snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
     AP::logger().Write_Message(buf);
     return buf;
 }

ArduCopter/UserCode.cpp

@@ -8,23 +8,7 @@ void Copter::userhook_init()
 {
     // put your initialisation code here
     // this will be called once at start-up
-    if (x30t != nullptr)
+
-    {
-        x30t->init();
-    }
-    else
-    {
-        if (x30t == nullptr)
-        {
-            gcs().send_text(MAV_SEVERITY_INFO,"x30t is null");
-            x30t = new AP_Camera_Serial_X30T();
-            if (x30t == nullptr || !x30t->init())
-            {
-                delete x30t;
-                x30t = nullptr;
-            }
-        }
-    }
 }
 #endif
 
@@ -46,38 +30,6 @@ void Copter::userhook_50Hz()
 void Copter::userhook_MediumLoop()
 {
     // put your 10Hz code here
-  if (x30t != nullptr)
-    {
-        int32_t lat = AP::gps().location().lat;
-        int32_t lng = AP::gps().location().lat;
-        int32_t alt = AP::gps().location().alt;
-        
-        uint8_t posdata[12];
-        posdata[0] = lat & 0xff;
-        posdata[1] = (lat >> 8) & 0xff;
-        posdata[2] = (lat >> 16) & 0xff;
-        posdata[3] = (lat >> 24) & 0xff;
-
-        posdata[4] = lng & 0xff;
-        posdata[5] = (lng >> 8) & 0xff;
-        posdata[6] = (lng >> 16) & 0xff;
-        posdata[7] = (lng >> 24) & 0xff;
-         
-        posdata[8] = alt& 0xff;
-        posdata[9] = (alt>> 8) & 0xff;
-        posdata[10] =(alt >> 16) & 0xff;
-        posdata[11] =(alt >> 24) & 0xff;
-       
-        x30t->sendPos(&posdata[0],X30T_MSG_TYPE_POS);
-
-
-       
-        
-  
-        
-
-    }
-
    
 }
 #endif

ArduCopter/events.cpp

@@ -102,6 +102,20 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
     } else {
         gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
     }
+        const AP_AHRS &_ahrs = AP::ahrs();
+        float home = 0.0;
+        Location loc;
+        if (_ahrs.get_position(loc)) {    
+            const Location &home_loc = _ahrs.get_home(); 
+            if (home_loc.lat != 0 || home_loc.lng != 0) {
+                // distance between vehicle and home_loc in meters
+                home = home_loc.get_distance(loc);
+                if(home < 10)
+                {
+                    desired_action = Failsafe_Action_None;
+                }
+            }
+        }
 
     // Battery FS options already use the Failsafe_Options enum. So use them directly.
     do_failsafe_action(desired_action, ModeReason::BATTERY_FAILSAFE);

ArduCopter/mode_rtl.cpp

@@ -66,6 +66,8 @@ void ModeRTL::run(bool disarm_on_land)
             }
             break;
         case RTL_FinalDescent:
+            gcs().send_text(MAV_SEVERITY_INFO,"complete:%d,_state:%d",_state_complete,_state);
+            set_mode(Mode::Number::BRAKE,ModeReason::MISSION_END);
             // do nothing
             break;
         case RTL_Land:

ArduCopter/motors.cpp

@@ -26,9 +26,10 @@ void Copter::arm_motors_check()
     }
 #endif
 
+
     int16_t yaw_in = channel_yaw->get_control_in();
-    int16_t pitch_in = channel_pitch->get_control_in();
+    // int16_t pitch_in = channel_pitch->get_control_in();
-    int16_t roll_in = channel_roll->get_control_in();
+    // int16_t roll_in = channel_roll->get_control_in();
     int16_t throttle_in = channel_throttle->get_control_in();
     static const uint16_t trim_trg_value = arming.get_rudder_arm_value()*40;
     static const int8_t con_arm_delay = arming.get_rudder_arm_time();
@@ -45,7 +46,8 @@ void Copter::arm_motors_check()
 
     // full right
     // if (yaw_in > 4000) {
-    if(yaw_in>trim_trg_value&&pitch_in>trim_trg_value&&roll_in< -trim_trg_value){
+    // if(yaw_in>trim_trg_value&&pitch_in>trim_trg_value&&roll_in< -trim_trg_value){
+    if(yaw_in>trim_trg_value){
         // increase the arming counter to a maximum of 1 beyond the auto trim counter
         if (arming_counter <= AUTO_TRIM_DELAY) {
             arming_counter++;
@@ -69,11 +71,12 @@ void Copter::arm_motors_check()
 
     // full left and rudder disarming is enabled
     // } else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {
-    } else if((yaw_in<-trim_trg_value&&pitch_in>trim_trg_value&&roll_in>trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){
+    // } else if((yaw_in<-trim_trg_value&&pitch_in>trim_trg_value&&roll_in>trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){
+    } else if((yaw_in<-trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){
         if (!flightmode->has_manual_throttle() && !ap.land_complete) {
             arming_counter = 0;
             return;
-        }
+        } 
 
         // increase the counter to a maximum of 1 beyond the disarm delay
         // if (arming_counter <= DISARM_DELAY) {

libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp

@@ -235,6 +235,7 @@ void AP_BattMonitor_Serial_BattGo::read()
 {
     requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA);
     uint32_t tnow = AP_HAL::micros();
+    static uint32_t last_prt;
     if (get_reading())
     {
         if (parseBattGo())
@@ -261,15 +262,17 @@ void AP_BattMonitor_Serial_BattGo::read()
             //...
             _state.healthy = _interim_state.healthy;
         }else{            
-            if(tnow-last_rev_batt_time>5000000){
+            if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){
                       _interim_state.healthy = false;
                       gcs().send_text(MAV_SEVERITY_INFO,"BATT_ERR:已经%d秒没有接收到智能电池 的串口数据！",(tnow-last_rev_batt_time)/1000000);
+                last_prt = tnow;
             }
         }
     }else{
-            if(tnow-last_rev_batt_time>5000000){
+            if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){
                       _interim_state.healthy = false;
                       gcs().send_text(MAV_SEVERITY_INFO,"BATT_ERR:已经%d秒没有接收到智能电池 的串口数据！",(tnow-last_rev_batt_time)/1000000);
+                last_prt = tnow;
             }
     }
 }

libraries/AP_GPS/GPS_Backend.cpp

@@ -110,7 +110,7 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
      //convert to time since Unix epoch
     uint32_t  ret =mkgmtime(timeinfo);
     //GPS epoch
-    ret -= 315964800; //315964800   1980-1-6  - 1970 1- 1
+    ret -= 315964800 + 18; //315964800   1980-1-6  - 1970 1- 1,+18s tiaomiao
 
     // // get time in seconds since unix epoch
     // uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day;

libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat

@@ -160,20 +160,20 @@ PD0 UART4_RX UART4 NODMA
 PD1 UART4_TX UART4 NODMA
 
 # # USART6 is telem3
-# PG9 USART6_RX USART6 NODMA
-# # we leave PG14 as an input to prevent it acting as a pullup
-# # on the IOMCU SBUS input
-# # PG14 USART6_TX USART6 NODMA
-# PG15 USART6_CTS USART6
-# PG8 USART6_RTS USART6
-
-# # USART6 is telem3 for cube
 PG9 USART6_RX USART6 NODMA
 # we leave PG14 as an input to prevent it acting as a pullup
 # on the IOMCU SBUS input
-PG14 USART6_TX USART6 NODMA
+# PG14 USART6_TX USART6 NODMA
-# PG15 USART6_CTS USART6
+PG15 USART6_CTS USART6
-# PG8 USART6_RTS USART6
+PG8 USART6_RTS USART6
+
+# # USART6 is telem3 for cube
+# PG9 USART6_RX USART6 NODMA
+# # we leave PG14 as an input to prevent it acting as a pullup
+# # on the IOMCU SBUS input
+# PG14 USART6_TX USART6 NODMA
+# # PG15 USART6_CTS USART6
+# # PG8 USART6_RTS USART6
 
 # UART7 is debug
 PF6 UART7_RX UART7 NODMA

libraries/AP_Notify/UAVCAN_RGB_LED.cpp

@@ -16,8 +16,9 @@
  */
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/system.h>
-
+ 
 #if HAL_WITH_UAVCAN
+// #if 1
 #include "UAVCAN_RGB_LED.h"
 
 #include <AP_UAVCAN/AP_UAVCAN.h>
@@ -54,10 +55,29 @@ bool UAVCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
     bool success = false;
     uint8_t can_num_drivers = AP::can().get_num_drivers();
 
+    static uint8_t cnt;
+    static uint8_t l_mode,l_red,l_green,l_blue;
+    rgb = {red, green, blue};
+    
+    cnt++;
+
+    if(l_mode != rgb_mode || l_red != rgb.r || l_green != rgb.g || l_blue != rgb.b || cnt > 50)
+    {
+        cnt =0;
+        _need_update = true;
+        l_mode = rgb_mode ;
+        l_red = rgb.r ;
+        l_green = rgb.g ;
+        l_blue = rgb.b;
+    }else{
+        return success;
+    }
+
     for (uint8_t i = 0; i < can_num_drivers; i++) {
         AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
         if (uavcan != nullptr) {
            // success = uavcan->led_write(_led_index, red, green, blue) || success;
+        //    success = uavcan->send_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success
            success = uavcan->send_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success
         }
     }

libraries/AP_Notify/UAVCAN_RGB_LED.h

@@ -13,7 +13,12 @@ protected:
     virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override;
     void set_rgb_mode(uint8_t mode) override;
 private:
+    void _timer(void);
     uint8_t _led_index;
      uint8_t rgb_mode;
+    bool _need_update;
+    struct {
+        uint8_t r, g, b;
+    } rgb;
 };