mirror of
https://github.com/vicliu624/trail-mate.git
synced 2026-08-07 03:09:43 +00:00
119 lines
2.3 KiB
C++
119 lines
2.3 KiB
C++
#include "gps/motion_policy.h"
|
|
|
|
#include "sys/clock.h"
|
|
|
|
namespace gps
|
|
{
|
|
|
|
namespace
|
|
{
|
|
MotionPolicy* g_instance = nullptr;
|
|
}
|
|
|
|
bool MotionPolicy::begin(IMotionHardware& motion, const MotionConfig& config)
|
|
{
|
|
if (enabled_ && motion_ != nullptr)
|
|
{
|
|
motion_->removeCallback(config_.sensor_id, motionEventCallback);
|
|
motion_->detachInterrupt();
|
|
}
|
|
|
|
motion_ = &motion;
|
|
config_ = config;
|
|
enabled_ = false;
|
|
|
|
if (!motion_->isReady())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
bool configured = motion_->configure(
|
|
config_.sensor_id,
|
|
config_.interrupt_ctrl,
|
|
motionEventCallback,
|
|
this);
|
|
if (!configured)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
motion_->attachInterrupt(sensorInterruptHandler);
|
|
|
|
last_motion_ms_ = 0;
|
|
last_sensor_poll_ms_ = sys::millis_now();
|
|
g_instance = this;
|
|
enabled_ = true;
|
|
return true;
|
|
}
|
|
|
|
void MotionPolicy::onSensorInterrupt()
|
|
{
|
|
sensor_irq_pending_ = true;
|
|
}
|
|
|
|
bool MotionPolicy::shouldUpdateSensor(uint32_t now_ms)
|
|
{
|
|
if (sensor_irq_pending_)
|
|
{
|
|
sensor_irq_pending_ = false;
|
|
return true;
|
|
}
|
|
return (now_ms - last_sensor_poll_ms_) >= config_.poll_interval_ms;
|
|
}
|
|
|
|
bool MotionPolicy::hasRecentMotion(uint32_t now_ms, uint32_t window_ms) const
|
|
{
|
|
if (!enabled_ || last_motion_ms_ == 0)
|
|
{
|
|
return false;
|
|
}
|
|
return (now_ms - last_motion_ms_) <= window_ms;
|
|
}
|
|
|
|
void MotionPolicy::markSensorUpdated(uint32_t now_ms)
|
|
{
|
|
last_sensor_poll_ms_ = now_ms;
|
|
}
|
|
|
|
bool MotionPolicy::shouldEnableGps(uint32_t now_ms)
|
|
{
|
|
if (!enabled_)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (motion_event_pending_)
|
|
{
|
|
motion_event_pending_ = false;
|
|
}
|
|
|
|
return (last_motion_ms_ > 0) &&
|
|
(now_ms - last_motion_ms_ < config_.idle_timeout_ms);
|
|
}
|
|
|
|
void MotionPolicy::sensorInterruptHandler()
|
|
{
|
|
if (g_instance != nullptr)
|
|
{
|
|
g_instance->onSensorInterrupt();
|
|
}
|
|
}
|
|
|
|
void MotionPolicy::motionEventCallback(uint8_t sensor_id, uint8_t* data, uint32_t size,
|
|
uint64_t* timestamp, void* user_data)
|
|
{
|
|
(void)sensor_id;
|
|
(void)data;
|
|
(void)size;
|
|
(void)timestamp;
|
|
auto* policy = static_cast<MotionPolicy*>(user_data);
|
|
if (policy == nullptr)
|
|
{
|
|
return;
|
|
}
|
|
policy->motion_event_pending_ = true;
|
|
policy->last_motion_ms_ = sys::millis_now();
|
|
}
|
|
|
|
} // namespace gps
|