146 lines
3.3 KiB
C++
146 lines
3.3 KiB
C++
#include <cstring>
|
|
#include <cstdint>
|
|
#include <cstdio>
|
|
|
|
#include "roverif.h"
|
|
#include "hal.h"
|
|
|
|
PWMIn RoverIf::pwmin;
|
|
Servos RoverIf::servos;
|
|
Link RoverIf::link;
|
|
Blinker RoverIf::blinker;
|
|
Supervisor RoverIf::supervisor;
|
|
uint8_t RoverIf::ticks_;
|
|
MinBitArray RoverIf::pending;
|
|
|
|
Timer blinker_timer;
|
|
Timer heartbeat_timer;
|
|
Timer pwmin_timer;
|
|
|
|
void RoverIf::fill_heartbeat(Protocol::Heartbeat* pmsg) {
|
|
*pmsg = {
|
|
.code = Protocol::Code::Heartbeat,
|
|
.version = 1,
|
|
.device_id = 2,
|
|
.ticks = HAL::ticks,
|
|
.state = (uint8_t)supervisor.state(),
|
|
};
|
|
}
|
|
|
|
void RoverIf::fill_pwmin(Protocol::Inputs* pmsg) {
|
|
pmsg->code = Protocol::Code::Inputs;
|
|
|
|
for (uint8_t i = 0; i < sizeof(pmsg->channels); i++) {
|
|
pmsg->channels[i] = pwmin.get(i);
|
|
}
|
|
|
|
static uint8_t offset = Servos::Low;
|
|
|
|
servos.set(0, offset);
|
|
if (++offset > Servos::High)
|
|
offset = Servos::Low;
|
|
}
|
|
|
|
void RoverIf::fill_pong(Protocol::Pong* pmsg) {
|
|
*pmsg = {
|
|
.code = Protocol::Code::Pong,
|
|
};
|
|
}
|
|
|
|
void Supervisor::changed() {
|
|
static const uint8_t patterns[][2] = {
|
|
[State::None] = { 0b101, 0 },
|
|
[State::Initial] = { 0b10000001, 0 },
|
|
[State::Remote] = { 0, 0b10000001 },
|
|
[State::RemoteArmed] = { 0, 0b10000101 },
|
|
[State::Pilot] = { 0, 0b11111110 },
|
|
[State::Shutdown] = { 0b1011, 0 },
|
|
};
|
|
|
|
int idx = static_cast<int>(state());
|
|
RoverIf::blinker.set(patterns[idx][0], patterns[idx][1]);
|
|
}
|
|
|
|
void RoverIf::tick() {
|
|
if (pwmin_timer.tick(HAL::TicksPerSecond / 10)) {
|
|
defer(Pending::PWMIn);
|
|
}
|
|
if (blinker_timer.tick(HAL::TicksPerSecond / 7)) {
|
|
RoverIf::blinker.tick();
|
|
}
|
|
if (heartbeat_timer.tick(HAL::TicksPerSecond / 2)) {
|
|
defer(Pending::Heartbeat);
|
|
}
|
|
RoverIf::supervisor.tick();
|
|
}
|
|
|
|
void RoverIf::handle_ping(const Protocol::Ping& msg) {
|
|
defer(Pending::Pong);
|
|
}
|
|
|
|
#define DISPATCH(_type, _handler) \
|
|
case Protocol::Code::_type: \
|
|
if (length == sizeof(Protocol::_type)) \
|
|
_handler(*(const Protocol::_type*)p); \
|
|
break
|
|
|
|
#define DISPATCH_PENDING(_code, _type, _handler) \
|
|
case Pending::_code: \
|
|
_handler((Protocol::_type*)pmsg); \
|
|
link.send(sizeof(Protocol::_type)); \
|
|
break
|
|
|
|
void RoverIf::poll() {
|
|
uint8_t length;
|
|
const void* p = link.peek(&length);
|
|
|
|
if (p != nullptr) {
|
|
switch (*(const Protocol::Code*)p) {
|
|
DISPATCH(Ping, handle_ping);
|
|
default:
|
|
break;
|
|
}
|
|
link.discard();
|
|
}
|
|
|
|
if (ticks_ != HAL::ticks) {
|
|
ticks_++;
|
|
tick();
|
|
}
|
|
|
|
if (!pending.is_empty()) {
|
|
void* pmsg = link.start();
|
|
|
|
if (pmsg != nullptr) {
|
|
Pending next = (Pending)pending.pop();
|
|
switch (next) {
|
|
DISPATCH_PENDING(PWMIn, Inputs, fill_pwmin);
|
|
DISPATCH_PENDING(Heartbeat, Heartbeat, fill_heartbeat);
|
|
DISPATCH_PENDING(Pong, Pong, fill_pong);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void RoverIf::init() {
|
|
HAL::init();
|
|
servos.init();
|
|
pwmin.init();
|
|
supervisor.init();
|
|
}
|
|
|
|
void RoverIf::run() {
|
|
HAL::start();
|
|
|
|
for (;;) {
|
|
HAL::poll();
|
|
poll();
|
|
HAL::wait();
|
|
}
|
|
}
|
|
|
|
void run() {
|
|
RoverIf::init();
|
|
RoverIf::run();
|
|
}
|