merge zr-v4-base2

ArduCopter/Copter.h

@@ -1039,12 +1039,14 @@ public:
     void zr_SlowLoop();
     void zr_set_uav_state_to_uavcan();
     void zr_camera_mkdir();
+    void zr_mkdir_in_takeoff();
     uint8_t BinarySearch2f(float a[], float value, int low, int high);
     int8_t in_debug_mode;
     uint8_t cacl_volt_pst;
     
     bool flow_start_sw;
     bool flow_index_sw;
+    uint8_t mkdir_step;
 };
 
 extern Copter copter;

ArduCopter/Parameters.cpp

@@ -493,6 +493,8 @@ const AP_Param::Info Copter::var_info[] = {
     // @User: Standard
     GSCALAR(acro_yaw_p,                 "ACRO_YAW_P",           ACRO_YAW_P),
 
+    GSCALAR(hardware_flag,   "HW_FLAG",    2),
+
 #if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
     // @Param: ACRO_BAL_ROLL
     // @DisplayName: Acro Balance Roll

ArduCopter/Parameters.h

@@ -397,7 +397,8 @@ public:
         k_param_zr_flow_type,
         k_param_zr_flow_sens,
         k_param_zr_flow_debug,
-        k_param_zr_flow_real_elevation
+        k_param_zr_flow_real_elevation,
+        k_param_hardware_flag
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
@@ -408,7 +409,6 @@ public:
     AP_Int16        sysid_board_name_1st;// modify by @Binsir
     AP_Int32        sysid_board_name_2nd;// modify by @Binsir
     AP_Int16        sysid_board_id; // modify by @Brown
-
     // Telemetry control
     //
     AP_Int16        sysid_this_mav;
@@ -456,8 +456,8 @@ public:
     AP_Int16        land_speed_3nd;
     AP_Int16        land_slow_2nd_alt;
     AP_Int16        land_slow_3nd_alt;
-    AP_Int8        land_lock_alt;
-    AP_Int8        land_lock_rpy;
+    AP_Int8         land_lock_alt;
+    AP_Int8         land_lock_rpy;
 
 
 
@@ -491,7 +491,7 @@ public:
     AP_Int8         fs_crash_check;
     AP_Float        fs_ekf_thresh;
     AP_Int16        gcs_pid_mask;
-
+    AP_Int32        hardware_flag;
 #if MODE_THROW_ENABLED == ENABLED
     AP_Int8         throw_motor_start;
 #endif

ArduCopter/UserCode.cpp

@@ -201,43 +201,39 @@ void Copter::zr_set_uav_state_to_uavcan()
             data.armd = motors->armed();
             data.fly_status = copter.updownStatus;
             data.gps_state = AP::gps().status();
-            data.wp_number = AP::mission()->get_current_nav_index();
+            data.wp_number = g.hardware_flag&0xFF;//硬件版本
             uavcan->set_zr_uav_state(data);
         }
     }
 #endif
 }
 
+
 void Copter::zr_camera_mkdir()
 {
-    static uint8_t mkdir_step = 0;
+   
     if (motors->armed())
     {
-        if (mkdir_step != 0)
-            return;
-            
-        Location loc;
-        if (AP::ahrs().get_position(loc))
+        switch(mkdir_step)
         {
-            int32_t alt_res;
-            if (loc.relative_alt)
-            {
-                alt_res = loc.alt;
-            }
-            else
-            {
-                alt_res = loc.alt - AP::ahrs().get_home().alt;
-            }
-            if (alt_res > 1000) //相对高度>10m 新建文件夹
+        case 0:
+            zr_mkdir_in_takeoff();
+            break;
+        case 1: //起飞已经新建文件夹,判断是否返航或者降落 是新建文件夹
+            if (control_mode == Mode::Number::LAND)
             {
-                mkdir_step = 1;
                 AP_Camera *cam = AP::camera();
                 if (cam != nullptr)
                 {
-                    cam->create_new_folder(10);
+                    cam->create_new_folder(9);
                 }
             }
-        }
+            break;
+        case 2:
+            break;
+        default:
+            break;
+        }  
     }
     else
     {
@@ -245,4 +241,30 @@ void Copter::zr_camera_mkdir()
     }
 }
 
+void Copter::zr_mkdir_in_takeoff()
+{
+    Location loc;
+    if (AP::ahrs().get_position(loc))
+    {
+        int32_t alt_res;
+        if (loc.relative_alt)
+        {
+            alt_res = loc.alt;
+        }
+        else
+        {
+            alt_res = loc.alt - AP::ahrs().get_home().alt;
+        }
+        if (alt_res > 1000) //相对高度>10m 新建文件夹
+        {
+            mkdir_step = 1;
+            AP_Camera *cam = AP::camera();
+            if (cam != nullptr)
+            {
+                cam->create_new_folder(10);
+            }
+        }
+    }
+}
+
 #endif

