修复完善照片分开拍照

ArduCopter/mode_auto.cpp

@@ -1172,7 +1172,9 @@ 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;
+   AP::camera()->cameras_take_enable = AP::camera()->cameras_take_enable_new;
+   AP::camera()->cameras_take_enable_new = cmd.content.location.cams_enable_bit;
+   //gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:WP:%d,p3:%d",cmd.index,AP::camera()->cameras_take_enable_new); //TODO DEBUG!!!
     // 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

libraries/AP_Camera/AP_Camera.cpp

@@ -148,6 +148,7 @@ void AP_Camera::trigger_pic()
     setup_feedback_callback();
 
     _image_index++;
+    //gcs().send_text(MAV_SEVERITY_INFO ,"NOTICE:num:%d,p3:%d", _image_index,cameras_take_enable);   
     switch (_trigger_type) {
     case AP_CAMERA_TRIGGER_TYPE_SERVO:
         servo_pic();                    // Servo operated camera
@@ -480,7 +481,7 @@ void AP_Camera::uavcan_pic()
     camera_can_msg.roll = ahrs.roll_sensor*1e-2f;
     camera_can_msg.yaw = ahrs.yaw_sensor*1e-2f;
     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
 }
@@ -662,6 +663,15 @@ void AP_Camera::create_new_folder()
 
 }
 
+void AP_Camera::set_image_index(uint16_t photo_index)
+{
+    _image_index = photo_index;
+}
+uint16_t AP_Camera::get_image_index()
+{
+    return _image_index;
+}
+
 // singleton instance
 AP_Camera *AP_Camera::_singleton;
 

libraries/AP_Camera/AP_Camera.h

@@ -76,7 +76,8 @@ public:
     void send_camera_zr_status(mavlink_channel_t chan);
     void create_new_folder();
     static const struct AP_Param::GroupInfo        var_info[];
