Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: added a new TAKEOFF flight mode 

takeoff then circle in direction of takeoff
This commit is contained in:
Andrew Tridgell 2019-05-04 19:56:07 +10:00
parent 6be5578621
commit ff400243e0
14 changed files with 179 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/ArduPlane.cpp
View File

@ -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.

1
ArduPlane/GCS_Mavlink.cpp
View File

@ -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;

1
ArduPlane/GCS_Plane.cpp
View File

@ -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;

20
ArduPlane/Parameters.cpp
View File

@ -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
};

2
ArduPlane/Parameters.h
View File

@ -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,

2
ArduPlane/Plane.h
View File

@ -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

3
ArduPlane/control_modes.cpp
View File

@ -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;
}

2
ArduPlane/events.cpp
View File

@ -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;
}

8
ArduPlane/is_flying.cpp
View File

@ -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));
}

30
ArduPlane/mode.h
View File

@ -1,5 +1,6 @@
#pragma once
#include <AP_Param/AP_Param.h>
#include <stdint.h>
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;
};

114
ArduPlane/mode_takeoff.cpp Normal file
View File

@ -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();
}
}

1
ArduPlane/reverse_thrust.cpp
View File

@ -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:

5
ArduPlane/servos.cpp
View File

@ -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() &&

3
ArduPlane/takeoff.cpp
View File

@ -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