Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: minor restructure of loiter at end of rtl and srtl

Browse Source
This commit is contained in:
Randy Mackay 2019-10-05 09:20:51 +09:00
parent 0963551d23
commit 65e4ee9e2b
3 changed files with 22 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
APMrover2/mode.h
View File

@ -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
};

30
APMrover2/mode_rtl.cpp
View File

@ -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;
}

26
APMrover2/mode_smart_rtl.cpp
View File

@ -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;
}