合并相机录像分支

ArduCopter/Parameters.cpp

@@ -57,6 +57,8 @@ const AP_Param::Info Copter::var_info[] = {
     GSCALAR(sysid_board_name_2nd,   "SYSID_BDNAME_P2",    1001),
     // 0:rs100, 1:M66,  2:RF610   3:zrzk.20qt2
     GSCALAR(sysid_type,   "SYSID_TYPE",   0 ),
+    
+    GSCALAR(sysid_board_id,   "SYSID_BOARD_ID",     100),
 
     // @Param: SYSID_THISMAV
     // @DisplayName: MAVLink system ID of this vehicle
@@ -141,6 +143,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Increment: 1
     // @User: Standard
     GSCALAR(zr_tk_delay,  "ZR_TK_DELAY", ZR_TK_DELAY),
+    GSCALAR(zr_use_rc,  "ZR_USE_RC", 1),
 
     // @Param: ZR_RTL_DELAY
     // @DisplayName: rtl  Altitude  when at final decent alt
@@ -1662,25 +1665,42 @@ const char* Copter::get_sysid_board_id(void)
      int32_t nameValue1 =(int32_t) g.sysid_board_name_1st;
      int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd;
      int8_t type = g.sysid_type;
+    char  name[6] = {' ',' ',' ',' ',' '};   
+    //  memset(name,0,6);
+     nameValue2 = nameValue2 & 0xffffff;
+    //  name[5] = "a";
+     name[4] = nameValue2&0xFF;
+     name[3] = nameValue2>>8&0xFF; 
+     name[2] = nameValue2>>16& 0xFF;   
+     name[1] = nameValue1&0xFF; 
+     name[0] = nameValue1>>8 & 0xFF;  
+
     switch (type)
     {
         case 0:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.8 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.10 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 1:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.8 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.10 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 2:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.8 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.10 ,ID: M610%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 3:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.8 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
+            // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.10 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.10 ,Board ID: ZRZK.%5s.%03d",name,(int)g.sysid_board_id);
+
+            break;
+        case 4:
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.10 ,Board ID: ZRZK.19QT2.%d",(int)nameValue2);
+            break;
+        case 5:
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.10 ,Board ID: %04d%04d",(int)nameValue1,(int)nameValue2);
             break;
         
         default:
             break;
     }
-    AP::logger().Write_Message(buf);
     return buf;
 }
 

ArduCopter/Parameters.h

@@ -205,6 +205,7 @@ public:
         k_param_sysid_board_name_1st =106, //add by @Binsir
         k_param_sysid_board_name_2nd=107, //add by @Binsir
         k_param_sysid_type=108, // modify by @Brown
+        k_param_sysid_board_id=109, // modify by @Brown
 
         // 110: Telemetry control
         //
@@ -391,6 +392,7 @@ public:
         k_param_zr_tk_req_alt,
         k_param_zr_tk_delay,
         k_param_zr_rtl_delay,
+        k_param_zr_use_rc,
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
@@ -399,7 +401,8 @@ public:
 
     AP_Int8        sysid_type; // modify by @Brown
     AP_Int16        sysid_board_name_1st;// modify by @Binsir
-    AP_Int16         sysid_board_name_2nd;// modify by @Binsir
+    AP_Int32         sysid_board_name_2nd;// modify by @Binsir
+    AP_Int16        sysid_board_id; // modify by @Brown
 
     // Telemetry control
     //
@@ -504,6 +507,8 @@ public:
     AP_Int32                 zr_tk_req_alt;
     AP_Int16                 zr_tk_delay;
     AP_Int16                 zr_rtl_delay;
+    AP_Int8                 zr_use_rc;
+    
     // Note: keep initializers here in the same order as they are declared
     // above.
     Parameters()

ArduCopter/mode.cpp

@@ -609,7 +609,8 @@ void Mode::land_run_horizontal_control()
             update_simple_mode();
 
             // convert pilot input to lean angles
-            // get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+            if(g.zr_use_rc)
+                get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
 
             // record if pilot has overridden roll or pitch
             if (!is_zero(target_roll) || !is_zero(target_pitch)) {
@@ -621,7 +622,8 @@ void Mode::land_run_horizontal_control()
         }
 
         // get pilot's desired yaw rate
-        // target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        if(g.zr_use_rc)
+            target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
             auto_yaw.set_mode(AUTO_YAW_HOLD);
         }

ArduCopter/mode_rtl.cpp

@@ -60,7 +60,7 @@ void ModeRTL::run(bool disarm_on_land)
             break;
         case RTL_LoiterAtHome:
             // if (rtl_path.land || copter.failsafe.radio) {
