From 9509e87f8b5407929a86e699cc78555e74a66c2e Mon Sep 17 00:00:00 2001 From: nagezeng Date: Sat, 2 Apr 2022 18:42:21 +0800 Subject: [PATCH] =?UTF-8?q?sdk=E6=8E=A7=E5=88=B6=E8=B0=83=E6=95=B4?= =?UTF-8?q?=EF=BC=8C=E9=A3=9E=E8=A1=8C=E6=B5=8B=E8=AF=95=E6=88=90=E5=8A=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Copter.h | 2 +- ArduCopter/version.h | 8 +-- ArduCopter/zr_app.cpp | 65 +++++++++++++++++------- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 3 +- libraries/AP_RangeFinder/RangeFinder.cpp | 3 +- modules/mavlink | 2 +- rs100.sh | 2 +- 7 files changed, 59 insertions(+), 26 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index e321e0b9a4..d1f624397f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -681,7 +681,7 @@ private: // zr_app.cpp bool start_takeoff(float alt); - bool set_target_location(const Location& target_loc); + bool set_target_location(const Location& target_loc,int32_t yaw_cd); bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt); bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel); bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative); diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 0fce9641e0..20c294471f 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -9,14 +9,14 @@ #undef GIT_VERSION #define GIT_VERSION "3" #endif -#define THISFIRMWARE "ZRUAV v4.1.16" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.3.0-dev" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,1,16,FIRMWARE_VERSION_TYPE_OFFICIAL +#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_OFFICIAL #define FW_MAJOR 4 -#define FW_MINOR 1 -#define FW_PATCH 16 +#define FW_MINOR 3 +#define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL /** * AVDv6: avoid.get_zr_mode() = 3时可以用Loiter模式测试自动避障 diff --git a/ArduCopter/zr_app.cpp b/ArduCopter/zr_app.cpp index dad4c1cd13..99e8717651 100644 --- a/ArduCopter/zr_app.cpp +++ b/ArduCopter/zr_app.cpp @@ -16,14 +16,15 @@ bool Copter::start_takeoff(float alt) } // set target location (for use by scripting) -bool Copter::set_target_location(const Location& target_loc) +bool Copter::set_target_location(const Location& target_loc,int32_t yaw_cd) { // exit if vehicle is not in Guided mode or Auto-Guided mode if (!flightmode->in_guided_mode()) { return false; } - return mode_guided.set_destination(target_loc); + // return mode_guided.set_destination(target_loc); + return mode_guided.set_destination(target_loc, true, (float)yaw_cd, true, 200.0, false); } // set target position (for use by scripting) @@ -56,7 +57,15 @@ bool Copter::set_target_velocity_NED(const Vector3f& vel_ned) void Copter::zr_app_10hz() { - + if(zr_serial_api.data_trans_init){ +// void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining) + uint16_t now_volt = uint16_t(battery.voltage() * 100); + zr_serial_api.get_vehicle_status((uint8_t)control_mode,(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct()); + } + +} +void Copter::zr_app_50hz(){ + if(zr_serial_api.data_trans_init){ zr_serial_api.update(); }else{ @@ -69,15 +78,6 @@ void Copter::zr_app_10hz() } } - if(zr_serial_api.data_trans_init){ -// void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining) - uint16_t now_volt = uint16_t(battery.voltage() * 100); - zr_serial_api.get_vehicle_status((uint8_t)control_mode,(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct()); - } - -} -void Copter::zr_app_50hz(){ - if(zr_serial_api.data_trans_init){ Vector3l pos_neu_cm; @@ -117,6 +117,8 @@ void Copter::zr_app_50hz(){ float yaw_deg; bool new_data = zr_serial_api.get_control_data((uint8_t)control_mode ,msg_type,data,yaw_deg); if(new_data){ + + int32_t ahrs_yaw_cd = wrap_360_cd(int32_t(yaw_deg * 100)); gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg); switch (msg_type) { @@ -140,21 +142,50 @@ void Copter::zr_app_50hz(){ target_loc.lng = data.y; target_loc.alt = (data.z + 500); // target_loc.alt = (data.z + 500)/100.0; - set_target_location(target_loc); + set_target_location(target_loc,ahrs_yaw_cd); gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt); } break; case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL: if(motors->armed()){ Vector3f target_vel; - target_vel.x = data.x / 100.0; - target_vel.y = data.y / 100.0; - target_vel.z = data.z / 100.0; + target_vel.x = data.x / 1.0; + target_vel.y = data.y / 1.0; + target_vel.z = data.z / 1.0; // target_loc.alt = (data.z + 500)/100.0; - set_target_velocity_NED(target_vel); + // set_target_velocity_NED(target_vel); + mode_guided.set_velocity(target_vel,true,ahrs_yaw_cd,false,0.0f,false,true); + gcs().send_text(MAV_SEVERITY_INFO,"set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z); } break; + case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_P: + if(motors->armed()){ + Vector3f target_vel; + target_vel.x = data.x / 1.0; + target_vel.y = data.y / 1.0; + target_vel.z = data.z / 1.0; + // float speed_forward = target_vel.x * ahrs.cos_yaw() + target_vel.y * ahrs.sin_yaw(); + // float speed_right = -target_vel.x * ahrs.sin_yaw() + target_vel.y * ahrs.cos_yaw(); + + // rotate from body-frame to NE frame + const float ne_x = target_vel.x * ahrs.cos_yaw() - target_vel.y * ahrs.sin_yaw(); + const float ne_y = target_vel.x * ahrs.sin_yaw() + target_vel.y * ahrs.cos_yaw(); + // target_loc.alt = (data.z + 500)/100.0; + // set_target_velocity_NED(target_vel); + + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return; + } + + // convert vector to neu in cm + const Vector3f vel_neu_cms(ne_x, ne_y, -target_vel.z); + mode_guided.set_velocity(vel_neu_cms,true,ahrs_yaw_cd,false,0.0f,false,true); + + gcs().send_text(MAV_SEVERITY_INFO,"set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd); + } + break; default: break; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index b10f255ccf..01cfd81228 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -337,7 +337,8 @@ void NavEKF2_core::InitialiseVariables() have_table_earth_field = false; // initialise pre-arm message - hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising"); + // hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising"); + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 初始化"); InitialiseVariablesMag(); } diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 10d5da45a2..98de9c634c 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -700,7 +700,8 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le { for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { if ((params[i].type != RangeFinder_TYPE_NONE) && (drivers[i] == nullptr)) { - hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1); + // hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1); + hal.util->snprintf(failure_msg, failure_msg_len, "未检测到测距传感器 %d", i + 1); return false; } } diff --git a/modules/mavlink b/modules/mavlink index 5fc095d170..86cf7601a7 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 5fc095d17043557589ecc6a28d62fb234081fc87 +Subproject commit 86cf7601a72f065f6c2dd8b10a602a73f827ac64 diff --git a/rs100.sh b/rs100.sh index bb628ec5e0..74f89d52be 100755 --- a/rs100.sh +++ b/rs100.sh @@ -1,3 +1,3 @@ ./waf configure --board rs100 ./waf copter -cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100-v4.1.16.px4 +cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100-sdk-v4.3.0-dev-FE.px4