From 351791a30d76a3c2af1779449db4aaf957e4f661 Mon Sep 17 00:00:00 2001 From: bin Date: Thu, 25 Feb 2021 13:39:04 +0800 Subject: [PATCH] add single camera take photo by waypoint cmd --- ArduCopter/mode_auto.cpp | 5 ++--- Tools/autotest/locations.txt | 2 +- libraries/AP_Camera/AP_Camera.cpp | 5 +++-- libraries/AP_Camera/AP_Camera.h | 2 +- libraries/AP_Common/Location.h | 4 ++-- libraries/AP_Mission/AP_Mission.cpp | 3 ++- modules/mavlink | 2 +- 7 files changed, 12 insertions(+), 11 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index a763c740c2..c908901b7d 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1172,15 +1172,14 @@ Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) { Location target_loc = loc_from_cmd(cmd); - + AP::camera()->cameras_take_enable = cmd.content.location.camerasEnableBit; // this will be used to remember the time in millis after we reach or pass the WP. loiter_time = 0; // this is the delay, stored in seconds loiter_time_max = cmd.p1; - // Set wp navigation target wp_start(target_loc); - + // if no delay as well as not final waypoint set the waypoint as "fast" AP_Mission::Mission_Command temp_cmd; bool fast_waypoint = false; diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index f00f9d115a..0207d2a395 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -1,6 +1,6 @@ #NAME=latitude,longitude,absolute-altitude,heading OSRF0=37.4003371,-122.0800351,0,353 -CYDS=24.6057183,118.4168828,20,0 +CYDS=24.6122682,118.0672663,1,0 CELIU=24.6054997,118.0614525,1,0 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270 CMAC=-35.363261,149.165230,584,353 diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index a08be3ec92..ed926b323e 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -479,7 +479,8 @@ void AP_Camera::uavcan_pic() camera_can_msg.pitch = ahrs.pitch_sensor*1e-2f; camera_can_msg.roll = ahrs.roll_sensor*1e-2f; camera_can_msg.yaw = ahrs.yaw_sensor*1e-2f; - send_msg_step = 1; + send_msg_step = 1; + gcs().send_text(MAV_SEVERITY_INFO ,"NOTICE:num:%d,p3:%d", _image_index,cameras_take_enable); _trigger_counter = constrain_int16(_trigger_duration*5,0,255); #endif } @@ -495,7 +496,7 @@ void AP_Camera::send_can_msg(void) switch (send_msg_step) { case 1: - uavcan->set_zr_cam_cmd(camera_state.id,camera_can_msg.mode,1,_image_index,0,0); + uavcan->set_zr_cam_cmd(camera_state.id,camera_can_msg.mode,1,_image_index,0,cameras_take_enable); send_msg_step = 2; break; case 2: diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index e4e3a525c4..85fc0f9915 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -95,7 +95,7 @@ public: uint16_t number_of_index(void) const { return _image_index; } uint16_t _image_index_log; // number of pictures taken to log @Brown - + uint8_t cameras_take_enable; //相机是否执行拍照 0有效 struct Camera_State { float temperature; diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 24214c4820..d0f3bc0212 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -15,12 +15,12 @@ public: uint8_t terrain_alt : 1; // this altitude is above terrain uint8_t origin_alt : 1; // this altitude is above ekf origin uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location - + uint8_t camerasEnableBit; // note that mission storage only stores 24 bits of altitude (~ +/- 83km) int32_t alt; int32_t lat; int32_t lng; - + //cameras enable take photo bits; 0 enable takephoto /// enumeration of possible altitude types enum class AltFrame { ABSOLUTE = 0, diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index c7a14ac4ce..ce5f1e90dd 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -756,6 +756,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ #else // delay at waypoint in seconds (this is for copters???) cmd.p1 = packet.param1; + cmd.content.location.camerasEnableBit = packet.param3; #endif } break; @@ -1011,7 +1012,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.content.location.lng = packet.y; cmd.content.location.alt = packet.z * 100.0f; // convert packet's alt (m) to cmd alt (cm) - + switch (packet.frame) { case MAV_FRAME_MISSION: diff --git a/modules/mavlink b/modules/mavlink index 18cab69509..0013b46db8 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019 +Subproject commit 0013b46db8cb437b6ad77d736263597d90a2262d