396 lines
13 KiB
C
396 lines
13 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"
|
|
#include <hw/buscfg.h>
|
|
#include "sensors.h"
|
|
#include "sdcard.h"
|
|
|
|
#define TAG "LoRa"
|
|
|
|
#define ACK_TIMEOUT_MS 250 // Wait 300ms for ACK
|
|
#define MAX_RETRIES 1
|
|
|
|
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 = 8;
|
|
uint8_t bandwidth = SX126X_LORA_BW_250_0;
|
|
uint8_t codingRate = SX126X_LORA_CR_4_8;
|
|
uint16_t preambleLength = 4;
|
|
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, uint32_t crc)
|
|
{
|
|
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;
|
|
packet->CRCCheck = crc;
|
|
strcpy(packet->syncPhrase, "PlechDole");
|
|
}
|
|
|
|
static void send_packet_without_retries(uint8_t *data, uint16_t size)
|
|
{
|
|
if (xSemaphoreTake(loraRadioMutex, portMAX_DELAY) == pdTRUE)
|
|
{
|
|
writeFile(sensFile, data, size);
|
|
|
|
if (!LoRaSend(data, size, SX126x_TXMODE_SYNC))
|
|
{
|
|
ESP_LOGW(TAG, "LoRaSend failed");
|
|
xSemaphoreGive(loraRadioMutex);
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGI(TAG, "%d byte packet sent", size);
|
|
xSemaphoreGive(loraRadioMutex);
|
|
}
|
|
}
|
|
}
|
|
|
|
static void send_packet_with_retries(uint8_t *data, uint16_t size)
|
|
{
|
|
if (xSemaphoreTake(loraRadioMutex, portMAX_DELAY) == pdTRUE)
|
|
{
|
|
writeFile(sensFile, data, size);
|
|
for (int retry = 0; retry <= MAX_RETRIES; retry++)
|
|
{
|
|
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;
|
|
uint32_t crc = esp_rom_crc32_le(0, (uint8_t *)&telemetryPacket, sizeof(telemetryPacket));
|
|
prepare_downbound_packet(&downboundPacket, DownlinkPacketType_Telemetry, missionTimer, crc);
|
|
|
|
uint16_t offset = 0;
|
|
memcpy(bufOut + offset, &downboundPacket, sizeof(downboundPacket));
|
|
offset += sizeof(downboundPacket);
|
|
memcpy(bufOut + offset, &telemetryPacket, sizeof(telemetryPacket));
|
|
offset += sizeof(telemetryPacket);
|
|
|
|
if (csvFile != NULL) {
|
|
fprintf(csvFile,
|
|
"%llu,%lu,%u,%u," // missionTimer, packetIndex, telemetryIndex, packetType
|
|
"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d," // MPU
|
|
"%u,%u,%d,%u," // CCS
|
|
"%u,%u,%u,%lu,%u,%lu,%u," // INA, BME basic
|
|
"%s,%s,%u,%u," // bools and gas index/range
|
|
"%.2f,%.2f,%.2f,%.2f," // Bosch env data
|
|
"%u,%.2f,%.2f,%.2f," // IAQ + scores
|
|
"%lu,%ld,%ld,%d,%u,%u,%u,%u," // GPS data
|
|
"%ld,%ld,%d," // Predicted GPS
|
|
"%ld,%ld,%ld,%ld," // ADC
|
|
"%d,%d,%d,%d," // Servo
|
|
"%u\n", // presentDevices
|
|
|
|
missionTimer,
|
|
downboundPacket.packetIndex,
|
|
telemetryPacket.telemetryIndex,
|
|
downboundPacket.packetType,
|
|
|
|
telemetryPacket.accelerationX, telemetryPacket.accelerationY, telemetryPacket.accelerationZ,
|
|
telemetryPacket.gyroX, telemetryPacket.gyroY, telemetryPacket.gyroZ,
|
|
telemetryPacket.magnetX, telemetryPacket.magnetY, telemetryPacket.magnetZ,
|
|
telemetryPacket.accelerometer_temperature,
|
|
|
|
telemetryPacket.eCO2, telemetryPacket.tvoc, telemetryPacket.currentCCS, telemetryPacket.rawCCSData,
|
|
|
|
telemetryPacket.volts, telemetryPacket.current, telemetryPacket.power,
|
|
telemetryPacket.temperature, telemetryPacket.humidity, telemetryPacket.pressure, telemetryPacket.gas,
|
|
|
|
telemetryPacket.gas_valid ? "Yes" : "No",
|
|
telemetryPacket.heater_stable ? "Yes" : "No",
|
|
telemetryPacket.gas_range, telemetryPacket.gas_index,
|
|
|
|
telemetryPacket.air_temperature, telemetryPacket.relative_humidity,
|
|
telemetryPacket.barometric_pressure, telemetryPacket.gas_resistance,
|
|
|
|
telemetryPacket.iaq_score,
|
|
telemetryPacket.temperature_score, telemetryPacket.humidity_score, telemetryPacket.gas_score,
|
|
|
|
telemetryPacket.time_seconds,
|
|
telemetryPacket.latitude_centi_degrees,
|
|
telemetryPacket.longitude_centi_degrees,
|
|
telemetryPacket.altitude_centi_meters,
|
|
telemetryPacket.fix_quality,
|
|
telemetryPacket.num_satellites,
|
|
telemetryPacket.date_yyddmm,
|
|
telemetryPacket.speed_centi_knots,
|
|
|
|
telemetryPacket.predicted_latitude_centi_degrees,
|
|
telemetryPacket.predicted_longitude_centi_degrees,
|
|
telemetryPacket.predicted_altitude_centi_meters,
|
|
|
|
telemetryPacket.NH3, telemetryPacket.CO, telemetryPacket.NO2, telemetryPacket.UVC,
|
|
|
|
telemetryPacket.currentServoA, telemetryPacket.targetServoA,
|
|
telemetryPacket.currentServoB, telemetryPacket.targetServoB,
|
|
|
|
telemetryPacket.presentDevices);
|
|
fflush(csvFile);
|
|
fsync(fileno(csvFile)); // Critical: this ensures actual write to disk
|
|
}
|
|
|
|
printf("Sending %d byte packet hehe\n", offset);
|
|
send_packet_without_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};
|
|
|
|
ACKPacket ackPacket = {
|
|
.packetIndex = ackIndex,
|
|
.crc32Checksum = crc32Checksum,
|
|
};
|
|
|
|
uint32_t crc = esp_rom_crc32_le(0, (uint8_t *)&ackPacket, sizeof(ackPacket));
|
|
|
|
DownBoundPacket downboundPacket;
|
|
prepare_downbound_packet(&downboundPacket, DownlinkPacketType_ACK, missionTimer, crc);
|
|
|
|
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 %ld", uplinkPacket.packetType, uplinkPacket.packetIndex);
|
|
|
|
uint8_t *payload = data + sizeof(UplinkPacket);
|
|
|
|
uint8_t payloadRXLen = len - sizeof(UplinkPacket);
|
|
|
|
uint32_t crc = esp_rom_crc32_le(0, payload, payloadRXLen);
|
|
|
|
if (crc != uplinkPacket.CRCCheck)
|
|
{
|
|
ESP_LOGE(TAG, "Received BAD CRC for packet %ld, crc is %ld, should be %ld", uplinkPacket.packetIndex, crc, uplinkPacket.CRCCheck);
|
|
return;
|
|
}
|
|
|
|
if (uplinkPacket.packetType == UplinkPacketType_ACK)
|
|
{
|
|
ESP_LOGI(TAG, "Received ACK for packet %ld", 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));
|
|
setPowerMode(sysCtrl.powerMode);
|
|
// 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 %ld packets", uplinkPacket.packetIndex - (packetIndexRX + 1));
|
|
packetIndexRX = uplinkPacket.packetIndex;
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGW(TAG, "Duplicate packet: %ld", (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)
|
|
{
|
|
|
|
// while (foundDevices[0] != 2) {
|
|
// vTaskDelay(10);
|
|
// }
|
|
|
|
// Initialize the semaphore for radio access (binary semaphore, 1 = available)
|
|
loraRadioMutex = xSemaphoreCreateMutex();
|
|
xSemaphoreGive(loraRadioMutex); // Set semaphore as available
|
|
|
|
ESP_LOGI(TAG, "lora_comms_task started");
|
|
setup_lora();
|
|
ESP_LOGI(TAG, "lora_comms_task continuing");
|
|
xTaskCreate(
|
|
lora_receive_task,
|
|
"LoraReceiveTask",
|
|
8192,
|
|
NULL,
|
|
(tskIDLE_PRIORITY + 2),
|
|
NULL);
|
|
ESP_LOGI(TAG, "loraInit");
|
|
while (1)
|
|
{
|
|
int64_t start_time = esp_timer_get_time();
|
|
|
|
if (packetReadiness)
|
|
{
|
|
ESP_LOGI(TAG, "Preparing telemetry");
|
|
|
|
prepare_and_send_telemetry(start_time);
|
|
}
|
|
|
|
const int64_t interval_us = ((powerMode == HIGH_POWER_MODE) ? 10000 : 10000000); // 10 ms or 10 000 ms
|
|
|
|
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));
|
|
}
|
|
}
|
|
} |