Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added Q_NAVALT_MIN

Browse Source 
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:
Andrew Tridgell 2022-05-24 16:37:14 +10:00
parent a30f239069
commit ae6b877fd3
2 changed files with 42 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduPlane/quadplane.cpp
View File

@ -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());

4
ArduPlane/quadplane.h
View File

@ -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