mirror of https://github.com/ArduPilot/ardupilot
Rover: minor restructure of loiter at end of rtl and srtl
This commit is contained in:
parent
0963551d23
commit
65e4ee9e2b
|
@ -518,16 +518,12 @@ 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
|
||||
bool _loitering; // true if loitering at end of RTL
|
||||
|
||||
};
|
||||
|
||||
|
@ -554,9 +550,6 @@ 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
|
||||
|
@ -569,7 +562,7 @@ protected:
|
|||
|
||||
bool _enter() override;
|
||||
bool _load_point;
|
||||
bool _srtl_loiter;
|
||||
bool _loitering; // true if loitering at end of SRTL
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -28,7 +28,7 @@ bool ModeRTL::_enter()
|
|||
}
|
||||
|
||||
sent_notification = false;
|
||||
|
||||
_loitering = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -46,20 +46,22 @@ void ModeRTL::update()
|
|||
}
|
||||
|
||||
// we have reached the destination
|
||||
// boats loiters, rovers stop
|
||||
// boats loiter, rovers stop
|
||||
if (!rover.is_boat()) {
|
||||
stop_vehicle();
|
||||
} else {
|
||||
// if not loitering yet, start loitering
|
||||
if (!_rtl_loiter) {
|
||||
if (!start_loiter()) {
|
||||
stop_vehicle();
|
||||
}
|
||||
if (!_loitering) {
|
||||
_loitering = rover.mode_loiter.enter();
|
||||
}
|
||||
// update stop or loiter
|
||||
if (_loitering) {
|
||||
rover.mode_loiter.update();
|
||||
} else {
|
||||
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());
|
||||
}
|
||||
|
@ -79,13 +81,3 @@ 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;
|
||||
}
|
||||
|
|
|
@ -28,6 +28,7 @@ bool ModeSmartRTL::_enter()
|
|||
|
||||
// init state
|
||||
smart_rtl_state = SmartRTL_WaitForPathCleanup;
|
||||
_loitering = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
@ -80,14 +81,15 @@ void ModeSmartRTL::update()
|
|||
if (!rover.is_boat()) {
|
||||
stop_vehicle();
|
||||
} else {
|
||||
// if not loitering yet, start loitering
|
||||
if (!_srtl_loiter) {
|
||||
if (!start_loiter()) {
|
||||
stop_vehicle();
|
||||
}
|
||||
// if not loitering yet, start loitering
|
||||
if (!_loitering) {
|
||||
_loitering = rover.mode_loiter.enter();
|
||||
}
|
||||
if (_loitering) {
|
||||
rover.mode_loiter.update();
|
||||
} else {
|
||||
stop_vehicle();
|
||||
}
|
||||
// otherwise update the loiter
|
||||
rover.mode_loiter.update();
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
@ -119,13 +121,3 @@ 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