Plane: Add is_landing/is_taking_off scripting support

This commit is contained in:
Zach Flathers 2023-06-17 10:34:08 -07:00 committed by Tom Pittenger
parent 669b58b017
commit 6177cbbfe2
4 changed files with 66 additions and 0 deletions

View File

@ -32,3 +32,31 @@ Plane::Plane(void)
Plane plane; Plane plane;
AP_Vehicle& vehicle = 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
return plane.quadplane.in_vtol_land_descent();
#endif // HAL_QUADPLANE_ENABLED
return control_mode->is_landing();
}
// returns true if vehicle is taking off. Only used by Lua scripts
bool Plane::is_taking_off() const
{
#if HAL_QUADPLANE_ENABLED
return plane.quadplane.in_vtol_takeoff();
#endif // HAL_QUADPLANE_ENABLED
return control_mode->is_taking_off();
}
#endif // AP_SCRIPTING_ENABLED

View File

@ -523,6 +523,7 @@ private:
float rudder_offset_pct; float rudder_offset_pct;
bool run_yaw_rate_controller; bool run_yaw_rate_controller;
} nav_scripting; } nav_scripting;
#endif #endif
struct { struct {
@ -1245,6 +1246,8 @@ public:
// allow for landing descent rate to be overridden by a script, may be -ve to climb // allow for landing descent rate to be overridden by a script, may be -ve to climb
bool set_land_descent_rate(float descent_rate) override; bool set_land_descent_rate(float descent_rate) override;
bool is_landing() const override;
bool is_taking_off() const override;
#endif // AP_SCRIPTING_ENABLED #endif // AP_SCRIPTING_ENABLED
}; };

View File

@ -128,6 +128,13 @@ public:
// handle a guided target request from GCS // handle a guided target request from GCS
virtual bool handle_guided_request(Location target_loc) { return false; } virtual bool handle_guided_request(Location target_loc) { return false; }
// true if is landing
virtual bool is_landing() const { return false; }
// true if is taking
virtual bool is_taking_off() const { return false; }
protected: protected:
// subclasses override this to perform checks before entering the mode // subclasses override this to perform checks before entering the mode
@ -209,6 +216,10 @@ public:
bool mode_allows_autotuning() const override { return true; } bool mode_allows_autotuning() const override { return true; }
bool is_landing() const override;
bool is_taking_off() const override;
protected: protected:
bool _enter() override; bool _enter() override;
@ -745,6 +756,8 @@ public:
bool does_auto_throttle() const override { return true; } bool does_auto_throttle() const override { return true; }
bool is_taking_off() const override { return true; }
// var_info for holding parameter information // var_info for holding parameter information
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];

View File

@ -159,3 +159,25 @@ bool ModeAuto::_pre_arm_checks(size_t buflen, char *buffer) const
// Note that this bypasses the base class checks // Note that this bypasses the base class checks
return true; return true;
} }
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 false;
}
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;
}