-    
+    void  set_image_index(uint16_t photo_index);
+    uint16_t get_image_index();
     // set if vehicle is in AUTO mode
     void set_is_auto_mode(bool enable)
     {
@@ -95,6 +96,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_new;
     uint8_t         cameras_take_enable;   //相机是否执行拍照 0有效
     struct Camera_State
     {

libraries/AP_Common/Location.h

@@ -15,7 +15,7 @@ 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;
+    uint8_t cams_enable_bit;
     // note that mission storage only stores 24 bits of altitude (~ +/- 83km)
     int32_t alt;
     int32_t lat;

libraries/AP_Mission/AP_Mission.cpp

@@ -501,6 +501,7 @@ struct PACKED PackedLocation {
     int32_t alt:24;                                     ///< param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
     int32_t lat;                                        ///< param 3 - Latitude * 10**7
     int32_t lng;                                        ///< param 4 - Longitude * 10**7
+    uint8_t cam_enable_bit;
 };
 
 union PackedContent {
@@ -509,11 +510,11 @@ union PackedContent {
 
     // raw bytes, for reading/writing to eeprom. Note that only 10
     // bytes are available if a 16 bit command ID is used
-    uint8_t bytes[12];
+    uint8_t bytes[13];
 
 };
 
-assert_storage_size<PackedContent, 12> assert_storage_size_PackedContent;
+assert_storage_size<PackedContent, 13> assert_storage_size_PackedContent;
 
 /// load_cmd_from_storage - load command from storage
 ///     true is return if successful
@@ -542,14 +543,14 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
     PackedContent packed_content {};
 
     const uint8_t b1 = _storage.read_byte(pos_in_storage);
-    if (b1 == 0) {
+    if (b1 == 0) {//cmd id>=256
         cmd.id = _storage.read_uint16(pos_in_storage+1);
         cmd.p1 = _storage.read_uint16(pos_in_storage+3);
-        _storage.read_block(packed_content.bytes, pos_in_storage+5, 10);
+        _storage.read_block(packed_content.bytes, pos_in_storage+5, 10); //last byte is not use
     } else {
         cmd.id = b1;
         cmd.p1 = _storage.read_uint16(pos_in_storage+1);
-        _storage.read_block(packed_content.bytes, pos_in_storage+3, 12);
+        _storage.read_block(packed_content.bytes, pos_in_storage+3, 13); 
     }
 
     if (stored_in_location(cmd.id)) {
@@ -570,12 +571,13 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
         cmd.content.location.alt = packed_content.location.alt;
         cmd.content.location.lat = packed_content.location.lat;
         cmd.content.location.lng = packed_content.location.lng;
+        cmd.content.location.cams_enable_bit = packed_content.location.cam_enable_bit;
     } else {
         // all other options in Content are assumed to be packed:
-        static_assert(sizeof(cmd.content) >= 12,
+        static_assert(sizeof(cmd.content) >= 13,
                       "content is big enough to take bytes");
         // (void *) cast to specify gcc that we know that we are copy byte into a non trivial type and leaving 4 bytes untouched
-        memcpy((void *)&cmd.content, packed_content.bytes, 12);
+        memcpy((void *)&cmd.content, packed_content.bytes, 13);
     }
 
     // set command's index to it's position in eeprom
@@ -625,6 +627,7 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd
 
     PackedContent packed {};
     if (stored_in_location(cmd.id)) {
+        
         // Location is not PACKED; field-wise copy it:
         packed.location.flags.relative_alt = cmd.content.location.relative_alt;
         packed.location.flags.loiter_ccw = cmd.content.location.loiter_ccw;
@@ -634,11 +637,20 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd
         packed.location.alt = cmd.content.location.alt;
         packed.location.lat = cmd.content.location.lat;
         packed.location.lng = cmd.content.location.lng;
+        if (cmd.id == MAV_CMD_NAV_WAYPOINT)
+        {
+            packed.location.cam_enable_bit = cmd.content.location.cams_enable_bit;
+        }
+        else
+        {
+            packed.location.cam_enable_bit = 0;
+        }
+
     } else {
         // all other options in Content are assumed to be packed:
-        static_assert(sizeof(packed.bytes) >= 12,
+        static_assert(sizeof(packed.bytes) >= 13,
                       "packed.bytes is big enough to take content");
-        memcpy(packed.bytes, &cmd.content, 12);
+        memcpy(packed.bytes, &cmd.content, 13);
     }
 
     // calculate where in storage the command should be placed
@@ -647,13 +659,14 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd
     if (cmd.id < 256) {
         _storage.write_byte(pos_in_storage, cmd.id);
         _storage.write_uint16(pos_in_storage+1, cmd.p1);
-        _storage.write_block(pos_in_storage+3, packed.bytes, 12);
+        _storage.write_block(pos_in_storage+3, packed.bytes, 13); 
     } else {
         // if the command ID is above 256 we store a 0 followed by the 16 bit command ID
         _storage.write_byte(pos_in_storage, 0);
         _storage.write_uint16(pos_in_storage+1, cmd.id);
         _storage.write_uint16(pos_in_storage+3, cmd.p1);
         _storage.write_block(pos_in_storage+5, packed.bytes, 10);
+        _storage.write_byte(pos_in_storage+15, 0);  //填充为0
     }
 
     // remember when the mission last changed
@@ -756,7 +769,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;
+        cmd.content.location.cams_enable_bit = (uint8_t)packet.param3;
 #endif
     }
         break;
@@ -1012,7 +1025,10 @@ 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)
-           
+        if (cmd.id == MAV_CMD_NAV_WAYPOINT)
+        {
+            cmd.content.location.cams_enable_bit = (uint8_t)packet.param3;
+        }
         switch (packet.frame) {
 
         case MAV_FRAME_MISSION:
@@ -1183,6 +1199,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
 #else
         // delay at waypoint in seconds
         packet.param1 = cmd.p1;
+         packet.param3 = (float)cmd.content.location.cams_enable_bit;
 #endif
         break;
 

libraries/AP_Mission/AP_Mission.h

@@ -22,7 +22,7 @@
 
 // definitions
 #define AP_MISSION_EEPROM_VERSION           0x65AE  // version number stored in first four bytes of eeprom.  increment this by one when eeprom format is changed
-#define AP_MISSION_EEPROM_COMMAND_SIZE      15      // size in bytes of all mission commands
+#define AP_MISSION_EEPROM_COMMAND_SIZE      16     // size in bytes of all mission commands
 
 #define AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS 15      // allow up to 15 do-jump commands
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -676,6 +676,7 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)
     } else {
         mavlink_msg_mission_item_int_decode(&msg, &packet);
     }
+
     const uint8_t current = packet.current;
     const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)packet.mission_type;
 

libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp

@@ -27,7 +27,6 @@ MAV_MISSION_RESULT MissionItemProtocol_Waypoints::append_item(const mavlink_miss
 {
     // sanity check for DO_JUMP command
     AP_Mission::Mission_Command cmd;
-
     const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
     if (res != MAV_MISSION_ACCEPTED) {
         return res;
@@ -110,7 +109,6 @@ uint16_t MissionItemProtocol_Waypoints::max_items() const {
 MAV_MISSION_RESULT MissionItemProtocol_Waypoints::replace_item(const mavlink_mission_item_int_t &mission_item_int)
 {
     AP_Mission::Mission_Command cmd;
-
     const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
     if (res != MAV_MISSION_ACCEPTED) {
         return res;

modules/mavlink

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