#include "NV7Rangefinder.h"
|
#include "modules/my_task_creator/TaskCreator.h"
|
|
#ifdef NV7RANGEFINDER_DISABLE_LOGS
|
#define log_i(...)
|
#define log_w(...)
|
#define log_e(...)
|
#endif
|
|
NV7Rangefinder::NV7Rangefinder() {}
|
|
void NV7Rangefinder::singleMeasure(Stream& port) {
|
const uint8_t cmd[] = {0xAE, 0xA7, 0x04, 0x00, 0x05, 0x09, 0xBC, 0xBE};
|
port.write(cmd, sizeof(cmd));
|
log_i("NV7Rangefinder: Single measure command sent");
|
}
|
|
void NV7Rangefinder::startContinuousMeasure(Stream& port) {
|
const uint8_t cmd[] = {0xAE, 0xA7, 0x04, 0x00, 0x0E, 0x12, 0xBC, 0xBE};
|
port.write(cmd, sizeof(cmd));
|
log_i("NV7Rangefinder: Continuous measure command sent");
|
}
|
|
void NV7Rangefinder::stopContinuousMeasure(Stream& port) {
|
const uint8_t cmd[] = {0xAE, 0xA7, 0x04, 0x00, 0x0F, 0x13, 0xBC, 0xBE};
|
port.write(cmd, sizeof(cmd));
|
log_i("NV7Rangefinder: Stop continuous measure command sent");
|
}
|
|
NV7Rangefinder::Measurement NV7Rangefinder::getLastMeasurement() {
|
return lastMeasurement;
|
}
|
|
void NV7Rangefinder::TaskNV7Rangefinder(void* pvParameters) {
|
TaskCreator* mCreator = (TaskCreator*)pvParameters;
|
NV7Rangefinder* self = &(mCreator->mNV7);
|
MavlinkTransmitter* transmitter = &(mCreator->mTransmitter);
|
|
constexpr size_t PACKET_LEN = 27;
|
uint8_t buffer[PACKET_LEN];
|
size_t idx = 0;
|
|
Stream* serial = nullptr;
|
|
#ifdef NV7RANGEFINDER_USE_SERIAL0
|
Serial.begin(9600, SERIAL_8N1, 16, 17);
|
serial = &Serial;
|
log_i("NV7Rangefinder: Using Serial0 @ %lu", 9600);
|
#else
|
SoftwareSerial softSerial;
|
softSerial.begin(9600, SWSERIAL_8N1, 16, 17);
|
serial = &softSerial;
|
log_i("NV7Rangefinder: Using SoftwareSerial @ %lu", 9600);
|
#endif
|
|
log_i("NV7Rangefinder: Read task started");
|
self->startContinuousMeasure(*serial);
|
vTaskDelay(10 / portTICK_PERIOD_MS);
|
|
while (true) {
|
while (serial->available()) {
|
uint8_t b = serial->read();
|
|
if (idx == 0 && b != 0xAE) continue;
|
if (idx == 1 && b != 0xA7) { idx = 0; continue; }
|
|
if (idx < PACKET_LEN) {
|
buffer[idx++] = b;
|
} else {
|
log_w("Buffer overflow, reset idx");
|
transmitter->SendDistanceSensor(0.0f, false, (float)RANGEFINDER_NV7_MAX_DISTANCE);
|
idx = 0;
|
continue;
|
}
|
|
if (idx == PACKET_LEN) {
|
if (buffer[25] != 0xBC || buffer[26] != 0xBE) {
|
// log_w("Invalid footer bytes");
|
transmitter->SendDistanceSensor(0.0f, false,(float)RANGEFINDER_NV7_MAX_DISTANCE);
|
idx = 0;
|
continue;
|
}
|
|
uint8_t checksum = 0;
|
for (int i = 2; i <= 23; i++) checksum += buffer[i];
|
|
if (checksum != buffer[24]) {
|
log_w("Checksum failed: got %02X, expected %02X", buffer[24], checksum);
|
transmitter->SendDistanceSensor(0.0f, false,(float)RANGEFINDER_NV7_MAX_DISTANCE);
|
idx = 0;
|
continue;
|
}
|
|
const uint8_t* d = &buffer[5];
|
self->lastMeasurement = {
|
.elevation = (int16_t)(d[0] << 8 | d[1]),
|
.distance = (d[2] << 8 | d[3]) / 10.0f,
|
.sineHeight = (int16_t)(d[4] << 8 | d[5]),
|
.horizDistance = (int16_t)(d[6] << 8 | d[7]),
|
.twoPointHeight = (int16_t)(d[8] << 8 | d[9]),
|
.azimuth = (int16_t)(d[10] << 8 | d[11]),
|
.horizAngle = (int16_t)(d[12] << 8 | d[13]),
|
.span = (int16_t)(d[14] << 8 | d[15]),
|
.speed = (int16_t)(d[16] << 8 | d[17]),
|
.unit = d[18],
|
.secondElevation = (int16_t)(d[19] << 8 | d[20]),
|
.isValid = true
|
};
|
|
log_i("Parsed: dist=%.1f (unit=%d)", self->lastMeasurement.distance, self->lastMeasurement.unit);
|
transmitter->SendDistanceSensor(self->lastMeasurement.distance, true, (float)RANGEFINDER_NV7_MAX_DISTANCE);
|
idx = 0;
|
}
|
}
|
|
vTaskDelay(10 / portTICK_PERIOD_MS);
|
}
|
}
|
