2026-09-28 18:14:00 +02:00
|
|
|
#include "RadioLink.h"
|
|
|
|
|
#include "Config.h"
|
|
|
|
|
#include <RF24.h>
|
|
|
|
|
#include <SPI.h>
|
|
|
|
|
|
|
|
|
|
static_assert(HXP_RADIO_DATA_RATE == RF24_250KBPS && HXP_RADIO_PA_LEVEL == RF24_PA_LOW,
|
|
|
|
|
"HexapodProtocol.h radio enum values out of sync with RF24.h");
|
|
|
|
|
|
|
|
|
|
static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);
|
|
|
|
|
|
|
|
|
|
bool RadioLink::begin() {
|
|
|
|
|
present_ = radio.begin();
|
|
|
|
|
if (present_) {
|
2026-11-17 14:42:37 +01:00
|
|
|
// The robot answers with Telemetry in its ack payloads, which needs
|
|
|
|
|
// dynamic payload lengths on both radios. Each end checks the length of
|
|
|
|
|
// what it receives against its own struct instead (see HexapodProtocol.h).
|
|
|
|
|
radio.enableAckPayload();
|
2026-09-28 18:14:00 +02:00
|
|
|
radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL);
|
|
|
|
|
radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE);
|
|
|
|
|
radio.setChannel(HXP_RADIO_CHANNEL);
|
|
|
|
|
radio.setRetries(RADIO_RETRY_DELAY, RADIO_RETRY_COUNT);
|
|
|
|
|
radio.openWritingPipe(HXP_RADIO_ADDRESS);
|
|
|
|
|
radio.stopListening();
|
|
|
|
|
}
|
|
|
|
|
return present_;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool RadioLink::send(const CmdPacket& p) {
|
|
|
|
|
if (!present_) return false;
|
|
|
|
|
bool acked = radio.write(&p, sizeof(p));
|
|
|
|
|
history_ = (history_ << 1) | (acked ? 1u : 0u);
|
|
|
|
|
if (acked) {
|
|
|
|
|
last_ack_ms_ = millis();
|
|
|
|
|
ever_acked_ = true;
|
|
|
|
|
}
|
2026-11-17 14:42:37 +01:00
|
|
|
// An ack payload lands in the RX FIFO. Drain it every time, so it can't fill up.
|
|
|
|
|
while (radio.available()) {
|
|
|
|
|
uint8_t len = radio.getDynamicPayloadSize();
|
|
|
|
|
if (len == sizeof(Telemetry)) {
|
|
|
|
|
radio.read(&telemetry_, sizeof(telemetry_));
|
|
|
|
|
has_telemetry_ = true;
|
|
|
|
|
telemetry_ms_ = millis();
|
|
|
|
|
} else if (len > 0) { // 0 = corrupt length; RF24 has flushed the FIFO already
|
|
|
|
|
uint8_t junk[32]; // another protocol version: ignore it
|
|
|
|
|
radio.read(junk, len <= sizeof(junk) ? len : sizeof(junk));
|
|
|
|
|
}
|
|
|
|
|
}
|
2026-09-28 18:14:00 +02:00
|
|
|
return acked;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void RadioLink::update(uint32_t now_ms) {
|
|
|
|
|
link_ok_ = present_ && ever_acked_ && (now_ms - last_ack_ms_) < LINK_LOST_MS;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint8_t RadioLink::qualityPct() const {
|
|
|
|
|
return (uint8_t)(__builtin_popcount(history_) * 100 / 32);
|
|
|
|
|
}
|