GPS change, Flow rotation

ArduCopter/UserCode.cpp

@@ -99,16 +99,8 @@ void Copter::userhook_SuperSlowLoop()
                 break;
         }
         cacl_volt_pst = 100 - cnt;
-        battery.reset_remaining(0, cacl_volt_pst);
+        battery.reset_remaining(1, cacl_volt_pst);
     } 
-    
-    // // check AHRS and GPS are within 10m of each other
-    //     const Location gps_loc = gps.location();
-    //     Location ahrs_loc;
-    //     if (AP::ahrs().get_position(ahrs_loc)) {
-    //         const float distance = gps_loc.get_distance(ahrs_loc);
-    //         gcs().send_text(MAV_SEVERITY_INFO, "GPS and AHRS differ by %4.1fm", (double)distance);
-    //     }
 
 }
 #endif

ArduCopter/mode_brake.cpp

@@ -91,7 +91,7 @@ void ModeBrake::run()
                     }               
                     else  if(countdown<=0){
                         gcs().send_text(MAV_SEVERITY_INFO," 执行继续任务");
-                       copter.updownStatus == UpDown_ContinueClimb;                    
+                       copter.updownStatus = UpDown_ContinueClimb;                    
                        set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
                     }
                 }

ArduCopter/mode_rtl.cpp

@@ -96,7 +96,7 @@ void ModeRTL::run(bool disarm_on_land)
                     }
                     else if(countdown<=0){
                        gcs().send_text(MAV_SEVERITY_INFO,"开始着陆");
-                        copter.updownStatus == UpDown_ContinueDescent;
+                        copter.updownStatus = UpDown_ContinueDescent;
                         _state = RTL_Land;
                     }
                  }

ArduCopter/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.0.4"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.0.4-rc1.0"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,4,FIRMWARE_VERSION_TYPE_OFFICIAL

libraries/AP_GPS/AP_GPS.cpp

@@ -282,6 +282,9 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Range: 5.0 30.0
     // @User: Advanced
     AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
+
+    AP_GROUPINFO("NUM_SW", 22, AP_GPS, _nunber_to_switch, 5),
+
 #endif
 
     AP_GROUPEND
@@ -782,7 +785,7 @@ void AP_GPS::update(void)
                 for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
                     // choose GPS with highest state or higher number of satellites
                     if ((state[i].status > state[primary_instance].status) ||
-                        ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
+                        ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats  + _nunber_to_switch))) {
                         primary_instance = i;
                         _last_instance_swap_ms = now;
                     }
@@ -800,11 +803,11 @@ void AP_GPS::update(void)
                         continue;
                     }
 
-                    bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
+                    bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats  + _nunber_to_switch);
 
                     if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
 
-                        bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
+                        bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats  + _nunber_to_switch + 2);
 
                         if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
                             (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) {

libraries/AP_GPS/AP_GPS.h

@@ -458,6 +458,7 @@ protected:
     AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
     AP_Int8 _blend_mask;
     AP_Float _blend_tc;
+    AP_Int8 _nunber_to_switch;
 
     uint32_t _log_gps_bit = -1;
 

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1024,7 +1024,8 @@ AP_GPS_UBLOX::_parse_gps(void)
         if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
             if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
                 (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
-                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
+                // next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
+                next_fix = AP_GPS::GPS_OK_FIX_3D;
             }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
                 next_fix = AP_GPS::GPS_OK_FIX_3D;
             }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
@@ -1065,7 +1066,8 @@ AP_GPS_UBLOX::_parse_gps(void)
         if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
             if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
                 (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
-                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
+                // next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
+                next_fix = AP_GPS::GPS_OK_FIX_3D;
             }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
                 next_fix = AP_GPS::GPS_OK_FIX_3D;
             }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
@@ -1166,8 +1168,10 @@ AP_GPS_UBLOX::_parse_gps(void)
                 break;
             case 3:
                 state.status = AP_GPS::GPS_OK_FIX_3D;
-                if (_buffer.pvt.flags & 0b00000010)  // diffsoln
+                if (_buffer.pvt.flags & 0b00000010){  // diffsoln
-                    state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
+                    // state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
+                    state.status = AP_GPS::GPS_OK_FIX_3D;
+                }
                 if (_buffer.pvt.flags & 0b01000000)  // carrsoln - float
                     state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
                 if (_buffer.pvt.flags & 0b10000000)  // carrsoln - fixed

libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp

@@ -66,8 +66,13 @@ void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t n
     if (_ap_uavcan == ap_uavcan && _node_id == node_id) {
         WITH_SEMAPHORE(_driver->_sem);
         _driver->new_data = true;
-        _driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]);
+        if(0){
-        _driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]);
+            _driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]);
+            _driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]);
+        }else{
+            _driver->flowRate = Vector2f(-cb.msg->flow_integral[1], cb.msg->flow_integral[0]);
+            _driver->bodyRate = Vector2f(-cb.msg->rate_gyro_integral[1], cb.msg->rate_gyro_integral[0]);
+        }
         _driver->integral_time = cb.msg->integration_interval;
         _driver->surface_quality = cb.msg->quality;
     }

zr_version

@@ -0,0 +1,10 @@
+zr-v4.0:   经过飞行测试，稳定版本
+zr-v4.0-dev:   新添加的功能，并入主分支前先验证
+
+zr-v4.0.1:
+
+zr-dev-4.0.2: NMEA 时间修复，日志目录名
+
+v4.0.3: 快速解锁
+
+v4.0.4： 分阶段降落，光流门限，限制GPS切换 _nunber_to_switch,