From ff400243e0af65f90500f83b8d554a4593a82d33 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 May 2019 19:56:07 +1000 Subject: [PATCH] Plane: added a new TAKEOFF flight mode takeoff then circle in direction of takeoff --- ArduPlane/ArduPlane.cpp | 3 +- ArduPlane/GCS_Mavlink.cpp | 1 + ArduPlane/GCS_Plane.cpp | 1 + ArduPlane/Parameters.cpp | 20 +++--- ArduPlane/Parameters.h | 2 +- ArduPlane/Plane.h | 2 + ArduPlane/control_modes.cpp | 3 + ArduPlane/events.cpp | 2 + ArduPlane/is_flying.cpp | 8 ++- ArduPlane/mode.h | 30 +++++++++ ArduPlane/mode_takeoff.cpp | 114 +++++++++++++++++++++++++++++++++++ ArduPlane/reverse_thrust.cpp | 1 + ArduPlane/servos.cpp | 5 +- ArduPlane/takeoff.cpp | 3 +- 14 files changed, 179 insertions(+), 16 deletions(-) create mode 100644 ArduPlane/mode_takeoff.cpp diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 6295fa2ab7..b6f5a5463d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -491,6 +491,7 @@ void Plane::update_navigation() case Mode::Number::LOITER: case Mode::Number::AVOID_ADSB: case Mode::Number::GUIDED: + case Mode::Number::TAKEOFF: update_loiter(radius); break; @@ -617,7 +618,7 @@ void Plane::update_flight_stage(void) } else { set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); } - } else { + } else if (control_mode != &mode_takeoff) { // If not in AUTO then assume normal operation for normal TECS operation. // This prevents TECS from being stuck in the wrong stage if you switch from // AUTO to, say, FBWB during a landing, an aborted landing or takeoff. diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 165e3b771a..5d90ffce08 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -44,6 +44,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case Mode::Number::AVOID_ADSB: case Mode::Number::GUIDED: case Mode::Number::CIRCLE: + case Mode::Number::TAKEOFF: case Mode::Number::QRTL: _base_mode = MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index ad902f6925..9fb6a665e9 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -81,6 +81,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) case Mode::Number::AVOID_ADSB: case Mode::Number::GUIDED: case Mode::Number::CIRCLE: + case Mode::Number::TAKEOFF: case Mode::Number::QRTL: rate_controlled = true; attitude_stabilized = true; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 8ecf1fecbe..be6d8ded53 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -538,49 +538,49 @@ const AP_Param::Info Plane::var_info[] = { // @Param: FLTMODE1 // @DisplayName: FlightMode1 // @Description: Flight mode for switch position 1 (910 to 1230 and above 2049) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), // @Param: FLTMODE2 // @DisplayName: FlightMode2 // @Description: Flight mode for switch position 2 (1231 to 1360) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO // @User: Standard GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), // @Param: FLTMODE3 // @DisplayName: FlightMode3 // @Description: Flight mode for switch position 3 (1361 to 1490) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO // @User: Standard GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), // @Param: FLTMODE4 // @DisplayName: FlightMode4 // @Description: Flight mode for switch position 4 (1491 to 1620) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO // @User: Standard GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), // @Param: FLTMODE5 // @DisplayName: FlightMode5 // @Description: Flight mode for switch position 5 (1621 to 1749) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO // @User: Standard GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), // @Param: FLTMODE6 // @DisplayName: FlightMode6 // @Description: Flight mode for switch position 6 (1750 to 2049) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), // @Param: INITIAL_MODE // @DisplayName: Initial flight mode // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO // @User: Advanced GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL), @@ -1089,7 +1089,11 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_OSD/AP_OSD.cpp GOBJECT(osd, "OSD", AP_OSD), #endif - + + // @Group: TKOFF_ + // @Path: mode_takeoff.cpp + GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff), + AP_VAREND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index eb41c96beb..7d48e86796 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -88,7 +88,7 @@ public: k_param_scheduler, k_param_relay, k_param_takeoff_throttle_delay, - k_param_skip_gyro_cal, // unused + k_param_mode_takeoff, // was skip_gyro_cal k_param_auto_fbw_steer, k_param_waypoint_max_radius, k_param_ground_steer_alt, diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2a2f39f649..faa10e1774 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -155,6 +155,7 @@ public: friend class ModeQRTL; friend class ModeQAcro; friend class ModeQAutotune; + friend class ModeTakeoff; Plane(void); @@ -288,6 +289,7 @@ private: ModeQRTL mode_qrtl; ModeQAcro mode_qacro; ModeQAutotune mode_qautotune; + ModeTakeoff mode_takeoff; // This is the state of the flight control system // There are multiple states defined such as MANUAL, FBW-A, AUTO diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index dec778e1cf..43153fe86c 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -70,6 +70,9 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::QAUTOTUNE: ret = &mode_qautotune; break; + case Mode::Number::TAKEOFF: + ret = &mode_takeoff; + break; } return ret; } diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index ad31949337..39db0e2c77 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -55,6 +55,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re break; case Mode::Number::CIRCLE: + case Mode::Number::TAKEOFF: case Mode::Number::RTL: case Mode::Number::QLAND: case Mode::Number::QRTL: @@ -123,6 +124,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t rea case Mode::Number::RTL: case Mode::Number::QLAND: case Mode::Number::QRTL: + case Mode::Number::TAKEOFF: case Mode::Number::INITIALISING: break; } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index d6b3168fe1..d69c919fb0 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -319,12 +319,14 @@ void Plane::crash_detection_update(void) /* * return true if we are in a pre-launch phase of an auto-launch, typically used in bungee launches */ -bool Plane::in_preLaunch_flight_stage(void) { +bool Plane::in_preLaunch_flight_stage(void) +{ + if (control_mode == &mode_takeoff && throttle_suppressed) { + return true; + } return (control_mode == &mode_auto && throttle_suppressed && flight_stage == AP_Vehicle::FixedWing::FLIGHT_NORMAL && mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF && !quadplane.is_vtol_takeoff(mission.get_current_nav_cmd().id)); } - - diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index c6aba6f3d3..1c20b23582 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -1,5 +1,6 @@ #pragma once +#include #include class Mode @@ -25,6 +26,7 @@ public: AUTO = 10, RTL = 11, LOITER = 12, + TAKEOFF = 13, AVOID_ADSB = 14, GUIDED = 15, INITIALISING = 16, @@ -449,3 +451,31 @@ protected: bool _enter() override; void _exit() override; }; + + +class ModeTakeoff: public Mode +{ +public: + ModeTakeoff(); + + Number mode_number() const override { return Number::TAKEOFF; } + const char *name() const override { return "TAKEOFF"; } + const char *name4() const override { return "TKOF"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + + // var_info for holding parameter information + static const struct AP_Param::GroupInfo var_info[]; + +protected: + AP_Int16 target_alt; + AP_Int16 target_dist; + AP_Int16 level_alt; + AP_Int8 level_pitch; + + int32_t initial_alt_cm; + bool takeoff_started; + + bool _enter() override; +}; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp new file mode 100644 index 0000000000..85db85915b --- /dev/null +++ b/ArduPlane/mode_takeoff.cpp @@ -0,0 +1,114 @@ +#include "mode.h" +#include "Plane.h" + +/* + mode takeoff parameters + */ +const AP_Param::GroupInfo ModeTakeoff::var_info[] = { + // @Param: ALT + // @DisplayName: Takeoff mode altitude + // @Description: This is the target altitude for TAKEOFF mode + // @Range: 0 200 + // @Increment: 1 + // @Units: m + // @User: User + AP_GROUPINFO("ALT", 1, ModeTakeoff, target_alt, 50), + + // @Param: LVL_ALT + // @DisplayName: Takeoff mode altitude level altitude + // @Description: This is the altitude below which wings are held level for TAKEOFF mode + // @Range: 0 50 + // @Increment: 1 + // @Units: m + // @User: User + AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 20), + + // @Param: LVL_PITCH + // @DisplayName: Takeoff mode altitude initial pitch + // @Description: This is the target pitch for the initial climb to TKOFF_LVL_ALT + // @Range: 0 30 + // @Increment: 1 + // @Units: deg + // @User: User + AP_GROUPINFO("LVL_PITCH", 3, ModeTakeoff, level_pitch, 15), + + // @Param: DIST + // @DisplayName: Takeoff mode distance + // @Description: This is the distance from the takeoff location where the plane will loiter. The loiter point will be in the direction of takeoff (the direction the plane is facing when the motor starts) + // @Range: 0 500 + // @Increment: 1 + // @Units: m + // @User: User + AP_GROUPINFO("DIST", 4, ModeTakeoff, target_dist, 200), + + AP_GROUPEND +}; + +ModeTakeoff::ModeTakeoff() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +bool ModeTakeoff::_enter() +{ + // the altitude to circle at is taken from the current altitude + plane.throttle_allows_nudging = true; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = true; + + takeoff_started = false; + + return true; +} + +void ModeTakeoff::update() +{ + if (!takeoff_started) { + // setup target location 1.5 times loiter radius from the + // takeoff point, at a height of TKOFF_ALT + const float dist = target_dist; + const float alt = target_alt; + const float direction = degrees(plane.ahrs.yaw); + + plane.prev_WP_loc = plane.current_loc; + plane.next_WP_loc = plane.current_loc; + plane.next_WP_loc.alt += alt*100.0; + plane.next_WP_loc.offset_bearing(direction, dist); + + initial_alt_cm = plane.current_loc.alt; + + plane.crash_state.is_crashed = false; + + plane.auto_state.takeoff_pitch_cd = level_pitch * 100; + + plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF); + + if (!plane.throttle_suppressed) { + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm at %.1fm to %.1f deg", + alt, dist, direction); + takeoff_started = true; + } + } + + // we finish the initial level takeoff if we climb past + // TKOFF_LVL_ALT or we pass the target location. The check for + // target location prevents us flying forever if we can't climb + if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && + (plane.current_loc.alt - initial_alt_cm >= level_alt*100 || + plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc))) { + // reached level alt + plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); + plane.complete_auto_takeoff(); + } + + if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) { + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100); + plane.takeoff_calc_roll(); + plane.takeoff_calc_pitch(); + } else { + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + plane.calc_throttle(); + } +} + diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index 0414456886..44ea9d9312 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -71,6 +71,7 @@ bool Plane::allow_reverse_thrust(void) const allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_RTL); break; case Mode::Number::CIRCLE: + case Mode::Number::TAKEOFF: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CIRCLE); break; case Mode::Number::CRUISE: diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 28eb02a5a7..192483c2b1 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -82,8 +82,9 @@ bool Plane::suppress_throttle(void) bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5); - if (control_mode == &mode_auto && - auto_state.takeoff_complete == false) { + if ((control_mode == &mode_auto && + auto_state.takeoff_complete == false) || + control_mode == &mode_takeoff) { uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000; if (is_flying() && diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 6df47085e4..3afc08eafb 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -169,7 +169,8 @@ void Plane::takeoff_calc_pitch(void) } if (aparm.stall_prevention != 0) { - if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF) { + if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF || + control_mode == &mode_takeoff) { // during takeoff we want to prioritise roll control over // pitch. Apply a reduction in pitch demand if our roll is // significantly off. The aim of this change is to