#include "modules/my_altimeter/Altimeter.h"
|
#include "modules/my_task_creator/TaskCreator.h"
|
|
#define INVALID_FRAME 0x61A8
|
|
uint8_t Altimiter::address = 0x66; // Initializing the default I2C address
|
double Altimiter::distance = 0.0; // Initializing the distance variable
|
|
void Altimiter::AltimiterRxTask(void *pvParameters)
|
{
|
log_v("Task Started"); // Logging task start
|
Wire.begin(SDA_PIN, SCL_PIN); // Initializing Wire library
|
Wire.setBufferSize(32); // Setting buffer size for Wire communication
|
TaskCreator *mCreator = (TaskCreator *)pvParameters; // Getting TaskCreator object
|
Altimiter *obj = &(mCreator->mAltimiter); // Getting Altimiter object from TaskCreator
|
MavlinkTransmitter* transmitter = &(mCreator->mTransmitter);
|
|
if(!obj->scan(obj->address))
|
{
|
vTaskDelete(NULL);
|
}
|
|
vTaskDelay(50); // Delay to allow initialization
|
uint32_t mCurrentTick=xTaskGetTickCount();
|
|
for (;;)
|
{
|
bool isValid = false;
|
|
Wire.requestFrom(static_cast<uint16_t>(obj->address), static_cast<size_t>(2), true);
|
|
while (Wire.available() >= 2) {
|
byte byteH = Wire.read();
|
byte byteL = Wire.read();
|
|
uint16_t raw = (byteH << 8) | byteL;
|
|
if (raw == INVALID_FRAME) {
|
isValid = false;
|
continue;
|
}
|
|
obj->distance = raw / 100.0f;
|
isValid = true;
|
}
|
|
uint32_t t = xTaskGetTickCount();
|
if (isValid && (t - mCurrentTick >= 100)) {
|
transmitter->SendDistanceSensor(obj->distance, isValid, (float)RANGEFINDER_SF30_MAX_DISTANCE);
|
mCurrentTick = t;
|
}
|
|
vTaskDelay(10);
|
}
|
}
|
|
bool Altimiter::scan(byte adress)
|
{
|
Wire.beginTransmission(adress);
|
byte error = Wire.endTransmission();
|
|
if (error == 0)
|
{
|
log_i("Altimiter found");
|
return true;
|
}
|
else
|
{
|
log_i("Altimiter not found");
|
return false;
|
}
|
}
|
