From 75a7a41876235a283c012974e8811f30c5d3b7a6 Mon Sep 17 00:00:00 2001 From: nagezeng Date: Fri, 10 Jun 2022 18:20:42 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=A8=A1=E5=BC=8F=E8=B0=83?= =?UTF-8?q?=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/mode_auto.cpp | 4 ++-- ArduCopter/mode_guided.cpp | 8 ++++---- ArduCopter/version.h | 6 +++--- ArduCopter/zr_app.cpp | 18 ++++++++++++++++++ libraries/AC_ZR_APP/AC_ZR_Serial_API.h | 3 ++- rs100.sh | 2 +- 6 files changed, 30 insertions(+), 11 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index e6e32eb07c..bbaeef51b8 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -888,7 +888,7 @@ void ModeAuto::wp_run() } // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -935,7 +935,7 @@ void ModeAuto::spline_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // get pilot's desired yaw rat target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 67da2c9f75..66a5da7539 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -386,7 +386,7 @@ void Mode::auto_takeoff_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } @@ -417,7 +417,7 @@ void ModeGuided::pos_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -459,7 +459,7 @@ void ModeGuided::vel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -512,7 +512,7 @@ void ModeGuided::posvel_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 2a32fa9474..872b5e2d03 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -9,14 +9,14 @@ #undef GIT_VERSION #define GIT_VERSION "Beta" #endif -#define THISFIRMWARE "ZRUAV v4.3.1" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.3.2" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,3,1,FIRMWARE_VERSION_TYPE_OFFICIAL +#define FIRMWARE_VERSION 4,3,2,FIRMWARE_VERSION_TYPE_OFFICIAL #define FW_MAJOR 4 #define FW_MINOR 3 -#define FW_PATCH 1 +#define FW_PATCH 2 #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 e1dde4de1e..6d058c1ee1 100644 --- a/ArduCopter/zr_app.cpp +++ b/ArduCopter/zr_app.cpp @@ -212,6 +212,24 @@ void Copter::zr_app_50hz(){ } } break; + case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_LOCK_YAW: + if(motors->armed()){ + Vector3f target_vel; + target_vel.x = 0; + target_vel.y = 0; + target_vel.z = 0; + // target_loc.alt = (data.z + 500)/100.0; + // set_target_velocity_NED(target_vel); + mode_guided.set_velocity(target_vel,false,ahrs_yaw_cd,false,0.0f,false,true); + if(zr_serial_api.get_param_debug()){ + + // zr_serial_api.print_data("set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z); + snprintf(buf, sizeof(buf), "set vel %.2f,%.2f,%.2f,lock yaw",target_vel.x,target_vel.y,target_vel.z); + zr_serial_api.print_msg(buf); + memset(buf,0,50); + } + } + break; case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE: { diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h index 921e705d43..47135c3a62 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h @@ -45,8 +45,9 @@ public: MSG_VEHICLE_STATUS, // 飞机状态 MSG_CONTROL_TKOFF , // 飞行控制：起飞 MSG_CONTROL_POS , // 飞行控制：位置控制模式 - MSG_CONTROL_VEL , // 飞行控制：速度控制模式 + MSG_CONTROL_VEL , // 飞行控制：速度控制模式，大地坐标系 MSG_CONTROL_VEL_P, // 飞行控制：速度控制模式 + MSG_CONTROL_VEL_LOCK_YAW , // 悬停，锁定航向 MSG_CONTROL_MODE, // 飞行控制：速度控制模式 }; diff --git a/rs100.sh b/rs100.sh index 74f89d52be..83809262f8 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-sdk-v4.3.0-dev-FE.px4 +cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100f-sdk-v4.3.2-beta.px4