335 lines
9.4 KiB
C
335 lines
9.4 KiB
C
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "../hw/sx1262.h"
|
|
#include "radio.h"
|
|
#include "esp_log.h"
|
|
#include "string.h"
|
|
#include "esp_timer.h"
|
|
#include "esp_rom_crc.h"
|
|
|
|
#define TAG "LoRa"
|
|
|
|
#define ACK_TIMEOUT_MS 500 // Wait 300ms for ACK
|
|
#define MAX_RETRIES 3
|
|
|
|
uint32_t packetIndexTX = 0;
|
|
uint32_t packetIndexRX = 0;
|
|
|
|
TelemetryPacket telemetryPacket;
|
|
uint8_t packetReadiness = 0;
|
|
|
|
volatile bool ackReceived = false;
|
|
uint32_t lastAckIndex = 0;
|
|
|
|
SemaphoreHandle_t loraRadioMutex;
|
|
|
|
void setup_lora(void)
|
|
{
|
|
LoRaInit();
|
|
|
|
int8_t txPowerInDbm = 20;
|
|
uint32_t frequencyInHz = 869525000;
|
|
float tcxoVoltage = 2.2;
|
|
bool useRegulatorLDO = true;
|
|
|
|
ESP_LOGW(TAG, "Enable TCXO");
|
|
LoRaDebugPrint(false);
|
|
|
|
if (LoRaBegin(frequencyInHz, txPowerInDbm, tcxoVoltage, useRegulatorLDO) != 0)
|
|
{
|
|
ESP_LOGE(TAG, "LoRa module not recognized. Halting.");
|
|
while (1)
|
|
{
|
|
vTaskDelay(1);
|
|
}
|
|
}
|
|
|
|
uint8_t spreadingFactor = 6;
|
|
uint8_t bandwidth = SX126X_LORA_BW_250_0;
|
|
uint8_t codingRate = SX126X_LORA_CR_4_8;
|
|
uint16_t preambleLength = 8;
|
|
bool crcOn = true;
|
|
bool invertIrq = false;
|
|
|
|
LoRaConfig(spreadingFactor, bandwidth, codingRate, preambleLength, 0, crcOn, invertIrq);
|
|
}
|
|
|
|
static void prepare_downbound_packet(DownBoundPacket *packet, uint8_t type, uint64_t missionTimer)
|
|
{
|
|
memset(packet, 0, sizeof(DownBoundPacket));
|
|
packet->missionTimer = missionTimer;
|
|
packet->packetIndex = packetIndexTX++;
|
|
ESP_LOGI(TAG_RADIO, "Sending downbound packet with index %ld", packetIndexTX - 1);
|
|
packet->packetType = type;
|
|
strcpy(packet->syncPhrase, "PlechDole");
|
|
}
|
|
|
|
static void send_packet_with_retries(uint8_t *data, uint16_t size)
|
|
{
|
|
for (int retry = 0; retry <= MAX_RETRIES; retry++)
|
|
{
|
|
if (xSemaphoreTake(loraRadioMutex, portMAX_DELAY) == pdTRUE)
|
|
{
|
|
if (!LoRaSend(data, size, SX126x_TXMODE_SYNC))
|
|
{
|
|
ESP_LOGW(TAG, "LoRaSend failed, retry %d", retry);
|
|
xSemaphoreGive(loraRadioMutex);
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGI(TAG, "%d byte packet sent (attempt %d)", size, retry + 1);
|
|
xSemaphoreGive(loraRadioMutex);
|
|
|
|
// Wait for ACK
|
|
ackReceived = false;
|
|
uint64_t start_wait = esp_timer_get_time();
|
|
while ((esp_timer_get_time() - start_wait) < (ACK_TIMEOUT_MS * 1000))
|
|
{
|
|
if (ackReceived)
|
|
{
|
|
ESP_LOGI(TAG, "ACK received for packet.");
|
|
return;
|
|
}
|
|
vTaskDelay(pdMS_TO_TICKS(10));
|
|
}
|
|
|
|
ESP_LOGW(TAG, "ACK timeout, retrying...");
|
|
}
|
|
}
|
|
}
|
|
|
|
ESP_LOGE(TAG, "Failed to send packet after %d retries", MAX_RETRIES);
|
|
}
|
|
|
|
void prepare_and_send_telemetry(uint64_t missionTimer)
|
|
{
|
|
uint8_t bufOut[256] = {0};
|
|
|
|
DownBoundPacket downboundPacket;
|
|
prepare_downbound_packet(&downboundPacket, DownlinkPacketType_Telemetry, missionTimer);
|
|
|
|
uint16_t offset = 0;
|
|
memcpy(bufOut + offset, &downboundPacket, sizeof(downboundPacket));
|
|
offset += sizeof(downboundPacket);
|
|
memcpy(bufOut + offset, &telemetryPacket, sizeof(telemetryPacket));
|
|
offset += sizeof(telemetryPacket);
|
|
|
|
send_packet_with_retries(bufOut, offset);
|
|
packetReadiness = 0;
|
|
}
|
|
|
|
static void build_and_send_ack(uint32_t ackIndex, uint32_t crc32Checksum, uint64_t missionTimer)
|
|
{
|
|
uint8_t bufOut[256] = {0};
|
|
|
|
DownBoundPacket downboundPacket;
|
|
prepare_downbound_packet(&downboundPacket, DownlinkPacketType_ACK, missionTimer);
|
|
|
|
ACKPacket ackPacket = {
|
|
.packetIndex = ackIndex,
|
|
.crc32Checksum = crc32Checksum,
|
|
};
|
|
|
|
uint16_t offset = 0;
|
|
memcpy(bufOut + offset, &downboundPacket, sizeof(downboundPacket));
|
|
offset += sizeof(downboundPacket);
|
|
memcpy(bufOut + offset, &ackPacket, sizeof(ackPacket));
|
|
offset += sizeof(ackPacket);
|
|
|
|
if (xSemaphoreTake(loraRadioMutex, portMAX_DELAY) == pdTRUE)
|
|
{
|
|
if (!LoRaSend(bufOut, offset, SX126x_TXMODE_SYNC))
|
|
{
|
|
ESP_LOGE(TAG, "Failed to send ACK");
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGI(TAG, "%d byte ACK sent", offset);
|
|
}
|
|
xSemaphoreGive(loraRadioMutex);
|
|
}
|
|
}
|
|
|
|
void process_uplink_packet(uint8_t *data, uint8_t len, uint64_t missionTimer)
|
|
{
|
|
if (len < sizeof(UplinkPacket))
|
|
{
|
|
ESP_LOGW(TAG, "Uplink packet too small: %d bytes", len);
|
|
return;
|
|
}
|
|
|
|
UplinkPacket uplinkPacket;
|
|
memcpy(&uplinkPacket, data, sizeof(UplinkPacket));
|
|
|
|
if (strcmp(UplinkSync, uplinkPacket.syncPhrase) != 0)
|
|
{
|
|
ESP_LOGW(TAG, "Invalid sync phrase");
|
|
return;
|
|
}
|
|
|
|
ESP_LOGI(TAG, "Got uplink packet of type %d, index %d", uplinkPacket.packetType, uplinkPacket.packetIndex);
|
|
|
|
uint8_t payloadRXLen = len - sizeof(UplinkPacket);
|
|
|
|
if (uplinkPacket.packetType == UplinkPacketType_ACK)
|
|
{
|
|
ESP_LOGI(TAG, "Received ACK for packet %d", uplinkPacket.packetIndex);
|
|
ackReceived = true;
|
|
lastAckIndex = uplinkPacket.packetIndex;
|
|
return;
|
|
}
|
|
|
|
if (uplinkPacket.packetIndex == packetIndexRX + 1)
|
|
{
|
|
ESP_LOGI(TAG, "Packet arrived in correct order");
|
|
packetIndexRX = uplinkPacket.packetIndex;
|
|
|
|
uint32_t crc = esp_rom_crc32_le(0, data + sizeof(UplinkPacket), payloadRXLen);
|
|
build_and_send_ack(packetIndexRX, crc, missionTimer);
|
|
|
|
switch (uplinkPacket.packetType)
|
|
{
|
|
case UplinkPacketType_SystemControl:
|
|
if (payloadRXLen == sizeof(SystemControlPacket))
|
|
{
|
|
SystemControlPacket sysCtrl;
|
|
memcpy(&sysCtrl, data + sizeof(UplinkPacket), sizeof(SystemControlPacket));
|
|
// TODO: Process sysCtrl
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGW(TAG, "SystemControlPacket size mismatch");
|
|
}
|
|
break;
|
|
|
|
case UplinkPacketType_Ping:
|
|
// TODO: handle Ping
|
|
break;
|
|
|
|
default:
|
|
ESP_LOGW(TAG, "Unknown uplink packet type %d", uplinkPacket.packetType);
|
|
break;
|
|
}
|
|
}
|
|
else if (uplinkPacket.packetIndex > packetIndexRX + 1)
|
|
{
|
|
ESP_LOGW(TAG, "Skipped %d packets", uplinkPacket.packetIndex - (packetIndexRX + 1));
|
|
packetIndexRX = uplinkPacket.packetIndex;
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGW(TAG, "Duplicate packet: %d", (packetIndexRX + 1) - uplinkPacket.packetIndex);
|
|
}
|
|
}
|
|
|
|
void lora_receive_task(void *pvParameters)
|
|
{
|
|
ESP_LOGI(TAG, "lora_receive_task started");
|
|
uint8_t bufIn[256];
|
|
while (1)
|
|
{
|
|
// Wait to take the semaphore before accessing LoRa
|
|
if (xSemaphoreTake(loraRadioMutex, 0) == pdTRUE)
|
|
{
|
|
uint8_t rxLen = LoRaReceive(bufIn, sizeof(bufIn));
|
|
if (rxLen > 0)
|
|
{
|
|
ESP_LOGI(TAG, "%d byte packet received", rxLen);
|
|
process_uplink_packet(bufIn, rxLen, esp_timer_get_time());
|
|
int8_t rssi, snr;
|
|
GetPacketStatus(&rssi, &snr);
|
|
ESP_LOGI(TAG, "rssi=%d[dBm], snr=%d[dB]", rssi, snr);
|
|
}
|
|
|
|
// Release the semaphore when done with LoRa RX
|
|
xSemaphoreGive(loraRadioMutex);
|
|
}
|
|
vTaskDelay(pdMS_TO_TICKS(10)); // Delay to prevent busy-waiting
|
|
}
|
|
}
|
|
|
|
void lora_comms_task(void *pvParameters)
|
|
{
|
|
|
|
ESP_LOGI(TAG, "lora_comms_task started");
|
|
// Initialize the semaphore for radio access (binary semaphore, 1 = available)
|
|
loraRadioMutex = xSemaphoreCreateMutex();
|
|
xSemaphoreGive(loraRadioMutex); // Set semaphore as available
|
|
|
|
const int64_t interval_us = 400000; // 400 ms
|
|
|
|
setup_lora();
|
|
xTaskCreate(
|
|
lora_receive_task,
|
|
"LoraReceiveTask",
|
|
8192,
|
|
NULL,
|
|
(tskIDLE_PRIORITY + 2),
|
|
NULL);
|
|
while (1)
|
|
{
|
|
int64_t start_time = esp_timer_get_time();
|
|
|
|
if (packetReadiness)
|
|
{
|
|
ESP_LOGI(TAG, "Preparing telemetry");
|
|
|
|
prepare_and_send_telemetry(start_time);
|
|
}
|
|
|
|
int64_t end_time = esp_timer_get_time();
|
|
int64_t elapsed = end_time - start_time;
|
|
|
|
if (elapsed < interval_us)
|
|
{
|
|
vTaskDelay(pdMS_TO_TICKS((interval_us - elapsed) / 1000));
|
|
}
|
|
}
|
|
}
|
|
|
|
// void lora_comms_task(void *pvParameters)
|
|
// {
|
|
// const int64_t interval_us = 400000; // 100 ms
|
|
// int64_t start_time, end_time, elapsed;
|
|
|
|
// setup_lora();
|
|
|
|
// uint8_t bufIn[256];
|
|
|
|
// while (1)
|
|
// {
|
|
// start_time = esp_timer_get_time();
|
|
|
|
// if (packetReadiness)
|
|
// {
|
|
// ESP_LOGI(TAG, "Preparing telemetry");
|
|
// prepare_and_send_telemetry(start_time);
|
|
// }
|
|
|
|
// uint8_t rxLen = LoRaReceive(bufIn, sizeof(bufIn));
|
|
// if (rxLen > 0)
|
|
// {
|
|
// ESP_LOGI(TAG, "%d byte packet received", rxLen);
|
|
// process_uplink_packet(bufIn, rxLen, start_time);
|
|
|
|
// int8_t rssi, snr;
|
|
// GetPacketStatus(&rssi, &snr);
|
|
// ESP_LOGI(TAG, "rssi=%d[dBm], snr=%d[dB]", rssi, snr);
|
|
// }
|
|
|
|
// int lost = GetPacketLost();
|
|
// if (lost != 0)
|
|
// {
|
|
// ESP_LOGW(TAG, "%d packets lost", lost);
|
|
// }
|
|
|
|
// end_time = esp_timer_get_time();
|
|
// elapsed = end_time - start_time;
|
|
|
|
// if (elapsed < interval_us)
|
|
// {
|
|
// vTaskDelay(pdMS_TO_TICKS((interval_us - elapsed) / 1000));
|
|
// }
|
|
// }
|
|
// }
|