mirror of https://github.com/ArduPilot/ardupilot
AP_Landing: abstract out init_start_nav_cnd work to landing lib
This reverts commit 3e89580383
and fixes it
This commit is contained in:
parent
b299b9c0bb
commit
03b5523b43
|
@ -186,6 +186,19 @@ void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Lo
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* initialize state for new nav command
|
||||
*/
|
||||
void AP_Landing::init_start_nav_cmd(void)
|
||||
{
|
||||
switch (type) {
|
||||
default:
|
||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||
type_slope_init_start_nav_cmd();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Restart a landing by first checking for a DO_LAND_START and
|
||||
jump there. Otherwise decrement waypoint so we would re-start
|
||||
|
|
|
@ -89,6 +89,7 @@ public:
|
|||
int8_t get_abort_throttle_enable(void) const { return abort_throttle_enable; }
|
||||
int8_t get_flap_percent(void) const { return flap_percent; }
|
||||
int8_t get_throttle_slewrate(void) const { return throttle_slewrate; }
|
||||
void init_start_nav_cmd(void);
|
||||
|
||||
|
||||
// Flag to indicate if we have landed.
|
||||
|
@ -110,14 +111,14 @@ public:
|
|||
// landing altitude offset (meters)
|
||||
float alt_offset;
|
||||
|
||||
// once landed, post some landing statistics to the GCS
|
||||
bool post_stats;
|
||||
|
||||
// denotes if a go-around has been commanded for landing
|
||||
bool commanded_go_around;
|
||||
|
||||
private:
|
||||
|
||||
// once landed, post some landing statistics to the GCS
|
||||
bool post_stats;
|
||||
|
||||
bool has_aborted_due_to_slope_recalc;
|
||||
|
||||
AP_Mission &mission;
|
||||
|
@ -157,5 +158,6 @@ private:
|
|||
|
||||
void type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm);
|
||||
void type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
|
||||
void type_slope_init_start_nav_cmd(void);
|
||||
|
||||
};
|
||||
|
|
|
@ -292,3 +292,15 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
|
|||
// stay within the range of the start and end locations in altitude
|
||||
constrain_target_altitude_location_fn(loc, prev_WP_loc);
|
||||
}
|
||||
|
||||
void AP_Landing::type_slope_init_start_nav_cmd(void)
|
||||
{
|
||||
complete = false;
|
||||
pre_flare = false;
|
||||
|
||||
// if a go around had been commanded, clear it now.
|
||||
commanded_go_around = false;
|
||||
|
||||
// once landed, post some landing statistics to the GCS
|
||||
post_stats = false;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue