ardupilot/ArduPlane/mode.cpp

123 lines
3.7 KiB
C++

#include "Plane.h"
Mode::Mode()
#if HAL_QUADPLANE_ENABLED
: quadplane(plane.quadplane),
pos_control(plane.quadplane.pos_control),
attitude_control(plane.quadplane.attitude_control),
loiter_nav(plane.quadplane.loiter_nav),
poscontrol(plane.quadplane.poscontrol)
#endif
{
}
void Mode::exit()
{
// call sub-classes exit
_exit();
}
bool Mode::enter()
{
#if AP_SCRIPTING_ENABLED
// reset nav_scripting.enabled
plane.nav_scripting.enabled = false;
#endif
// 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;
plane.steer_state.locked_course = false;
// 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 OFFBOARD_GUIDED == ENABLED
plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane.
plane.guided_state.last_target_alt = 0;
#endif
#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();
// set VTOL auto state
plane.auto_state.vtol_mode = is_vtol_mode();
plane.auto_state.vtol_loiter = false;
// initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
plane.new_airspeed_cm = -1;
#if HAL_QUADPLANE_ENABLED
quadplane.mode_enter();
#endif
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 = does_auto_throttle();
#if HAL_ADSB_ENABLED
plane.adsb.set_is_auto_mode(does_auto_navigation());
#endif
// reset steering integrator on mode change
plane.steerController.reset_I();
// update RC failsafe, as mode change may have necessitated changing the failsafe throttle
plane.control_failsafe();
}
return enter_result;
}
bool Mode::is_vtol_man_throttle() const
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.tailsitter.is_in_fw_flight() &&
plane.quadplane.assisted_flight) {
// We are a tailsitter that has fully transitioned to Q-assisted forward flight.
// In this case the forward throttle directly drives the vertical throttle so
// set vertical throttle state to match the forward throttle state. Confusingly the booleans are inverted,
// forward throttle uses 'does_auto_throttle' whereas vertical uses 'is_vtol_man_throttle'.
return !does_auto_throttle();
}
#endif
return false;
}