53 lines
2.0 KiB
C++
53 lines
2.0 KiB
C++
#pragma once
|
|
|
|
#include "fgc/IImuSource.h"
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <chrono>
|
|
#include <cmath>
|
|
|
|
namespace fgc {
|
|
|
|
// Simulated IMU for development without hardware: synthesizes a slowly varying,
|
|
// physically plausible full sample (orientation sweeps, ~1 g on accZ, small
|
|
// gyro/mag) so the Sensors panel and its expanded view animate.
|
|
class MockImuSource : public IImuSource {
|
|
public:
|
|
void start() override {
|
|
start_ = clock::now();
|
|
LOG_INFO << "[mock] IMU started";
|
|
}
|
|
void stop() override { LOG_INFO << "[mock] IMU stopped"; }
|
|
|
|
bool connected() const override { return true; }
|
|
|
|
std::optional<ImuSample> sample() override {
|
|
const double t = std::chrono::duration<double>(clock::now() - start_).count();
|
|
ImuSample s;
|
|
s.valid = true;
|
|
s.roll_deg = static_cast<float>(15.0 * std::sin(t * 0.5));
|
|
s.pitch_deg = static_cast<float>(10.0 * std::sin(t * 0.3 + 1.0));
|
|
s.yaw_deg = static_cast<float>(std::fmod(t * 8.0, 360.0)); // slow spin
|
|
// Gravity tilts with roll/pitch; small free accel noise.
|
|
const double r = s.roll_deg * M_PI / 180.0, p = s.pitch_deg * M_PI / 180.0;
|
|
s.acc[0] = static_cast<float>(9.81 * -std::sin(p));
|
|
s.acc[1] = static_cast<float>(9.81 * std::sin(r) * std::cos(p));
|
|
s.acc[2] = static_cast<float>(9.81 * std::cos(r) * std::cos(p));
|
|
s.gyr[0] = static_cast<float>(0.13 * std::cos(t * 0.5));
|
|
s.gyr[1] = static_cast<float>(0.05 * std::cos(t * 0.3 + 1.0));
|
|
s.gyr[2] = 0.14f;
|
|
s.mag[0] = static_cast<float>(std::cos(s.yaw_deg * M_PI / 180.0));
|
|
s.mag[1] = static_cast<float>(-std::sin(s.yaw_deg * M_PI / 180.0));
|
|
s.mag[2] = 0.35f;
|
|
s.temp_c = static_cast<float>(24.5 + 0.5 * std::sin(t * 0.05));
|
|
s.sample_counter = static_cast<uint16_t>(static_cast<unsigned>(t * 100.0) & 0xFFFF);
|
|
return s;
|
|
}
|
|
|
|
private:
|
|
using clock = std::chrono::steady_clock;
|
|
clock::time_point start_ = clock::now();
|
|
};
|
|
|
|
} // namespace fgc
|