Rover: follow mode restores offsets to zero on exit

This commit is contained in:
Randy Mackay 2019-10-04 10:24:36 +09:00
parent e4336551cd
commit 0ab5ebbd9b
2 changed files with 7 additions and 0 deletions

View File

@ -630,6 +630,7 @@ public:
protected: protected:
bool _enter() override; bool _enter() override;
void _exit() override;
}; };
class ModeSimple : public Mode class ModeSimple : public Mode

View File

@ -14,6 +14,12 @@ bool ModeFollow::_enter()
return true; return true;
} }
// exit handling
void ModeFollow::_exit()
{
g2.follow.clear_offsets_if_required();
}
void ModeFollow::update() void ModeFollow::update()
{ {
// stop vehicle if no speed estimate // stop vehicle if no speed estimate