ArduCopter/mode_rtl.cpp

@@ -59,6 +59,16 @@ void ModeRTL::run(bool disarm_on_land)
             loiterathome_start();
             break;
         case RTL_LoiterAtHome:
+
+            if (copter.mkdir_step == 1)
+            {
+                AP_Camera *camera = AP::camera();
+                if (camera != nullptr)
+                {
+                    camera->create_new_folder(9);
+                    copter.mkdir_step = 2;
+                }
+            }
             // if (rtl_path.land || copter.failsafe.radio) {
             if (g.rtl_alt_final <= 0) {
                 land_start();
@@ -229,17 +239,15 @@ void ModeRTL::loiterathome_start()
     _loiter_start_time = millis();
 
     // yaw back to initial take-off heading yaw unless pilot has already overridden yaw
-    if(auto_yaw.default_mode(true) != AUTO_YAW_HOLD) {
+    if (auto_yaw.default_mode(true) != AUTO_YAW_HOLD)
+    {
         auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW);
-    } else {
-        auto_yaw.set_mode(AUTO_YAW_HOLD);
     }
-
-    AP_Camera *camera = AP::camera();
-    if (camera != nullptr)
+    else
     {
-        camera->create_new_folder(9);
+        auto_yaw.set_mode(AUTO_YAW_HOLD);
     }
+    
 }
 
 // rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller

libraries/AP_Camera/AP_Camera.cpp

@@ -479,7 +479,7 @@ void AP_Camera::uavcan_pic()
         AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
         if (uavcan != nullptr)
         {
-            uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index, 0, cameras_take_enable);
+            uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index&0xff, _image_index>>8, cameras_take_enable);
         }
     }
     _trigger_counter = constrain_int16(_trigger_duration * 5, 0, 255);

libraries/AP_GPS/AP_GPS.cpp

@@ -66,7 +66,7 @@ const uint32_t AP_GPS::_baudrates[] = { 115200U,  38400U, 57600U, 230400U};
 // initialisation blobs to send to the GPS to try to get it into the
 // right mode
 // const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
-const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
+const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY ;
 
 AP_GPS *AP_GPS::_singleton;
 
@@ -569,32 +569,32 @@ void AP_GPS::detect_instance(uint8_t instance)
 #if !HAL_MINIMIZE_FEATURES
         // we drop the MTK drivers when building a small build as they are so rarely used
         // and are surprisingly large
-        else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) &&
+        else if ((_type[instance] == GPS_TYPE_MTK19) &&
                  AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) {
             new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]);
-        } else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) &&
+        } else if ((_type[instance] == GPS_TYPE_MTK) &&
                    AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) {
             new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
         }
 #endif
-        else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
+        else if ((_type[instance] == GPS_TYPE_SBP) &&
                  AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
             new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]);
         }
-        else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
+        else if ((_type[instance] == GPS_TYPE_SBP) &&
                  AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
             new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
         }
 #if !HAL_MINIMIZE_FEATURES
-        else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
+        else if ((_type[instance] == GPS_TYPE_SIRF) &&
                  AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
             new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]);
         }
 #endif
-        else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) &&
+        else if ((_type[instance] == GPS_TYPE_ERB) &&
                  AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
             new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]);
-        } else if ((_type[instance] == GPS_TYPE_NMEA ||
+        } else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA ||
                     _type[instance] == GPS_TYPE_HEMI || _type[instance] == GPS_TYPE_SINO || _type[instance] == GPS_TYPE_UNICO) &&
                    AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
             new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -38,6 +38,13 @@
 
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS.h>
+
+#if HAL_WITH_UAVCAN
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+#include <AP_Camera/AP_Camera.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 // optionally log all NMEA data for debug purposes
@@ -153,6 +160,35 @@ int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p)
     return ret;
 }
 
+uint32_t AP_GPS_NMEA::_parse_decimal_1000(const char *p)
+{
+    char *endptr = nullptr;
+    long ret = 1000 * strtol(p, &endptr, 10);
+    if (ret >= (long)INT32_MAX) {
+        return INT32_MAX;
+    }
+    if (ret <= (long)INT32_MIN) {
+        return INT32_MIN;
+    }
+    if (endptr == nullptr || *endptr != '.') {
+        return ret;
+    }
+
+    if (isdigit(endptr[1])) {
+        ret += 100 * DIGIT_TO_VAL(endptr[1]);
+        if (isdigit(endptr[2])) {
+            ret += 10 * DIGIT_TO_VAL(endptr[2]);
+            if (isdigit(endptr[3])) {
+                ret += DIGIT_TO_VAL(endptr[3]);
+                if (isdigit(endptr[4])) {
+                    ret += (DIGIT_TO_VAL(endptr[4]) >= 5);
+                }
+            }
+        }
+    }
+    return ret;
+}
+
 /*
   parse a NMEA latitude/longitude degree value. The result is in degrees*1e7
  */
