mission auto resume ok

ArduCopter/APM_Config.h

@@ -67,8 +67,8 @@
 //#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
 //#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
 //#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
-//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
-//#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
-//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
-//#define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
+#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
+#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
+#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
+#define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
 //#define USER_PARAMS_ENABLED ENABLED                       // to enable user parameters

ArduCopter/UserCode.cpp

@@ -33,6 +33,17 @@ void Copter::userhook_MediumLoop()
 void Copter::userhook_SlowLoop()
 {
     // put your 3.3Hz code here
+    /********** @Brown,for get camera pos **********/
+    static int32_t last_pos_lat;
+    static int32_t last_pos_lng;
+
+    camera.get_last_trigger_pos(last_pos_lat,last_pos_lng);
+    // mission.set_last_trigger_pos(last_pos_lat,last_pos_lng);
+    if(mode_auto.mission._type_resume)
+        mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),last_pos_lat,last_pos_lng);
+    else
+        mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),current_loc.lat,current_loc.lng);
+    /********** @Brown,for get camera pos **********/
 }
 #endif
 

ArduCopter/mode.cpp

@@ -532,6 +532,19 @@ void Mode::land_run_vertical_control(bool pause_descent)
     const float precland_acceptable_error = 15.0f;
     const float precland_min_descent_speed = 10.0f;
 
+
+// modify by @Brown , Staging land speed control
+        if(get_alt_above_ground_cm() < 100)
+            g.land_speed = 20;
+        else if(get_alt_above_ground_cm() < 500)
+            g.land_speed = 30;
+        else if(get_alt_above_ground_cm() < 1000)
+            g.land_speed = 50;
+        else if(get_alt_above_ground_cm() < 2000)
+            g.land_speed = 100;
+        else if(get_alt_above_ground_cm() < 3000)
+            g.land_speed = 150;
+
     float cmb_rate = 0;
     if (!pause_descent) {
         float max_land_descent_velocity;

Tools/autotest/locations.txt

@@ -1,5 +1,6 @@
 #NAME=latitude,longitude,absolute-altitude,heading
 OSRF0=37.4003371,-122.0800351,0,353
+CYDS=24.6122682,118.0672663,1,0
 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270
 CMAC=-35.363261,149.165230,584,353
 CMAC2=-35.362889,149.165221,584,270

libraries/AP_Camera/AP_Camera.cpp

@@ -408,10 +408,14 @@ void AP_Camera::log_picture()
         gcs().send_message(MSG_CAMERA_FEEDBACK);
         if (logger->should_log(log_camera_bit)) {
             logger->Write_Camera(current_loc);
+            last_pos_lat = current_loc.lat;
+            last_pos_lng = current_loc.lng;
         }
     } else {
         if (logger->should_log(log_camera_bit)) {
             logger->Write_Trigger(current_loc);
+            last_pos_lat = current_loc.lat;
+            last_pos_lng = current_loc.lng;
         }
     }
 }

libraries/AP_Camera/AP_Camera.h

@@ -76,6 +76,8 @@ public:
         CAMERA_TYPE_BMMCC
     };
 
+    void get_last_trigger_pos(int32_t &lat,int32_t &lng){ lat=last_pos_lat;lng=last_pos_lng; } // @Brown,for get camera pos
+
 private:
 
     static AP_Camera *_singleton;
@@ -132,6 +134,9 @@ private:
     {
         return _feedback_pin > 0;
     }
+    
+    int32_t last_pos_lat; // @Brown,for get camera pos
+    int32_t last_pos_lng;
 
 };
 

libraries/AP_Mission/AP_Mission.cpp

@@ -31,6 +31,21 @@ const AP_Param::GroupInfo AP_Mission::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("OPTIONS",  2, AP_Mission, _options, AP_MISSION_OPTIONS_DEFAULT),
 
+
+    // @Param: AUTO_RESUME
+    // @DisplayName: Mission auto resume mission
+    // @Description: Bitmask of what options to use in missions.
+    // @Values: 0:disable, 1:enable
+    // @User: Advanced
+    AP_GROUPINFO("AUTO_RESUME",  3, AP_Mission, _auto_resume, 1),
+
+    // @Param: TYPE_RESUME
+    // @DisplayName: resume mission type,return to last gps pos or camera pos.
+    // @Description: Bitmask of what options to use in missions.
+    // @Values: 0:gps pos, 1:camera pos
+    // @User: Advanced
+    AP_GROUPINFO("TYPE_RESUME",  4, AP_Mission, _type_resume, 1),
+
     AP_GROUPEND
 };
 
@@ -1979,3 +1994,146 @@ AP_Mission *mission()
 }
 
 }
+
+
+
+/**
+ *  Modify  by @Brown
+ */ 
+void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,int32_t r_lng)
+{
+    Mission_Command cmd_rtl = {}; // 存储返航点坐标信息
+    Mission_Command cmd_temp = {};
+    
+    static uint8_t need_change_cmd = 0;     // 判斷是否需要進入航點改寫程序
+
+    uint16_t cmd_truncate = 0;  // 去除剩余的航点
+    static uint16_t cmd_total = 0;  // 去除剩余的航点
+    static uint16_t cmd_rtl_index = 0; // rtl返航点需要改写的航点序号
+    
+    uint16_t cmd_i = 0;
+    uint16_t new_cmd_i = 0;
+  	uint8_t cmd_step = 0;
+    int32_t takeoff_alt = 0;
+    int32_t rtlwp_alt = 0;
+
+
+    // int32_t cam_lat,cam_lng;
+    // get_last_trigger_pos(cam_lat,cam_lng);
+    
+    if(f_mode == 3 && need_change_cmd == 0)
+    {
+        need_change_cmd = 1;
+        // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "enter resume dec");
+
+    }else if(need_change_cmd == 1 && _auto_resume && 6 == f_mode)
+    {
+        cmd_rtl_index = _nav_cmd.index;
+
+        cmd_rtl_index -= 1;
+        // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "resume %d",cmd_rtl_index);
+        if(cmd_rtl_index < 6 ||  num_commands() - cmd_rtl_index < 4)// 飛行的航點小於6則直接推出，否則讀取當前航點；飞完所有航点，也直接退出
+        {
+		    need_change_cmd = 0;
+            // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[Resume] Need more waypoint,now:%d, need:6",cmd_rtl_index);
+            return;
+        }
+        //  存储当前的POS到航点命令
+        // cmd_rtl.content.location.alt = current_loc.alt * 10,        // millimeters above sea level
+ 
+        for(int i = cmd_rtl_index; i < cmd_rtl_index+3;i++)
+        {
+            read_cmd_from_storage(i,cmd_rtl);   // 读取原本航点
+
+            // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "read[%d]: %d",i,cmd_rtl.id);
+            if (is_nav_cmd(cmd_rtl)) {   // 是否是Waypoint
+                break;
+            }
+            
+        }
+
+        // cmd_rtl.content.location.alt = user_cmd_state.content.location.alt; // 獲取經緯度和高度值
+        cmd_rtl.content.location.lat = r_lat;
+        cmd_rtl.content.location.lng = r_lng;
+        cmd_rtl.id = MAV_CMD_NAV_WAYPOINT;
+        // cmd_rtl.content.location.options = 0;
+        cmd_rtl.p1 = 2;     // 到航点后延时2s
+        // cmd_rtl.content.location.flags.relative_alt = 1;
+
+        replace_cmd(cmd_rtl_index, cmd_rtl);
+        // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[Resume] Replace wp: %d",cmd_rtl_index);
+
+		need_change_cmd = 2;
+    }
+
+    
+    // 飞机上锁，则开始改写航点
+    if (!is_arm && need_change_cmd == 2) {
+        cmd_total = num_commands();
+        // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[Resume] --- Star replace wp ,total:%d ---",cmd_total);
+
+        for(cmd_i = 1; cmd_i < cmd_total; cmd_i++)
+        {
+            read_cmd_from_storage(cmd_i,cmd_temp);
+                // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[%d] step:%d, id:%d ",cmd_i,cmd_step ,cmd_temp.id);
+            switch (cmd_step)
+            {
+                // gcs_send_text_fmt(MAV_SEVERITY_INFO, "[d]t:%d,i:%d \n ",cmd_step,cmd_rtl_i);
+
+	  			case 0:
+		  			if(cmd_temp.id == MAV_CMD_NAV_TAKEOFF)
+		  			{
+		  				cmd_step = 1;
+                        takeoff_alt = cmd_temp.content.location.alt;
+		  			}
+		  			break;
+	  			case 1:
+		  			if(cmd_temp.id == MAV_CMD_NAV_WAYPOINT)
+		  			{
+  			            read_cmd_from_storage(cmd_rtl_index,cmd_temp);
+                        // cmd_temp.content.location.flags.relative_alt = 1;
+                        rtlwp_alt = cmd_temp.content.location.alt;
+                        if(rtlwp_alt < takeoff_alt)
+                        {
+                            cmd_temp.content.location.alt = takeoff_alt;       
+                            cmd_rtl_index-=1;                  
+                        } 
+		  				replace_cmd(cmd_i, cmd_temp);     // 替換 // 獲取第一個航點的序號
+
+		  				cmd_step = 2;
+                        new_cmd_i = cmd_i;
+		  			}
+		  			break;
+	  			case 2:
+		  			if(cmd_temp.id == MAV_CMD_DO_SET_CAM_TRIGG_DIST)
+					{
+                        // cmd_rtl_i = cmd_i;
+		  				cmd_step = 3;
+                        new_cmd_i++;
+                        replace_cmd(new_cmd_i,cmd_temp);
+                        cmd_i = cmd_rtl_index; // 從返航點開始繼續讀取航點
+	        		}
+		  			break;
+                case 3:
+                    
+                    // cmd_temp.content.location.flags.relative_alt = 1;
+                        new_cmd_i++;
+                        replace_cmd(new_cmd_i,cmd_temp);
+                    if( MAV_CMD_NAV_LAND == cmd_temp.id || MAV_CMD_NAV_RETURN_TO_LAUNCH == cmd_temp.id )    
+                    {
+                        // cmd_rtl_i = cmd_i;
+                        cmd_i = cmd_total;
+                        // GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "[c]c:%d,i:%d \n ",cmd_rtl_i,cmd_i);
+                    }
+                    break;
+            }
+        }
+            // cmd_truncate = cmd_total - cmd_rtl_index + cmd_rtl_i;
+            cmd_truncate = new_cmd_i + 1;
+            // new mission arriving, truncate mission to be the same length
+            truncate(cmd_truncate);
+            // gcs_send_text_fmt(MAV_SEVERITY_INFO, "[d]t:%d,i:%d \n ",total_cmd,cmd_rtl_i);
+            need_change_cmd = 0;
+        // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[Resume] --- Finish replace wp,truncate :%d ---",cmd_truncate);
+    }
+}

