Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added support for NAV_PACKAGE_PLACE mission item

Browse Source 
this works in conjunction with a lua script to allow for a wide
variety of package place options
This commit is contained in:
Andrew Tridgell 2023-01-17 17:47:03 +11:00
parent 510d979b79
commit 2989e2deba
7 changed files with 157 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/AP_Arming.cpp
View File

@ -413,7 +413,7 @@ bool AP_Arming_Plane::mission_checks(bool report)
if (!plane.mission.read_cmd_from_storage(i, cmd)) {
break;
}
if ((cmd.id == MAV_CMD_NAV_VTOL_LAND || cmd.id == MAV_CMD_NAV_LAND) &&
if (plane.is_land_command(cmd.id) &&
prev_cmd.id == MAV_CMD_NAV_WAYPOINT) {
const float dist = cmd.content.location.get_distance(prev_cmd.content.location);
const float tecs_land_speed = plane.TECS_controller.get_land_airspeed();

13
ArduPlane/ArduPlane.cpp
View File

@ -801,6 +801,19 @@ bool Plane::set_velocity_match(const Vector2f &velocity)
return false;
}
// allow for override of land descent rate
bool Plane::set_land_descent_rate(float descent_rate)
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_land_descent()) {
quadplane.poscontrol.override_descent_rate = descent_rate;
quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis();
return true;
}
#endif
return false;
}
#endif // AP_SCRIPTING_ENABLED
// correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL

3
ArduPlane/GCS_Mavlink.cpp
View File

