82 lines
3.1 KiB
C++
82 lines
3.1 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;
|
|
}
|
|
|
|
// Synthetic config mirroring the real handshake (Euler + calibrated, 100 Hz),
|
|
// so the expanded view's IMU CONFIG section renders without hardware.
|
|
std::optional<ImuDeviceConfig> config() const override {
|
|
ImuDeviceConfig c;
|
|
c.valid = true;
|
|
c.product_code = "MTi-28A53G35 (mock)";
|
|
c.has_device_id = true;
|
|
c.device_id = 0x00990ABC;
|
|
c.firmware = "2.8.1 build 0";
|
|
c.has_output_mode = true;
|
|
c.output_mode = kOutputMode;
|
|
c.out_temperature = c.out_calibrated = c.out_orientation = true;
|
|
c.has_output_settings = true;
|
|
c.output_settings = kOutputSettings;
|
|
c.orientation_mode = "Euler";
|
|
c.timestamp_mode = "Sample counter";
|
|
c.data_format = "Float";
|
|
c.has_period = true;
|
|
c.period = 1152; // 100 Hz
|
|
c.sample_rate_hz = 100.0f;
|
|
c.has_scenario = true;
|
|
c.scenario_type = 39;
|
|
c.scenario_version = 11;
|
|
c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
|
|
{41, 11, "Dynamic"}};
|
|
c.scenario_label = "General";
|
|
return c;
|
|
}
|
|
|
|
private:
|
|
using clock = std::chrono::steady_clock;
|
|
clock::time_point start_ = clock::now();
|
|
};
|
|
|
|
} // namespace fgc
|