AP_Landing: Add more interfaces from vehicle code
Fixs up some documentation about expected return values
This commit is contained in:
parent
0ad46c2f72
commit
c10fe2e7a4
@ -239,6 +239,10 @@ bool AP_Landing::is_flaring(void) const
|
||||
}
|
||||
|
||||
// return true while the aircraft is performing a landing approach
|
||||
// when true the vehicle will:
|
||||
// - disable ground steering
|
||||
// - call setup_landing_glide_slope() and adjust_landing_slope_for_rangefinder_bump()
|
||||
// - will be considered flying if sink rate > 0.2, and can trigger crash detection
|
||||
bool AP_Landing::is_on_approach(void) const
|
||||
{
|
||||
if (!flags.in_progress) {
|
||||
@ -253,7 +257,23 @@ bool AP_Landing::is_on_approach(void) const
|
||||
}
|
||||
}
|
||||
|
||||
// return true while the aircraft is allowed to perform ground steering
|
||||
bool AP_Landing::is_ground_steering_allowed(void) const
|
||||
{
|
||||
if (!flags.in_progress) {
|
||||
return true;
|
||||
}
|
||||
|
||||
switch (type) {
|
||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||
return type_slope_is_on_approach();
|
||||
default:
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// return true when at the last stages of a land when an impact with the ground is expected soon
|
||||
// when true is_flying knows that the vehicle was expecting to stop flying, possibly because of a hard impact
|
||||
bool AP_Landing::is_expecting_impact(void) const
|
||||
{
|
||||
if (!flags.in_progress) {
|
||||
@ -353,6 +373,20 @@ int32_t AP_Landing::constrain_roll(const int32_t desired_roll_cd, const int32_t
|
||||
}
|
||||
}
|
||||
|
||||
// returns true if landing provided a Location structure with the current target altitude
|
||||
bool AP_Landing::get_target_altitude_location(Location &location)
|
||||
{
|
||||
if (!flags.in_progress) {
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (type) {
|
||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Determine how aligned heading_deg is with the wind. Return result
|
||||
* is 1.0 when perfectly aligned heading into wind, -1 when perfectly
|
||||
@ -394,7 +428,10 @@ int32_t AP_Landing::get_target_airspeed_cm(void)
|
||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||
return type_slope_get_target_airspeed_cm();
|
||||
default:
|
||||
return SpdHgt_Controller->get_land_airspeed();
|
||||
// don't return the landing airspeed, because if type is invalid we have
|
||||
// no postive indication that the land airspeed has been configured or
|
||||
// how it was meant to be utilized
|
||||
return SpdHgt_Controller->get_target_airspeed();
|
||||
}
|
||||
}
|
||||
|
||||
@ -450,3 +487,20 @@ void AP_Landing::log(void) const
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* returns true when throttle should be suppressed while landing
|
||||
*/
|
||||
bool AP_Landing::is_throttle_suppressed(void) const
|
||||
{
|
||||
if (!flags.in_progress) {
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (type) {
|
||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||
return type_slope_is_throttle_suppressed();
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -78,8 +78,11 @@ public:
|
||||
bool request_go_around(void);
|
||||
bool is_flaring(void) const;
|
||||
bool is_on_approach(void) const;
|
||||
bool is_ground_steering_allowed(void) const;
|
||||
bool is_throttle_suppressed(void) const;
|
||||
void handle_flight_stage_change(const bool _in_landing_stage);
|
||||
int32_t constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
|
||||
bool get_target_altitude_location(Location &location);
|
||||
|
||||
// helper functions
|
||||
bool restart_landing_sequence(void);
|
||||
@ -121,7 +124,6 @@ private:
|
||||
// calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc)
|
||||
float slope;
|
||||
|
||||
|
||||
AP_Mission &mission;
|
||||
AP_AHRS &ahrs;
|
||||
AP_SpdHgtControl *SpdHgt_Controller;
|
||||
@ -184,5 +186,5 @@ private:
|
||||
bool type_slope_is_flaring(void) const;
|
||||
bool type_slope_is_on_approach(void) const;
|
||||
bool type_slope_is_expecting_impact(void) const;
|
||||
|
||||
bool type_slope_is_throttle_suppressed(void) const;
|
||||
};
|
||||
|
@ -388,3 +388,8 @@ void AP_Landing::type_slope_log(void) const
|
||||
(double)initial_slope,
|
||||
(double)alt_offset);
|
||||
}
|
||||
|
||||
bool AP_Landing::type_slope_is_throttle_suppressed(void) const
|
||||
{
|
||||
return type_slope_stage == SLOPE_STAGE_FINAL;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user