libraries/AP_Mission/AP_Mission.h

@@ -480,6 +480,14 @@ public:
     // returns true if the mission contains the requested items
     bool contains_item(MAV_CMD command) const;
 
+
+    /*********************************/
+    // @Brown
+    AP_Int8                _type_resume;
+    AP_Int8                _auto_resume;    
+    void auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t lat,int32_t lng);
+    void set_last_trigger_pos(int32_t lat,int32_t lng){ last_pos_lat = lat;last_pos_lng = lng; }
+    /*********************************/
     // user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -586,6 +594,9 @@ private:
     bool start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd);
     bool start_command_camera(const AP_Mission::Mission_Command& cmd);
     bool start_command_parachute(const AP_Mission::Mission_Command& cmd);
+
+    int32_t last_pos_lat; // @Brown,for get camera pos
+    int32_t last_pos_lng;
 };
 
 namespace AP {

libraries/AP_Notify/ToneAlarm.cpp

@@ -47,9 +47,11 @@ const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] {
 #define AP_NOTIFY_TONE_LOUD_POS_FEEDBACK 5
     { "MFT100L4>A#B#", false },
 #define AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED 6
-    { "MFT100L4>G#6A#6B#4", false },
+    // { "MFT100L4>G#6A#6B#4", false },
+    { "MFT200L4<G#6A#6B#6O4C#6D#4O3G#6A#6B#6O4C#6D#1", false }, // modify by @Brown 
 #define AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED 7
-    { "MFT200L4<G#6A#6B#4", false },
+    // { "MFT200L4<G#6A#6B#4", false },
+    { "MFT200L4<G#6A#6B#6O4C#6D#4O3G#6A#6B#6O4C#6D#1", false },
 #define AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED 8
     { "MFT100L4>A#A#A#A#", false },
 #define AP_NOTIFY_TONE_QUIET_ARMING_WARNING 9
@@ -93,7 +95,8 @@ const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] {
 #define AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED 28
     { "MFT200L4<B#4A#6G#6", false },
 #define AP_NOTIFY_TONE_STARTUP 29
-    { "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false },
+    // { "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false },
+    { "MFT240L8 O4L4fdL2a", false }, // modify by @Brown 
 #define AP_NOTIFY_TONE_NO_SDCARD 30
     { "MNBGG", false },
 };