115 lines
3.4 KiB
C++
115 lines
3.4 KiB
C++
#include "Tactility/hal/gps/Satellites.h"
|
|
|
|
#include <algorithm>
|
|
|
|
#define TAG "satellites"
|
|
|
|
namespace tt::hal::gps {
|
|
|
|
constexpr inline bool hasTimeElapsed(TickType_t now, TickType_t timeInThePast, TickType_t expireTimeInTicks) {
|
|
return (TickType_t)(now - timeInThePast) >= expireTimeInTicks;
|
|
}
|
|
|
|
SatelliteStorage::SatelliteRecord* SatelliteStorage::findRecord(int number) {
|
|
auto result = records | std::views::filter([number](auto& record) {
|
|
return record.inUse && record.data.nr == number;
|
|
});
|
|
|
|
if (!result.empty()) {
|
|
return &result.front();
|
|
} else {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
SatelliteStorage::SatelliteRecord* SatelliteStorage::findUnusedRecord() {
|
|
auto lock = mutex.asScopedLock();
|
|
lock.lock();
|
|
|
|
auto result = records | std::views::filter([](auto& record) {
|
|
return !record.inUse;
|
|
});
|
|
|
|
if (!result.empty()) {
|
|
auto* record = &result.front();
|
|
record->inUse = true;
|
|
TT_LOG_D(TAG, "Found unused record");
|
|
return record;
|
|
} else {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
SatelliteStorage::SatelliteRecord* SatelliteStorage::findRecordToRecycle() {
|
|
auto lock = mutex.asScopedLock();
|
|
lock.lock();
|
|
|
|
int candidate_index = -1;
|
|
auto candidate_age = portMAX_DELAY;
|
|
TickType_t expire_duration = kernel::secondsToTicks(recycleTimeSeconds);
|
|
TickType_t now = kernel::getTicks();
|
|
for (int i = 0; i < records.size(); ++i) {
|
|
// First try to find a record that is "old enough"
|
|
if (hasTimeElapsed(now, records[i].lastUpdated, expire_duration)) {
|
|
TT_LOG_D(TAG, "! [%d] %lu < %lu", i, records[i].lastUpdated, expire_duration);
|
|
candidate_index = i;
|
|
break;
|
|
}
|
|
|
|
// Otherwise keep finding the oldest record
|
|
if (records[i].inUse && records[i].lastUpdated < candidate_age) {
|
|
candidate_index = i;
|
|
candidate_age = records[i].lastUpdated;
|
|
TT_LOG_D(TAG, "? [%d] %lu < %lu", i, records[i].lastUpdated, candidate_age);
|
|
}
|
|
}
|
|
|
|
assert(candidate_index != -1);
|
|
|
|
TT_LOG_D(TAG, "Recycled record %d", candidate_index);
|
|
|
|
return &records[candidate_index];
|
|
}
|
|
|
|
SatelliteStorage::SatelliteRecord* SatelliteStorage::findWithFallback(int number) {
|
|
auto lock = mutex.asScopedLock();
|
|
lock.lock();
|
|
|
|
if (auto* found_record = findRecord(number)) {
|
|
return found_record;
|
|
} else if (auto* unused_record = findUnusedRecord()) {
|
|
return unused_record;
|
|
} else {
|
|
return findRecordToRecycle();
|
|
}
|
|
}
|
|
|
|
void SatelliteStorage::notify(const minmea_sat_info& data) {
|
|
auto lock = mutex.asScopedLock();
|
|
lock.lock();
|
|
|
|
auto* record = findWithFallback(data.nr);
|
|
if (record != nullptr) {
|
|
record->inUse = true;
|
|
record->lastUpdated = kernel::getTicks();
|
|
record->data = data;
|
|
TT_LOG_D(TAG, "Updated satellite %d: elevation %d, azimuth %d, snr %d", record->data.nr, record->data.elevation, record->data.elevation, record->data.snr);
|
|
}
|
|
}
|
|
|
|
void SatelliteStorage::getRecords(const std::function<void(const minmea_sat_info&)>& onRecord) const {
|
|
auto lock = mutex.asScopedLock();
|
|
lock.lock();
|
|
|
|
TickType_t expire_duration = kernel::secondsToTicks(recentTimeSeconds);
|
|
TickType_t now = kernel::getTicks();
|
|
|
|
for (auto& record: records) {
|
|
if (record.inUse && !hasTimeElapsed(now, record.lastUpdated, expire_duration)) {
|
|
onRecord(record.data);
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace tt::hal::gps
|