Plane: fixup is_landing and is_taking_off members

This commit is contained in:
Tom Pittenger 2023-06-21 12:30:05 -07:00 committed by Tom Pittenger
parent b35b94f553
commit 4eeb1f18d3
6 changed files with 29 additions and 46 deletions

View File

@ -851,6 +851,28 @@ bool Plane::set_land_descent_rate(float descent_rate)
return false;
}
// returns true if vehicle is landing.
bool Plane::is_landing() const
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_land_descent()) {
return true;
}
#endif
return control_mode->is_landing();
}
// returns true if vehicle is taking off.
bool Plane::is_taking_off() const
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_takeoff()) {
return true;
}
#endif
return control_mode->is_taking_off();
}
#endif // AP_SCRIPTING_ENABLED
// correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL

View File

@ -32,25 +32,3 @@ Plane::Plane(void)
Plane plane;
AP_Vehicle& vehicle = plane;
#if AP_SCRIPTING_ENABLED
// returns true if vehicle is landing. Only used by Lua scripts
bool Plane::is_landing() const
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_land_descent()) {
return true;
}
#endif
return control_mode->is_landing();
}
// returns true if vehicle is taking off. Only used by Lua scripts
bool Plane::is_taking_off() const
{
return control_mode->is_taking_off() || (started_flying_ms > 0 && millis() - started_flying_ms < 30000);
}
#endif // AP_SCRIPTING_ENABLED

View File

@ -523,7 +523,6 @@ private:
float rudder_offset_pct;
bool run_yaw_rate_controller;
} nav_scripting;
#endif
struct {

View File

@ -225,3 +225,8 @@ void Mode::reset_controllers()
plane.steer_state.locked_course = false;
plane.steer_state.locked_course_err = 0;
}
bool Mode::is_taking_off() const
{
return (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF);
}

View File

@ -132,7 +132,7 @@ public:
virtual bool is_landing() const { return false; }
// true if is taking
virtual bool is_taking_off() const { return false; }
virtual bool is_taking_off() const;
protected:
@ -218,8 +218,6 @@ public:
bool is_landing() const override;
bool is_taking_off() const override;
protected:
bool _enter() override;
@ -756,8 +754,6 @@ public:
bool does_auto_throttle() const override { return true; }
bool is_taking_off() const override { return true; }
// var_info for holding parameter information
static const struct AP_Param::GroupInfo var_info[];

View File

@ -162,22 +162,5 @@ bool ModeAuto::_pre_arm_checks(size_t buflen, char *buffer) const
bool ModeAuto::is_landing() const
{
switch(plane.mission.get_current_nav_cmd().id) {
case MAV_CMD_NAV_LAND:
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_DO_VTOL_TRANSITION:
return true;
}
return plane.mission.get_in_landing_sequence_flag();
}
bool ModeAuto::is_taking_off() const
{
switch(plane.mission.get_current_nav_cmd().id) {
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_NAV_VTOL_TAKEOFF:
case MAV_CMD_DO_VTOL_TRANSITION:
return true;
}
return false;
return (plane.flight_stage == AP_FixedWing::FlightStage::LAND);
}