#include "modules/my_altimeter/GRF500.h"
|
#include "modules/my_task_creator/TaskCreator.h"
|
#include "modules/my_mavlink_tx/MavlinkTransmitter.h"
|
|
uint8_t GRF500::address = 0x66;
|
double GRF500::distance = 0.0;
|
|
GRF500::GRF500(uint8_t addr)
|
: registerModeActivated(false), lastReadMs(0)
|
{
|
address = addr;
|
}
|
|
bool GRF500::begin()
|
{
|
log_v("GRF500 begin");
|
Wire.begin(SDA_PIN, SCL_PIN); // SDA, SCL
|
|
log_v("Scanning I2C bus for devices...");
|
bool deviceFound = false;
|
for (uint8_t addr = 1; addr < 127; ++addr) {
|
if (scan(addr)) {
|
log_i("I2C device found at 0x%02X", addr);
|
deviceFound = true;
|
}
|
}
|
if (!deviceFound) {
|
log_i("No I2C devices found");
|
return false;
|
}
|
|
// Verify GRF500 at configured address
|
log_i("Checking GRF500 at address 0x%02X", address);
|
if (!scan(address)) {
|
log_i("GRF500 not responding at 0x%02X", address);
|
return false;
|
}
|
log_i("GRF500 responsive at 0x%02X", address);
|
vTaskDelay(pdMS_TO_TICKS(50));
|
|
enableRegisterMode();
|
registerModeActivated = checkRegisterMode();
|
log_i("Register mode %s", registerModeActivated ? "OK" : "FAIL");
|
if (!registerModeActivated) return false;
|
|
readDistance();
|
return true;
|
}
|
|
void GRF500::Task(void *pvParameters)
|
{
|
log_v("GRF500 Task started");
|
TaskCreator *creator = static_cast<TaskCreator*>(pvParameters);
|
GRF500 *sensor = &(creator->mGRF500);
|
MavlinkTransmitter *transmitter = &(creator->mTransmitter);
|
|
if (!sensor->begin()) {
|
log_i("GRF500 initialization failed");
|
vTaskDelete(NULL);
|
return;
|
}
|
|
log_i("GRF500 initialized successfully");
|
uint32_t lastTick = xTaskGetTickCount();
|
for (;;) {
|
sensor->readMeasurements();
|
bool valid = (distance > 0);
|
|
uint32_t now = xTaskGetTickCount();
|
if (valid && (now - lastTick >= 100)) {
|
transmitter->SendDistanceSensor(distance, valid, (float)RANGEFINDER_GRF500_MAX_DISTANCE);
|
lastTick = now;
|
}
|
vTaskDelay(pdMS_TO_TICKS(10));
|
}
|
}
|
|
void GRF500::update()
|
{
|
uint32_t now = millis();
|
if (now - lastReadMs < 100) return;
|
lastReadMs = now;
|
if (!registerModeActivated) return;
|
|
readMeasurements();
|
}
|
|
bool GRF500::scan(uint8_t addr)
|
{
|
Wire.beginTransmission(addr);
|
return (Wire.endTransmission() == 0);
|
}
|
|
void GRF500::enableRegisterMode()
|
{
|
Wire.beginTransmission(address);
|
Wire.write((uint8_t)120);
|
Wire.write((uint8_t)0xAA);
|
Wire.write((uint8_t)0xAA);
|
if (Wire.endTransmission() != 0) {
|
log_i("Register-mode write error");
|
}
|
}
|
|
bool GRF500::checkRegisterMode()
|
{
|
Wire.beginTransmission(address);
|
Wire.write((uint8_t)120);
|
if (Wire.endTransmission() != 0) return false;
|
if (Wire.requestFrom(address, (uint8_t)2) != 2) return false;
|
|
uint8_t a = Wire.read();
|
uint8_t b = Wire.read();
|
return (a == 0xCC && b == 0);
|
}
|
|
void GRF500::readDistance()
|
{
|
Wire.beginTransmission(address);
|
Wire.write((uint8_t)27);
|
if (Wire.endTransmission() != 0) {
|
log_i("Distance write error");
|
return;
|
}
|
if (Wire.requestFrom(address, (uint8_t)4) == 4) {
|
uint32_t raw = 0;
|
raw |= (uint32_t)Wire.read();
|
raw |= (uint32_t)Wire.read() << 8;
|
raw |= (uint32_t)Wire.read() << 16;
|
raw |= (uint32_t)Wire.read() << 24;
|
distance = raw / 100.0;
|
log_i("Distance: %.2f m", distance);
|
}
|
}
|
|
void GRF500::readMeasurements()
|
{
|
Wire.beginTransmission(address);
|
Wire.write((uint8_t)44);
|
if (Wire.endTransmission() != 0) {
|
log_i("Measurements write error");
|
return;
|
}
|
if (Wire.requestFrom(address, (uint8_t)8) == 8) {
|
int16_t first = Wire.read() | (Wire.read() << 8);
|
int16_t strength1 = Wire.read() | (Wire.read() << 8);
|
int16_t last = Wire.read() | (Wire.read() << 8);
|
int16_t strength2 = Wire.read() | (Wire.read() << 8);
|
|
distance = first / 10.0;
|
// log_i("First=%d Last=%d S1=%d S2=%d", first, last, strength1, strength2);
|
}
|
}
|
