99 lines
2.1 KiB
C++
99 lines
2.1 KiB
C++
#include "supervisor.h"
|
|
#include "roverif.h"
|
|
|
|
#include <algorithm>
|
|
|
|
/* Goals:
|
|
|
|
* Throttle shutdown on start.
|
|
* Throttle shutdown when returning to remote.
|
|
* Clear throttle shutdown by going to idle.
|
|
* None, Remote, or Pilot is in control.
|
|
* Can enter pilot mode when enabled by a switch on the remote.
|
|
* Enter pilot mode immediately without shutdowns.
|
|
|
|
*/
|
|
|
|
Supervisor::Supervisor()
|
|
: in_control_(InControl::Invalid), in_shutdown_(true) {
|
|
}
|
|
|
|
void Supervisor::init() {
|
|
change(InControl::Initial);
|
|
changed();
|
|
}
|
|
|
|
void Supervisor::update_remote(bool throttle_high, bool pilot_allowed) {
|
|
bool old = pilot_allowed_;
|
|
|
|
throttle_high_ = throttle_high;
|
|
pilot_allowed_ = pilot_allowed;
|
|
|
|
remote_seen_.start(500);
|
|
remote_ok_ = true;
|
|
check();
|
|
|
|
if (old != pilot_allowed_) {
|
|
changed();
|
|
}
|
|
}
|
|
|
|
void Supervisor::update_pilot(bool want_control) {
|
|
pilot_wants_ = want_control;
|
|
|
|
pilot_seen_.start(500);
|
|
pilot_ok_ = true;
|
|
check();
|
|
}
|
|
|
|
void Supervisor::tick() {
|
|
remote_seen_.tick();
|
|
pilot_seen_.tick();
|
|
|
|
if (!remote_seen_.running()) {
|
|
remote_ok_ = false;
|
|
check();
|
|
}
|
|
|
|
if (!pilot_seen_.running()) {
|
|
pilot_ok_ = false;
|
|
check();
|
|
}
|
|
}
|
|
|
|
void Supervisor::check() {
|
|
if (remote_ok_ && pilot_ok_) {
|
|
if (pilot_wants_ && pilot_allowed_ && !in_shutdown_) {
|
|
change(InControl::Pilot);
|
|
} else {
|
|
change(InControl::Remote);
|
|
}
|
|
} else if (remote_ok_) {
|
|
change(InControl::Remote);
|
|
} else {
|
|
change(InControl::None);
|
|
}
|
|
|
|
if (in_control_ == InControl::Remote) {
|
|
if (in_shutdown_ && !throttle_high_) {
|
|
in_shutdown_ = false;
|
|
changed();
|
|
}
|
|
}
|
|
}
|
|
|
|
void Supervisor::change(InControl next) {
|
|
if (in_control_ == InControl::Initial && next == InControl::None) {
|
|
// Stay in initial.
|
|
} else if (in_control_ != next) {
|
|
if (next != InControl::Pilot) {
|
|
if (!in_shutdown_) {
|
|
shutdown();
|
|
}
|
|
in_shutdown_ = true;
|
|
}
|
|
in_control_ = next;
|
|
changed();
|
|
}
|
|
}
|