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

Plane: Move navigation functions into flight mode classes.

This commit is contained in:
Samuel Tabor 2020-07-29 16:01:34 +01:00 committed by Peter Barker
parent f14c9db568
commit 06eea6ed9f
12 changed files with 141 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
ArduPlane/ArduPlane.cpp
View File

@ -434,102 +434,12 @@ void Plane::update_control_mode(void)
effective_mode->update();
}
/*
call the navigate method of the active flight mode
*/
void Plane::update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
uint16_t radius = 0;
uint16_t qrtl_radius = abs(g.rtl_radius);
if (qrtl_radius == 0) {
qrtl_radius = abs(aparm.loiter_radius);
}
switch (control_mode->mode_number()) {
case Mode::Number::AUTO:
if (ahrs.home_is_set()) {
mission.update();
}
break;
case Mode::Number::RTL:
if (quadplane.available() && quadplane.rtl_mode == 1 &&
(nav_controller->reached_loiter_target() ||
current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc) ||
auto_state.wp_distance < MAX(qrtl_radius, quadplane.stopping_distance())) &&
AP_HAL::millis() - last_mode_change_ms > 1000) {
/*
for a quadplane in RTL mode we switch to QRTL when we
are within the maximum of the stopping distance and the
RTL_RADIUS
*/
set_mode(mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
break;
} else if (g.rtl_autoland == 1 &&
!auto_state.checked_for_autoland &&
reached_loiter_target() &&
labs(altitude_error_cm) < 1000) {
// we've reached the RTL point, see if we have a landing sequence
if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
mission.set_force_resume(true);
set_mode(mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND);
}
// prevent running the expensive jump_to_landing_sequence
// on every loop
auto_state.checked_for_autoland = true;
}
else if (g.rtl_autoland == 2 &&
!auto_state.checked_for_autoland) {
// Go directly to the landing sequence
if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
mission.set_force_resume(true);
set_mode(mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND);
}
// prevent running the expensive jump_to_landing_sequence
// on every loop
auto_state.checked_for_autoland = true;
}
radius = abs(g.rtl_radius);
if (radius > 0) {
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
}
// fall through to LOITER
FALLTHROUGH;
case Mode::Number::LOITER:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::TAKEOFF:
update_loiter(radius);
break;
case Mode::Number::CRUISE:
update_cruise();
break;
case Mode::Number::MANUAL:
case Mode::Number::STABILIZE:
case Mode::Number::TRAINING:
case Mode::Number::INITIALISING:
case Mode::Number::ACRO:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CIRCLE:
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
case Mode::Number::QRTL:
case Mode::Number::QAUTOTUNE:
case Mode::Number::QACRO:
// nothing to do
break;
}
control_mode->navigate();
}
/*

5
ArduPlane/mode.cpp
View File

@ -85,3 +85,8 @@ bool Mode::enter()
return enter_result;
}
void Mode::navigate()
{
_navigate();
}

13
ArduPlane/mode.h
View File

@ -74,6 +74,9 @@ public:
// true if mode can have terrain following disabled by switch
virtual bool allows_terrain_disable() const { return false; }
// navigation
void navigate();
protected:
// subclasses override this to perform checks before entering the mode
@ -81,6 +84,9 @@ protected:
// subclasses override this to perform any required cleanup when exiting the mode
virtual void _exit() { return; }
// subclasses override this if they require navigation.
virtual void _navigate() { return; }
};
@ -115,6 +121,7 @@ protected:
bool _enter() override;
void _exit() override;
void _navigate() override;
};
@ -149,6 +156,7 @@ public:
protected:
bool _enter() override;
void _navigate() override;
};
class ModeCircle: public Mode
@ -184,6 +192,7 @@ public:
protected:
bool _enter() override;
void _navigate() override;
};
class ModeManual : public Mode
@ -218,6 +227,7 @@ public:
protected:
bool _enter() override;
void _navigate() override;
};
class ModeStabilize : public Mode
@ -319,6 +329,7 @@ public:
protected:
bool _enter() override;
void _navigate() override;
};
class ModeAvoidADSB : public Mode
@ -335,6 +346,7 @@ public:
protected:
bool _enter() override;
void _navigate() override;
};
class ModeQStabilize : public Mode
@ -498,4 +510,5 @@ protected:
Location start_loc;
bool _enter() override;
void _navigate() override;
};

7
ArduPlane/mode_auto.cpp
View File

@ -88,3 +88,10 @@ void ModeAuto::update()
}
}
void ModeAuto::_navigate()
{
if (AP::ahrs().home_is_set()) {
plane.mission.update();
}
}

5
ArduPlane/mode_avoidADSB.cpp
View File

@ -11,3 +11,8 @@ void ModeAvoidADSB::update()
plane.mode_guided.update();
}
void ModeAvoidADSB::_navigate()
{
plane.mode_loiter.navigate();
}

32
ArduPlane/mode_cruise.cpp
View File

@ -40,3 +40,35 @@ void ModeCruise::update()
plane.update_fbwb_speed_height();
}
/*
handle CRUISE mode, locking heading to GPS course when we have
sufficient ground speed, and no aileron or rudder input
*/
void ModeCruise::_navigate()
{
if (!plane.cruise_state.locked_heading &&
plane.channel_roll->get_control_in() == 0 &&
plane.rudder_input() == 0 &&
plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
plane.gps.ground_speed() >= 3 &&
plane.cruise_state.lock_timer_ms == 0) {
// user wants to lock the heading - start the timer
plane.cruise_state.lock_timer_ms = millis();
}
if (plane.cruise_state.lock_timer_ms != 0 &&
(millis() - plane.cruise_state.lock_timer_ms) > 500) {
// lock the heading after 0.5 seconds of zero heading input
// from user
plane.cruise_state.locked_heading = true;
plane.cruise_state.lock_timer_ms = 0;
plane.cruise_state.locked_heading_cd = plane.gps.ground_course_cd();
plane.prev_WP_loc = plane.current_loc;
}
if (plane.cruise_state.locked_heading) {
plane.next_WP_loc = plane.prev_WP_loc;
// always look 1km ahead
plane.next_WP_loc.offset_bearing(plane.cruise_state.locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
}
}

5
ArduPlane/mode_guided.cpp
View File

@ -28,3 +28,8 @@ void ModeGuided::update()
}
}
void ModeGuided::_navigate()
{
plane.mode_loiter.navigate();
}

