mirror of https://github.com/ArduPilot/ardupilot
Plane: added Q_NAVALT_MIN
this is equivalent to copters WP_NAVALT_MIN parameter for takeoff. Not implemented for land yet this is useful for vehicles with significant GPS velocity noise on takeoff, preventing dragging the landing gear
This commit is contained in:
parent
a30f239069
commit
ae6b877fd3
|
@ -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());
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue