Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: add `get_stopping_point` helper and pass stopping point to `jump_to_landing_sequence`

This commit is contained in:
Iampete1 2024-03-27 17:56:58 +00:00 committed by Andrew Tridgell
parent c8fc90cca3
commit eadcf9b09e
4 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/mode.cpp
View File

@ -1062,3 +1062,12 @@ uint16_t Mode::get_pilot_speed_dn()
{
return copter.get_pilot_speed_dn();
}
// Return stopping point as a location with above origin alt frame
Location Mode::get_stopping_point() const
{
Vector3p stopping_point_NEU;
copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy());
copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z);
return Location { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN };
}

3
ArduCopter/mode.h
View File

@ -197,6 +197,9 @@ protected:
void zero_throttle_and_hold_attitude();
void make_safe_ground_handling(bool force_throttle_unlimited = false);
// Return stopping point as a location with above origin alt frame
Location get_stopping_point() const;
// functions to control normal landing. pause_descent is true if vehicle should not descend
void land_run_horizontal_control();
void land_run_vertical_control(bool pause_descent = false);

2
ArduCopter/mode_auto.cpp
View File

@ -216,7 +216,7 @@ bool ModeAuto::allows_weathervaning() const
// Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
{
if (mission.jump_to_landing_sequence()) {
if (mission.jump_to_landing_sequence(get_stopping_point())) {
mission.set_force_resume(true);
// if not already in auto switch to auto
if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) {

5
ArduCopter/mode_rtl.cpp
View File

@ -387,10 +387,7 @@ void ModeRTL::land_run(bool disarm_on_land)
void ModeRTL::build_path()
{
// origin point is our stopping point
Vector3p stopping_point;
pos_control->get_stopping_point_xy_cm(stopping_point.xy());
pos_control->get_stopping_point_z_cm(stopping_point.z);
rtl_path.origin_point = Location(stopping_point.tofloat(), Location::AltFrame::ABOVE_ORIGIN);
rtl_path.origin_point = get_stopping_point();
rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
// compute return target