@@ -253,25 +289,49 @@ bool AP_GPS_NMEA::_term_complete()
                 switch (_sentence_type) {
                 case _GPS_SENTENCE_MARK:
                 case _GPS_SENTENCE_EVENT:
-                    if(get_new_mark){                        
-                        // gcs().send_text(MAV_SEVERITY_INFO, "%d, %.4f, %f , %f , %f\n",gps_mark.week,gps_mark.second,gps_mark.lat,gps_mark.lon,gps_mark.hgt);
+                    if (get_new_mark)
+                    {
+                        gcs().send_text(MAV_SEVERITY_INFO, "%d, %.4f, %f , %f , %f\n", gps_mark.week, gps_mark.second, gps_mark.lat, gps_mark.lon, gps_mark.hgt);
                         get_new_mark = false;
-                            AP_Logger *logger = AP_Logger::get_singleton();
 
-                            if ( logger!=nullptr )
+#if HAL_WITH_UAVCAN
+                        uint8_t can_num_drivers = AP::can().get_num_drivers();
+                        for (uint8_t i = 0; i < can_num_drivers; i++)
+                        {
+                            AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
+                            if (uavcan != nullptr)
                             {
-                                Vector3d  llh = Vector3d(gps_mark.lat,gps_mark.lon, gps_mark.hgt);
-                                Vector3f  sigma = Vector3f(gps_mark.lat_sigma,gps_mark.lon_sigma, gps_mark.hgt_sigma);
-                                    AP::logger().Write_RTK_Mark_Pos(
-                                    gps_mark.week,
-                                    gps_mark.second,
-                                    llh,
-                                    gps_mark.undulation,
-                                    sigma,
-                                    gps_mark.diff_age,
-                                    gps_mark.sol_age
-                                    );
+                                ppk_pos_data_t ppk_pos;
+                                memset(&ppk_pos,0,sizeof(ppk_pos_data_t));
+                                ppk_pos.week = gps_mark.week;
+                                ppk_pos.weekms = gps_mark.second;
+                                ppk_pos.latitude_deg_1e7 = (int32_t)(gps_mark.lat * (int32_t)10000000UL);
+                                ppk_pos.longitude_deg_1e7 = (int32_t)(gps_mark.lon * (int32_t)10000000UL);
+                                ppk_pos.height_abs_cm = gps_mark.hgt * 100;
+                                AP_Camera *camera = AP::camera();
+                                if (camera != nullptr)
+                                {
+                                   ppk_pos.index =camera->get_image_index();
+                                }
+                                uavcan->set_zr_ppk_pos(ppk_pos);
                             }
+                        }
+#endif
+                        AP_Logger *logger = AP_Logger::get_singleton();
+
+                        if (logger != nullptr)
+                        {
+                            Vector3d llh = Vector3d(gps_mark.lat, gps_mark.lon, gps_mark.hgt);
+                            Vector3f sigma = Vector3f(gps_mark.lat_sigma, gps_mark.lon_sigma, gps_mark.hgt_sigma);
+                            AP::logger().Write_RTK_Mark_Pos(
+                                gps_mark.week,
+                                gps_mark.second,
+                                llh,
+                                gps_mark.undulation,
+                                sigma,
+                                gps_mark.diff_age,
+                                gps_mark.sol_age);
+                        }
                     }
                 }
                 
@@ -492,11 +552,7 @@ bool AP_GPS_NMEA::_term_complete()
     }
            
 #if 1
-static uint32_t cnt;
-cnt++;
-// if ((_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT ))
-//         gcs().send_text(MAV_SEVERITY_INFO, "%d,t:%d, n:%d,%d: %s",cnt,_sentence_type,_term_number,_term[0],_term);
-    // rtk event mark decode
+
     if ((_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )) {
         switch (_sentence_type + _term_number) {
         // operational status
@@ -504,66 +560,54 @@ cnt++;
         case _GPS_SENTENCE_MARK + 5: // week
         case _GPS_SENTENCE_EVENT + 5: // week
             gps_mark.week = atol(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%d",_term,gps_mark.week);
             break; 
         case _GPS_SENTENCE_MARK + 6: // second
         case _GPS_SENTENCE_EVENT + 6: // second
-            gps_mark.second = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.second);
+            gps_mark.second = _parse_decimal_1000(_term); 
+            // gcs().send_text(MAV_SEVERITY_INFO, "event:%s:%ld,%ld",_term,gps_mark.second,_parse_decimal_100(_term));
             break;
         case _GPS_SENTENCE_MARK + 12: // lat
         case _GPS_SENTENCE_EVENT + 22: // lat
             gps_mark.lat = atof(_term);  
-            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lat);
             break;
         case _GPS_SENTENCE_MARK + 13: // lon
         case _GPS_SENTENCE_EVENT + 23:
             gps_mark.lon = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lon);
             break;
         case _GPS_SENTENCE_MARK + 14: // hgt
         case _GPS_SENTENCE_EVENT + 24:
             gps_mark.hgt = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lon);
             break;
         case _GPS_SENTENCE_MARK + 15: // undulation
         case _GPS_SENTENCE_EVENT + 25:
             gps_mark.undulation = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
             break;
         case _GPS_SENTENCE_MARK + 17: // lat_sigma
         case _GPS_SENTENCE_EVENT + 27:
             gps_mark.lat_sigma = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
             break;
         case _GPS_SENTENCE_MARK + 18: // lon_sigma
         case _GPS_SENTENCE_EVENT + 28:
             gps_mark.lon_sigma = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
             break;
         case _GPS_SENTENCE_MARK + 19: // hgt_sigma
         case _GPS_SENTENCE_EVENT + 29:
             gps_mark.hgt_sigma = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
             break;
         case _GPS_SENTENCE_MARK + 21: // diff_age
         case _GPS_SENTENCE_EVENT + 31:
             gps_mark.diff_age = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
             break;
         case _GPS_SENTENCE_MARK + 22: // sol_age
         case _GPS_SENTENCE_EVENT + 32:
             gps_mark.sol_age = atof(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
             break;
         case _GPS_SENTENCE_MARK + 23: // SVs
         case _GPS_SENTENCE_EVENT + 33:
             gps_mark.SVs = atol(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
         case _GPS_SENTENCE_MARK + 24: // solnSVs
         case _GPS_SENTENCE_EVENT + 34:
             gps_mark.solnSVs = atol(_term); 
-            // gcs().send_text(MAV_SEVERITY_INFO, "--- %d,%s \n",_term_number,_term);
             get_new_mark = true;
             
             break;

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -93,6 +93,7 @@ private:
     ///					multiplied by 100.
     ///
     static int32_t _parse_decimal_100(const char *p);
+    static uint32_t _parse_decimal_1000(const char *p);
 
     /// Parses the current term as a NMEA-style degrees + minutes
     /// value with up to four decimal digits.
@@ -149,7 +150,7 @@ private:
     struct mark_s
     {
         uint16_t    week;
-        float       second;
+        uint32_t       second;
         double       lat;
         double       lon;
         double       hgt;
@@ -176,7 +177,7 @@ private:
     static const char _initialisation_blob[];
 
     bool _send_message( void *msg, uint16_t size);
-    u_long markCRC;
+    unsigned long markCRC;
         
     const unsigned long aulCrcTable[256] =
     {
@@ -227,9 +228,12 @@ private:
 };
 
 #define AP_GPS_NMEA_HEMISPHERE_INIT_STRING \
-        "log com1 gpgga ontime 0.1\r\n" /* GGA at 5Hz */                            \
-        "log com1 gprmc ontime 0.1\r\n" /* RMC at 5Hz */                            \
-        "log com1 gpvtg ontime 0.2\r\n" /* VTG at 5Hz */
+        "interfacemode com1 compass compass on\r\n" /* Prefix of GP on the HDT message */      \
+        "COM COM2 230400\r\n" /* GGA at 5Hz */                            \
+        "COM COM3 460800\r\n" /* RMC at 5Hz */                            \
+        "COM COM1 115200\r\n" /* VTG at 5Hz */                            \
+        "interfacemode com1 auto auto on\r\n" /* HDT at 5Hz */                            \
+        "saveconfig\r\n" /* Enable SBAS */
 
 
 
@@ -242,10 +246,21 @@ private:
         "saveconfig\r\n" /* Enable SBAS */
 
 #define AP_GPS_NMEA_UNICORE_INIT_STRING \
-        "interfacemode com1 compass compass on\r\n" /* Prefix of GP on the HDT message */      \
-        "log com1 gpgga ontime 0.1\r\n" /* GGA at 5Hz */                            \
-        "log com1 gprmc ontime 0.1\r\n" /* HDT at 5Hz */                            \
-        "log com1 gpvtg ontime 0.2\r\n" /* RMC at 5Hz */                            \
-        "LOG COM1 EVENTALLA ONCHANGED\r\n" /* VTG at 5Hz */                            \
-        "saveconfig\r\n" /* Enable SBAS */
-        
+        "UNLOG COM1\r\n" /* Prefix of GP on the HDT message */      \
+        "log com1 gpgga ontime 0.1\r\n" /* Prefix of GP on the HDT message */      \
+        "log com1 gprmc ontime 0.1\r\n" /* GGA at 5Hz */                            \
+        "log com1 gpvtg ontime 0.1\r\n" /* HDT at 5Hz */                            \
+        "LOG COM1 EVENTALLA ONCHANGED\r\n" /* VTG at 5Hz */                         
+#if 0
+        "LOG COM1 EVENTMARKA ONCHANGED\r\n" /* RMC at 5Hz */ 
+        "log com3 GPSEPHEMB ontime 300\r\n" \
+        "log com3 GLOEPHEMERISB ontime 300\r\n" \
+        "log com3 BD2EPHEMB ontime 300\r\n" \
+        "log com3 rangecmpb ontime 0.1\r\n" \
+        "LOG COM3 EVENTALLB ONCHANGED\r\n" \
+        "LOG COM3 EVENTMARKB ONCHANGED\r\n" \
+        "LOG COM3 EVENTALLA ONCHANGED\r\n" \
+        "CONFIG PPS ENABLE GPS POSITIVE 500000 1000 0 0\r\n" \
+        "log com3 gpgga ontime 0.05\r\n" \
+        // "saveconfig\r\n" /* Enable SBAS */ 
+#endif 

libraries/AP_GPS/GPS_Backend.cpp

@@ -16,8 +16,9 @@
 #include "AP_GPS.h"
 #include "GPS_Backend.h"
 #include <AP_Logger/AP_Logger.h>
-
+#include <AP_RTC/AP_RTC.h>
 #include <assert.h>
+#include <time.h>
 #define GPS_BACKEND_DEBUGGING 0
 
 #if GPS_BACKEND_DEBUGGING
@@ -77,44 +78,25 @@ int16_t AP_GPS_Backend::swap_int16(int16_t v) const
  */
 void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
 {
-    uint8_t year, mon, day, hour, min, sec;
-    uint16_t msec;
+    struct tm tm {};
 
-    year = bcd_date % 100;
-    mon  = (bcd_date / 100) % 100;
-    day  = bcd_date / 10000;
+    tm.tm_year = 100U + bcd_date % 100U;
+    tm.tm_mon  = ((bcd_date / 100U) % 100U)-1;
+    tm.tm_mday = bcd_date / 10000U;
 
     uint32_t v = bcd_milliseconds;
-    msec = v % 1000; v /= 1000;
-    sec  = v % 100; v /= 100;
-    min  = v % 100; v /= 100;
-    hour = v % 100;
-
-    time_t rawtime; 
-    struct tm* timeinfo;
-    time(&rawtime);
-    timeinfo = gmtime(&rawtime);
-    timeinfo->tm_year = year+2000 - 1900;
-    timeinfo->tm_mon = mon - 1;    //months since January - [0,11]
-    timeinfo->tm_mday = day;          //day of the month - [1,31] 
-    timeinfo->tm_hour = hour;         //hours since midnight - [0,23]
-    timeinfo->tm_min = min;          //minutes after the hour - [0,59]
-    timeinfo->tm_sec = sec;          //seconds after the minute - [0,59]
-    
-     //convert to time since Unix epoch
-    uint32_t  ret =mkgmtime(timeinfo);
-    //GPS epoch
-    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;
-    // ret += year*365 + 10501;
-    // ret = ret*24 + hour;
-    // ret = ret*60 + min;
-    // ret = ret*60 + sec;
-
-    // // convert to time since GPS epoch
-    // ret -= 272764785UL;
+    uint16_t msec = v % 1000U; v /= 1000U;
+    tm.tm_sec = v % 100U; v /= 100U;
+    tm.tm_min = v % 100U; v /= 100U;
+    tm.tm_hour = v % 100U;
+
+    // convert from time structure to unix time
+    time_t unix_time = AP::rtc().mktime(&tm);
+
+    // convert to time since GPS epoch
+    const uint32_t unix_to_GPS_secs = 315964800UL;
+    const uint16_t leap_seconds_unix = GPS_LEAPSECONDS_MILLIS/1000U;
+    uint32_t ret = unix_time + leap_seconds_unix - unix_to_GPS_secs;
 
     // get GPS week and time
     state.time_week = ret / AP_SEC_PER_WEEK;
@@ -301,19 +283,3 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
     }
 }
 
-
-    time_t AP_GPS_Backend::mkgmtime(struct tm *unixdate)
-{
-    assert(unixdate != nullptr);
-
-    time_t     fakeUnixTime = mktime(unixdate); 
-    struct tm *fakeDate     = gmtime(&fakeUnixTime);
-
-    int32_t nOffset = fakeDate->tm_hour - unixdate->tm_hour;
-    if (nOffset > 12)
-    {
-        nOffset = 24 - nOffset;
-    }
-    return fakeUnixTime - nOffset * 3600;
-}
-

libraries/AP_GPS/GPS_Backend.h

@@ -20,8 +20,6 @@
 
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_RTC/JitterCorrection.h>
-#include <time.h>
-#include <sys/time.h>
 #include "AP_GPS.h"
 
 class AP_GPS_Backend
@@ -100,7 +98,6 @@ protected:
 
     void check_new_itow(uint32_t itow, uint32_t msg_length);
     
-      time_t mkgmtime(struct tm *unixdate);
 private:
     // itow from previous message
     uint32_t _last_itow;

libraries/AP_Logger/AP_Logger.h

@@ -287,7 +287,7 @@ public:
     void Write_OABendyRuler(bool active, float target_yaw, float margin, const Location &final_dest, const Location &oa_dest);
     void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest);
     void Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2,uint16_t cam3,uint16_t cam4,uint16_t cam5,uint8_t flag);