-            if (g.rtl_alt_final == 0) {
+            if (g.rtl_alt_final <= 0) {
                 land_start();
             }else{
                 descent_start();
@@ -192,7 +192,8 @@ void ModeRTL::climb_return_run()
     float target_yaw_rate = 0;
     if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
-        // target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        if(g.zr_use_rc)
+            target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
             auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
@@ -249,7 +250,8 @@ void ModeRTL::loiterathome_run()
     float target_yaw_rate = 0;
     if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
-        // target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        if(g.zr_use_rc)
+            target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
             auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
@@ -334,7 +336,8 @@ void ModeRTL::descent_run()
             update_simple_mode();
 
             // convert pilot input to lean angles
-            // get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+        if(g.zr_use_rc)
+            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
 
             // record if pilot has overridden roll or pitch
             if (!is_zero(target_roll) || !is_zero(target_pitch)) {
@@ -346,7 +349,8 @@ void ModeRTL::descent_run()
         }
 
         // get pilot's desired yaw rate
-        // target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        if(g.zr_use_rc)
+            target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
     }
 
     // set motors to full range

ArduCopter/version.h

@@ -6,12 +6,12 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.0.8-RC8"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.0.10-RC1"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,0,8,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 4,0,10,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
 #define FW_MINOR 0
-#define FW_PATCH 8
+#define FW_PATCH 10
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

ArduCopter/zr_flight.cpp

@@ -25,9 +25,7 @@ bool Copter::zr_radio_valid(){
     if (!rc().ignore_ctrl_channel() && (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::LAND)){
         if(abs(channel_roll->get_control_in())>750 || \
             abs(channel_pitch->get_control_in())>750 || \
-            abs(channel_yaw->get_control_in())>750 || \
-            channel_throttle->get_control_in()<380 || \
-            channel_throttle->get_control_in()>590  )
+            abs(channel_yaw->get_control_in())>750)
         {
             // rc().set_disable_ch();
             gcs().send_text(MAV_SEVERITY_INFO,"注意遥控通道是否在中位");
@@ -103,8 +101,10 @@ void Copter::zr_SuperSlowLoop(){
          if(before_fly){
             before_fly = false;
             camera.create_new_folder();
+            gcs().send_text(MAV_SEVERITY_INFO,"%s",get_sysid_board_id());
         }
         relay.on(3);
+        gcs().send_message(MSG_ZR_FLYING_STATUS);
         // camera.set_in_arm_mode(true);
 
     }else{
@@ -138,7 +138,6 @@ void Copter::zr_SuperSlowLoop(){
         }
     }
 
-    gcs().send_message(MSG_ZR_FLYING_STATUS);
 
 }
 #endif

hexa-zrv4.sh

@@ -1,3 +1,3 @@
 ./waf configure --board zr-hexa
-./waf --targets bin/arducopter --upload
-sudo cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.8-rc8.px4
+./waf copter
+cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.9-rc12.px4

libraries/AP_Camera/AP_Camera.cpp

@@ -603,28 +603,23 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan)
         {
             delt_pic_num += abs(num - camera_state.num[i]); // 判断各个相机照片数是否相等
         }
-        if (last_delt != delt_pic_num)
-        {
-            delt_cnt += 1;
-            if (delt_cnt > 5)
-            { // Mavlink一次可能会发两三个chan，取5确保是两次计算差值满足条件
+        if(delt_pic_num == 0){ 
+            delt_cnt = 0;
+            last_delt = 0;
+        }
+        if(last_delt != delt_pic_num){
+            delt_cnt+=1;
+            if(delt_cnt > 5){ // Mavlink一次可能会发两三个chan，取5确保是两次计算差值满足条件
                 // gcs().send_text(MAV_SEVERITY_INFO, "照片数差异(%d):%d,%d,%d,%d,%d",delt_pic_num,camera_state.num[0],camera_state.num[1],camera_state.num[2],camera_state.num[3],camera_state.num[4]);
-                if (delt_pic_num > 3)
-                {
-                    gcs().send_text(MAV_SEVERITY_CRITICAL, "%d:照片数差异大,总差值：%d", camera_state.num[0], delt_pic_num);
+                if(delt_pic_num > 5){
+                    gcs().send_text(MAV_SEVERITY_CRITICAL, "%d:照片数差异大,总差值：%d",camera_state.num[0],delt_pic_num);
                     feedback_gimbal = 20;
-                }
-                else
+                }else
                 {
-                    gcs().send_text(MAV_SEVERITY_INFO, "照片数差异(%d):%d", delt_pic_num, camera_state.num[0]);
+                    gcs().send_text(MAV_SEVERITY_INFO, "照片数差异(%d):%d",delt_pic_num,camera_state.num[0]);
                 }
-                last_delt = delt_pic_num;
-                delt_cnt = 0;
             }
         }
-        else
-        {
-        }
     }
     mavlink_camera_zr_status_t msg;
     memset(&msg, 0, sizeof(msg));

libraries/AP_GPS/GPS_Backend.cpp

@@ -90,12 +90,6 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
     min  = v % 100; v /= 100;
     hour = v % 100;
 
-    // int8_t rmon = mon - 2;
-    // if (0 >= rmon) {    
-    //     rmon += 12;
-    //     year -= 1;
-    // }
-
     time_t rawtime; 
     struct tm* timeinfo;
     time(&rawtime);

libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm

@@ -71,6 +71,7 @@ BATT_MONITOR 4
 BATT_SERIAL_NUM -1
 BATT_VOLT_PIN 0
 
+BRD_BOOT_DELAY 2000
 BRD_PWM_COUNT 0
 BRD_RTC_TYPES 1
 BRD_RTC_TZ_MIN 0
@@ -98,7 +99,19 @@ CAM_SERVO_ON 1900
 CAM_TRIGG_DIST 0
 CAM_TRIGG_TYPE 1
 CAM_TYPE 0
-
+CAN_D1_PROTOCOL 1
+CAN_D1_UC_ESC_BM 0
+CAN_D1_UC_NODE 10
+CAN_D1_UC_SRV_BM 0
+CAN_D1_UC_SRV_RT 50
+CAN_D2_PROTOCOL 1
+CAN_P1_BITRATE 1000000
+CAN_P1_DRIVER 1
+CAN_P2_BITRATE 1000000
+CAN_P2_DRIVER 1
+CAN_SLCAN_CPORT 0
+CAN_SLCAN_SERNUM -1
+CAN_SLCAN_TIMOUT 0
 CHUTE_ENABLED 0
 CIRCLE_RADIUS 1000
 CIRCLE_RATE 20
@@ -268,6 +281,7 @@ SERIAL6_PROTOCOL -1
 SERIAL7_BAUD 115200
 SERIAL7_OPTIONS 0
 SERIAL7_PROTOCOL 2
+SYSID_TYPE 2
 WP_YAW_BEHAVIOR 1
 WPNAV_ACCEL 100
 WPNAV_ACCEL_Z 100

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

@@ -121,10 +121,10 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
                 // if checksum matches extract contents
                 if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
 
-                    uint16_t strength = ((uint16_t)linebuf[5] << 8) | linebuf[4];
-                    if(strength < 100 || strength == 0xFFFF){
-                        return false;
-                    }
+                    // uint16_t strength = ((uint16_t)linebuf[5] << 8) | linebuf[4];
+                    // if(strength < 100 || strength == 0xFFFF){
+                    //     return false;
+                    // }
                     // calculate distance
                     uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
                     if (dist >= BENEWAKE_DIST_MAX_CM) {

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -88,7 +88,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
 
     // @Group: 4_
     // @Path: AP_RangeFinder_Wasp.cpp
-    AP_SUBGROUPVARPTR(drivers[0], "4_",  60, RangeFinder, backend_var_info[3]),
+    AP_SUBGROUPVARPTR(drivers[3], "4_",  60, RangeFinder, backend_var_info[3]),
 #endif
 
 #if RANGEFINDER_MAX_INSTANCES > 4
@@ -556,15 +556,22 @@ bool RangeFinder::has_orientation(enum Rotation orientation) const
 // find first range finder instance with the specified orientation
 AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) const
 {
+    // first try for a rangefinder that is in range
     for (uint8_t i=0; i<num_instances; i++) {
         AP_RangeFinder_Backend *backend = get_backend(i);
-        if (backend == nullptr) {
-            continue;
+        if (backend != nullptr &&
+            backend->orientation() == orientation &&
+            backend->status() == RangeFinder_Good) {
+            return backend;
         }
-        if (backend->orientation() != orientation) {
-            continue;
+    }
+    // if none in range then return first with correct orientation
+    for (uint8_t i=0; i<num_instances; i++) {
+        AP_RangeFinder_Backend *backend = get_backend(i);
+        if (backend != nullptr &&
+            backend->orientation() == orientation) {
+            return backend;
         }
-        return backend;
     }
     return nullptr;
 }
@@ -676,30 +683,16 @@ void RangeFinder::Log_RFND()
         if (s == nullptr) {
             continue;
         }
-        if (i == 0)
-        {
-            const struct log_RFND pkt = {
+
+        const struct log_RFND pkt = {
                 LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
-                time_us : AP_HAL::micros64(),
-                instance : i,
-                dist : s->distance_cm(),
-                status : (uint8_t)s->status(),
-                orient : s->orientation(),
-            };
-            AP::logger().WriteBlock(&pkt, sizeof(pkt));
-        }
-        else
-        {
-            const struct log_RFND pkt2 = {
-                LOG_PACKET_HEADER_INIT(LOG_RFND2_MSG),
-                time_us : AP_HAL::micros64(),
-                instance : i,
-                dist : s->distance_cm(),
-                status : (uint8_t)s->status(),
-                orient : s->orientation(),
-            };
-            AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
-        }
+                time_us      : AP_HAL::micros64(),
+                instance     : i,
+                dist         : s->distance_cm(),
+                status       : (uint8_t)s->status(),
+                orient       : s->orientation(),
+        };
+        AP::logger().WriteBlock(&pkt, sizeof(pkt));
     }
 }
 

modules/mavlink

@@ -1 +1 @@
-Subproject commit 0013b46db8cb437b6ad77d736263597d90a2262d
+Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019