diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 19712a020d..0a68a37a9e 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -663,6 +663,15 @@ void Plane::update_load_factor(void) } aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll))); + if (quadplane.in_transition() && + (quadplane.options & QuadPlane::OPTION_LEVEL_TRANSITION)) { + /* + the user has asked for transitions to be kept level to + within LEVEL_ROLL_LIMIT + */ + roll_limit_cd = MIN(roll_limit_cd, g.level_roll_limit*100); + } + if (!aparm.stall_prevention) { // stall prevention is disabled return; diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 08b32b6c11..9f7915f049 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -603,7 +603,7 @@ void Plane::rangefinder_height_update(void) (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND || control_mode == QLAND || control_mode == QRTL || - (control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND)) && + (control_mode == AUTO && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) && g.rangefinder_landing) { rangefinder_state.in_use = true; gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 6259edb9cb..73877c0985 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -32,7 +32,8 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) AP_Mission::Mission_Command next_nav_cmd; const uint16_t next_index = mission.get_current_nav_index() + 1; - auto_state.wp_is_land_approach = mission.get_next_nav_cmd(next_index, next_nav_cmd) && (next_nav_cmd.id == MAV_CMD_NAV_LAND); + auto_state.wp_is_land_approach = mission.get_next_nav_cmd(next_index, next_nav_cmd) && (next_nav_cmd.id == MAV_CMD_NAV_LAND) && + !quadplane.is_vtol_land(next_nav_cmd.id); gcs().send_text(MAV_SEVERITY_INFO, "Executing nav command ID #%i",cmd.id); } else { @@ -43,6 +44,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_TAKEOFF: crash_state.is_crashed = false; + if (quadplane.is_vtol_takeoff(cmd.id)) { + return quadplane.do_vtol_takeoff(cmd); + } do_takeoff(cmd); break; @@ -51,6 +55,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_LAND: // LAND to Waypoint + if (quadplane.is_vtol_land(cmd.id)) { + crash_state.is_crashed = false; + return quadplane.do_vtol_land(cmd); + } do_land(cmd); break; @@ -238,12 +246,18 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret switch(cmd.id) { case MAV_CMD_NAV_TAKEOFF: + if (quadplane.is_vtol_takeoff(cmd.id)) { + return quadplane.verify_vtol_takeoff(cmd); + } return verify_takeoff(); case MAV_CMD_NAV_WAYPOINT: return verify_nav_wp(cmd); case MAV_CMD_NAV_LAND: + if (quadplane.is_vtol_land(cmd.id)) { + return quadplane.verify_vtol_land(); + } if (flight_stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_ABORT_LAND) { return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed); @@ -349,6 +363,9 @@ void Plane::do_RTL(int32_t rtl_altitude) DataFlash.Log_Write_Mode(control_mode, control_mode_reason); } +/* + start a NAV_TAKEOFF command + */ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) { prev_WP_loc = current_loc; diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 94094fdb44..135318b981 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -303,7 +303,8 @@ bool Plane::in_preLaunch_flight_stage(void) { return (control_mode == AUTO && throttle_suppressed && flight_stage == AP_Vehicle::FixedWing::FLIGHT_NORMAL && - mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF); + mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF && + !quadplane.is_vtol_takeoff(mission.get_current_nav_cmd().id)); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1f3a5d9cae..1d5057db38 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -399,6 +399,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Description: This controls the mavlink type given in HEARTBEAT messages. For some GCS types a particular setting will be needed for correct operation. // @Values: 0:AUTO,1:FIXED_WING,2:QUADROTOR,3:COAXIAL,4:HELICOPTER,7:AIRSHIP,8:FREE_BALLOON,9:ROCKET,10:GROUND_ROVER,11:SURFACE_BOAT,12:SUBMARINE,16:FLAPPING_WING,17:KITE,19:VTOL_DUOROTOR,20:VTOL_QUADROTOR,21:VTOL_TILTROTOR AP_GROUPINFO("MAV_TYPE", 57, QuadPlane, mav_type, 0), + + // @Param: OPTIONS + // @DisplayName: quadplane options + // @Description: This provides a set of additional control options for quadplanes. LevelTransition means that the wings should be held level to within LEVEL_ROLL_LIMIT degrees during transition to fixed wing flight. If AllowFWTakeoff is disabled then fixed wing takeoff on quadplanes will instead perform a VTOL takeoff. If AllowFWLand is disabled then fixed wing land on quadplanes will instead perform a VTOL land. + // @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand + AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_GROUPEND }; @@ -1463,7 +1469,7 @@ void QuadPlane::check_throttle_suppression(void) } // allow for takeoff - if (plane.control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_TAKEOFF) { + if (plane.control_mode == AUTO && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) { return; } @@ -1631,7 +1637,8 @@ bool QuadPlane::in_vtol_auto(void) if (plane.auto_state.vtol_mode) { return true; } - switch (plane.mission.get_current_nav_cmd().id) { + uint16_t id = plane.mission.get_current_nav_cmd().id; + switch (id) { case MAV_CMD_NAV_VTOL_LAND: case MAV_CMD_NAV_VTOL_TAKEOFF: return true; @@ -1640,6 +1647,10 @@ bool QuadPlane::in_vtol_auto(void) case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_LOITER_TO_ALT: return plane.auto_state.vtol_loiter; + case MAV_CMD_NAV_TAKEOFF: + return is_vtol_takeoff(id); + case MAV_CMD_NAV_LAND: + return is_vtol_land(id); default: return false; } @@ -1970,9 +1981,18 @@ void QuadPlane::control_auto(const Location &loc) motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - switch (plane.mission.get_current_nav_cmd().id) { + uint16_t id = plane.mission.get_current_nav_cmd().id; + switch (id) { case MAV_CMD_NAV_VTOL_TAKEOFF: - takeoff_controller(); + case MAV_CMD_NAV_TAKEOFF: + if (is_vtol_takeoff(id)) { + takeoff_controller(); + } + break; + case MAV_CMD_NAV_LAND: + if (is_vtol_land(id)) { + vtol_position_controller(); + } break; case MAV_CMD_NAV_VTOL_LAND: case MAV_CMD_NAV_LOITER_UNLIM: @@ -2440,3 +2460,43 @@ uint8_t QuadPlane::get_mav_type(void) const } return uint8_t(mav_type.get()); } + +/* + return true if current mission item is a vtol takeoff +*/ +bool QuadPlane::is_vtol_takeoff(uint16_t id) const +{ + if (id == MAV_CMD_NAV_VTOL_TAKEOFF) { + return true; + } + if (id == MAV_CMD_NAV_TAKEOFF && available() && (options & OPTION_ALLOW_FW_TAKEOFF) == 0) { + // treat fixed wing takeoff as VTOL takeoff + return true; + } + return false; +} + +/* + return true if current mission item is a vtol land +*/ +bool QuadPlane::is_vtol_land(uint16_t id) const +{ + if (id == MAV_CMD_NAV_VTOL_LAND) { + return true; + } + if (id == MAV_CMD_NAV_LAND && available() && (options & OPTION_ALLOW_FW_LAND) == 0) { + // treat fixed wing land as VTOL land + return true; + } + return false; +} + +/* + return true if we are in a transition to fwd flight from hover + */ +bool QuadPlane::in_transition(void) const +{ + return available() && assisted_flight && + (transition_state == TRANSITION_AIRSPEED_WAIT || + transition_state == TRANSITION_TIMER); +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index d945461cfb..a66457ec4d 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -49,6 +49,11 @@ public: bool in_assisted_flight(void) const { return available() && assisted_flight; } + + /* + return true if we are in a transition to fwd flight from hover + */ + bool in_transition(void) const; bool handle_do_vtol_transition(enum MAV_VTOL_STATE state); @@ -304,7 +309,7 @@ private: // are we in a guided takeoff? bool guided_takeoff:1; - + struct { // time when motors reached lower limit uint32_t lower_limit_start_ms; @@ -418,6 +423,24 @@ private: // adjust altitude target smoothly void adjust_alt_target(float target_cm); + + // additional options + AP_Int32 options; + enum { + OPTION_LEVEL_TRANSITION=(1<<0), + OPTION_ALLOW_FW_TAKEOFF=(1<<1), + OPTION_ALLOW_FW_LAND=(1<<2), + }; + + /* + return true if current mission item is a vtol takeoff + */ + bool is_vtol_takeoff(uint16_t id) const; + + /* + return true if current mission item is a vtol landing + */ + bool is_vtol_land(uint16_t id) const; public: void motor_test_output(); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index c22d992b5c..f6f093e6d8 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -463,7 +463,8 @@ void Plane::exit_mode(enum FlightMode mode) if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); - if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) + if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && + !quadplane.is_vtol_land(mission.get_current_nav_cmd().id)) { landing.restart_landing_sequence(); }