mirror of https://github.com/ArduPilot/ardupilot
Rover: change RTL/SRTL from circling to loitering for boats
This commit is contained in:
parent
2b97f18e27
commit
81745f55d2
|
@ -518,11 +518,17 @@ public:
|
|||
float get_distance_to_destination() const override { return _distance_to_destination; }
|
||||
bool reached_destination() const override;
|
||||
|
||||
// vehicle start loiter
|
||||
bool start_loiter();
|
||||
|
||||
protected:
|
||||
|
||||
bool _enter() override;
|
||||
|
||||
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
|
||||
|
@ -548,6 +554,9 @@ public:
|
|||
// save current position for use by the smart_rtl flight mode
|
||||
void save_position();
|
||||
|
||||
// vehicle start loiter
|
||||
bool start_loiter();
|
||||
|
||||
protected:
|
||||
|
||||
// Safe RTL states
|
||||
|
@ -560,8 +569,10 @@ protected:
|
|||
|
||||
bool _enter() override;
|
||||
bool _load_point;
|
||||
bool _srtl_loiter;
|
||||
};
|
||||
|
||||
|
||||
|
||||
class ModeSteering : public Mode
|
||||
{
|
||||
|
|
|
@ -46,13 +46,20 @@ void ModeRTL::update()
|
|||
}
|
||||
|
||||
// we have reached the destination
|
||||
// boats keep navigating, rovers stop
|
||||
if (rover.is_boat()) {
|
||||
navigate_to_waypoint();
|
||||
} else {
|
||||
// boats loiters, rovers stop
|
||||
if (!rover.is_boat()) {
|
||||
stop_vehicle();
|
||||
}
|
||||
} else {
|
||||
// if not loitering yet, start loitering
|
||||
if (!_rtl_loiter) {
|
||||
if (!start_loiter()) {
|
||||
stop_vehicle();
|
||||
}
|
||||
}
|
||||
|
||||
// update the loiter
|
||||
rover.mode_loiter.update();
|
||||
}
|
||||
// update distance to 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();
|
||||
}
|
||||
|
||||
bool ModeRTL::start_loiter()
|
||||
{
|
||||
if (rover.mode_loiter.enter()) {
|
||||
_rtl_loiter = true;
|
||||
return true;
|
||||
}
|
||||
_rtl_loiter= false;
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -75,12 +75,19 @@ void ModeSmartRTL::update()
|
|||
case SmartRTL_StopAtHome:
|
||||
case SmartRTL_Failure:
|
||||
_reached_destination = true;
|
||||
if (rover.is_boat()) {
|
||||
// boats attempt to hold position at home
|
||||
navigate_to_waypoint();
|
||||
// we have reached the destination
|
||||
// boats loiters, rovers stop
|
||||
if (!rover.is_boat()) {
|
||||
stop_vehicle();
|
||||
} else {
|
||||
// rovers stop
|
||||
stop_vehicle();
|
||||
// if not loitering yet, start loitering
|
||||
if (!_srtl_loiter) {
|
||||
if (!start_loiter()) {
|
||||
stop_vehicle();
|
||||
}
|
||||
}
|
||||
// otherwise update the loiter
|
||||
rover.mode_loiter.update();
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
@ -112,3 +119,13 @@ void ModeSmartRTL::save_position()
|
|||
const bool save_pos = (rover.control_mode != &rover.mode_smartrtl);
|
||||
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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue