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

Rover: change RTL/SRTL from circling to loitering for boats

Browse Source
This commit is contained in:
Henry Wurzburg 2019-10-04 13:26:56 -05:00 committed by Andrew Tridgell
parent 2b97f18e27
commit 81745f55d2
3 changed files with 55 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
APMrover2/mode.h
View File

@ -518,11 +518,17 @@ public:
float get_distance_to_destination() const override { return _distance_to_destination; } float get_distance_to_destination() const override { return _distance_to_destination; }
bool reached_destination() const override; bool reached_destination() const override;
// vehicle start loiter
bool start_loiter();
protected: protected:
bool _enter() override; bool _enter() override;
bool sent_notification; // used to send one time notification to ground station bool sent_notification; // used to send one time notification to ground station
bool _rtl_loiter; //if loitering at end of RTL
}; };
class ModeSmartRTL : public Mode class ModeSmartRTL : public Mode
@ -548,6 +554,9 @@ public:
// save current position for use by the smart_rtl flight mode // save current position for use by the smart_rtl flight mode
void save_position(); void save_position();
// vehicle start loiter
bool start_loiter();
protected: protected:
// Safe RTL states // Safe RTL states
@ -560,9 +569,11 @@ protected:
bool _enter() override; bool _enter() override;
bool _load_point; bool _load_point;
bool _srtl_loiter;
}; };
class ModeSteering : public Mode class ModeSteering : public Mode
{ {
public: public:

23
APMrover2/mode_rtl.cpp
View File

@ -46,13 +46,20 @@ void ModeRTL::update()
} }
// we have reached the destination // we have reached the destination
// boats keep navigating, rovers stop // boats loiters, rovers stop
if (rover.is_boat()) { if (!rover.is_boat()) {
navigate_to_waypoint(); stop_vehicle();
} else { } else {
// if not loitering yet, start loitering
if (!_rtl_loiter) {
if (!start_loiter()) {
stop_vehicle(); stop_vehicle();
} }
}
// update the loiter
rover.mode_loiter.update();
}
// update distance to destination // update distance to destination
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination()); _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
} }
@ -72,3 +79,13 @@ bool ModeRTL::reached_destination() const
{ {
return g2.wp_nav.reached_destination(); return g2.wp_nav.reached_destination();
} }
bool ModeRTL::start_loiter()
{
if (rover.mode_loiter.enter()) {
_rtl_loiter = true;
return true;
}
_rtl_loiter= false;
return false;
}

27
APMrover2/mode_smart_rtl.cpp
View File

@ -75,12 +75,19 @@ void ModeSmartRTL::update()
case SmartRTL_StopAtHome: case SmartRTL_StopAtHome:
case SmartRTL_Failure: case SmartRTL_Failure:
_reached_destination = true; _reached_destination = true;
if (rover.is_boat()) { // we have reached the destination
// boats attempt to hold position at home // boats loiters, rovers stop
navigate_to_waypoint(); if (!rover.is_boat()) {
} else {
// rovers stop
stop_vehicle(); stop_vehicle();
} else {
// if not loitering yet, start loitering
if (!_srtl_loiter) {
if (!start_loiter()) {
stop_vehicle();
}
}
// otherwise update the loiter
rover.mode_loiter.update();
} }
break; break;
} }
@ -112,3 +119,13 @@ void ModeSmartRTL::save_position()
const bool save_pos = (rover.control_mode != &rover.mode_smartrtl); const bool save_pos = (rover.control_mode != &rover.mode_smartrtl);
g2.smart_rtl.update(true, save_pos); g2.smart_rtl.update(true, save_pos);
} }
bool ModeSmartRTL::start_loiter()
{
if (rover.mode_loiter.enter()) {
_srtl_loiter = true;
return true;
}
_srtl_loiter= false;
return false;
}