81 lines
1.6 KiB
C++
81 lines
1.6 KiB
C++
#include "supervisor.h"
|
|
#include "roverif.h"
|
|
|
|
#include <algorithm>
|
|
|
|
const Switch::Fixed Supervisor::mode_fixed_ = {
|
|
{ 0, 6310, 9160, 12000 }
|
|
};
|
|
|
|
|
|
Supervisor::Supervisor()
|
|
: state_(State::None), remote_ok_(false), pilot_ok_(false) {
|
|
}
|
|
|
|
void Supervisor::init() {
|
|
change(State::Initial);
|
|
}
|
|
|
|
void Supervisor::set_remote(const uint16_t* channels, int count) {
|
|
if (std::abs(channels[ThrottleChannel] - LostThrottle) > 10) {
|
|
remote_seen_.start(500);
|
|
remote_ok_ = true;
|
|
}
|
|
|
|
mode_.update(mode_fixed_, channels[4]);
|
|
pilot_allowed_ = (mode_.position() == 3);
|
|
|
|
check();
|
|
}
|
|
|
|
void Supervisor::set_pilot(
|
|
bool in_control, const uint16_t* channels, int count) {
|
|
}
|
|
|
|
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_ && pilot_allowed_ && pilot_wants_) {
|
|
change(State::Pilot);
|
|
} else if (remote_ok_ && pilot_allowed_) {
|
|
change(State::RemoteArmed);
|
|
} else if (remote_ok_) {
|
|
change(State::Remote);
|
|
} else {
|
|
if (state_ != State::Initial) {
|
|
change(State::Shutdown);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Supervisor::change(State next) {
|
|
if (state_ != next) {
|
|
state_ = next;
|
|
changed();
|
|
|
|
if (state_ == State::Shutdown) {
|
|
/* PENDING: Use NUM_ELEMS */
|
|
shutdown();
|
|
}
|
|
}
|
|
}
|
|
|
|
void Supervisor::shutdown() {
|
|
}
|
|
|
|
void Supervisor::update(const uint16_t* channels, int count) {
|
|
}
|