diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index f2da406219..3648089b8b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -442,6 +442,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Increment: 0.05 // @User: Standard AP_GROUPINFO("LAND_ALTCHG", 31, QuadPlane, landing_detect.detect_alt_change, 0.2), + + // @Param: NAVALT_MIN + // @DisplayName: Minimum navigation altitude + // @Description: This is the altitude in meters above which navigation begins in auto takeoff. Below this altitude the target roll and pitch will be zero. A value of zero disables the feature + // @Range: 0 5 + // @User: Advanced + AP_GROUPINFO("NAVALT_MIN", 32, QuadPlane, takeoff_navalt_min, 0), AP_GROUPEND }; @@ -2790,16 +2797,40 @@ void QuadPlane::takeoff_controller(void) vel = poscontrol.velocity_match * 100; } - pos_control->set_accel_desired_xy_cmss(zero); - pos_control->set_vel_desired_xy_cms(vel); - pos_control->input_vel_accel_xy(vel, zero); + /* + support zeroing roll/pitch during early part of takeoff. This + can help particularly with poor GPS velocity data + */ + bool no_navigation = false; + if (takeoff_navalt_min > 0) { + uint32_t now = AP_HAL::millis(); + const float alt = plane.current_loc.alt*0.01; + if (takeoff_last_run_ms == 0 || + now - takeoff_last_run_ms > 1000) { + takeoff_start_alt = alt; + } + takeoff_last_run_ms = now; + if (alt - takeoff_start_alt < takeoff_navalt_min) { + no_navigation = true; + } + } + + if (no_navigation) { + plane.nav_roll_cd = 0; + plane.nav_pitch_cd = 0; + pos_control->relax_velocity_controller_xy(); + } else { + pos_control->set_accel_desired_xy_cmss(zero); + pos_control->set_vel_desired_xy_cms(vel); + pos_control->input_vel_accel_xy(vel, zero); + + // nav roll and pitch are controller by position controller + plane.nav_roll_cd = pos_control->get_roll_cd(); + plane.nav_pitch_cd = pos_control->get_pitch_cd(); + } run_xy_controller(); - // nav roll and pitch are controller by position controller - plane.nav_roll_cd = pos_control->get_roll_cd(); - plane.nav_pitch_cd = pos_control->get_pitch_cd(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ef47aa1ca3..7444e773a0 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -558,6 +558,10 @@ private: float last_land_final_agl; + // min alt for navigation in takeoff + AP_Float takeoff_navalt_min; + uint32_t takeoff_last_run_ms; + float takeoff_start_alt; // oneshot with duration ARMING_DELAY_MS used by quadplane to delay spoolup after arming: // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set