RadioModule.cpp

#include "RadioModule.h"

// NRF24 Pins
#define NRF_CE   4
#define NRF_CSN  5

#define FAILSAFE_TIMEOUT_MS 5000

RF24 radio(NRF_CE, NRF_CSN);
const byte rxAddress[6] = "CTRL1";
const byte txAddress[6] = "CTRL2";

// Local state pointers - made static to prevent linker conflicts
static FlightState* localState;
static SemaphoreHandle_t localMutex;

unsigned long lastPacketTime = 0;
unsigned long lastTelemetrySent = 0;

ControlPacket rxPkt;

void setupRadio(FlightState* statePtr, SemaphoreHandle_t mutexHandle) {
  localState = statePtr;
  localMutex = mutexHandle;

  // Initialize standard VSPI for NRF24
  SPI.begin(18, 19, 23, NRF_CSN);

  if (!radio.begin()) {
    Serial.println("NRF24 NOT FOUND! Halting radio setup.");
    return; 
  }

  radio.setChannel(100);
  radio.setPALevel(RF24_PA_MAX);
  radio.setDataRate(RF24_250KBPS);
  radio.setAutoAck(false);
  radio.disableCRC();
  radio.enableDynamicPayloads();

  radio.openReadingPipe(1, rxAddress);
  radio.openWritingPipe(txAddress);
  radio.startListening();

  lastPacketTime = millis();
  Serial.println("NRF24 Initialized on Core 0");
}

// This function loops forever on Core 0
void radioTask(void *pvParameters) {
  while (true) {
    bool packetReceived = false;

    // 1. Check for incoming control packets
    while (radio.available()) {
      uint8_t len = radio.getDynamicPayloadSize();
      if (len == sizeof(ControlPacket)) {
        radio.read(&rxPkt, sizeof(rxPkt));
        lastPacketTime = millis();
        packetReceived = true;
      } else {
        radio.flush_rx();
      }
    }

    // 2. Safely transfer received data to Shared State & Grab Telemetry
    if (xSemaphoreTake(localMutex, pdMS_TO_TICKS(5)) == pdTRUE) {
      
      // Update Failsafe flag based on timeout
      if (millis() - lastPacketTime >= FAILSAFE_TIMEOUT_MS) {
        localState->failsafeActive = true;
      } else {
        localState->failsafeActive = false;
        
        // If we got a packet, update the shared controls
        if (packetReceived) {
          localState->control = rxPkt;
        }
      }

      // Read telemetry to send back (copy locally to minimize mutex lock time)
      TelemetryPacket txPkt = localState->telemetry;
      
      xSemaphoreGive(localMutex); // Unlock Mutex

      // 3. Send Telemetry if needed (e.g., every 500ms or after RX)
      if (packetReceived && (millis() - lastTelemetrySent >= 500)) {
        radio.stopListening();
        radio.write(&txPkt, sizeof(txPkt));
        radio.startListening();
        lastTelemetrySent = millis();
      }
    }

    // Yield to the RTOS scheduler (crucial to prevent watchdog resets on Core 0)
    vTaskDelay(pdMS_TO_TICKS(5)); 
  }
}