diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index fca35f8d088fe..477ceb8b168aa 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -320,6 +320,13 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c // rising edge of delay_arming oneshot delay_arming = true; + + //capture heading for autoland mode if option is set + if (plane.aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM) { + plane.takeoff_state.takeoff_initial_direction = wrap_360((plane.ahrs.yaw_sensor * 0.01f) + plane.mode_autoland.landing_dir_off); + plane.takeoff_state.takeoff_direction_initialized = true; + gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.takeoff_initial_direction)); + } send_arm_disarm_statustext("Throttle armed"); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 24d8fe8d1f009..9dfc473684d26 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -170,7 +170,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_OPTIONS // @DisplayName: Takeoff options // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. - // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. + // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. 1: When set, if there is a healthy compass available (whether or not its being "USED"), the compass heading will be captured at arming and used for the AUTOLAND mode's initial takeoff direction instead of capturing ground course in NAV_TAKEOFF or Mode TAKEOFF. // @User: Advanced ASCALAR(takeoff_options, "TKOFF_OPTIONS", 0), diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 7ed81c1800b3c..28f6312256089 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -382,7 +382,9 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) // zero locked course steer_state.locked_course_err = 0; steer_state.hold_course_cd = -1; - takeoff_state.takeoff_direction_initialized = false; + if (!(aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) { + plane.takeoff_state.takeoff_direction_initialized = false; + } auto_state.baro_takeoff_alt = barometer.get_altitude(); } @@ -557,9 +559,11 @@ bool Plane::verify_takeoff() gps.ground_speed() > min_gps_speed && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { auto_state.takeoff_speed_time_ms = millis(); - takeoff_state.takeoff_initial_direction = gps.ground_course(); + if (!(aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) { + takeoff_state.takeoff_initial_direction = wrap_360(gps.ground_course() + mode_autoland.landing_dir_off); takeoff_state.takeoff_direction_initialized = true; - gcs().send_text(MAV_SEVERITY_INFO, "Takeoff initial direction= %u",int(takeoff_state.takeoff_initial_direction)); + gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.takeoff_initial_direction)); + } } if (auto_state.takeoff_speed_time_ms != 0 && millis() - auto_state.takeoff_speed_time_ms >= 2000) { diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 10727fc0a71f4..c4a044998c359 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -885,6 +885,7 @@ class ModeAutoLand: public Mode AP_Int16 final_wp_alt; AP_Int16 final_wp_dist; + AP_Int16 landing_dir_off; protected: bool _enter() override; diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index e9d13858035a2..be0e6befad617 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -24,6 +24,15 @@ const AP_Param::GroupInfo ModeAutoLand::var_info[] = { // @User: Standard AP_GROUPINFO("WP_DIST", 2, ModeAutoLand, final_wp_dist, 400), + // @Param: DIR_OFF + // @DisplayName: Landing direction offset from takeoff + // @Description: The captured takeoff direction (at arming,if TKOFF_OPTION bit1 is set, or after ground course is established in autotakeoffs)is offset by this amount to create a different landing direction and approach. + // @Range: -360 360 + // @Increment: 1 + // @Units: m + // @User: Standard + AP_GROUPINFO("DIR_OFF", 3, ModeAutoLand, landing_dir_off, 0), + AP_GROUPEND }; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 54b09a80fc0aa..cad6260ffcb73 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -134,7 +134,9 @@ void ModeTakeoff::update() plane.takeoff_state.throttle_max_timer_ms = millis(); takeoff_mode_setup = true; plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function. - plane.takeoff_state.takeoff_direction_initialized = false; + if (!(plane.aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) { + plane.takeoff_state.takeoff_direction_initialized = false; + } } } } @@ -148,9 +150,9 @@ void ModeTakeoff::update() const float min_gps_speed = GPS_GND_CRS_MIN_SPD; if (!(plane.takeoff_state.takeoff_direction_initialized) && (plane.gps.ground_speed() > min_gps_speed) && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { - plane.takeoff_state.takeoff_initial_direction = plane.gps.ground_course(); + plane.takeoff_state.takeoff_initial_direction = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off); plane.takeoff_state.takeoff_direction_initialized = true; - gcs().send_text(MAV_SEVERITY_INFO, "Takeoff initial direction= %u",int(plane.takeoff_state.takeoff_initial_direction)); + gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.takeoff_initial_direction)); } // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index d7617e2f5696b..992b2fd4416d4 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -22,9 +22,11 @@ bool Plane::auto_takeoff_check(void) return false; } - // Reset states if process has been interrupted + // Reset states if process has been interrupted, except takeoff_direction_initialized if set + bool takeoff_dir_initialized = takeoff_state.takeoff_direction_initialized; if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) { memset(&takeoff_state, 0, sizeof(takeoff_state)); + takeoff_state.takeoff_direction_initialized = takeoff_dir_initialized; //restore dir init state return false; }