mirror of https://github.com/ArduPilot/ardupilot
Copter: rename WP_TKOFF_NAV_ALT to WP_NAVALT_MIN
this is in preparation for using the same parameter for landing as well
This commit is contained in:
parent
8f1f9481eb
commit
89c10a2db9
|
@ -963,12 +963,12 @@ const AP_Param::Info Copter::var_info[] = {
|
||||||
*/
|
*/
|
||||||
const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
||||||
|
|
||||||
// @Param: TKOFF_NAV_ALT
|
// @Param: WP_NAVALT_MIN
|
||||||
// @DisplayName: Takeoff navigation altitude
|
// @DisplayName: Minimum navigation altitude
|
||||||
// @Description: This is the altitude in meters above the takeoff point that attitude changes for navigation can begin
|
// @Description: This is the altitude in meters above which for navigation can begin. This applies in auto takeoff and auto landing.
|
||||||
// @Range: 0 5
|
// @Range: 0 5
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("WP_TKOFF_NAV_ALT", 1, ParametersG2, takeoff_nav_alt, 0),
|
AP_GROUPINFO("WP_NAVALT_MIN", 1, ParametersG2, wp_navalt_min, 0),
|
||||||
|
|
||||||
// @Group: BTN_
|
// @Group: BTN_
|
||||||
// @Path: ../libraries/AP_Button/AP_Button.cpp
|
// @Path: ../libraries/AP_Button/AP_Button.cpp
|
||||||
|
|
|
@ -546,7 +546,7 @@ public:
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
// altitude at which nav control can start in takeoff
|
// altitude at which nav control can start in takeoff
|
||||||
AP_Float takeoff_nav_alt;
|
AP_Float wp_navalt_min;
|
||||||
|
|
||||||
// button checking
|
// button checking
|
||||||
AP_Button button;
|
AP_Button button;
|
||||||
|
|
|
@ -139,7 +139,7 @@ void Copter::auto_takeoff_start(const Location& dest_loc)
|
||||||
// clear i term when we're taking off
|
// clear i term when we're taking off
|
||||||
set_throttle_takeoff();
|
set_throttle_takeoff();
|
||||||
|
|
||||||
// get initial alt for TKOFF_NAV_ALT
|
// get initial alt for WP_NAVALT_MIN
|
||||||
auto_takeoff_set_start_alt();
|
auto_takeoff_set_start_alt();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -74,7 +74,7 @@ bool Copter::guided_takeoff_start(float final_alt_above_home)
|
||||||
// clear i term when we're taking off
|
// clear i term when we're taking off
|
||||||
set_throttle_takeoff();
|
set_throttle_takeoff();
|
||||||
|
|
||||||
// get initial alt for WP_TKOFF_NAV_ALT
|
// get initial alt for WP_NAVALT_MIN
|
||||||
auto_takeoff_set_start_alt();
|
auto_takeoff_set_start_alt();
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -150,21 +150,21 @@ void Copter::auto_takeoff_set_start_alt(void)
|
||||||
auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude();
|
auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude();
|
||||||
|
|
||||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
|
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
|
||||||
// we are not flying, add the takeoff_nav_alt
|
// we are not flying, add the wp_navalt_min
|
||||||
auto_takeoff_no_nav_alt_cm += g2.takeoff_nav_alt * 100;
|
auto_takeoff_no_nav_alt_cm += g2.wp_navalt_min * 100;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
call attitude controller for automatic takeoff, limiting roll/pitch
|
call attitude controller for automatic takeoff, limiting roll/pitch
|
||||||
if below takeoff_nav_alt
|
if below wp_navalt_min
|
||||||
*/
|
*/
|
||||||
void Copter::auto_takeoff_attitude_run(float target_yaw_rate)
|
void Copter::auto_takeoff_attitude_run(float target_yaw_rate)
|
||||||
{
|
{
|
||||||
float nav_roll, nav_pitch;
|
float nav_roll, nav_pitch;
|
||||||
|
|
||||||
if (g2.takeoff_nav_alt > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) {
|
if (g2.wp_navalt_min > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) {
|
||||||
// we haven't reached the takeoff navigation altitude yet
|
// we haven't reached the takeoff navigation altitude yet
|
||||||
nav_roll = 0;
|
nav_roll = 0;
|
||||||
nav_pitch = 0;
|
nav_pitch = 0;
|
||||||
|
|
Loading…
Reference in New Issue