-    void Write_RTK_Mark_Pos(uint16_t week, float second,const Vector3d& llh,float undulation,const Vector3f& sigma,float diff_age,float sol_age);
+   void Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,float undulation,const Vector3f& sigma,float diff_age,float sol_age);
 
     void Write(const char *name, const char *labels, const char *fmt, ...);
     void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

libraries/AP_Logger/LogFile.cpp

@@ -1066,7 +1066,7 @@ void AP_Logger::Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2
       WriteBlock(&pkt, sizeof(pkt));
 }
 
-void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, float second,const Vector3d& llh,float undulation,const Vector3f& sigma,float diff_age,float sol_age)
+void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,float undulation,const Vector3f& sigma,float diff_age,float sol_age)
 {
     const AP_AHRS &ahrs = AP::ahrs();
 
@@ -1092,4 +1092,4 @@ void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, float second,const Vector3d& l
         solnSVs     : 0
     };
       WriteBlock(&pkt, sizeof(pkt)); 
-}
+}

libraries/AP_Logger/LogStructure.h

@@ -1248,7 +1248,7 @@ struct  PACKED log_rtk_mark_pos
     int16_t pitch;
     uint16_t yaw;
     uint16_t    week;
-    float       second;
+    uint32_t       second;
     double       lat;
     double       lon;
     double       hgt;
