From 57b31b889a47c4b318d048db376bb2941918d7fd Mon Sep 17 00:00:00 2001 From: "Brown.Z" Date: Fri, 17 Jun 2022 10:02:47 +0800 Subject: [PATCH] =?UTF-8?q?=E6=95=B0=E5=AD=97=E7=BB=BF=E5=9C=9F=EF=BC=8C?= =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=81=9C=E4=B8=8B=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/mode_guided.cpp | 10 +++--- ArduCopter/zr_app.cpp | 45 ++++++++++++++++++++++++ libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp | 14 ++++++-- libraries/AC_ZR_APP/AC_ZR_Serial_API.h | 4 ++- sitl.sh | 2 +- 5 files changed, 66 insertions(+), 9 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a1950e3141..794c2f42e3 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -169,7 +169,7 @@ void ModeGuided::wp_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && !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)) { @@ -648,7 +648,7 @@ void ModeGuided::pos_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && !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)) { @@ -707,7 +707,7 @@ void ModeGuided::accel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && !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)) { @@ -771,7 +771,7 @@ void ModeGuided::velaccel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && !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)) { @@ -847,7 +847,7 @@ void ModeGuided::posvelaccel_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && !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/zr_app.cpp b/ArduCopter/zr_app.cpp index bd1233351c..9a81bb8d8d 100644 --- a/ArduCopter/zr_app.cpp +++ b/ArduCopter/zr_app.cpp @@ -186,6 +186,51 @@ 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",target_vel.x,target_vel.y,target_vel.z); + gcs().send_text(MAV_SEVERITY_INFO,"%s",buf); + memset(buf,0,50); + } + } + break; + case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE: + { + + if(flightmode->mode_number() != Mode::Number::GUIDED){ + set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING); + } + uint8_t user_mode = (data.x); + if(user_mode == 3 || user_mode == 4 || user_mode == 6 || user_mode == 17){ + bool change_ok = set_mode(user_mode, ModeReason::SCRIPTING); + + if(zr_serial_api.get_param_debug() && change_ok){ + // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt); + snprintf(buf, sizeof(buf), "Change mode %d",user_mode); + zr_serial_api.print_msg(buf); + memset(buf,0,50); + } + }else{ + if(zr_serial_api.get_param_debug()){ + // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt); + snprintf(buf, sizeof(buf), "Unsupported mode %d",user_mode); + zr_serial_api.print_msg(buf); + memset(buf,0,50); + } + } + + } + break; default: break; diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp index eaa6531f2d..6cbbe8d6b5 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #define ZR_API_DEBUG 1 @@ -294,6 +295,15 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l if(control_data_last_time == last_time){ // 数据没更新，直接退出 return false; } + AP::logger().Write("APID", "TimeUS,Mode,Type,X,Y,Z,Yaw", "QBBiiif", + AP_HAL::micros64(), //Q + mode, //B + flight_control_parser.msg.type, //B + flight_control_parser.msg.x, //i + flight_control_parser.msg.y, //i + flight_control_parser.msg.z, //i + flight_control_parser.msg.yaw); //f + last_time = control_data_last_time; WITH_SEMAPHORE(sem); if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头 @@ -316,7 +326,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l return false; // 校验失败 } type = flight_control_parser.msg.type; - if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL_P ){ + if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_MODE ){ set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误 if(get_param_debug()){ @@ -324,7 +334,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l } return false; // 控制类型错误 } - if(type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4){ + if(type != ZR_Msg_Type::MSG_CONTROL_MODE && type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4 ){ set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE); if(get_param_debug()){ Debug("mode error: %d",mode); diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h index 351b2b201b..de4cc7e982 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h @@ -46,7 +46,9 @@ public: MSG_CONTROL_TKOFF , // 飞行控制：起飞 MSG_CONTROL_POS , // 飞行控制：位置控制模式 MSG_CONTROL_VEL , // 飞行控制：速度控制模式 - MSG_CONTROL_VEL_P // 飞行控制：速度控制模式 + MSG_CONTROL_VEL_P, // 飞行控制：速度控制模式 + MSG_CONTROL_VEL_LOCK_YAW , // 飞行控制：速度控制模式，大地坐标系，锁定航向 + MSG_CONTROL_MODE, // 飞行控制：速度控制模式 }; diff --git a/sitl.sh b/sitl.sh index b33e8af432..0051be338f 100755 --- a/sitl.sh +++ b/sitl.sh @@ -1 +1 @@ - ./Tools/autotest/sim_vehicle.py -j4 -L CYDS2 --console --out 192.168.0.51:14552 --map -A "--serial5=uart:/dev/pts/1:230400" -v ArduCopter + ./Tools/autotest/sim_vehicle.py -j4 -L CYDS2 --console --out 192.168.0.51:14552 --map -A "--serial5=uart:/dev/ttyS1:230400" -v ArduCopter