From 76c8b1452570f70b1b765b62d7f9e35a0796c6aa Mon Sep 17 00:00:00 2001 From: nagezeng Date: Thu, 23 Jun 2022 21:13:22 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=BF=E7=9C=9F=E6=B5=8B=E8=AF=95=E5=90=8E?= =?UTF-8?q?=E8=B0=83=E6=95=B4=EF=BC=8Cuavcan=E6=B6=88=E6=81=AF=E5=8F=91?= =?UTF-8?q?=E9=80=81=E6=88=90=E5=8A=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/GCS_Mavlink.cpp | 22 ++++++++++ ArduCopter/UserCode.cpp | 42 ++++++++++++++++++++ ArduCopter/mode_auto.cpp | 14 +++++++ ArduCopter/version.h | 8 ++-- libraries/AP_Camera/AP_Camera.cpp | 8 ++-- libraries/AP_Mission/AP_Mission.cpp | 6 ++- libraries/AP_Mission/AP_Mission_Commands.cpp | 1 + libraries/AP_UAVCAN/AP_UAVCAN.cpp | 2 +- modules/mavlink | 2 +- px4v3.sh | 5 +-- 10 files changed, 95 insertions(+), 15 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 95d107ee0d..1cf418c704 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -705,6 +705,28 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ } return MAV_RESULT_FAILED; + case MAV_CMD_DO_GIMBAL_CONTROL: + { + // param3 : target angle [-180~180] + // param2 : speed during change [deg per second] + // param3 : direction (-1:ccw, +1:cw) + // param4 : relative offset (1) or absolute angle (0) + gcs().send_text(MAV_SEVERITY_INFO, "gcs gimbal:%.2f,%.2f,%.2f,%.2f",packet.param1,packet.param2,packet.param3,packet.param4); + float angle = is_positive(packet.param3)?packet.param3:packet.param3+360.0f; + if ((packet.param3 >= -180.0f) && + (packet.param3 <= 180.0f)) { + copter.flightmode->auto_yaw.set_fixed_yaw( + angle, + 0, + 0, + 0); + + handle_command_camera(packet); + return MAV_RESULT_ACCEPTED; + } + } + return MAV_RESULT_FAILED; + case MAV_CMD_DO_CHANGE_SPEED: // param1 : unused // param2 : new speed in m/s diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 9699ead2df..25c62d1ca6 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -126,6 +126,48 @@ void Copter::userhook_SuperSlowLoop() camera.take_picture(); } #endif + if(1) + { + static uint8_t ccnt; + static float cpitch = 0.0; + static float cyaw = 0.0; + static uint8_t step = 0; + ccnt+=1; + if (ccnt < 3) + { + return; + } + + switch (step) + { + case 0: + cpitch = 0.0f; + cyaw = 0.0f; + step +=1; + break; + case 1: + cpitch = -30.0f; + cyaw = 1.0f; + step +=1; + break; + case 2: + cpitch = -60.0f; + cyaw = 2.0f; + step +=1; + break; + case 3: + cpitch = -90.0f; + cyaw = 3.0f; + step =0; + break; + + default: + break; + } + camera.control_att(cpitch,cyaw,1.0f); + ccnt = 0; + + } } #endif diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index c0484ed8c8..2db85caf25 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -491,6 +491,19 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_yaw(cmd); break; + case MAV_CMD_DO_GIMBAL_CONTROL: // 115 + { + float angle = is_positive(cmd.content.gimbal_control.yaw)?cmd.content.gimbal_control.yaw:cmd.content.gimbal_control.yaw+360.0f; + auto_yaw.set_fixed_yaw( + angle, + 0, + (angle - ahrs.yaw_sensor/100.0f), + 0); + + // gcs().send_text(MAV_SEVERITY_INFO, "**GIMBAL yaw:%.2f,%d",angle,ahrs.yaw_sensor); + } + break; + /// /// do commands /// @@ -719,6 +732,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_CONDITION_YAW: + case MAV_CMD_DO_GIMBAL_CONTROL: cmd_complete = verify_yaw(); break; diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 3bfaf19aa3..48fa39b0e8 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -5,15 +5,15 @@ #endif #include "ap_version.h" -#define GIT_VERSION "2" -#define THISFIRMWARE "ZRUAV v4.1.18" //"ArduCopter V4.0.0" +#define GIT_VERSION "3" +#define THISFIRMWARE "ZRUAV v4.1.19" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,1,18,FIRMWARE_VERSION_TYPE_OFFICIAL +#define FIRMWARE_VERSION 4,1,19,FIRMWARE_VERSION_TYPE_OFFICIAL #define FW_MAJOR 4 #define FW_MINOR 1 -#define FW_PATCH 18 +#define FW_PATCH 19 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL /** * AVDv6: avoid.get_zr_mode() = 3时可以用Loiter模式测试自动避障 diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index e84c804714..5ea49fce06 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -292,13 +292,12 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo void AP_Camera::control_att(float pitch,float yaw,float shooting_cmd ) { // take picture - _camera_pitch = pitch; - _camera_pitch = yaw; + _camera_yaw = yaw; if (is_equal(shooting_cmd,1.0f)) { trigger_pic(); } - gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f,trg:%d",_camera_pitch,_camera_yaw,shooting_cmd); + gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f,trg:%f",_camera_pitch,_camera_yaw,shooting_cmd); } /* @@ -516,7 +515,7 @@ void AP_Camera::uavcan_pic() void AP_Camera::uavcan_pic_att() { - gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f",_camera_pitch,_camera_yaw); + // gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f",_camera_pitch,_camera_yaw); //TODO: 增加发送云台角度控制 #if HAL_WITH_UAVCAN uint8_t mode = 12; @@ -527,6 +526,7 @@ void AP_Camera::uavcan_pic_att() if (uavcan != nullptr) { uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index&0xff, _image_index>>8, 0); + uavcan->set_cam_angular(camera_state.id,2,_camera_pitch,0,_camera_yaw); } } _trigger_counter = constrain_int16(_trigger_duration * 5, 0, 255); diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index d90edeb569..261f9d9013 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -289,7 +289,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd) case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_DO_PARACHUTE: - case MAV_CMD_DO_GIMBAL_CONTROL: + // case MAV_CMD_DO_GIMBAL_CONTROL: return true; default: return _cmd_verify_fn(cmd); @@ -311,10 +311,12 @@ bool AP_Mission::start_command(const Mission_Command& cmd) case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - case MAV_CMD_DO_GIMBAL_CONTROL: return start_command_camera(cmd); case MAV_CMD_DO_PARACHUTE: return start_command_parachute(cmd); + case MAV_CMD_DO_GIMBAL_CONTROL: + start_command_camera(cmd); + return _cmd_start_fn(cmd); default: return _cmd_start_fn(cmd); } diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 52de9b3d70..2217607a25 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -101,6 +101,7 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd) return true; case MAV_CMD_DO_GIMBAL_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| + //TODO 这里调用到相机控制 camera->control_att(cmd.content.gimbal_control.pitch, cmd.content.gimbal_control.yaw, cmd.content.gimbal_control.shooting_cmd); diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 4d4f9a6292..ab506f61f8 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -483,7 +483,7 @@ void AP_UAVCAN::loop(void) } } zr_cam_cmd_send(); - //cam_aular_send(); + cam_aular_send(); //cam_geopoi_send(); zr_rgb_led_send(); led_out_send(); diff --git a/modules/mavlink b/modules/mavlink index 959dea22da..73dc48ca0a 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 959dea22dafbae0e5682cd2191e608b45a7566a9 +Subproject commit 73dc48ca0ab259e16ec940db7aec95e44383c877 diff --git a/px4v3.sh b/px4v3.sh index 1f9212edbf..580f5ca5ae 100755 --- a/px4v3.sh +++ b/px4v3.sh @@ -1,4 +1,3 @@ -git submodule update -./waf configure --board CubeBlack +./waf configure --board fmuv3 ./waf copter -cp build/CubeBlack/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/dev-px4v3.px4 +cp build/fmuv3/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/编译/dev-px4v3.px4