Tom Pittenger
6 years ago
committed by
Andrew Tridgell
55 changed files with 1688 additions and 996 deletions
@ -0,0 +1,76 @@
@@ -0,0 +1,76 @@
|
||||
#include "Plane.h" |
||||
|
||||
Mode::Mode() |
||||
{ |
||||
} |
||||
|
||||
void Mode::exit() |
||||
{ |
||||
// call sub-classes exit
|
||||
_exit(); |
||||
} |
||||
|
||||
bool Mode::enter() |
||||
{ |
||||
// cancel inverted flight
|
||||
plane.auto_state.inverted_flight = false; |
||||
|
||||
// don't cross-track when starting a mission
|
||||
plane.auto_state.next_wp_crosstrack = false; |
||||
|
||||
// reset landing check
|
||||
plane.auto_state.checked_for_autoland = false; |
||||
|
||||
// zero locked course
|
||||
plane.steer_state.locked_course_err = 0; |
||||
|
||||
// reset crash detection
|
||||
plane.crash_state.is_crashed = false; |
||||
plane.crash_state.impact_detected = false; |
||||
|
||||
// reset external attitude guidance
|
||||
plane.guided_state.last_forced_rpy_ms.zero(); |
||||
plane.guided_state.last_forced_throttle_ms = 0; |
||||
|
||||
#if CAMERA == ENABLED |
||||
plane.camera.set_is_auto_mode(this == &plane.mode_auto); |
||||
#endif |
||||
|
||||
// zero initial pitch and highest airspeed on mode change
|
||||
plane.auto_state.highest_airspeed = 0; |
||||
plane.auto_state.initial_pitch_cd = plane.ahrs.pitch_sensor; |
||||
|
||||
// disable taildrag takeoff on mode change
|
||||
plane.auto_state.fbwa_tdrag_takeoff_mode = false; |
||||
|
||||
// start with previous WP at current location
|
||||
plane.prev_WP_loc = plane.current_loc; |
||||
|
||||
// new mode means new loiter
|
||||
plane.loiter.start_time_ms = 0; |
||||
|
||||
// record time of mode change
|
||||
plane.last_mode_change_ms = AP_HAL::millis(); |
||||
|
||||
// assume non-VTOL mode
|
||||
plane.auto_state.vtol_mode = false; |
||||
plane.auto_state.vtol_loiter = false; |
||||
|
||||
// reset steering integrator on mode change
|
||||
plane.steerController.reset_I(); |
||||
|
||||
bool enter_result = _enter(); |
||||
|
||||
if (enter_result) { |
||||
// -------------------
|
||||
// these must be done AFTER _enter() because they use the results to set more flags
|
||||
|
||||
// start with throttle suppressed in auto_throttle modes
|
||||
plane.throttle_suppressed = plane.auto_throttle_mode; |
||||
|
||||
plane.adsb.set_is_auto_mode(plane.auto_navigation_mode); |
||||
} |
||||
|
||||
return enter_result; |
||||
} |
||||
|
||||
@ -0,0 +1,433 @@
@@ -0,0 +1,433 @@
|
||||
#pragma once |
||||
|
||||
#include <stdint.h> |
||||
|
||||
class Mode |
||||
{ |
||||
public: |
||||
|
||||
/* Do not allow copies */ |
||||
Mode(const Mode &other) = delete; |
||||
Mode &operator=(const Mode&) = delete; |
||||
|
||||
// Auto Pilot modes
|
||||
// ----------------
|
||||
enum Number { |
||||
MANUAL = 0, |
||||
CIRCLE = 1, |
||||
STABILIZE = 2, |
||||
TRAINING = 3, |
||||
ACRO = 4, |
||||
FLY_BY_WIRE_A = 5, |
||||
FLY_BY_WIRE_B = 6, |
||||
CRUISE = 7, |
||||
AUTOTUNE = 8, |
||||
AUTO = 10, |
||||
RTL = 11, |
||||
LOITER = 12, |
||||
AVOID_ADSB = 14, |
||||
GUIDED = 15, |
||||
INITIALISING = 16, |
||||
QSTABILIZE = 17, |
||||
QHOVER = 18, |
||||
QLOITER = 19, |
||||
QLAND = 20, |
||||
QRTL = 21, |
||||
QAUTOTUNE = 22, |
||||
QACRO = 23, |
||||
}; |
||||
|
||||
// Constructor
|
||||
Mode(); |
||||
|
||||
// enter this mode, always returns true/success
|
||||
bool enter(); |
||||
|
||||
// perform any cleanups required:
|
||||
void exit(); |
||||
|
||||
// returns a unique number specific to this mode
|
||||
virtual Number mode_number() const = 0; |
||||
|
||||
// returns full text name
|
||||
virtual const char *name() const = 0; |
||||
|
||||
// returns a string for this flightmode, exactly 4 bytes
|
||||
virtual const char *name4() const = 0; |
||||
|
||||
//
|
||||
// methods that sub classes should override to affect movement of the vehicle in this mode
|
||||
//
|
||||
|
||||
// convert user input to targets, implement high level control for this mode
|
||||
virtual void update() = 0; |
||||
|
||||
protected: |
||||
|
||||
// subclasses override this to perform checks before entering the mode
|
||||
virtual bool _enter() { return true; } |
||||
|
||||
// subclasses override this to perform any required cleanup when exiting the mode
|
||||
virtual void _exit() { return; } |
||||
}; |
||||
|
||||
|
||||
class ModeAcro : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Mode::Number mode_number() const override { return Mode::Number::ACRO; } |
||||
const char *name() const override { return "ACRO"; } |
||||
const char *name4() const override { return "ACRO"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeAuto : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::AUTO; } |
||||
const char *name() const override { return "AUTO"; } |
||||
const char *name4() const override { return "AUTO"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
void _exit() override; |
||||
}; |
||||
|
||||
|
||||
class ModeAutoTune : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::AUTOTUNE; } |
||||
const char *name() const override { return "AUTOTUNE"; } |
||||
const char *name4() const override { return "ATUN"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
void _exit() override; |
||||
}; |
||||
|
||||
class ModeGuided : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::GUIDED; } |
||||
const char *name() const override { return "GUIDED"; } |
||||
const char *name4() const override { return "GUID"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeCircle: public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::CIRCLE; } |
||||
const char *name() const override { return "CIRCLE"; } |
||||
const char *name4() const override { return "CIRC"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeLoiter : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::LOITER; } |
||||
const char *name() const override { return "LOITER"; } |
||||
const char *name4() const override { return "LOIT"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeManual : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::MANUAL; } |
||||
const char *name() const override { return "MANUAL"; } |
||||
const char *name4() const override { return "MANU"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
void _exit() override; |
||||
}; |
||||
|
||||
|
||||
class ModeRTL : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::RTL; } |
||||
const char *name() const override { return "RTL"; } |
||||
const char *name4() const override { return "RTL "; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeStabilize : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::STABILIZE; } |
||||
const char *name() const override { return "STABILIZE"; } |
||||
const char *name4() const override { return "STAB"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeTraining : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::TRAINING; } |
||||
const char *name() const override { return "TRAINING"; } |
||||
const char *name4() const override { return "TRAN"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeInitializing : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::INITIALISING; } |
||||
const char *name() const override { return "INITIALISING"; } |
||||
const char *name4() const override { return "INIT"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override { } |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeFBWA : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::FLY_BY_WIRE_A; } |
||||
const char *name() const override { return "FLY_BY_WIRE_A"; } |
||||
const char *name4() const override { return "FBWA"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
bool _enter() override; |
||||
|
||||
protected: |
||||
|
||||
}; |
||||
|
||||
class ModeFBWB : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::FLY_BY_WIRE_B; } |
||||
const char *name() const override { return "FLY_BY_WIRE_B"; } |
||||
const char *name4() const override { return "FBWB"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeCruise : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::CRUISE; } |
||||
const char *name() const override { return "CRUISE"; } |
||||
const char *name4() const override { return "CRUS"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeAvoidADSB : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::AVOID_ADSB; } |
||||
const char *name() const override { return "AVOID_ADSB"; } |
||||
const char *name4() const override { return "AVOI"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeQStabilize : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::QSTABILIZE; } |
||||
const char *name() const override { return "QSTABILIZE"; } |
||||
const char *name4() const override { return "QSTB"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
// used as a base class for all Q modes
|
||||
bool _enter() override; |
||||
|
||||
protected: |
||||
|
||||
}; |
||||
|
||||
class ModeQHover : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::QHOVER; } |
||||
const char *name() const override { return "QHOVER"; } |
||||
const char *name4() const override { return "QHOV"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeQLoiter : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::QLOITER; } |
||||
const char *name() const override { return "QLOITER"; } |
||||
const char *name4() const override { return "QLOT"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeQLand : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::QLAND; } |
||||
const char *name() const override { return "QLAND"; } |
||||
const char *name4() const override { return "QLND"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeQRTL : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::QRTL; } |
||||
const char *name() const override { return "QRTL"; } |
||||
const char *name4() const override { return "QRTL"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeQAcro : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::QACRO; } |
||||
const char *name() const override { return "QACO"; } |
||||
const char *name4() const override { return "QACRO"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
|
||||
class ModeQAutotune : public Mode |
||||
{ |
||||
public: |
||||
|
||||
Number mode_number() const override { return Number::QAUTOTUNE; } |
||||
const char *name() const override { return "QAUTOTUNE"; } |
||||
const char *name4() const override { return "QATN"; } |
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override; |
||||
|
||||
protected: |
||||
|
||||
bool _enter() override; |
||||
}; |
||||
@ -0,0 +1,29 @@
@@ -0,0 +1,29 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeAcro::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = false; |
||||
plane.auto_navigation_mode = false; |
||||
plane.acro_state.locked_roll = false; |
||||
plane.acro_state.locked_pitch = false; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeAcro::update() |
||||
{ |
||||
// handle locked/unlocked control
|
||||
if (plane.acro_state.locked_roll) { |
||||
plane.nav_roll_cd = plane.acro_state.locked_roll_err; |
||||
} else { |
||||
plane.nav_roll_cd = plane.ahrs.roll_sensor; |
||||
} |
||||
if (plane.acro_state.locked_pitch) { |
||||
plane.nav_pitch_cd = plane.acro_state.locked_pitch_cd; |
||||
} else { |
||||
plane.nav_pitch_cd = plane.ahrs.pitch_sensor; |
||||
} |
||||
} |
||||
|
||||
@ -0,0 +1,80 @@
@@ -0,0 +1,80 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeAuto::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = true; |
||||
plane.auto_throttle_mode = true; |
||||
plane.auto_navigation_mode = true; |
||||
if (plane.quadplane.available() && plane.quadplane.enable == 2) { |
||||
plane.auto_state.vtol_mode = true; |
||||
} else { |
||||
plane.auto_state.vtol_mode = false; |
||||
} |
||||
plane.next_WP_loc = plane.prev_WP_loc = plane.current_loc; |
||||
// start or resume the mission, based on MIS_AUTORESET
|
||||
plane.mission.start_or_resume(); |
||||
|
||||
plane.g2.soaring_controller.init_cruising(); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeAuto::_exit() |
||||
{ |
||||
if (plane.mission.state() == AP_Mission::MISSION_RUNNING) { |
||||
plane.mission.stop(); |
||||
|
||||
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && |
||||
!plane.quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id)) |
||||
{ |
||||
plane.landing.restart_landing_sequence(); |
||||
} |
||||
} |
||||
plane.auto_state.started_flying_in_auto_ms = 0; |
||||
} |
||||
|
||||
void ModeAuto::update() |
||||
{ |
||||
if (plane.mission.state() != AP_Mission::MISSION_RUNNING) { |
||||
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
|
||||
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
|
||||
plane.set_mode(plane.mode_rtl, MODE_REASON_MISSION_END); |
||||
gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission"); |
||||
return; |
||||
} |
||||
|
||||
uint16_t nav_cmd_id = plane.mission.get_current_nav_cmd().id; |
||||
|
||||
if (plane.quadplane.in_vtol_auto()) { |
||||
plane.quadplane.control_auto(plane.next_WP_loc); |
||||
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || |
||||
(nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) { |
||||
plane.takeoff_calc_roll(); |
||||
plane.takeoff_calc_pitch(); |
||||
plane.calc_throttle(); |
||||
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) { |
||||
plane.calc_nav_roll(); |
||||
plane.calc_nav_pitch(); |
||||
|
||||
// allow landing to restrict the roll limits
|
||||
plane.nav_roll_cd = plane.landing.constrain_roll(plane.nav_roll_cd, plane.g.level_roll_limit*100UL); |
||||
|
||||
if (plane.landing.is_throttle_suppressed()) { |
||||
// if landing is considered complete throttle is never allowed, regardless of landing type
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); |
||||
} else { |
||||
plane.calc_throttle(); |
||||
} |
||||
} else { |
||||
// we are doing normal AUTO flight, the special cases
|
||||
// are for takeoff and landing
|
||||
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) { |
||||
plane.steer_state.hold_course_cd = -1; |
||||
} |
||||
plane.calc_nav_roll(); |
||||
plane.calc_nav_pitch(); |
||||
plane.calc_throttle(); |
||||
} |
||||
} |
||||
|
||||
@ -0,0 +1,24 @@
@@ -0,0 +1,24 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeAutoTune::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = false; |
||||
plane.auto_navigation_mode = false; |
||||
plane.autotune_start(); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeAutoTune::_exit() |
||||
{ |
||||
// restore last gains
|
||||
plane.autotune_restore(); |
||||
} |
||||
|
||||
void ModeAutoTune::update() |
||||
{ |
||||
plane.mode_fbwa.update(); |
||||
} |
||||
|
||||
@ -0,0 +1,13 @@
@@ -0,0 +1,13 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeAvoidADSB::_enter() |
||||
{ |
||||
return plane.mode_guided.enter(); |
||||
} |
||||
|
||||
void ModeAvoidADSB::update() |
||||
{ |
||||
plane.mode_guided.update(); |
||||
} |
||||
|
||||
@ -0,0 +1,26 @@
@@ -0,0 +1,26 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeCircle::_enter() |
||||
{ |
||||
// the altitude to circle at is taken from the current altitude
|
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = true; |
||||
plane.auto_navigation_mode = true; |
||||
plane.next_WP_loc.alt = plane.current_loc.alt; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeCircle::update() |
||||
{ |
||||
// we have no GPS installed and have lost radio contact
|
||||
// or we just want to fly around in a gentle circle w/o GPS,
|
||||
// holding altitude at the altitude we set when we
|
||||
// switched into the mode
|
||||
plane.nav_roll_cd = plane.roll_limit_cd / 3; |
||||
plane.update_load_factor(); |
||||
plane.calc_nav_pitch(); |
||||
plane.calc_throttle(); |
||||
} |
||||
|
||||
@ -0,0 +1,40 @@
@@ -0,0 +1,40 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeCruise::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = true; |
||||
plane.auto_navigation_mode = false; |
||||
plane.cruise_state.locked_heading = false; |
||||
plane.cruise_state.lock_timer_ms = 0; |
||||
|
||||
// for ArduSoar soaring_controller
|
||||
plane.g2.soaring_controller.init_cruising(); |
||||
|
||||
plane.set_target_altitude_current(); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeCruise::update() |
||||
{ |
||||
/*
|
||||
in CRUISE mode we use the navigation code to control |
||||
roll when heading is locked. Heading becomes unlocked on |
||||
any aileron or rudder input |
||||
*/ |
||||
if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) { |
||||
plane.cruise_state.locked_heading = false; |
||||
plane.cruise_state.lock_timer_ms = 0; |
||||
} |
||||
|
||||
if (!plane.cruise_state.locked_heading) { |
||||
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd; |
||||
plane.update_load_factor(); |
||||
} else { |
||||
plane.calc_nav_roll(); |
||||
} |
||||
plane.update_fbwb_speed_height(); |
||||
} |
||||
|
||||
@ -0,0 +1,45 @@
@@ -0,0 +1,45 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeFBWA::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = false; |
||||
plane.auto_navigation_mode = false; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeFBWA::update() |
||||
{ |
||||
// set nav_roll and nav_pitch using sticks
|
||||
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd; |
||||
plane.update_load_factor(); |
||||
float pitch_input = plane.channel_pitch->norm_input(); |
||||
if (pitch_input > 0) { |
||||
plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max_cd; |
||||
} else { |
||||
plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min_cd); |
||||
} |
||||
plane.adjust_nav_pitch_throttle(); |
||||
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); |
||||
if (plane.fly_inverted()) { |
||||
plane.nav_pitch_cd = -plane.nav_pitch_cd; |
||||
} |
||||
if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) { |
||||
// FBWA failsafe glide
|
||||
plane.nav_roll_cd = 0; |
||||
plane.nav_pitch_cd = 0; |
||||
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); |
||||
} |
||||
if (plane.g.fbwa_tdrag_chan > 0) { |
||||
// check for the user enabling FBWA taildrag takeoff mode
|
||||
bool tdrag_mode = (RC_Channels::get_radio_in(plane.g.fbwa_tdrag_chan-1) > 1700); |
||||
if (tdrag_mode && !plane.auto_state.fbwa_tdrag_takeoff_mode) { |
||||
if (plane.auto_state.highest_airspeed < plane.g.takeoff_tdrag_speed1) { |
||||
plane.auto_state.fbwa_tdrag_takeoff_mode = true; |
||||
plane.gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,26 @@
@@ -0,0 +1,26 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeFBWB::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = true; |
||||
plane.auto_navigation_mode = false; |
||||
|
||||
// for ArduSoar soaring_controller
|
||||
plane.g2.soaring_controller.init_cruising(); |
||||
|
||||
plane.set_target_altitude_current(); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeFBWB::update() |
||||
{ |
||||
// Thanks to Yury MonZon for the altitude limit code!
|
||||
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd; |
||||
plane.update_load_factor(); |
||||
plane.update_fbwb_speed_height(); |
||||
|
||||
} |
||||
|
||||
@ -0,0 +1,30 @@
@@ -0,0 +1,30 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeGuided::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = true; |
||||
plane.auto_throttle_mode = true; |
||||
plane.auto_navigation_mode = true; |
||||
plane.guided_throttle_passthru = false; |
||||
/*
|
||||
when entering guided mode we set the target as the current |
||||
location. This matches the behaviour of the copter code |
||||
*/ |
||||
plane.guided_WP_loc = plane.current_loc; |
||||
plane.set_guided_WP(); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeGuided::update() |
||||
{ |
||||
if (plane.auto_state.vtol_loiter && plane.quadplane.available()) { |
||||
plane.quadplane.guided_update(); |
||||
} else { |
||||
plane.calc_nav_roll(); |
||||
plane.calc_nav_pitch(); |
||||
plane.calc_throttle(); |
||||
} |
||||
} |
||||
|
||||
@ -0,0 +1,12 @@
@@ -0,0 +1,12 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeInitializing::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = true; |
||||
plane.auto_throttle_mode = true; |
||||
plane.auto_navigation_mode = false; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
@ -0,0 +1,25 @@
@@ -0,0 +1,25 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeLoiter::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = true; |
||||
plane.auto_throttle_mode = true; |
||||
plane.auto_navigation_mode = true; |
||||
plane.do_loiter_at_location(); |
||||
|
||||
if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) { |
||||
plane.g2.soaring_controller.init_thermalling(); |
||||
plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path
|
||||
} |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeLoiter::update() |
||||
{ |
||||
plane.calc_nav_roll(); |
||||
plane.calc_nav_pitch(); |
||||
plane.calc_throttle(); |
||||
} |
||||
|
||||
@ -0,0 +1,26 @@
@@ -0,0 +1,26 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeManual::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = false; |
||||
plane.auto_navigation_mode = false; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeManual::_exit() |
||||
{ |
||||
if (plane.g.auto_trim > 0) { |
||||
plane.trim_radio(); |
||||
} |
||||
} |
||||
|
||||
void ModeManual::update() |
||||
{ |
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz()); |
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz()); |
||||
plane.steering_control.steering = plane.steering_control.rudder = plane.channel_rudder->get_control_in_zero_dz(); |
||||
} |
||||
|
||||
@ -0,0 +1,14 @@
@@ -0,0 +1,14 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeQAcro::_enter() |
||||
{ |
||||
//return false;
|
||||
return plane.mode_qstabilize._enter(); |
||||
} |
||||
|
||||
void ModeQAcro::update() |
||||
{ |
||||
plane.mode_qstabilize.update(); |
||||
} |
||||
|
||||
@ -0,0 +1,13 @@
@@ -0,0 +1,13 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeQAutotune::_enter() |
||||
{ |
||||
return plane.mode_qstabilize._enter(); |
||||
} |
||||
|
||||
void ModeQAutotune::update() |
||||
{ |
||||
plane.mode_qstabilize.update(); |
||||
} |
||||
|
||||
@ -0,0 +1,14 @@
@@ -0,0 +1,14 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeQHover::_enter() |
||||
{ |
||||
return plane.mode_qstabilize._enter(); |
||||
} |
||||
|
||||
void ModeQHover::update() |
||||
{ |
||||
plane.mode_qstabilize.update(); |
||||
} |
||||
|
||||
|
||||
@ -0,0 +1,13 @@
@@ -0,0 +1,13 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeQLand::_enter() |
||||
{ |
||||
return plane.mode_qstabilize._enter(); |
||||
} |
||||
|
||||
void ModeQLand::update() |
||||
{ |
||||
plane.mode_qstabilize.update(); |
||||
} |
||||
|
||||
@ -0,0 +1,14 @@
@@ -0,0 +1,14 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeQLoiter::_enter() |
||||
{ |
||||
return plane.mode_qstabilize._enter(); |
||||
} |
||||
|
||||
void ModeQLoiter::update() |
||||
{ |
||||
plane.mode_qstabilize.update(); |
||||
} |
||||
|
||||
|
||||
@ -0,0 +1,13 @@
@@ -0,0 +1,13 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeQRTL::_enter() |
||||
{ |
||||
return plane.mode_qstabilize._enter(); |
||||
} |
||||
|
||||
void ModeQRTL::update() |
||||
{ |
||||
plane.mode_qstabilize.update(); |
||||
} |
||||
|
||||
@ -0,0 +1,40 @@
@@ -0,0 +1,40 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeQStabilize::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = true; |
||||
plane.auto_navigation_mode = false; |
||||
if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { |
||||
plane.control_mode = plane.previous_mode; |
||||
} else { |
||||
plane.auto_throttle_mode = false; |
||||
plane.auto_state.vtol_mode = true; |
||||
} |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeQStabilize::update() |
||||
{ |
||||
// set nav_roll and nav_pitch using sticks
|
||||
int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); |
||||
plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit; |
||||
plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit); |
||||
float pitch_input = plane.channel_pitch->norm_input(); |
||||
// Scale from normalized input [-1,1] to centidegrees
|
||||
if (plane.quadplane.tailsitter_active()) { |
||||
// For tailsitters, the pitch range is symmetrical: [-Q_ANGLE_MAX,Q_ANGLE_MAX]
|
||||
plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max; |
||||
} else { |
||||
// pitch is further constrained by LIM_PITCH_MIN/MAX which may impose
|
||||
// tighter (possibly asymmetrical) limits than Q_ANGLE_MAX
|
||||
if (pitch_input > 0) { |
||||
plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max); |
||||
} else { |
||||
plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max); |
||||
} |
||||
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); |
||||
} |
||||
} |
||||
|
||||
@ -0,0 +1,21 @@
@@ -0,0 +1,21 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeRTL::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = true; |
||||
plane.auto_throttle_mode = true; |
||||
plane.auto_navigation_mode = true; |
||||
plane.prev_WP_loc = plane.current_loc; |
||||
plane.do_RTL(plane.get_RTL_altitude()); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeRTL::update() |
||||
{ |
||||
plane.calc_nav_roll(); |
||||
plane.calc_nav_pitch(); |
||||
plane.calc_throttle(); |
||||
} |
||||
|
||||
@ -0,0 +1,18 @@
@@ -0,0 +1,18 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeStabilize::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = false; |
||||
plane.auto_navigation_mode = false; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeStabilize::update() |
||||
{ |
||||
plane.nav_roll_cd = 0; |
||||
plane.nav_pitch_cd = 0; |
||||
} |
||||
|
||||
@ -0,0 +1,44 @@
@@ -0,0 +1,44 @@
|
||||
#include "mode.h" |
||||
#include "Plane.h" |
||||
|
||||
bool ModeTraining::_enter() |
||||
{ |
||||
plane.throttle_allows_nudging = false; |
||||
plane.auto_throttle_mode = false; |
||||
plane.auto_navigation_mode = false; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ModeTraining::update() |
||||
{ |
||||
plane.training_manual_roll = false; |
||||
plane.training_manual_pitch = false; |
||||
plane.update_load_factor(); |
||||
|
||||
// if the roll is past the set roll limit, then
|
||||
// we set target roll to the limit
|
||||
if (plane.ahrs.roll_sensor >= plane.roll_limit_cd) { |
||||
plane.nav_roll_cd = plane.roll_limit_cd; |
||||
} else if (plane.ahrs.roll_sensor <= -plane.roll_limit_cd) { |
||||
plane.nav_roll_cd = -plane.roll_limit_cd; |
||||
} else { |
||||
plane.training_manual_roll = true; |
||||
plane.nav_roll_cd = 0; |
||||
} |
||||
|
||||
// if the pitch is past the set pitch limits, then
|
||||
// we set target pitch to the limit
|
||||
if (plane.ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) { |
||||
plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd; |
||||
} else if (plane.ahrs.pitch_sensor <= plane.pitch_limit_min_cd) { |
||||
plane.nav_pitch_cd = plane.pitch_limit_min_cd; |
||||
} else { |
||||
plane.training_manual_pitch = true; |
||||
plane.nav_pitch_cd = 0; |
||||
} |
||||
if (plane.fly_inverted()) { |
||||
plane.nav_pitch_cd = -plane.nav_pitch_cd; |
||||
} |
||||
} |
||||
|
||||
Loading…
Reference in new issue