Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -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 committed by Andrew Tridgell
parent 81745f55d2
commit b866b1f412
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; } 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 _loitering; // true if loitering at end of RTL
bool _rtl_loiter; //if loitering at end of RTL
}; };
@ -554,9 +550,6 @@ 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
@ -569,7 +562,7 @@ protected:
bool _enter() override; bool _enter() override;
bool _load_point; 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; sent_notification = false;
_loitering = false;
return true; return true;
} }
@ -46,20 +46,22 @@ void ModeRTL::update()
} }
// we have reached the destination // we have reached the destination
// boats loiters, rovers stop // boats loiter, rovers stop
if (!rover.is_boat()) { if (!rover.is_boat()) {
stop_vehicle(); stop_vehicle();
} else { } else {
// if not loitering yet, start loitering // if not loitering yet, start loitering
if (!_rtl_loiter) { if (!_loitering) {
if (!start_loiter()) { _loitering = rover.mode_loiter.enter();
stop_vehicle(); }
} // update stop or loiter
if (_loitering) {
rover.mode_loiter.update();
} else {
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());
} }
@ -79,13 +81,3 @@ 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;
}

26
APMrover2/mode_smart_rtl.cpp
View File

@ -28,6 +28,7 @@ bool ModeSmartRTL::_enter()
// init state // init state
smart_rtl_state = SmartRTL_WaitForPathCleanup; smart_rtl_state = SmartRTL_WaitForPathCleanup;
_loitering = false;
return true; return true;
} }
@ -80,14 +81,15 @@ void ModeSmartRTL::update()
if (!rover.is_boat()) { if (!rover.is_boat()) {
stop_vehicle(); stop_vehicle();
} else { } else {
// if not loitering yet, start loitering // if not loitering yet, start loitering
if (!_srtl_loiter) { if (!_loitering) {
if (!start_loiter()) { _loitering = rover.mode_loiter.enter();
stop_vehicle(); }
} if (_loitering) {
rover.mode_loiter.update();
} else {
stop_vehicle();
} }
// otherwise update the loiter
rover.mode_loiter.update();
} }
break; break;
} }
@ -119,13 +121,3 @@ 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;
}