add single camera take photo by waypoint cmd

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;

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

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:

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;

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,

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:

modules/mavlink

@@ -1 +1 @@
-Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019
+Subproject commit 0013b46db8cb437b6ad77d736263597d90a2262d