57 lines
2.4 KiB
C++
57 lines
2.4 KiB
C++
#pragma once
|
|
|
|
#include "fgc/IImuSource.h"
|
|
#include "fgc/ImuMount.h"
|
|
|
|
#include <memory>
|
|
#include <utility>
|
|
|
|
namespace fgc {
|
|
|
|
// Wraps any IImuSource and puts its readings into the gimbal's NED body frame:
|
|
// the device's earth reference is re-referenced to NED, and the [IMU] x/y/z mount
|
|
// carries the body frame onto the gimbal's. Both are needed and they are not
|
|
// interchangeable - see applyFrames().
|
|
//
|
|
// This sits directly on the factory's output, which is the whole point: every
|
|
// consumer - the calibration routine, the self-test suite, the TUI, `imu status`,
|
|
// the MQTT path - already reads through IImuSource::sample(), so one wrap covers
|
|
// all of them and no consumer needs to know a mount exists. It wraps the mock as
|
|
// readily as the MTi, which is what makes the feature exercisable with
|
|
// --mock-imu on a machine with no hardware.
|
|
//
|
|
// Application skips the wrapper entirely when there is nothing to do (the device
|
|
// already reports NED and the mount is the identity), so such a rig carries
|
|
// neither the indirection nor the arithmetic.
|
|
class MountedImuSource : public IImuSource {
|
|
public:
|
|
MountedImuSource(std::unique_ptr<IImuSource> inner, ImuReference reference, ImuMount mount)
|
|
: inner_(std::move(inner)), reference_(reference), mount_(mount) {}
|
|
|
|
void start() override { inner_->start(); }
|
|
void stop() override { inner_->stop(); }
|
|
bool connected() const override { return inner_->connected(); }
|
|
|
|
std::optional<ImuSample> sample() override {
|
|
auto s = inner_->sample();
|
|
if (!s) return s;
|
|
return applyFrames(reference_, mount_, *s);
|
|
}
|
|
|
|
// Everything below describes the device, not a frame, so it passes straight
|
|
// through. The mount is a host-side correction and never reaches the MTi.
|
|
std::optional<ImuDeviceConfig> config() const override { return inner_->config(); }
|
|
std::vector<std::string> configProblems() const override { return inner_->configProblems(); }
|
|
void refreshConfig() override { inner_->refreshConfig(); }
|
|
void noRotation(int seconds) override { inner_->noRotation(seconds); }
|
|
void headingReset() override { inner_->headingReset(); }
|
|
bool setFilterProfile(int type) override { return inner_->setFilterProfile(type); }
|
|
|
|
private:
|
|
std::unique_ptr<IImuSource> inner_;
|
|
ImuReference reference_;
|
|
ImuMount mount_;
|
|
};
|
|
|
|
} // namespace fgc
|