diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 25a43be25cab36..d9048b93200cd0 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -321,6 +321,15 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c // rising edge of delay_arming oneshot delay_arming = true; +#if MODE_AUTOLAND_ENABLED + //capture heading for autoland mode if option is set and using a compass + if (plane.ahrs.use_compass() && plane.tkoff_option_is_set(AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) { + plane.takeoff_state.initial_direction.heading = wrap_360(plane.ahrs.yaw_sensor * 0.01f); + plane.takeoff_state.initial_direction.initialized = true; + gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading)); + } +#endif + send_arm_disarm_statustext("Throttle armed"); return true; @@ -381,6 +390,7 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec #if MODE_AUTOLAND_ENABLED // takeoff direction always cleared on disarm plane.takeoff_state.initial_direction.initialized = false; + plane.takeoff_direction_check_completed = false; #endif send_arm_disarm_statustext("Throttle disarmed"); return true; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 6e6991fd54b017..f11dbbc97cef5d 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 in use 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 or other modes. // @User: Advanced ASCALAR(takeoff_options, "TKOFF_OPTIONS", 0), diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 2fb6cb1eebdc51..0a0783354fd0f2 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -514,6 +514,10 @@ void Plane::update_control_mode(void) update_fly_forward(); control_mode->update(); + +#if MODE_AUTOLAND_ENABLED + check_takeoff_direction(); +#endif } @@ -1058,7 +1062,30 @@ void Plane::update_quicktune(void) quicktune.update(control_mode->supports_quicktune()); } #endif +#if MODE_AUTOLAND_ENABLED +/* + In certain pilot controlled modes other than NAV_TAKEOFF or Mode TAKEOFF,takeoff direction is initialized after arm when sufficient altitude and ground speed is obtained, then captured takeoff direction + offset used as landing direction in AUTOLAND +*/ +void Plane::check_takeoff_direction() +{ + if (takeoff_direction_check_completed || takeoff_state.initial_direction.initialized) { + return; + } + //set autoland direction if for GPS heading + if (control_mode->allows_autoland_direction_capture() && (gps.ground_speed() > GPS_GND_CRS_MIN_SPD)) { + set_autoland_direction(); + } +} +// Sets autoland direction using ground course + offest parameter +void Plane::set_autoland_direction() +{ + takeoff_state.initial_direction.heading = wrap_360(gps.ground_course() + mode_autoland.landing_dir_off); + takeoff_state.initial_direction.initialized = true; + takeoff_direction_check_completed = true; + gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.initial_direction.heading)); +} +#endif /* constructor for main Plane class */ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b272a13b11ff12..eaeece728856c5 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1311,6 +1311,12 @@ class Plane : public AP_Vehicle { // last target alt we passed to tecs int32_t tecs_target_alt_cm; +#if MODE_AUTOLAND_ENABLED + // used to set takeoff direction for AUTOLAND mode in some cases + void check_takeoff_direction(void); + bool takeoff_direction_check_completed; + void set_autoland_direction(void); +#endif public: void failsafe_check(void); @@ -1333,6 +1339,11 @@ class Plane : public AP_Vehicle { #endif // AP_SCRIPTING_ENABLED + bool tkoff_option_is_set(AP_FixedWing::TakeoffOption option) const { + return (aparm.takeoff_options & int32_t(option)) != 0; + } + + }; extern Plane plane; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index af99693635b98a..44c2e9cc8640ae 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -382,9 +382,6 @@ 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; -#if MODE_AUTOLAND_ENABLED - takeoff_state.initial_direction.initialized = false; -#endif auto_state.baro_takeoff_alt = barometer.get_altitude(); } @@ -559,11 +556,6 @@ 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(); -#if MODE_AUTOLAND_ENABLED - plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off); - takeoff_state.initial_direction.initialized = true; - gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.initial_direction.heading)); -#endif } 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 6e5d0b56327f0a..1a05bbaacb4182 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -156,6 +156,11 @@ class Mode // true if voltage correction should be applied to throttle virtual bool use_battery_compensation() const; + +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + virtual bool allows_autoland_direction_capture() const { return false; } +#endif #if AP_QUICKTUNE_ENABLED // does this mode support VTOL quicktune? @@ -262,6 +267,11 @@ class ModeAuto : public Mode void run() override; +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + #if AP_PLANE_GLIDER_PULLUP_ENABLED bool in_pullup() const { return pullup.in_pullup(); } #endif @@ -449,6 +459,11 @@ class ModeManual : public Mode // true if voltage correction should be applied to throttle bool use_battery_compensation() const override { return false; } +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + }; @@ -495,6 +510,11 @@ class ModeStabilize : public Mode void run() override; +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + private: void stabilize_stick_mixing_direct(); @@ -552,6 +572,11 @@ class ModeFBWA : public Mode void run() override; +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + }; class ModeFBWB : public Mode @@ -788,6 +813,11 @@ class ModeQAcro : public Mode bool is_vtol_man_throttle() const override { return true; } virtual bool is_vtol_man_mode() const override { return true; } +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + // methods that affect movement of the vehicle in this mode void update() override; @@ -844,6 +874,11 @@ class ModeTakeoff: public Mode bool does_auto_throttle() const override { return true; } +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + // var_info for holding parameter information static const struct AP_Param::GroupInfo var_info[]; diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index e9ec9e8e964425..7e5a2553760db4 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -63,7 +63,6 @@ bool ModeAutoLand::_enter() gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set,must use autotakeoff"); return false; } - plane.prev_WP_loc = plane.current_loc; plane.set_target_altitude_current(); diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 8f18f6d1b2eeb8..9eebcd1c119ade 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -134,9 +134,6 @@ 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. -#if MODE_AUTOLAND_ENABLED - plane.takeoff_state.initial_direction.initialized = false; -#endif } } } @@ -145,16 +142,6 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); takeoff_mode_setup = false; } -#if MODE_AUTOLAND_ENABLED - // set initial_direction.heading - const float min_gps_speed = GPS_GND_CRS_MIN_SPD; - if (!(plane.takeoff_state.initial_direction.initialized) && (plane.gps.ground_speed() > min_gps_speed) - && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { - plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off); - plane.takeoff_state.initial_direction.initialized = true; - gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading)); - } -#endif // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we // pass the target location. The check for target location prevents us // flying towards a wrong location if we can't climb. diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index 8135a7d1603520..df1ca843947d64 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -56,5 +56,6 @@ struct AP_FixedWing { // Bitfields of TKOFF_OPTIONS enum class TakeoffOption { THROTTLE_RANGE = (1U << 0), // Unset: Max throttle. Set: Throttle range. + AUTOLAND_DIR_ON_ARM = (1U << 1), // set dir for autoland on arm if compass in use. }; };