355 lines
8.9 KiB
C++
355 lines
8.9 KiB
C++
#include <cstring>
|
|
#include <cstdint>
|
|
#include <cstdio>
|
|
|
|
#include "roverif.h"
|
|
#include "hal.h"
|
|
#include "version.h"
|
|
|
|
Servos RoverIf::servos;
|
|
PWMIn RoverIf::pwmin;
|
|
IMU RoverIf::imu;
|
|
Link RoverIf::link;
|
|
Blinker RoverIf::blinker;
|
|
Supervisor RoverIf::supervisor;
|
|
uint8_t RoverIf::ticks_;
|
|
uint8_t RoverIf::pwmin_cycles_;
|
|
MinBitArray RoverIf::pending_;
|
|
uint8_t RoverIf::pilot_supplied_;
|
|
uint8_t RoverIf::demands_;
|
|
|
|
Timer imu_timer;
|
|
Timer check_timer;
|
|
Timer blinker_timer;
|
|
Timer heartbeat_timer;
|
|
Timer pwmin_limiter;
|
|
Timer state_timer;
|
|
|
|
void RoverIf::update_servos(const int8_t* pdemands, bool from_pilot) {
|
|
Supervisor::InControl in_control = supervisor.in_control();
|
|
bool use_pilot = (in_control == Supervisor::InControl::Pilot);
|
|
|
|
if (from_pilot) {
|
|
pilot_supplied_ = 0;
|
|
|
|
if (use_pilot) {
|
|
demands_++;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < servos.NumChannels; i++) {
|
|
if (i == ThrottleChannel && supervisor.in_shutdown()) {
|
|
servos.set(i, Servos::Mid);
|
|
} else if (from_pilot) {
|
|
if (pdemands[i] > Protocol::Demand::Reserved) {
|
|
pilot_supplied_ |= 1 << i;
|
|
|
|
if (use_pilot) {
|
|
servos.set(i, Servos::Mid + pdemands[i]);
|
|
}
|
|
}
|
|
} else {
|
|
if (pdemands[i] > PWMIn::Missing) {
|
|
switch (in_control) {
|
|
case Supervisor::InControl::Remote:
|
|
servos.set(i, Servos::Mid + pdemands[i]);
|
|
break;
|
|
case Supervisor::InControl::Pilot:
|
|
if ((pilot_supplied_ & (1 << i)) == 0) {
|
|
servos.set(i, Servos::Mid + pdemands[i]);
|
|
}
|
|
break;
|
|
default:
|
|
// No control.
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void RoverIf::fill_heartbeat(Protocol::Heartbeat* pmsg) {
|
|
*pmsg = {
|
|
.code = Protocol::Code::Heartbeat,
|
|
.version = 1,
|
|
.device_id = 2,
|
|
};
|
|
}
|
|
|
|
void RoverIf::fill_pwmin(Protocol::Input* pmsg) {
|
|
pmsg->code = Protocol::Code::Inputs;
|
|
pmsg->reference = Protocol::Input::Reference12MHz;
|
|
|
|
for (uint8_t i = 0; i < sizeof(pmsg->channels); i++) {
|
|
pmsg->channels[i] = pwmin.get(i);
|
|
}
|
|
}
|
|
|
|
bool RoverIf::fill_imu(Protocol::IMU* pmsg) {
|
|
pmsg->code = Protocol::Code::IMU;
|
|
return imu.read(pmsg);
|
|
}
|
|
|
|
template<typename T>
|
|
static inline void set_flag(T* pnow, T next) {
|
|
*pnow = (T)(static_cast<int>(*pnow) | static_cast<int>(next));
|
|
}
|
|
|
|
void RoverIf::fill_state(Protocol::State* pmsg) {
|
|
*pmsg = {
|
|
.code = Protocol::Code::State,
|
|
.flags = Protocol::State::Flags::None,
|
|
};
|
|
|
|
if (supervisor.remote_ok()) {
|
|
set_flag(&pmsg->flags, Protocol::State::Flags::RemoteOK);
|
|
}
|
|
if (supervisor.in_shutdown()) {
|
|
set_flag(&pmsg->flags, Protocol::State::Flags::InShutdown);
|
|
}
|
|
if (supervisor.in_control() == Supervisor::InControl::Pilot) {
|
|
set_flag(&pmsg->flags, Protocol::State::Flags::InControl);
|
|
}
|
|
if (supervisor.pilot_allowed()) {
|
|
set_flag(&pmsg->flags, Protocol::State::Flags::PilotAllowed);
|
|
}
|
|
}
|
|
|
|
void RoverIf::fill_pong(Protocol::Pong* pmsg) {
|
|
*pmsg = {
|
|
.code = Protocol::Code::Pong,
|
|
|
|
};
|
|
}
|
|
|
|
void RoverIf::fill_counters(Protocol::Counters* pmsg) {
|
|
*pmsg = {
|
|
.code = Protocol::Code::Counters,
|
|
.demands = demands_,
|
|
.sent = link.sent,
|
|
.received = link.received,
|
|
.rx_errors = link.rx_errors,
|
|
};
|
|
}
|
|
|
|
void RoverIf::fill_version(Protocol::Version* pmsg) {
|
|
pmsg->code = Protocol::Code::Version;
|
|
|
|
uint8_t at;
|
|
for (at = 0; at < sizeof(pmsg->version) && version[at]; at++) {
|
|
pmsg->version[at] = version[at];
|
|
}
|
|
|
|
for (; at < sizeof(pmsg->version); at++) {
|
|
pmsg->version[at] = '\0';
|
|
}
|
|
}
|
|
|
|
void RoverIf::supervisor_changed() {
|
|
pilot_supplied_ = 0;
|
|
defer(Pending::State);
|
|
}
|
|
|
|
void Supervisor::changed() {
|
|
uint8_t red = 0;
|
|
uint8_t green = 0;
|
|
|
|
switch (in_control()) {
|
|
case InControl::Initial:
|
|
red = 0b10000001;
|
|
break;
|
|
case InControl::None:
|
|
red = 0b11111110;
|
|
break;
|
|
case InControl::Remote:
|
|
green = pilot_allowed() ? 0b10001010 : 0b10000010;
|
|
break;
|
|
case InControl::Pilot:
|
|
green = 0b11111110;
|
|
break;
|
|
case InControl::Invalid:
|
|
default:
|
|
red = 0b101;
|
|
break;
|
|
}
|
|
|
|
if (in_shutdown() && red == 0) {
|
|
red = 0b01;
|
|
}
|
|
RoverIf::blinker.set(red, green);
|
|
RoverIf::supervisor_changed();
|
|
}
|
|
|
|
void Supervisor::shutdown() {
|
|
RoverIf::servos.set(RoverIf::ThrottleChannel, Servos::Mid);
|
|
RoverIf::supervisor_changed();
|
|
}
|
|
|
|
void RoverIf::tick() {
|
|
if (imu_timer.tick(50)) {
|
|
defer(Pending::IMU);
|
|
}
|
|
|
|
pwmin_limiter.tick();
|
|
RoverIf::supervisor.tick();
|
|
|
|
if (blinker_timer.tick(1000/7)) {
|
|
RoverIf::blinker.tick();
|
|
}
|
|
if (state_timer.tick(500)) {
|
|
defer(Pending::State);
|
|
}
|
|
if (heartbeat_timer.tick(500)) {
|
|
defer(Pending::Heartbeat);
|
|
defer(Pending::Counters);
|
|
}
|
|
if (check_timer.tick(500)) {
|
|
imu.check();
|
|
}
|
|
}
|
|
|
|
#define DISPATCH_REQUEST(_name) \
|
|
case Protocol::Code::_name: defer(Pending::_name)
|
|
|
|
void RoverIf::handle_request(const Protocol::Request& msg) {
|
|
switch (msg.requested) {
|
|
DISPATCH_REQUEST(Pong);
|
|
DISPATCH_REQUEST(Version);
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void RoverIf::handle_demand(const Protocol::Demand& msg) {
|
|
supervisor.update_pilot(msg.flags == Protocol::Demand::Flags::TakeControl);
|
|
update_servos(msg.channels, true);
|
|
}
|
|
|
|
#define DISPATCH_MSG(_type, _handler) \
|
|
case Protocol::Code::_type: \
|
|
if (length == sizeof(Protocol::_type)) \
|
|
_handler(*(const Protocol::_type*)p); \
|
|
break
|
|
|
|
void RoverIf::poll_link() {
|
|
uint8_t length;
|
|
const void* p = link.peek(&length);
|
|
|
|
if (p != nullptr) {
|
|
switch (*(const Protocol::Code*)p) {
|
|
DISPATCH_MSG(Request, handle_request);
|
|
DISPATCH_MSG(Demand, handle_demand);
|
|
default:
|
|
break;
|
|
}
|
|
link.discard();
|
|
}
|
|
}
|
|
|
|
void RoverIf::poll_ticks() {
|
|
if (ticks_ != HAL::ticks) {
|
|
ticks_++;
|
|
|
|
static_assert((HAL::TickPrescaler & (HAL::TickPrescaler-1)) == 0,
|
|
"Tick prescaler must be a power of 2.");
|
|
if ((ticks_ & (HAL::TickPrescaler-1)) == 0) {
|
|
tick();
|
|
}
|
|
}
|
|
}
|
|
|
|
void RoverIf::poll_pwmin() {
|
|
uint8_t cycles = pwmin.cycles;
|
|
if (cycles != pwmin_cycles_) {
|
|
pwmin_cycles_ = cycles;
|
|
|
|
if (pwmin.get(ShutdownChannel) < -PWMIn::Full*2/3) {
|
|
// Treat as missing.
|
|
} else {
|
|
supervisor.update_remote(
|
|
abs(pwmin.get(ThrottleChannel)) > PWMIn::Full/5,
|
|
pwmin.get(SwitchChannel) > -PWMIn::Full/2);
|
|
|
|
int8_t channels[PWMIn::NumChannels];
|
|
pwmin.get_all(channels);
|
|
update_servos(channels, false);
|
|
|
|
if (!pwmin_limiter.running()) {
|
|
pwmin_limiter.start(50);
|
|
defer(Pending::PWMIn);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#define DISPATCH_PENDING(_code, _type, _handler) \
|
|
case Pending::_code: \
|
|
_handler((Protocol::_type*)pmsg); \
|
|
link.send(sizeof(Protocol::_type)); \
|
|
break
|
|
|
|
#define DISPATCH_PENDINGIF(_code, _type, _handler) \
|
|
case Pending::_code: \
|
|
if (_handler((Protocol::_type*)pmsg)) { \
|
|
link.send(sizeof(Protocol::_type)); \
|
|
} \
|
|
break
|
|
|
|
void RoverIf::poll_pending() {
|
|
if (!pending_.is_empty()) {
|
|
void* pmsg = link.start();
|
|
|
|
if (pmsg != nullptr) {
|
|
Pending next = (Pending)pending_.pop();
|
|
switch (next) {
|
|
DISPATCH_PENDING(PWMIn, Input, fill_pwmin);
|
|
DISPATCH_PENDINGIF(IMU, IMU, fill_imu);
|
|
DISPATCH_PENDING(State, State, fill_state);
|
|
DISPATCH_PENDING(Heartbeat, Heartbeat, fill_heartbeat);
|
|
DISPATCH_PENDING(Pong, Pong, fill_pong);
|
|
DISPATCH_PENDING(Counters, Counters, fill_counters);
|
|
DISPATCH_PENDING(Version, Version, fill_version);
|
|
default:
|
|
assert(false);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void RoverIf::poll() {
|
|
poll_link();
|
|
poll_ticks();
|
|
poll_pwmin();
|
|
poll_pending();
|
|
}
|
|
|
|
void RoverIf::init() {
|
|
// Need at least 10 ms resolution.
|
|
static_assert(HAL::TicksPerSecond >= 100, "Tick rate is too low.");
|
|
// Need to count to at least half a second.
|
|
static_assert(HAL::TicksPerSecond <= 256*2, "Tick rate is too high.");
|
|
static_assert(int(Pending::Max) <= 8, "Too many Pending entries.");
|
|
|
|
HAL::init();
|
|
servos.init();
|
|
pwmin.init();
|
|
imu.init();
|
|
supervisor.init();
|
|
}
|
|
|
|
void RoverIf::run() {
|
|
HAL::start();
|
|
|
|
for (;;) {
|
|
HAL::poll();
|
|
poll();
|
|
HAL::wait();
|
|
}
|
|
}
|
|
|
|
int main() {
|
|
RoverIf::init();
|
|
RoverIf::run();
|
|
return 0;
|
|
}
|