@@ -1462,7 +1462,7 @@ struct  PACKED log_rtk_mark_pos
     { LOG_CAM_FEEDBACK_MSG, sizeof(log_Camera_FeedBack), \
       "CAMF","QHHHHHHB","TimeUS,Index,CAM1,CAM2,CAM3,CAM4,CAM5,FLAG", "s-------", "F-------" },\
     { LOG_RTK_MARK_POS, sizeof(log_rtk_mark_pos), \
-      "MARK","QhhHHfdddffffffBB","TimeUS,Roll,Pitch,Yaw,Week,Second,Lat,Lon,Hgt,Undulation,Lat_s,Lon_s,Hgt_s,Diff_age,Sol_age,SVs,SolnSVs", "sddh-------------", "F----------------" }
+      "MARK","QhhHHIdddffffffBB","TimeUS,R,P,Y,Week,Secms,Lat,Lon,Hgt,Undu,Lat_s,Lon_s,Hgt_s,D_age,S_age,SVs,SoSVs", "sddh-------------", "F----------------" }
 // messages for more advanced boards
 #define LOG_EXTRA_STRUCTURES \
     { LOG_IMU2_MSG, sizeof(log_IMU), \
@@ -1751,6 +1751,7 @@ enum LogMessages : uint8_t {
 
     LOG_CSRV_MSG,
     LOG_CESC_MSG,
+
     LOG_BAR2_MSG,
     LOG_ARSP_MSG,
     LOG_ATTITUDE_MSG,

libraries/AP_RTC/AP_RTC.cpp

@@ -3,6 +3,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS.h>
+#include <time.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -63,6 +64,7 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
         // can't allow time to go backwards, ever
         return;
     }
+    WITH_SEMAPHORE(rsem);
 
     rtc_shift = tmp;
 
@@ -73,8 +75,10 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
 
     rtc_source_type = type;
 
+#ifndef HAL_NO_GCS
     // update signing timestamp
     GCS_MAVLINK::update_signing_timestamp(time_utc_usec);
+#endif
 }
 
 bool AP_RTC::get_utc_usec(uint64_t &usec) const
@@ -86,7 +90,7 @@ bool AP_RTC::get_utc_usec(uint64_t &usec) const
     return true;
 }
 
-bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms)
+bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
 {
      // get time of day in ms
     uint64_t time_ms = 0;
@@ -109,7 +113,7 @@ bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uin
     return true;
 }
 
