From e9fe85bbff0212904383a2c050970ccc5cc36362 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Jan 2020 11:05:51 +1100 Subject: [PATCH 01/12] AP_RTC: added get_semaphore() --- libraries/AP_RTC/AP_RTC.cpp | 1 + libraries/AP_RTC/AP_RTC.h | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index c3e978d3fb..781e1602fb 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -63,6 +63,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; diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h index 373cc6b62d..d0c9f79a37 100644 --- a/libraries/AP_RTC/AP_RTC.h +++ b/libraries/AP_RTC/AP_RTC.h @@ -49,9 +49,15 @@ public: 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; From 1b79d134faa0a3aa572fc94c9a89ceb30b6b6703 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 30 Mar 2020 09:49:54 +1100 Subject: [PATCH 02/12] AP_RTC: use GCS_SEND_TEXT() --- libraries/AP_RTC/AP_RTC.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index 781e1602fb..e765a4e986 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -74,8 +74,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 From 7afafa7e1e1b59dc5b4dbeb0bb4156456063bb8f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 Oct 2020 21:34:40 +1100 Subject: [PATCH 03/12] AP_RTC: added mktime(), used by AP_Filesystem and AP_GPS --- libraries/AP_RTC/AP_RTC.cpp | 43 +++++++++++++++++++++++++++++++++++++ libraries/AP_RTC/AP_RTC.h | 3 +++ 2 files changed, 46 insertions(+) diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index e765a4e986..ef4af285fb 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -3,6 +3,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -205,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; diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h index d0c9f79a37..3f160e6ff6 100644 --- a/libraries/AP_RTC/AP_RTC.h +++ b/libraries/AP_RTC/AP_RTC.h @@ -44,6 +44,9 @@ public: 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; From 384f3f3839b59ec4ff12f365f9a103a9b80166ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 1 Feb 2021 13:26:33 -0300 Subject: [PATCH 04/12] AP_RTC: Add missing const in member functions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- libraries/AP_RTC/AP_RTC.cpp | 4 ++-- libraries/AP_RTC/AP_RTC.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index ef4af285fb..a439882a34 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -90,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; @@ -113,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; diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h index 3f160e6ff6..814b7b9640 100644 --- a/libraries/AP_RTC/AP_RTC.h +++ b/libraries/AP_RTC/AP_RTC.h @@ -38,9 +38,9 @@ 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); From d26584798a2557f7d507b40903c3aeb922bc326f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Aug 2020 15:05:14 +1000 Subject: [PATCH 05/12] =?UTF-8?q?AP=5FGPS:=20=E6=81=A2=E5=A4=8D=E4=B8=BA?= =?UTF-8?q?=E5=AE=98=E6=96=B94.1=E7=9A=84=E5=91=A8=E7=A7=92=E8=AE=A1?= =?UTF-8?q?=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_GPS/GPS_Backend.cpp | 70 ++++++++------------------------ libraries/AP_GPS/GPS_Backend.h | 3 -- libraries/AP_RTC/AP_RTC.h | 2 +- 3 files changed, 19 insertions(+), 56 deletions(-) diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index b00e33576f..eb99acba26 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -16,8 +16,9 @@ #include "AP_GPS.h" #include "GPS_Backend.h" #include - +#include #include +#include #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; -} - diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 7c7a9329f3..973d7611aa 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -20,8 +20,6 @@ #include #include -#include -#include #include "AP_GPS.h" class AP_GPS_Backend @@ -99,7 +97,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; diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h index 814b7b9640..da3908ec42 100644 --- a/libraries/AP_RTC/AP_RTC.h +++ b/libraries/AP_RTC/AP_RTC.h @@ -3,7 +3,7 @@ #include #include - +#include class AP_RTC { public: From 19bf9484b75514d479d9ffc2014aa2c6099233d1 Mon Sep 17 00:00:00 2001 From: binsir Date: Sat, 22 May 2021 17:29:16 +0800 Subject: [PATCH 06/12] =?UTF-8?q?=E7=B2=BE=E7=AE=80=E5=AE=9E=E6=97=B6uavca?= =?UTF-8?q?n=20pos=20=E6=B6=88=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 3 +-- .../dsdl/zrzk/equipment/uav/26134.Position.uavcan | 9 ++------- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 01485f130f..10b5ebd90c 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -1197,11 +1197,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); } } diff --git a/libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26134.Position.uavcan b/libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26134.Position.uavcan index 6995769ee5..129966c7fb 100644 --- a/libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26134.Position.uavcan +++ b/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 \ No newline at end of file +int24 height_abs_cm \ No newline at end of file From 35c4d73fa9e3e3ddb799bfd89f1d0d41009bad11 Mon Sep 17 00:00:00 2001 From: binsir Date: Tue, 25 May 2021 10:46:05 +0800 Subject: [PATCH 07/12] =?UTF-8?q?=E9=99=8D=E8=90=BD=20=E7=9B=B8=E6=9C=BA?= =?UTF-8?q?=E6=96=B0=E5=BB=BA=E6=96=87=E4=BB=B6=E5=A4=B9=E6=9B=B4=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Copter.h | 2 ++ ArduCopter/UserCode.cpp | 60 ++++++++++++++++++++++++++++------------- ArduCopter/mode_rtl.cpp | 22 ++++++++++----- 3 files changed, 58 insertions(+), 26 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 67027a8cf7..5fd5b015f5 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -1022,9 +1022,11 @@ 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; + uint8_t mkdir_step; }; extern Copter copter; diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 11ec4068e4..cd00d75bb3 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -144,36 +144,32 @@ void Copter::zr_set_uav_state_to_uavcan() #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 { @@ -181,4 +177,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 diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index ae65594b1a..98b756a306 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/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 From 4f9b939d869aa5be8a39dee1682ce9525821a146 Mon Sep 17 00:00:00 2001 From: binsir Date: Fri, 18 Jun 2021 18:33:29 +0800 Subject: [PATCH 08/12] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E7=A1=AC=E4=BB=B6?= =?UTF-8?q?=E7=89=88=E6=9C=AC=E6=A0=87=E5=BF=97=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 2 ++ ArduCopter/Parameters.h | 14 +++++++------- ArduCopter/UserCode.cpp | 2 +- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b89c2bdfae..951a7f7060 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -488,6 +488,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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ce9278ed85..d7562f4e97 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -393,17 +393,17 @@ public: k_param_zr_tk_delay, k_param_zr_rtl_delay, k_param_zr_use_rc, + k_param_hardware_flag, // the k_param_* space is 9-bits in size // 511: reserved }; AP_Int16 format_version; - AP_Int8 sysid_type; // modify by @Brown + AP_Int8 sysid_type; // modify by @Brown AP_Int16 sysid_board_name_1st;// modify by @Binsir - AP_Int32 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 // AP_Int16 sysid_this_mav; @@ -451,8 +451,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; @@ -486,7 +486,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 @@ -507,7 +507,7 @@ public: AP_Int32 zr_tk_req_alt; AP_Int16 zr_tk_delay; AP_Int16 zr_rtl_delay; - AP_Int8 zr_use_rc; + AP_Int8 zr_use_rc; // Note: keep initializers here in the same order as they are declared // above. diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index cd00d75bb3..51cb8c4ad9 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -137,7 +137,7 @@ 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); } } From b4949bf0f9c59a4309e15a1a19f246c98e99e398 Mon Sep 17 00:00:00 2001 From: binsir Date: Sat, 19 Jun 2021 14:43:04 +0800 Subject: [PATCH 09/12] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E7=9B=B8=E6=9C=BA?= =?UTF-8?q?=E6=8B=8D=E7=85=A7=E6=8C=87=E4=BB=A4=20=E5=8F=91=E9=80=81?= =?UTF-8?q?=E6=8B=8D=E7=85=A7=E5=BA=8F=E5=8F=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_Camera/AP_Camera.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 2afe470cec..065e66d6cb 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -478,7 +478,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); From d98b7aeeb026aa26e2f182c7811f3447f0a82b23 Mon Sep 17 00:00:00 2001 From: binsir Date: Sat, 19 Jun 2021 17:53:09 +0800 Subject: [PATCH 10/12] =?UTF-8?q?=E6=9B=B4=E6=94=B9mavlink=20=E7=9B=B8?= =?UTF-8?q?=E6=9C=BA=E5=8E=86=E5=8F=B2=E6=80=BB=E6=95=B0=20id=20=E4=B8=BA2?= =?UTF-8?q?24?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index 0013b46db8..6d62818f97 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 0013b46db8cb437b6ad77d736263597d90a2262d +Subproject commit 6d62818f975c25ffc507904658d8510d3de53c80 From e96c36d140ee69bc38ff4c8bd61bdb6ca7884104 Mon Sep 17 00:00:00 2001 From: zbr Date: Thu, 15 Apr 2021 23:04:30 +0800 Subject: [PATCH 11/12] =?UTF-8?q?=E5=A2=9E=E5=8A=A0nmea=E8=A7=A3=E6=9E=90m?= =?UTF-8?q?ark=EF=BC=8C=E9=80=9A=E8=BF=87event=E5=81=9A=E6=97=B6=E9=97=B4?= =?UTF-8?q?=E5=90=8C=E6=AD=A5=EF=BC=8C=E8=AE=B0=E5=BD=95RTKpos?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_GPS/AP_GPS.cpp | 28 ++-- libraries/AP_GPS/AP_GPS.h | 22 +++- libraries/AP_GPS/AP_GPS_NMEA.cpp | 202 ++++++++++++++++++++++++++++- libraries/AP_GPS/AP_GPS_NMEA.h | 115 +++++++++++++++- libraries/AP_GPS/GPS_Backend.h | 1 + libraries/AP_Logger/AP_Logger.h | 1 + libraries/AP_Logger/LogFile.cpp | 30 ++++- libraries/AP_Logger/LogStructure.h | 74 +++++++++-- modules/mavlink | 2 +- zr-v4.sh | 2 +- 10 files changed, 443 insertions(+), 34 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 35d92b1e96..0298ba7010 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -60,11 +60,13 @@ extern const AP_HAL::HAL &hal; // baudrates to try to detect GPSes with -const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U}; +// const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U}; +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; @@ -532,7 +534,11 @@ void AP_GPS::detect_instance(uint8_t instance) if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { if (_type[instance] == GPS_TYPE_HEMI) { send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); - } else { + }else if (_type[instance] == GPS_TYPE_SINO) { + send_blob_start(instance, AP_GPS_NMEA_SINO_INIT_STRING, strlen(AP_GPS_NMEA_SINO_INIT_STRING)); + }else if (_type[instance] == GPS_TYPE_UNICO) { + send_blob_start(instance, AP_GPS_NMEA_UNICORE_INIT_STRING, strlen(AP_GPS_NMEA_UNICORE_INIT_STRING)); + }else { send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } } @@ -562,33 +568,33 @@ 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 || - _type[instance] == GPS_TYPE_HEMI) && + } 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]); } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index b01a09857b..37f344c7ea 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -92,6 +92,8 @@ public: GPS_TYPE_MAV = 14, GPS_TYPE_NOVA = 15, GPS_TYPE_HEMI = 16, // hemisphere NMEA + GPS_TYPE_SINO = 17, // sino NMEA + GPS_TYPE_UNICO = 18, // unicorecomm NMEA }; /// GPS status codes @@ -166,7 +168,22 @@ public: uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999) int32_t rtk_iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses }; - + struct rtk_mark_s + { + uint16_t week; + float second; + double lat; + double lon; + double hgt; + float undulation; + float lat_sigma; + float lon_sigma; + float hgt_sigma; + float diff_age; + float sol_age; + uint8_t SVs; + uint8_t solnSVs; + }; /// Startup initialisation. void init(const AP_SerialManager& serial_manager); @@ -486,7 +503,8 @@ private: GPS_State state[GPS_MAX_INSTANCES]; AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS]; AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS]; - + // rtk_mark_s rtk_mark[GPS_MAX_INSTANCES]; + /// primary GPS instance uint8_t primary_instance; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 5e0631ca2c..697f2e42f0 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -36,6 +36,8 @@ #include "AP_GPS_NMEA.h" +#include +#include extern const AP_HAL::HAL& hal; // optionally log all NMEA data for debug purposes @@ -82,7 +84,10 @@ bool AP_GPS_NMEA::_decode(char c) switch (c) { case ',': // term terminators + case ';': // term terminators _parity ^= c; + + markCRC = aulCrcTable[(markCRC ^ c) & 0xff] ^ (markCRC >> 8); FALLTHROUGH; case '\r': case '\n': @@ -97,8 +102,10 @@ bool AP_GPS_NMEA::_decode(char c) return valid_sentence; case '$': // sentence begin + case '#': // sentence begin _term_number = _term_offset = 0; _parity = 0; + markCRC = 0; _sentence_type = _GPS_SENTENCE_OTHER; _is_checksum_term = false; _gps_data_good = false; @@ -109,8 +116,11 @@ bool AP_GPS_NMEA::_decode(char c) // ordinary characters if (_term_offset < sizeof(_term) - 1) _term[_term_offset++] = c; - if (!_is_checksum_term) + if (!_is_checksum_term){ _parity ^= c; + + markCRC = aulCrcTable[(markCRC ^ c) & 0xff] ^ (markCRC >> 8); + } return valid_sentence; } @@ -143,6 +153,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 */ @@ -233,7 +272,47 @@ bool AP_GPS_NMEA::_term_complete() return false; } const uint8_t checksum = (nibble_high << 4u) | nibble_low; - if (checksum == _parity) { + if(_gps_data_good && (_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )){ + if(get_new_mark){ + const uint8_t mark_crc = markCRC >> 24; + + // gcs().send_text(MAV_SEVERITY_INFO, "week: %d, %f,crc:0x%02x--0x%02x--0x%02x",gps_mark.week,gps_mark.second,checksum,_parity,mark_crc); + // gcs().send_text(MAV_SEVERITY_INFO, "llh: %f , %f , %f\n",gps_mark.lat,gps_mark.lon,gps_mark.hgt); + if(checksum == mark_crc){ + 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); + get_new_mark = false; + 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 + ); + } + } + } + + return _have_new_message(); + }else{ + + gcs().send_text(MAV_SEVERITY_INFO, "crc err:0x%02x--0x%02x--0x%02x",checksum,_parity,mark_crc); + } + get_new_mark = false; + } + } + if (checksum == _parity ) { if (_gps_data_good) { uint32_t now = AP_HAL::millis(); switch (_sentence_type) { @@ -296,6 +375,14 @@ bool AP_GPS_NMEA::_term_complete() state.gps_yaw = wrap_360(_new_gps_yaw*0.01f); state.have_gps_yaw = true; break; + // case _GPS_SENTENCE_MARK: + // case _GPS_SENTENCE_EVENT: + // if(get_new_mark){ + // gcs().send_text(MAV_SEVERITY_INFO, "week: %d, %f",gps_mark.week,gps_mark.second); + // gcs().send_text(MAV_SEVERITY_INFO, "llh: %f , %f , %f\n",gps_mark.lat,gps_mark.lon,gps_mark.hgt); + // get_new_mark = false; + // } + // break; } } else { switch (_sentence_type) { @@ -328,8 +415,10 @@ bool AP_GPS_NMEA::_term_complete() const char *term_type = &_term[2]; if (strcmp(term_type, "RMC") == 0) { _sentence_type = _GPS_SENTENCE_RMC; + // gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type); } else if (strcmp(term_type, "GGA") == 0) { _sentence_type = _GPS_SENTENCE_GGA; + // gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type); } else if (strcmp(term_type, "HDT") == 0) { _sentence_type = _GPS_SENTENCE_HDT; // HDT doesn't have a data qualifier @@ -339,6 +428,20 @@ bool AP_GPS_NMEA::_term_complete() // VTG may not contain a data qualifier, presume the solution is good // unless it tells us otherwise. _gps_data_good = true; + // gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type); + } else if (strcmp(term_type, "ENTALLA") == 0) { + _sentence_type = _GPS_SENTENCE_EVENT; + // VTG may not contain a data qualifier, presume the solution is good + // unless it tells us otherwise. + _gps_data_good = true; + // gcs().send_text(MAV_SEVERITY_INFO, "get: %s",term_type); + } else if (strcmp(term_type, "RKPOSA") == 0) { + _sentence_type = _GPS_SENTENCE_MARK; + // VTG may not contain a data qualifier, presume the solution is good + // unless it tells us otherwise. + + // gcs().send_text(MAV_SEVERITY_INFO, "get: %s",term_type); + _gps_data_good = true; } else { _sentence_type = _GPS_SENTENCE_OTHER; } @@ -416,10 +519,92 @@ bool AP_GPS_NMEA::_term_complete() break; } } + +#if 1 + if ((_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )) { + switch (_sentence_type + _term_number) { + // operational status + // + case _GPS_SENTENCE_MARK + 5: // week + case _GPS_SENTENCE_EVENT + 5: // week + gps_mark.week = atol(_term); + break; + case _GPS_SENTENCE_MARK + 6: // second + case _GPS_SENTENCE_EVENT + 6: // 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); + break; + case _GPS_SENTENCE_MARK + 13: // lon + case _GPS_SENTENCE_EVENT + 23: + gps_mark.lon = atof(_term); + break; + case _GPS_SENTENCE_MARK + 14: // hgt + case _GPS_SENTENCE_EVENT + 24: + gps_mark.hgt = atof(_term); + break; + case _GPS_SENTENCE_MARK + 15: // undulation + case _GPS_SENTENCE_EVENT + 25: + gps_mark.undulation = atof(_term); + break; + case _GPS_SENTENCE_MARK + 17: // lat_sigma + case _GPS_SENTENCE_EVENT + 27: + gps_mark.lat_sigma = atof(_term); + break; + case _GPS_SENTENCE_MARK + 18: // lon_sigma + case _GPS_SENTENCE_EVENT + 28: + gps_mark.lon_sigma = atof(_term); + break; + case _GPS_SENTENCE_MARK + 19: // hgt_sigma + case _GPS_SENTENCE_EVENT + 29: + gps_mark.hgt_sigma = atof(_term); + break; + case _GPS_SENTENCE_MARK + 21: // diff_age + case _GPS_SENTENCE_EVENT + 31: + gps_mark.diff_age = atof(_term); + break; + case _GPS_SENTENCE_MARK + 22: // sol_age + case _GPS_SENTENCE_EVENT + 32: + gps_mark.sol_age = atof(_term); + break; + case _GPS_SENTENCE_MARK + 23: // SVs + case _GPS_SENTENCE_EVENT + 33: + gps_mark.SVs = atol(_term); + case _GPS_SENTENCE_MARK + 24: // solnSVs + case _GPS_SENTENCE_EVENT + 34: + gps_mark.solnSVs = atol(_term); + get_new_mark = true; + + break; + } + } +#endif return false; } +bool +AP_GPS_NMEA::_send_message( void *msg, uint16_t size) +{ + if (port->txspace() < size) { + return false; + } + + port->write((const uint8_t *)msg, size); + return true; +} + + +const char AP_GPS_NMEA::_initialisation_blob[] = AP_GPS_NMEA_HEMISPHERE_INIT_STRING; + +void AP_GPS_NMEA::send_init_blob(uint8_t instance, AP_GPS &gps) +{ + gps.send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); +} + /* detect a NMEA GPS. Adds one byte, and returns true if the stream matches a NMEA string @@ -458,3 +643,16 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data) } return false; } + + +// Calculate and return the CRC for usA binary buffer +// unsigned long CalculateCRC32(u_char *szBuf, int iSize) +// { +// int iIndex; +// u_long ulCRC = 0; +// for (iIndex=0; iIndex> 8); +// } +// return ulCRC; +// } \ No newline at end of file diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 7bc9a7c5cd..b2e9c47963 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -64,7 +64,8 @@ public: static bool _detect(struct NMEA_detect_state &state, uint8_t data); const char *name() const override { return "NMEA"; } - + static void send_init_blob(uint8_t instance, AP_GPS &gps); + private: /// Coding for the GPS sentences that the parser handles enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value. @@ -72,6 +73,8 @@ private: _GPS_SENTENCE_GGA = 64, _GPS_SENTENCE_VTG = 96, _GPS_SENTENCE_HDT = 128, + _GPS_SENTENCE_MARK = 160, + _GPS_SENTENCE_EVENT = 200, _GPS_SENTENCE_OTHER = 0 }; @@ -90,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. @@ -143,6 +147,23 @@ private: uint32_t _last_VTG_ms = 0; uint32_t _last_HDT_ms = 0; + struct mark_s + { + uint16_t week; + uint32_t second; + double lat; + double lon; + double hgt; + float undulation; + float lat_sigma; + float lon_sigma; + float hgt_sigma; + float diff_age; + float sol_age; + uint8_t SVs; + uint8_t solnSVs; + }gps_mark; + bool get_new_mark; /// @name Init strings /// In ::init, an attempt is made to configure the GPS /// unit to send just the messages that we are interested @@ -154,12 +175,92 @@ private: //@} static const char _initialisation_blob[]; + + bool _send_message( void *msg, uint16_t size); + unsigned long markCRC; + + const unsigned long aulCrcTable[256] = + { + 0x00000000UL, 0x77073096UL, 0xee0e612cUL, 0x990951baUL, 0x076dc419UL,0x706af48fUL, + 0xe963a535UL, 0x9e6495a3UL, 0x0edb8832UL, 0x79dcb8a4UL, 0xe0d5e91eUL,0x97d2d988UL, + 0x09b64c2bUL, 0x7eb17cbdUL, 0xe7b82d07UL, 0x90bf1d91UL, 0x1db71064UL,0x6ab020f2UL, + 0xf3b97148UL, 0x84be41deUL, 0x1adad47dUL, 0x6ddde4ebUL, 0xf4d4b551UL,0x83d385c7UL, + 0x136c9856UL, 0x646ba8c0UL, 0xfd62f97aUL, 0x8a65c9ecUL, 0x14015c4fUL,0x63066cd9UL, + 0xfa0f3d63UL, 0x8d080df5UL, 0x3b6e20c8UL, 0x4c69105eUL, 0xd56041e4UL,0xa2677172UL, + 0x3c03e4d1UL, 0x4b04d447UL, 0xd20d85fdUL, 0xa50ab56bUL, 0x35b5a8faUL,0x42b2986cUL, + 0xdbbbc9d6UL, 0xacbcf940UL, 0x32d86ce3UL, 0x45df5c75UL, 0xdcd60dcfUL,0xabd13d59UL, + 0x26d930acUL, 0x51de003aUL, 0xc8d75180UL, 0xbfd06116UL, 0x21b4f4b5UL,0x56b3c423UL, + 0xcfba9599UL, 0xb8bda50fUL, 0x2802b89eUL, 0x5f058808UL, 0xc60cd9b2UL,0xb10be924UL, + 0x2f6f7c87UL, 0x58684c11UL, 0xc1611dabUL, 0xb6662d3dUL, 0x76dc4190UL,0x01db7106UL, + 0x98d220bcUL, 0xefd5102aUL, 0x71b18589UL, 0x06b6b51fUL, 0x9fbfe4a5UL,0xe8b8d433UL, + 0x7807c9a2UL, 0x0f00f934UL, 0x9609a88eUL, 0xe10e9818UL, 0x7f6a0dbbUL,0x086d3d2dUL, + 0x91646c97UL, 0xe6635c01UL, 0x6b6b51f4UL, 0x1c6c6162UL, 0x856530d8UL,0xf262004eUL, + 0x6c0695edUL, 0x1b01a57bUL, 0x8208f4c1UL, 0xf50fc457UL, 0x65b0d9c6UL,0x12b7e950UL, + 0x8bbeb8eaUL, 0xfcb9887cUL, 0x62dd1ddfUL, 0x15da2d49UL, 0x8cd37cf3UL,0xfbd44c65UL, + 0x4db26158UL, 0x3ab551ceUL, 0xa3bc0074UL, 0xd4bb30e2UL, 0x4adfa541UL,0x3dd895d7UL, + 0xa4d1c46dUL, 0xd3d6f4fbUL, 0x4369e96aUL, 0x346ed9fcUL, 0xad678846UL,0xda60b8d0UL, + 0x44042d73UL, 0x33031de5UL, 0xaa0a4c5fUL, 0xdd0d7cc9UL, 0x5005713cUL,0x270241aaUL, + 0xbe0b1010UL, 0xc90c2086UL, 0x5768b525UL, 0x206f85b3UL, 0xb966d409UL,0xce61e49fUL, + 0x5edef90eUL, 0x29d9c998UL, 0xb0d09822UL, 0xc7d7a8b4UL, 0x59b33d17UL,0x2eb40d81UL, + 0xb7bd5c3bUL, 0xc0ba6cadUL, 0xedb88320UL, 0x9abfb3b6UL, 0x03b6e20cUL,0x74b1d29aUL, + 0xead54739UL, 0x9dd277afUL, 0x04db2615UL, 0x73dc1683UL, 0xe3630b12UL,0x94643b84UL, + 0x0d6d6a3eUL, 0x7a6a5aa8UL, 0xe40ecf0bUL, 0x9309ff9dUL, 0x0a00ae27UL,0x7d079eb1UL, + 0xf00f9344UL, 0x8708a3d2UL, 0x1e01f268UL, 0x6906c2feUL, 0xf762575dUL,0x806567cbUL, + 0x196c3671UL, 0x6e6b06e7UL, 0xfed41b76UL, 0x89d32be0UL, 0x10da7a5aUL,0x67dd4accUL, + 0xf9b9df6fUL, 0x8ebeeff9UL, 0x17b7be43UL, 0x60b08ed5UL, 0xd6d6a3e8UL,0xa1d1937eUL, + 0x38d8c2c4UL, 0x4fdff252UL, 0xd1bb67f1UL, 0xa6bc5767UL, 0x3fb506ddUL,0x48b2364bUL, + 0xd80d2bdaUL, 0xaf0a1b4cUL, 0x36034af6UL, 0x41047a60UL, 0xdf60efc3UL,0xa867df55UL, + 0x316e8eefUL, 0x4669be79UL, 0xcb61b38cUL, 0xbc66831aUL, 0x256fd2a0UL,0x5268e236UL, + 0xcc0c7795UL, 0xbb0b4703UL, 0x220216b9UL, 0x5505262fUL, 0xc5ba3bbeUL,0xb2bd0b28UL, + 0x2bb45a92UL, 0x5cb36a04UL, 0xc2d7ffa7UL, 0xb5d0cf31UL, 0x2cd99e8bUL,0x5bdeae1dUL, + 0x9b64c2b0UL, 0xec63f226UL, 0x756aa39cUL, 0x026d930aUL, 0x9c0906a9UL,0xeb0e363fUL, + 0x72076785UL, 0x05005713UL, 0x95bf4a82UL, 0xe2b87a14UL, 0x7bb12baeUL,0x0cb61b38UL, + 0x92d28e9bUL, 0xe5d5be0dUL, 0x7cdcefb7UL, 0x0bdbdf21UL, 0x86d3d2d4UL,0xf1d4e242UL, + 0x68ddb3f8UL, 0x1fda836eUL, 0x81be16cdUL, 0xf6b9265bUL, 0x6fb077e1UL,0x18b74777UL, + 0x88085ae6UL, 0xff0f6a70UL, 0x66063bcaUL, 0x11010b5cUL, 0x8f659effUL,0xf862ae69UL, + 0x616bffd3UL, 0x166ccf45UL, 0xa00ae278UL, 0xd70dd2eeUL, 0x4e048354UL,0x3903b3c2UL, + 0xa7672661UL, 0xd06016f7UL, 0x4969474dUL, 0x3e6e77dbUL, 0xaed16a4aUL,0xd9d65adcUL, + 0x40df0b66UL, 0x37d83bf0UL, 0xa9bcae53UL, 0xdebb9ec5UL, 0x47b2cf7fUL,0x30b5ffe9UL, + 0xbdbdf21cUL, 0xcabac28aUL, 0x53b39330UL, 0x24b4a3a6UL, 0xbad03605UL,0xcdd70693UL, + 0x54de5729UL, 0x23d967bfUL, 0xb3667a2eUL, 0xc4614ab8UL, 0x5d681b02UL,0x2a6f2b94UL, + 0xb40bbe37UL, 0xc30c8ea1UL, 0x5a05df1bUL, 0x2d02ef8dUL + }; }; #define AP_GPS_NMEA_HEMISPHERE_INIT_STRING \ - "$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */ \ - "$JASC,GPGGA,5\r\n" /* GGA at 5Hz */ \ - "$JASC,GPRMC,5\r\n" /* RMC at 5Hz */ \ - "$JASC,GPVTG,5\r\n" /* VTG at 5Hz */ \ - "$JASC,GPHDT,5\r\n" /* HDT at 5Hz */ \ - "$JMODE,SBASR,YES\r\n" /* Enable SBAS */ + "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 */ + + + +#define AP_GPS_NMEA_SINO_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 markposa onnew\r\n" /* RMC at 5Hz */ \ + "log com1 marktimea onnew\r\n" /* VTG at 5Hz */ \ + "interfacemode com1 auto auto on\r\n" /* HDT at 5Hz */ \ + "saveconfig\r\n" /* Enable SBAS */ + +#define AP_GPS_NMEA_UNICORE_INIT_STRING \ + "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 diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 973d7611aa..49ef95a89f 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -69,6 +69,7 @@ protected: AP_HAL::UARTDriver *port; ///< UART we are attached to AP_GPS &gps; ///< access to frontend (for parameters) AP_GPS::GPS_State &state; ///< public state for this instance + // AP_GPS::rtk_mark_s &rtk_mark; /// @Brown for rtk mark // common utility functions int32_t swap_int32(int32_t v) const; diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index b998ffd725..613a017a50 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -285,6 +285,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, 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, ...); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 97686a0201..56d4d1ead4 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -1027,4 +1027,32 @@ void AP_Logger::Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2 flag:flag }; WriteBlock(&pkt, sizeof(pkt)); -} \ No newline at end of file +} + +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(); + + struct log_rtk_mark_pos pkt + { + LOG_PACKET_HEADER_INIT(LOG_RTK_MARK_POS), + time_us : AP_HAL::micros64(), + roll : (int16_t)ahrs.roll_sensor, + pitch : (int16_t)ahrs.pitch_sensor, + yaw : (uint16_t)ahrs.yaw_sensor, + week :week, + second :second, + lat :llh.x, + lon :llh.y , + hgt :llh.z , + undulation :undulation , + lat_sigma :sigma.x , + lon_sigma :sigma.y , + hgt_sigma :sigma.z , + diff_age :diff_age , + sol_age :sol_age , + SVs :0 , + solnSVs : 0 + }; + WriteBlock(&pkt, sizeof(pkt)); +} diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 27dafa5281..84bd5e34a7 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1217,6 +1217,30 @@ struct PACKED log_Camera_FeedBack uint8_t flag; }; + +struct PACKED log_rtk_mark_pos +{ + LOG_PACKET_HEADER; + uint64_t time_us; + int16_t roll; + int16_t pitch; + uint16_t yaw; + uint16_t week; + uint32_t second; + double lat; + double lon; + double hgt; + float undulation; + float lat_sigma; + float lon_sigma; + float hgt_sigma; + float diff_age; + float sol_age; + uint8_t SVs; + uint8_t solnSVs; +}; + + // FMT messages define all message formats other than FMT // UNIT messages define units which can be referenced by FMTU messages // FMTU messages associate types (e.g. centimeters/second/second) to FMT message fields @@ -1414,7 +1438,9 @@ struct PACKED log_Camera_FeedBack { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \ "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" },\ { LOG_CAM_FEEDBACK_MSG, sizeof(log_Camera_FeedBack), \ - "CAMF","QHHHHHHB","TimeUS,Index,CAM1,CAM2,CAM3,CAM4,CAM5,FLAG", "s-------", "F-------" } + "CAMF","QHHHHHHB","TimeUS,Index,CAM1,CAM2,CAM3,CAM4,CAM5,FLAG", "s-------", "F-------" },\ + { LOG_RTK_MARK_POS, sizeof(log_rtk_mark_pos), \ + "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), \ @@ -1688,14 +1714,14 @@ enum LogMessages : uint8_t { LOG_GPS_UBX2_MSG, LOG_GPS2_UBX1_MSG, LOG_GPS2_UBX2_MSG, - LOG_ESC1_MSG, - LOG_ESC2_MSG, - LOG_ESC3_MSG, - LOG_ESC4_MSG, - LOG_ESC5_MSG, - LOG_ESC6_MSG, - LOG_ESC7_MSG, - LOG_ESC8_MSG, + // LOG_ESC1_MSG, + // LOG_ESC2_MSG, + // LOG_ESC3_MSG, + // LOG_ESC4_MSG, + // LOG_ESC5_MSG, + // LOG_ESC6_MSG, + // LOG_ESC7_MSG, + // LOG_ESC8_MSG, LOG_BAR2_MSG, LOG_ARSP_MSG, LOG_ATTITUDE_MSG, @@ -1790,11 +1816,41 @@ enum LogMessages : uint8_t { LOG_OA_DIJKSTRA_MSG, LOG_RFND2_MSG, LOG_CAM_FEEDBACK_MSG, + LOG_RTK_MARK_POS, _LOG_LAST_MSG_ }; static_assert(_LOG_LAST_MSG_ <= 255, "Too many message formats"); +enum LogMessages_back : uint8_t { + + LOG_ESC1_MSG = 154, + LOG_ESC2_MSG, + LOG_ESC3_MSG, + LOG_ESC4_MSG, + LOG_ESC5_MSG, + LOG_ESC6_MSG, + LOG_ESC7_MSG, + LOG_ESC8_MSG, + + // LOG_CURRENT3_MSG, + // LOG_CURRENT4_MSG, + // LOG_CURRENT5_MSG, + // LOG_CURRENT6_MSG, + // LOG_CURRENT7_MSG, + // LOG_CURRENT8_MSG, + // LOG_CURRENT9_MSG, + // LOG_CURRENT_CELLS_MSG, + // LOG_CURRENT_CELLS2_MSG, + // LOG_CURRENT_CELLS3_MSG, + // LOG_CURRENT_CELLS4_MSG, + // LOG_CURRENT_CELLS5_MSG, + // LOG_CURRENT_CELLS6_MSG, + // LOG_CURRENT_CELLS7_MSG, + // LOG_CURRENT_CELLS8_MSG, + // LOG_CURRENT_CELLS9_MSG +}; + enum LogOriginType { ekf_origin = 0, ahrs_home = 1 diff --git a/modules/mavlink b/modules/mavlink index 6d62818f97..72b407db39 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 6d62818f975c25ffc507904658d8510d3de53c80 +Subproject commit 72b407db3991ed74631fa59a5e826615f569819f diff --git a/zr-v4.sh b/zr-v4.sh index 22edc39615..c577c4d58e 100755 --- a/zr-v4.sh +++ b/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/qusd-zrv4-v4.0.8-rc8.px4 +cp ./build/zr-v4/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/quard-zrv4-gps.px4 From 7b806a66441c01024e73c221a4f2ee1449a33289 Mon Sep 17 00:00:00 2001 From: binsir Date: Wed, 7 Jul 2021 09:37:29 +0800 Subject: [PATCH 12/12] =?UTF-8?q?=E6=B7=BB=E5=8A=A0ppkpos=E5=88=B0uavcan?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_GPS/AP_GPS_NMEA.cpp | 61 ++++++++++++++----- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 34 ++++++++++- libraries/AP_UAVCAN/AP_UAVCAN.h | 24 +++++++- .../zrzk/equipment/uav/26135.PPKPOS.uavcan | 7 +++ 4 files changed, 109 insertions(+), 17 deletions(-) create mode 100644 libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26135.PPKPOS.uavcan diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 697f2e42f0..a7807e47c7 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -38,6 +38,13 @@ #include #include + +#if HAL_WITH_UAVCAN +#include +#include +#include +#endif + extern const AP_HAL::HAL& hal; // optionally log all NMEA data for debug purposes @@ -282,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); + } } } diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 10b5ebd90c..7291f1aa56 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -58,6 +58,8 @@ #include #include +#include + #include #include @@ -144,7 +146,7 @@ static uavcan::Publisher* zr_uav_state[MAX_NUMBER_O static uavcan::Publisher* zr_uav_timestamp[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher* zr_uav_euler[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher * zr_uav_pos[MAX_NUMBER_OF_CAN_DRIVERS]; - +static uavcan::Publisher* zr_uav_ppk[MAX_NUMBER_OF_CAN_DRIVERS]; // subscribers // handler SafteyButton @@ -359,6 +361,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) zr_uav_pos[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(100)); zr_uav_pos[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + zr_uav_ppk[driver_index] = new uavcan::Publisher(*_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(*_node); if (safety_button_listener[driver_index]) { safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button)); @@ -487,6 +493,7 @@ void AP_UAVCAN::loop(void) zr_uav_version_send(); zr_uav_pos_send(); zr_uav_euler_send(); + zr_uav_ppk_send(); AP::uavcan_server().verify_nodes(this); } } @@ -1235,4 +1242,29 @@ void AP_UAVCAN::handle_zr_camera_sum(AP_UAVCAN *ap_uavcan, uint8_t node_id, cons camera->update_zr_cam_sum(pkt); } +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 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index aca872952b..cdff8c6f80 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -65,6 +65,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. @@ -94,6 +105,8 @@ public: uint8_t hardware_patch; // bit len 8 } zr_uav_version_data_t; + + typedef enum { ZR_UAVCAN_REQUEST_TIMESTAMP=1, @@ -131,6 +144,7 @@ public: void set_zr_timestamp(uint8_t id,uint64_t timestamp); 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_ppk_pos(ppk_pos_data_t &data); template class RegistryBinder { protected: @@ -215,6 +229,7 @@ private: void zr_uav_version_send(); void zr_uav_pos_send(); void zr_uav_euler_send(); + void zr_uav_ppk_send(); uavcan::PoolAllocator _node_allocator; // UAVCAN parameters @@ -238,6 +253,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 ///// @@ -345,9 +361,15 @@ private: uint8_t param2; // bit len 8 uint8_t param3; // bit len 8 uint8_t param4; // bit len 8 - } zr_request_t; + 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; diff --git a/libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26135.PPKPOS.uavcan b/libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26135.PPKPOS.uavcan new file mode 100644 index 0000000000..d781bd4f85 --- /dev/null +++ b/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 \ No newline at end of file