Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: restore initial climb to RTL

Browse Source
This commit is contained in:
rmackay9 2012-12-17 16:17:49 +09:00
parent c1abd96ef2
commit 1332ed3b5c
2 changed files with 29 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
ArduCopter/commands_logic.pde
View File

@ -219,28 +219,21 @@ static bool verify_may()
static void do_RTL(void)
{
// set rtl state
rtl_state = RTL_STATE_RETURNING_HOME;
rtl_state = RTL_STATE_INITIAL_CLIMB;
// set roll, pitch and yaw modes
set_roll_pitch_mode(RTL_RP);
set_yaw_mode(RTL_YAW);
set_yaw_mode(YAW_HOLD); // first stage of RTL is the initial climb so just hold current yaw
set_throttle_mode(RTL_THR);
// set navigation mode
wp_control = WP_MODE;
wp_control = LOITER_MODE;
// so we know where we are navigating from
// initial climb starts at current location
next_WP = current_loc;
// Set navigation target to home
set_next_WP(&home);
// override altitude to RTL altitude
set_new_altitude(get_RTL_alt());
// output control mode to the ground station
// -----------------------------------------
gcs_send_message(MSG_HEARTBEAT);
}
/********************************************************************************/
@ -480,6 +473,26 @@ static bool verify_RTL()
switch( rtl_state ) {
case RTL_STATE_INITIAL_CLIMB:
// rely on verify_altitude function to update alt_change_flag when we've reached the target
if(alt_change_flag == REACHED_ALT) {
// Set navigation target to home
set_next_WP(&home);
// override target altitude to RTL altitude
set_new_altitude(get_RTL_alt());
// set navigation mode
wp_control = WP_MODE;
// set yaw mode
set_yaw_mode(RTL_YAW);
// advance to next rtl state
rtl_state = RTL_STATE_RETURNING_HOME;
}
break;
case RTL_STATE_RETURNING_HOME:
// if we've reached home initiate loiter
if (wp_distance <= g.waypoint_radius * 100 || check_missed_wp()) {

9
ArduCopter/defines.h
View File

@ -214,10 +214,11 @@
#define WP_OPTION_NEXT_CMD 128
// RTL state
#define RTL_STATE_RETURNING_HOME 0
#define RTL_STATE_LOITERING_AT_HOME 1
#define RTL_STATE_FINAL_DESCENT 2
#define RTL_STATE_LAND 3
#define RTL_STATE_INITIAL_CLIMB 0
#define RTL_STATE_RETURNING_HOME 1
#define RTL_STATE_LOITERING_AT_HOME 2
#define RTL_STATE_FINAL_DESCENT 3
#define RTL_STATE_LAND 4
//repeating events
#define RELAY_TOGGLE 5