-bool AP_RTC::get_local_time(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms)
+bool AP_RTC::get_local_time(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
 {
      // get local time of day in ms
     uint64_t time_ms = 0;
@@ -202,6 +206,48 @@ uint32_t AP_RTC::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms
 }
 
 
+/*
+  mktime replacement from Samba
+ */
+time_t AP_RTC::mktime(const struct tm *t)
+{
+    time_t epoch = 0;
+    int n;
+    int mon [] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }, y, m, i;
+    const unsigned MINUTE = 60;
+    const unsigned HOUR = 60*MINUTE;
+    const unsigned DAY = 24*HOUR;
+    const unsigned YEAR = 365*DAY;
+
+    if (t->tm_year < 70) {
+        return (time_t)-1;
+    }
+
+    n = t->tm_year + 1900 - 1;
+    epoch = (t->tm_year - 70) * YEAR +
+            ((n / 4 - n / 100 + n / 400) - (1969 / 4 - 1969 / 100 + 1969 / 400)) * DAY;
+
+    y = t->tm_year + 1900;
+    m = 0;
+
+    for (i = 0; i < t->tm_mon; i++) {
+        epoch += mon [m] * DAY;
+        if (m == 1 && y % 4 == 0 && (y % 100 != 0 || y % 400 == 0)) {
+            epoch += DAY;
+        }
+
+        if (++m > 11) {
+            m = 0;
+            y++;
+        }
+    }
+
+    epoch += (t->tm_mday - 1) * DAY;
+    epoch += t->tm_hour * HOUR + t->tm_min * MINUTE + t->tm_sec;
+
+    return epoch;
+}
+
 // singleton instance
 AP_RTC *AP_RTC::_singleton;
 

libraries/AP_RTC/AP_RTC.h

@@ -3,7 +3,7 @@
 #include <AP_Param/AP_Param.h>
 
 #include <stdint.h>
-
+#include <time.h>
 class AP_RTC {
 
 public:
@@ -38,20 +38,29 @@ public:
     /*
       get time in UTC hours, minutes, seconds and milliseconds
      */
-    bool get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms);
+    bool get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const;
     
-    bool get_local_time(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms);
+    bool get_local_time(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const;
 
     uint32_t get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms);
 
+    // replacement for mktime()
+    static time_t mktime(const struct tm *t);
+
     // get singleton instance
     static AP_RTC *get_singleton() {
         return _singleton;
     }
 
+    // allow threads to lock against RTC update
+    HAL_Semaphore &get_semaphore(void) {
+        return rsem;
+    }
+    
 private:
 
     static AP_RTC *_singleton;
+    HAL_Semaphore_Recursive rsem;
 
     source_type rtc_source_type = SOURCE_NONE;
     int64_t rtc_shift;

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -61,6 +61,8 @@
 
 #include <zrzk/protocol/Message.hpp>
 #include <zrzk/protocol/Request.hpp>
+#include <zrzk/equipment/uav/PPKPOS.hpp>
+
 #include <uavcan/equipment/camera_gimbal/GEOPOICommand.hpp>
 #include <uavcan/equipment/camera_gimbal/AngularCommand.hpp>
 
@@ -149,6 +151,7 @@ static uavcan::Publisher<zrzk::equipment::uav::Euler>* zr_uav_euler[MAX_NUMBER_O
 static uavcan::Publisher<zrzk::equipment::uav::Position> * zr_uav_pos[MAX_NUMBER_OF_CAN_DRIVERS];
 
 static uavcan::Publisher<zrzk::equipment::flow::Water> *zr_flow_water[MAX_NUMBER_OF_CAN_DRIVERS];
+static uavcan::Publisher<zrzk::equipment::uav::PPKPOS>* zr_uav_ppk[MAX_NUMBER_OF_CAN_DRIVERS]; 
 // subscribers
 
 // handler SafteyButton
@@ -377,6 +380,9 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     zr_flow_water[driver_index] = new uavcan::Publisher<zrzk::equipment::flow::Water>(*_node);
     zr_flow_water[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(100));
     zr_flow_water[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    zr_uav_ppk[driver_index] = new uavcan::Publisher<zrzk::equipment::uav::PPKPOS>(*_node);
+    zr_uav_ppk[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(100));
+    zr_uav_ppk[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
 
     safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
     if (safety_button_listener[driver_index]) {
@@ -523,6 +529,7 @@ void AP_UAVCAN::loop(void)
         zr_uav_pos_send();
         zr_uav_euler_send();
         zr_flow_water_send();
+        zr_uav_ppk_send();
         AP::uavcan_server().verify_nodes(this);
     }
 }
@@ -1263,11 +1270,10 @@ void AP_UAVCAN::zr_uav_pos_send()
             zrzk::equipment::uav::Position msg;
 
             WITH_SEMAPHORE(uav_pos_sem);
+            msg.p1 =0;
             msg.latitude_deg_1e7 = loc.lat;
             msg.longitude_deg_1e7 = loc.lng;
             msg.height_abs_cm = altitude;
-            msg.need_wait_data = false; //FIXME 留待以后ppk数据来更新
-            msg.usec = AP::gps().time_epoch_usec() / 1000;
             zr_uav_pos[_driver_index]->broadcast(msg);
         }
     }
