From f977ba92b79f5ac6d3086e458b204879c2c5eb2a Mon Sep 17 00:00:00 2001 From: z Date: Tue, 31 Dec 2019 17:11:43 +0800 Subject: [PATCH] mission auto resume ok --- ArduCopter/APM_Config.h | 8 +- ArduCopter/UserCode.cpp | 11 ++ ArduCopter/mode.cpp | 13 +++ Tools/autotest/locations.txt | 1 + libraries/AP_Camera/AP_Camera.cpp | 4 + libraries/AP_Camera/AP_Camera.h | 5 + libraries/AP_Mission/AP_Mission.cpp | 158 ++++++++++++++++++++++++++++ libraries/AP_Mission/AP_Mission.h | 11 ++ libraries/AP_Notify/ToneAlarm.cpp | 9 +- 9 files changed, 213 insertions(+), 7 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index c28a027b9b..1c87164bfc 100644 --- a/ArduCopter/APM_Config.h +++ b/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 diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 46070d47fa..0803c691f4 100644 --- a/ArduCopter/UserCode.cpp +++ b/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 diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index a2a3da4b9c..8656ee8c92 100644 --- a/ArduCopter/mode.cpp +++ b/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; diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index fe12ec0816..bd01ca4516 100644 --- a/Tools/autotest/locations.txt +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index a2f2227516..106a431434 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/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; } } } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 4f4baa350c..ea08d4ce59 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/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; }; diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index c9f05dbd5a..51378d4b98 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/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); + } +} \ No newline at end of file diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 9034942fba..b04f74588e 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/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 { diff --git a/libraries/AP_Notify/ToneAlarm.cpp b/libraries/AP_Notify/ToneAlarm.cpp index 768e60d833..61ff9f7ed1 100644 --- a/libraries/AP_Notify/ToneAlarm.cpp +++ b/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 }, + { "MFT200L4A#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