7
ArduPlane/mode_loiter.cpp
View File

@ -80,3 +80,10 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd)
}
return false;
}
void ModeLoiter::_navigate()
{
// Zero indicates to use WP_LOITER_RAD
plane.update_loiter(0);
}

57
ArduPlane/mode_rtl.cpp
View File

@ -34,3 +34,60 @@ void ModeRTL::update()
}
}
void ModeRTL::_navigate()
{
uint16_t qrtl_radius = abs(plane.g.rtl_radius);
if (qrtl_radius == 0) {
qrtl_radius = abs(plane.aparm.loiter_radius);
}
if (plane.quadplane.available() && plane.quadplane.rtl_mode == 1 &&
(plane.nav_controller->reached_loiter_target() ||
plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc) ||
plane.auto_state.wp_distance < MAX(qrtl_radius, plane.quadplane.stopping_distance())) &&
AP_HAL::millis() - plane.last_mode_change_ms > 1000) {
/*
for a quadplane in RTL mode we switch to QRTL when we
are within the maximum of the stopping distance and the
RTL_RADIUS
*/
plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
return;
}
if (plane.g.rtl_autoland == 1 &&
!plane.auto_state.checked_for_autoland &&
plane.reached_loiter_target() &&
labs(plane.altitude_error_cm) < 1000) {
// we've reached the RTL point, see if we have a landing sequence
if (plane.mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
plane.mission.set_force_resume(true);
plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND);
}
// prevent running the expensive jump_to_landing_sequence
// on every loop
plane.auto_state.checked_for_autoland = true;
}
else if (plane.g.rtl_autoland == 2 &&
!plane.auto_state.checked_for_autoland) {
// Go directly to the landing sequence
if (plane.mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
plane.mission.set_force_resume(true);
plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND);
}
// prevent running the expensive jump_to_landing_sequence
// on every loop
plane.auto_state.checked_for_autoland = true;
}
uint16_t radius = abs(plane.g.rtl_radius);
if (radius > 0) {
plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1;
}
plane.update_loiter(radius);
}

5
ArduPlane/mode_takeoff.cpp
View File

@ -131,3 +131,8 @@ void ModeTakeoff::update()
}
}
void ModeTakeoff::_navigate()
{
plane.mode_loiter.navigate();
}

33
ArduPlane/navigation.cpp
View File

@ -278,39 +278,6 @@ void Plane::update_loiter(uint16_t radius)
}
}
/*
handle CRUISE mode, locking heading to GPS course when we have
sufficient ground speed, and no aileron or rudder input
*/
void Plane::update_cruise()
{
if (!cruise_state.locked_heading &&
channel_roll->get_control_in() == 0 &&
rudder_input() == 0 &&
gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
gps.ground_speed() >= 3 &&
cruise_state.lock_timer_ms == 0) {
// user wants to lock the heading - start the timer
cruise_state.lock_timer_ms = millis();
}
if (cruise_state.lock_timer_ms != 0 &&
(millis() - cruise_state.lock_timer_ms) > 500) {
// lock the heading after 0.5 seconds of zero heading input
// from user
cruise_state.locked_heading = true;
cruise_state.lock_timer_ms = 0;
cruise_state.locked_heading_cd = gps.ground_course_cd();
prev_WP_loc = current_loc;
}
if (cruise_state.locked_heading) {
next_WP_loc = prev_WP_loc;
// always look 1km ahead
next_WP_loc.offset_bearing(cruise_state.locked_heading_cd*0.01f, prev_WP_loc.get_distance(current_loc) + 1000);
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
}
}
/*
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The

1
ArduPlane/quadplane.h
View File

@ -30,6 +30,7 @@ public:
friend class Mode;
friend class ModeAuto;
friend class ModeRTL;
friend class ModeAvoidADSB;
friend class ModeGuided;
friend class ModeQHover;