From a2fb61e5e47aefe538d3af902b7fb4b784fc1ce7 Mon Sep 17 00:00:00 2001 From: "Brown.Z" Date: Mon, 15 Aug 2022 18:57:44 +0800 Subject: [PATCH] =?UTF-8?q?Rover:=E5=A2=9E=E5=8A=A0zr=5Fapp=E6=96=87?= =?UTF-8?q?=E4=BB=B6=EF=BC=8C=E6=89=A7=E8=A1=8C1hz,10hz,50hz=E5=BE=AA?= =?UTF-8?q?=E7=8E=AF,=20parameter=E5=A2=9E=E5=8A=A0zr=5Fapp=E5=8F=82?= =?UTF-8?q?=E6=95=B0=20GCS=5FMAVLink:=E5=A2=9E=E5=8A=A0=E5=8F=91=E9=80=81?= =?UTF-8?q?=E8=AE=BE=E5=A4=87ID=20AC=5FZR=5FApp:=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E8=8E=B7=E5=8F=96=E7=89=88=E6=9C=AC=E5=8F=B7=E3=80=81ID?= =?UTF-8?q?=E5=8F=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Rover/GCS_Mavlink.cpp | 12 ++++++++++++ Rover/GCS_Mavlink.h | 2 ++ Rover/Parameters.cpp | 1 + Rover/Parameters.h | 2 ++ Rover/Rover.cpp | 5 +++++ Rover/Rover.h | 6 ++++++ Rover/wscript | 1 + Rover/zr_app.cpp | 27 +++++++++++++++++++++++++++ libraries/AC_ZR_APP/AC_ZR_App.cpp | 28 +++++++++++++++++++++++++++- libraries/AC_ZR_APP/AC_ZR_App.h | 1 + libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 3 ++- 12 files changed, 88 insertions(+), 2 deletions(-) create mode 100644 Rover/zr_app.cpp diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index a16a252542..e94eb4f3a5 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -1133,3 +1133,15 @@ uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const return 0; } #endif // HAL_HIGH_LATENCY2_ENABLED + +/** + * @brief send device id + * @ + * @return char* + */ +char* GCS_MAVLINK_Rover::gcs_get_sysid_id() +{ + static char buf[50]; + memcpy(buf,rover.zr_app.get_sysid_board_id(),50); + return buf; +} diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index 2b900f2f6d..ea985b7d11 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -30,6 +30,8 @@ protected: void send_nav_controller_output() const override; void send_pid_tuning() override; + + char* gcs_get_sysid_id() override ; private: diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index e86e02ed85..3643a90ffb 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -382,6 +382,7 @@ const AP_Param::Info Rover::var_info[] = { // @Path: ../libraries/AP_OSD/AP_OSD.cpp GOBJECT(osd, "OSD", AP_OSD), #endif + GOBJECT(zr_app, "ZR", AC_ZR_App), // @Group: // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp diff --git a/Rover/Parameters.h b/Rover/Parameters.h index d18e32f5f5..f7811c1a2f 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -226,6 +226,8 @@ public: // 254,255: reserved k_param_vehicle = 257, // vehicle common block of parameters + + k_param_zr_app = 258, // 253 - Logging Group }; AP_Int16 format_version; diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 0b2f28033d..5551c32dcd 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -129,6 +129,11 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { #if ADVANCED_FAILSAFE == ENABLED SCHED_TASK(afs_fs_check, 10, 200, 129), #endif + + + SCHED_TASK(zr_app_50hz, 50, 200, 132), + SCHED_TASK(zr_app_10hz, 10, 200, 135), + SCHED_TASK(zr_app_1hz, 1, 200, 138), }; diff --git a/Rover/Rover.h b/Rover/Rover.h index 83a7f59ff3..427bd45a18 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -37,6 +37,7 @@ #include #include #include +#include #if AP_SCRIPTING_ENABLED #include @@ -374,6 +375,7 @@ private: "_failsafe_priorities is missing the sentinel"); + AC_ZR_App zr_app; public: void failsafe_check(); // Motor test @@ -388,6 +390,10 @@ public: // Simple mode float simple_sin_yaw; + + void zr_app_50hz(); + void zr_app_10hz(); + void zr_app_1hz(); }; extern Rover rover; diff --git a/Rover/wscript b/Rover/wscript index 5d887bd32c..c8aade03b0 100644 --- a/Rover/wscript +++ b/Rover/wscript @@ -25,6 +25,7 @@ def build(bld): 'AP_WindVane', 'AR_Motors', 'AP_Torqeedo', + 'AC_ZR_APP', ], ) diff --git a/Rover/zr_app.cpp b/Rover/zr_app.cpp new file mode 100644 index 0000000000..45401b9100 --- /dev/null +++ b/Rover/zr_app.cpp @@ -0,0 +1,27 @@ +#include "Rover.h" + + +void Rover::zr_app_1hz() +{ + static bool before_fly = true; + if(arming.is_armed()){ + if(before_fly){ + before_fly = false; + gcs().send_text(MAV_SEVERITY_INFO,"%s",zr_app.get_sysid_board_id()); + } + relay.on(3); // 电子开关，保持通路 + // camera.set_in_arm_mode(true); + }else{ + relay.off(3); + } + +} + +void Rover::zr_app_10hz() +{ + + +} +void Rover::zr_app_50hz(){ + +} \ No newline at end of file diff --git a/libraries/AC_ZR_APP/AC_ZR_App.cpp b/libraries/AC_ZR_APP/AC_ZR_App.cpp index 7c8056526a..53f4ba366c 100644 --- a/libraries/AC_ZR_APP/AC_ZR_App.cpp +++ b/libraries/AC_ZR_APP/AC_ZR_App.cpp @@ -13,7 +13,9 @@ along with this program. If not, see . */ #include "AC_ZR_App.h" +#include +#include AC_ZR_App *AC_ZR_App::_singleton; @@ -252,4 +254,28 @@ uint8_t AC_ZR_App::get_battery_capacity(uint8_t type,float volt) break; } return bat_cnt; -} \ No newline at end of file +} + + +const char* AC_ZR_App::get_sysid_board_id(void) +{ + static char buf[50]; + uint8_t type = sysid_type; + int32_t boad_id =(int32_t) sysid_board_id; + switch (type) + { + case 1: + snprintf(buf, sizeof(buf), "Version: %u.%u.%u ,ID: M%07d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)boad_id); + break; + case 2: + snprintf(buf, sizeof(buf), "Version: %u.%u.%u ,ID: H%07d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)boad_id); + break; + + default: + snprintf(buf, sizeof(buf), "Version: %u.%u.%u ,ID: M%07d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)boad_id); + break; + } + + + return buf; +} diff --git a/libraries/AC_ZR_APP/AC_ZR_App.h b/libraries/AC_ZR_APP/AC_ZR_App.h index cafa5bbe98..af6d49bdb8 100644 --- a/libraries/AC_ZR_APP/AC_ZR_App.h +++ b/libraries/AC_ZR_APP/AC_ZR_App.h @@ -44,6 +44,7 @@ public: uint16_t get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn); uint8_t get_battery_capacity(uint8_t type,float volt); + const char* get_sysid_board_id(void); static const struct AP_Param::GroupInfo var_info[]; protected: diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index cfb1941ace..037f04e57e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -398,6 +398,8 @@ public: MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us); + virtual char* gcs_get_sysid_id() { return nullptr; } + protected: bool mavlink_coordinate_frame_to_location_alt_frame(MAV_FRAME coordinate_frame, diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 3b75c77187..58a680bb8e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3956,7 +3956,8 @@ void GCS_MAVLINK::send_banner() if (hal.rcout->get_output_mode_banner(banner_msg, sizeof(banner_msg))) { send_text(MAV_SEVERITY_INFO, "%s", banner_msg); } - + + send_text(MAV_SEVERITY_INFO, " %s", gcs_get_sysid_id()); #if HAL_INS_ENABLED // output any fast sampling status messages for (uint8_t i = 0; i < INS_MAX_BACKENDS; i++) {