#include "RadioLink.h" #include "Config.h" #include #include 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_) { // 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(); 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; } // 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)); } } 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); }