Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Quadplane: add loiter to alt then QLAND mode

Browse Source
This commit is contained in:
Iampete1 2021-09-19 00:15:41 +01:00 committed by Andrew Tridgell
parent d566b24c04
commit 1148bb1234
8 changed files with 87 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/GCS_Mavlink.cpp
View File

@ -58,6 +58,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
case Mode::Number::TAKEOFF:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QRTL:
case Mode::Number::LOITER_ALT_QLAND:
#endif
_base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_STABILIZE_ENABLED;

1
ArduPlane/GCS_Plane.cpp
View File

@ -84,6 +84,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
case Mode::Number::TAKEOFF:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QRTL:
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::THERMAL:
rate_controlled = true;

4
ArduPlane/Plane.h
View File

@ -166,6 +166,7 @@ public:
friend class ModeQAutotune;
friend class ModeTakeoff;
friend class ModeThermal;
friend class ModeLoiterAltQLand;
Plane(void);
@ -299,6 +300,9 @@ private:
#if HAL_SOARING_ENABLED
ModeThermal mode_thermal;
#endif
#if HAL_QUADPLANE_ENABLED
ModeLoiterAltQLand mode_lotier_qland;
#endif
// This is the state of the flight control system
// There are multiple states defined such as MANUAL, FBW-A, AUTO

6
ArduPlane/control_modes.cpp
View File

@ -88,6 +88,12 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
ret = &mode_thermal;
#endif
break;
#if HAL_QUADPLANE_ENABLED
case Mode::Number::LOITER_ALT_QLAND:
ret = &mode_lotier_qland;
break;
#endif // HAL_QUADPLANE_ENABLED
}
return ret;
}

6
ArduPlane/events.cpp
View File

@ -68,6 +68,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::LOITER:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::THERMAL:
if(g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) {
failsafe.saved_mode_number = control_mode->mode_number();
@ -112,6 +115,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
case Mode::Number::TRAINING:
case Mode::Number::CIRCLE:
case Mode::Number::LOITER:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::THERMAL:
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
#if PARACHUTE == ENABLED

23
ArduPlane/mode.h
View File

@ -51,6 +51,9 @@ public:
QACRO = 23,
#endif
THERMAL = 24,
#if HAL_QUADPLANE_ENABLED
LOITER_ALT_QLAND = 25,
#endif
};
// Constructor
@ -269,6 +272,26 @@ protected:
bool _enter() override;
};
#if HAL_QUADPLANE_ENABLED
class ModeLoiterAltQLand : public ModeLoiter
{
public:
Number mode_number() const override { return Number::LOITER_ALT_QLAND; }
const char *name() const override { return "Loiter to QLAND"; }
const char *name4() const override { return "L2QL"; }
protected:
bool _enter() override;
void navigate() override;
private:
void switch_qland();
};
#endif // HAL_QUADPLANE_ENABLED
class ModeManual : public Mode
{
public:

45
ArduPlane/mode_LoiterAltQLand.cpp Normal file
View File

@ -0,0 +1,45 @@
#include "mode.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
bool ModeLoiterAltQLand::_enter()
{
if (plane.previous_mode->is_vtol_mode() || plane.quadplane.in_vtol_mode()) {
plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_IN_VTOL);
return true;
}
ModeLoiter::_enter();
#if AP_TERRAIN_AVAILABLE
if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
} else {
plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
}
#else
plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
#endif
switch_qland();
return true;
}
void ModeLoiterAltQLand::navigate()
{
switch_qland();
ModeLoiter::navigate();
}
void ModeLoiterAltQLand::switch_qland()
{
ftype dist;
if (!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) {
plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);
}
}
#endif

1
ArduPlane/quadplane.h
View File

@ -52,6 +52,7 @@ public:
friend class ModeQStabilize;
friend class ModeQAutotune;
friend class ModeQAcro;
friend class ModeLoiterAltQLand;
QuadPlane(AP_AHRS &_ahrs);