@ -1039,8 +1039,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
{
uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
bool is_in_landing = (plane.flight_stage == AP_FixedWing::FlightStage::LAND) ||
(mission_id == MAV_CMD_NAV_LAND) ||
(mission_id == MAV_CMD_NAV_VTOL_LAND);
plane.is_land_command(mission_id);
if (is_in_landing) {
// fly a user planned abort pattern if available
if (plane.mission.jump_to_abort_landing_sequence()) {

16
ArduPlane/Plane.h
View File

@ -938,6 +938,13 @@ private:
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
bool is_land_command(uint16_t cmd) const;
/*
return true if in a specific AUTO mission command
*/
bool in_auto_mission_id(uint16_t command) const;
// Delay the next navigation command
struct {
uint32_t time_max_ms;
@ -956,12 +963,6 @@ private:
// set home location and store it persistently:
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
// quadplane.cpp
#if HAL_QUADPLANE_ENABLED
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);
#endif
// control_modes.cpp
void read_control_switch();
uint8_t readSwitch(void) const;
@ -1229,6 +1230,9 @@ public:
bool get_target_location(Location& target_loc) override;
bool update_target_location(const Location &old_loc, const Location &new_loc) override;
bool set_velocity_match(const Vector2f &velocity) override;
// 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;
#endif // AP_SCRIPTING_ENABLED
};

25
ArduPlane/commands_logic.cpp
View File

@ -98,6 +98,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
return quadplane.do_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_PAYLOAD_PLACE:
if (quadplane.landing_with_fixed_wing_spiral_approach()) {
// the user wants to approach the landing in a fixed wing flight mode
// the waypoint will be used as a loiter_to_alt
@ -243,7 +244,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
case MAV_CMD_NAV_LAND:
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_vtol_land(cmd.id)) {
return quadplane.verify_vtol_land();
return quadplane.verify_vtol_land();
}
#endif
if (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
@ -286,6 +287,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
case MAV_CMD_NAV_VTOL_TAKEOFF:
return quadplane.verify_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_PAYLOAD_PLACE:
if (quadplane.landing_with_fixed_wing_spiral_approach() && !verify_landing_vtol_approach(cmd)) {
// verify_landing_vtol_approach will return true once we have completed the approach,
// in which case we fall over to normal vtol landing code
@ -1276,3 +1278,24 @@ bool Plane::nav_scripting_enable(uint8_t mode)
return nav_scripting.enabled;
}
#endif // AP_SCRIPTING_ENABLED
/*
return true if this is a LAND command
note that we consider a PAYLOAD_PLACE to be a land command as it
follows the landing logic for quadplanes
*/
bool Plane::is_land_command(uint16_t command) const
{
return
command == MAV_CMD_NAV_VTOL_LAND ||
command == MAV_CMD_NAV_LAND ||
command == MAV_CMD_NAV_PAYLOAD_PLACE;
}
/*
return true if in a specific AUTO mission command
*/
bool Plane::in_auto_mission_id(uint16_t command) const
{
return control_mode == &mode_auto && mission.get_current_nav_id() == command;
}

97
ArduPlane/quadplane.cpp
View File

@ -1209,6 +1209,13 @@ bool QuadPlane::is_flying_vtol(void) const
*/
float QuadPlane::landing_descent_rate_cms(float height_above_ground)
{
if (poscontrol.last_override_descent_ms != 0) {
const uint32_t now = AP_HAL::millis();
if (now - poscontrol.last_override_descent_ms < 1000) {
return poscontrol.override_descent_rate*100;
}
}
if (poscontrol.get_state() == QPOS_LAND_FINAL) {
// when in final use descent rate for final even if alt has climbed again
height_above_ground = MIN(height_above_ground, land_final_alt);
@ -2099,6 +2106,7 @@ bool QuadPlane::in_vtol_auto(void) const
return is_vtol_takeoff(id);
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_LAND:
case MAV_CMD_NAV_PAYLOAD_PLACE:
return is_vtol_land(id);
default:
return false;
@ -2285,6 +2293,11 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
} else if (s == QPOS_LAND_DESCEND) {
// reset throttle descent control
qp.thr_ctrl_land = false;
qp.land_descend_start_alt = plane.current_loc.alt*0.01;
last_override_descent_ms = 0;
} else if (s == QPOS_LAND_ABORT) {
// reset throttle descent control
qp.thr_ctrl_land = false;
} else if (s == QPOS_LAND_FINAL) {
// remember last pos reset to handle GPS glitch in LAND_FINAL
Vector2f rpos;
@ -2674,6 +2687,7 @@ void QuadPlane::vtol_position_controller(void)
}
case QPOS_POSITION2:
case QPOS_LAND_ABORT:
case QPOS_LAND_DESCEND: {
setup_target_position();
/*
@ -2825,6 +2839,7 @@ void QuadPlane::vtol_position_controller(void)
}
case QPOS_LAND_DESCEND:
case QPOS_LAND_ABORT:
case QPOS_LAND_FINAL: {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (poscontrol.get_state() == QPOS_LAND_FINAL) {
@ -2832,6 +2847,10 @@ void QuadPlane::vtol_position_controller(void)
ahrs.set_touchdown_expected(true);
}
}
if (poscontrol.get_state() == QPOS_LAND_ABORT) {
set_climb_rate_cms(wp_nav->get_default_speed_up());
break;
}
const float descent_rate_cms = landing_descent_rate_cms(height_above_ground);
pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0);
break;
@ -3044,7 +3063,21 @@ void QuadPlane::control_auto(void)
}
if (poscontrol.get_state() > QPOS_APPROACH) {
if (!plane.arming.get_delay_arming()) {
bool should_run_motors = false;
// don't run the motors if in an arming delay
if (plane.arming.get_delay_arming()) {
should_run_motors = false;
}
// don't run motors if we are in the wait state for payload place
if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::SHUT_DOWN &&
plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
poscontrol.get_state() == QPOS_LAND_COMPLETE) {
should_run_motors = false;
}
if (should_run_motors) {
set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
}
@ -3058,6 +3091,7 @@ void QuadPlane::control_auto(void)
}
break;
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_PAYLOAD_PLACE:
case MAV_CMD_NAV_LAND:
if (is_vtol_land(id)) {
vtol_position_controller();
@ -3283,6 +3317,15 @@ bool QuadPlane::check_land_complete(void)
if (land_detector(4000)) {
poscontrol.set_state(QPOS_LAND_COMPLETE);
gcs().send_text(MAV_SEVERITY_INFO,"Land complete");
if (plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE)) {
// for payload place with full landing we shutdown motors
// and wait for the lua script to trigger a climb (using
// landing abort) or disarm
set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
return false;
}
if (plane.control_mode != &plane.mode_auto ||
!plane.mission.continue_after_land()) {
// disarm on land unless we have MIS_OPTIONS setup to
@ -3384,6 +3427,23 @@ bool QuadPlane::verify_vtol_land(void)
gcs().send_text(MAV_SEVERITY_INFO,"Land final started");
}
// at land_final_alt begin final landing
if (poscontrol.get_state() == QPOS_LAND_ABORT &&
plane.current_loc.alt*0.01 >= land_descend_start_alt) {
// continue to next WP, if there is one
return true;
}
if (plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
(poscontrol.get_state() == QPOS_LAND_DESCEND ||
poscontrol.get_state() == QPOS_LAND_FINAL)) {
const auto &cmd = plane.mission.get_current_nav_cmd();
if (cmd.p1 > 0 && plane.current_loc.alt*0.01 < land_descend_start_alt - cmd.p1*0.01) {
gcs().send_text(MAV_SEVERITY_INFO,"Payload place aborted");
poscontrol.set_state(QPOS_LAND_ABORT);
}
}
if (check_land_complete() && plane.mission.continue_after_land()) {
gcs().send_text(MAV_SEVERITY_INFO,"Mission continue");
return true;
@ -3730,7 +3790,7 @@ bool QuadPlane::is_vtol_takeoff(uint16_t id) const
*/
bool QuadPlane::is_vtol_land(uint16_t id) const
{
if (id == MAV_CMD_NAV_VTOL_LAND) {
if (id == MAV_CMD_NAV_VTOL_LAND || id == MAV_CMD_NAV_PAYLOAD_PLACE) {
if (landing_with_fixed_wing_spiral_approach()) {
return plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING;
} else {
@ -3879,12 +3939,13 @@ bool QuadPlane::in_vtol_land_approach(void) const
*/
bool QuadPlane::in_vtol_land_descent(void) const
{
const auto state = poscontrol.get_state();
if (plane.control_mode == &plane.mode_qrtl &&
(poscontrol.get_state() == QPOS_LAND_DESCEND || poscontrol.get_state() == QPOS_LAND_FINAL)) {
(state == QPOS_LAND_DESCEND || state == QPOS_LAND_FINAL || state == QPOS_LAND_ABORT)) {
return true;
}
if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
(poscontrol.get_state() == QPOS_LAND_DESCEND || poscontrol.get_state() == QPOS_LAND_FINAL)) {
(state == QPOS_LAND_DESCEND || state == QPOS_LAND_FINAL || state == QPOS_LAND_ABORT)) {
return true;
}
return false;
@ -4367,6 +4428,11 @@ bool QuadPlane::landing_with_fixed_wing_spiral_approach(void) const
{
const AP_Mission::Mission_Command cmd = plane.mission.get_current_nav_cmd();
if (cmd.id == MAV_CMD_NAV_PAYLOAD_PLACE &&
option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) {
return true;
}
return ((cmd.id == MAV_CMD_NAV_VTOL_LAND) &&
(option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH) ||
cmd.p1 == NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH));
@ -4413,4 +4479,27 @@ void QuadPlane::setup_rp_fw_angle_gains(void)
attitude_control->set_angle_P_scale(gain_scale);
}
/*
abort landing, used by scripting for payload place and ship landing abort
will return false if not in a landing descent
*/
bool QuadPlane::abort_landing(void)
{
if (poscontrol.get_state() == QPOS_LAND_ABORT) {
// already aborted?
return false;
}
// special case for payload place with full landing
const bool payload_place_landed =
plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
poscontrol.get_state() == QPOS_LAND_COMPLETE;
if (!payload_place_landed && !in_vtol_land_descent()) {
return false;
}
poscontrol.set_state(QuadPlane::QPOS_LAND_ABORT);
return true;
}
#endif // HAL_QUADPLANE_ENABLED

20
ArduPlane/quadplane.h
View File

@ -97,6 +97,9 @@ public:
return available() && assisted_flight;
}
// abort landing, only valid when in a VTOL landing descent
bool abort_landing(void);
/*
return true if we are in a transition to fwd flight from hover
*/
@ -172,6 +175,11 @@ public:
// Check if servo auto trim is allowed
bool allow_servo_auto_trim();
/*
are we in the descent phase of a VTOL landing?
*/
bool in_vtol_land_descent(void) const;
private:
AP_AHRS &ahrs;
@ -461,6 +469,7 @@ private:
QPOS_POSITION1,
QPOS_POSITION2,
QPOS_LAND_DESCEND,
QPOS_LAND_ABORT,
QPOS_LAND_FINAL,
QPOS_LAND_COMPLETE
};
@ -491,6 +500,9 @@ private:
float target_accel;
uint32_t last_pos_reset_ms;
bool overshoot;
float override_descent_rate;
uint32_t last_override_descent_ms;
private:
uint32_t last_state_change_ms;
enum position_control_state state;
@ -574,6 +586,9 @@ private:
float last_land_final_agl;
// AHRS alt for land abort and package place, meters
float land_descend_start_alt;
// min alt for navigation in takeoff
AP_Float takeoff_navalt_min;
uint32_t takeoff_last_run_ms;
@ -603,11 +618,6 @@ private:
*/
bool in_vtol_land_approach(void) const;
/*
are we in the descent phase of a VTOL landing?
*/
bool in_vtol_land_descent(void) const;
/*
are we in the final landing phase of a VTOL landing?
*/