#include "modules/my_altimeter/NRA15.h"
|
#include "modules/my_task_creator/TaskCreator.h"
|
|
// Protocol constants
|
#define START_SEQUENCE 0xAA // same for L and H bit
|
#define END_SEQUENCE 0x55 // same for L and H bit
|
|
// Message IDs
|
#define MSG_TARGET_INFO 0x070C
|
|
// Initializing the static variables
|
double NRA15::distance = 0.0;
|
Stream* NRA15::serial = nullptr;
|
|
#ifndef NRA15_USE_HARDWARE_SERIAL
|
SoftwareSerial* NRA15::softSerial = nullptr;
|
#endif
|
|
// Destructor
|
NRA15::~NRA15() {
|
#ifndef NRA15_USE_HARDWARE_SERIAL
|
if (softSerial) {
|
delete softSerial;
|
softSerial = nullptr;
|
}
|
#endif
|
}
|
|
void NRA15::NRA15RxTask(void *pvParameters)
|
{
|
log_i("NRA15 Task Started"); // Logging task start
|
|
// Initialize Serial (Hardware or Software)
|
#ifdef NRA15_USE_HARDWARE_SERIAL
|
// Using HardwareSerial (Serial0)
|
Serial.begin(115200, SERIAL_8N1, NRA15_RX_PIN, NRA15_TX_PIN);
|
serial = &Serial;
|
log_i("NRA15: Using HardwareSerial @ 115200 on GPIO%d/%d", NRA15_RX_PIN, NRA15_TX_PIN);
|
#else
|
// Using SoftwareSerial
|
softSerial = new SoftwareSerial(NRA15_RX_PIN, NRA15_TX_PIN);
|
softSerial->begin(115200);
|
serial = softSerial;
|
log_i("NRA15: Using SoftwareSerial @ 115200 on GPIO%d/%d", NRA15_RX_PIN, NRA15_TX_PIN);
|
#endif
|
|
// Getting TaskCreator object
|
TaskCreator *mCreator = (TaskCreator *)pvParameters;
|
NRA15 *obj = &(mCreator->mNRA15); // Getting NRA15 object from TaskCreator
|
MavlinkTransmitter *transmitter = &(mCreator->mTransmitter);
|
|
vTaskDelay(50); // Delay to allow initialization
|
|
uint32_t lastMavlinkTime = xTaskGetTickCount();
|
uint32_t lastValidDataTime = xTaskGetTickCount();
|
|
// Local variables for packet assembly
|
uint8_t buffer[14]; // Setting buffer size for UART communication
|
uint8_t bufferIndex = 0;
|
bool isValid = false;
|
|
for (;;)
|
{
|
// Read data from Serial
|
while (serial->available()) {
|
uint8_t byte = serial->read();
|
|
if (bufferIndex < 2) {
|
if (byte == START_SEQUENCE) {
|
buffer[bufferIndex++] = byte;
|
} else {
|
bufferIndex = 0; // Reset
|
}
|
}
|
else if (bufferIndex >= 2 && bufferIndex < 14) {
|
buffer[bufferIndex++] = byte;
|
|
// Complete packet received
|
if (bufferIndex == 14) {
|
// Validate end sequence
|
if (buffer[12] == END_SEQUENCE && buffer[13] == END_SEQUENCE) {
|
// Get Message ID
|
uint16_t msgId = buffer[2] | (buffer[3] << 8);
|
|
if (msgId == MSG_TARGET_INFO) {
|
// Parse target information
|
uint8_t rangeH = buffer[6];
|
uint8_t rangeL = buffer[7];
|
|
// Calculate distance in meters
|
obj->distance = (rangeH * 256 + rangeL) * 0.01;
|
isValid = true;
|
lastValidDataTime = xTaskGetTickCount();
|
log_v("NRA15: %.2fm", obj->distance);
|
}
|
} else {
|
log_v("NRA15: Invalid end sequence");
|
}
|
|
bufferIndex = 0; // Reset for next packet
|
}
|
}
|
}
|
|
|
uint32_t currentTime = xTaskGetTickCount();
|
|
//Timeout check
|
if ((currentTime - lastMavlinkTime) > pdMS_TO_TICKS(500)) {
|
if (isValid) {
|
isValid = false;
|
log_w("NRA15: no target (timeout)");
|
}
|
}
|
|
// Send MAVLink distance sensor message every 100ms
|
if ((currentTime - lastMavlinkTime) >= pdMS_TO_TICKS(100)) {
|
transmitter->SendDistanceSensor(
|
obj->distance,
|
isValid,
|
(float)RANGEFINDER_NRA15_MAX_DISTANCE
|
);
|
lastMavlinkTime = currentTime;
|
}
|
|
vTaskDelay(pdMS_TO_TICKS(10)); // 10ms delay
|
}
|
}
|