@@ -1357,5 +1363,29 @@ void AP_UAVCAN::set_zr_flow_water(const uint8_t *data, uint16_t len)
 
     _zr_water_flow.need_send = true;
 }
+void AP_UAVCAN::set_zr_ppk_pos(ppk_pos_data_t &data)
+{
+    WITH_SEMAPHORE(_zr_ppk_pos.sem);
+    memcpy(&_zr_ppk_pos.data, &data, sizeof(ppk_pos_data_t));
+    _zr_ppk_pos.need_send = true;
+}
+
+void AP_UAVCAN::zr_uav_ppk_send()
+{
+    if (!_zr_ppk_pos.need_send)
+        return;
+
+    zrzk::equipment::uav::PPKPOS msg;
+    WITH_SEMAPHORE(_zr_ppk_pos.sem);
+    msg.index = _zr_ppk_pos.data.index;
+    msg.week = _zr_ppk_pos.data.week;
+    msg.weekms = _zr_ppk_pos.data.weekms;
+    msg.latitude_deg_1e7 = _zr_ppk_pos.data.latitude_deg_1e7;
+    msg.longitude_deg_1e7 = _zr_ppk_pos.data.longitude_deg_1e7;
+    msg.height_abs_cm = _zr_ppk_pos.data.height_abs_cm;
+    msg.cam_number = _zr_ppk_pos.data.cam_number;
+    zr_uav_ppk[_driver_index]->broadcast(msg);
+    _zr_ppk_pos.need_send = false;
+}
 
 #endif // HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -68,6 +68,17 @@ typedef struct
     uint8_t wp_number;
     bool armd; // bit len 1
 } zr_uav_state_data_t;
+
+typedef struct
+{
+    uint32_t week;              // bit len 32
+    uint32_t weekms;            // bit len 32
+    uint32_t latitude_deg_1e7;  // bit len 32
+    uint32_t longitude_deg_1e7; // bit len 32
+    uint32_t height_abs_cm;     // bit len 24
+    uint16_t index;             // bit len 16
+    uint8_t cam_number;
+} ppk_pos_data_t;
 /*
     Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
     the Callback will invoke registery to register the node as separate backend.
@@ -97,6 +108,8 @@ public:
         uint8_t hardware_patch; // bit len 8
     } zr_uav_version_data_t;
 
+  
+
    typedef enum
     {
         ZR_UAVCAN_REQUEST_TIMESTAMP=1,
@@ -139,6 +152,7 @@ public:
     void set_zr_uav_state(zr_uav_state_data_t &data);
     void set_zr_uav_version(zr_uav_version_data_t &data);
     void set_zr_flow_water(const uint8_t *data, uint16_t len);
+    void set_zr_ppk_pos(ppk_pos_data_t &data);
     template <typename DataType_>
     class RegistryBinder {
     protected:
@@ -224,6 +238,7 @@ private:
     void  zr_uav_pos_send();
     void  zr_uav_euler_send();
     void  zr_flow_water_send();
+    void  zr_uav_ppk_send();
     uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
 
     // UAVCAN parameters
@@ -247,6 +262,7 @@ private:
     uint8_t _SRV_armed;
     uint32_t _SRV_last_send_us;
     uint32_t _pos_last_send_ms;
+    uint32_t _ppk_last_send_ms;
     HAL_Semaphore SRV_sem;
     HAL_Semaphore uav_pos_sem;
     ///// LED /////
@@ -363,6 +379,13 @@ private:
         bool need_send;
     } _zr_water_flow;
 
+    struct
+    {
+        HAL_Semaphore sem;
+        ppk_pos_data_t data;
+        bool need_send;
+    } _zr_ppk_pos;
+
     // safety status send state
     uint32_t _last_safety_state_ms;
     uint32_t _last_uav_state_ms;

libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26134.Position.uavcan

@@ -1,9 +1,4 @@
-uint5 cam_photo_bit
-bool  intant_data
-bool  home_data
-bool  need_wait_data
-uint16 index
+uint8 p1
 int32 longitude_deg_1e7   
 int32 latitude_deg_1e7
-int24 height_abs_cm
-truncated uint56 usec
+int24 height_abs_cm

libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26135.PPKPOS.uavcan

@@ -0,0 +1,7 @@
+uint8 cam_number
+uint16 index
+uint16 week
+uint32 weekms
+int32 latitude_deg_1e7
+int32 longitude_deg_1e7
+int24 height_abs_cm

zr-v4.sh

@@ -1,3 +1,3 @@
 ./waf configure --board zr-v4
 ./waf copter
-cp ./build/zr-v4/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/flow-zrv4.1.11.px4
+cp ./build/zr-v4/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/flow-zrv4.1.15.px4