86 lines
3.6 KiB
C++
86 lines
3.6 KiB
C++
#pragma once
|
|
|
|
#include "fgc/MtiProtocol.h" // ImuSample
|
|
|
|
#include <array>
|
|
|
|
namespace fgc {
|
|
|
|
// How the IMU is physically bolted to the gimbal.
|
|
//
|
|
// Both frames are NED body frames:
|
|
//
|
|
// X = forward Y = right Z = down
|
|
// roll about X pitch about Y yaw about Z (right-hand rule)
|
|
//
|
|
// x/y/z are degrees, and they are the rotation applied TO THE IMU to bring it
|
|
// into alignment with the gimbal - the direction the operator reads off the
|
|
// installation "to get from the IMU to the gimbal". An IMU that physically sits
|
|
// yawed +90 deg is therefore corrected with z = -90.
|
|
//
|
|
// Composition is intrinsic Z-X-Y: turn about Z, then about the X axis as it now
|
|
// stands, then the new Y.
|
|
struct ImuMount {
|
|
double x_deg = 0.0; // roll - about forward
|
|
double y_deg = 0.0; // pitch - about right
|
|
double z_deg = 0.0; // yaw - about down
|
|
|
|
// No rotation at all. The common case, and worth testing for: an unmounted
|
|
// rig then pays nothing and its samples pass through bit for bit.
|
|
bool identity() const;
|
|
};
|
|
|
|
// Row-major 3x3. m[r][c].
|
|
using Mat3 = std::array<std::array<double, 3>, 3>;
|
|
|
|
// The rotation carrying the device's local earth frame onto NED. Identity for a
|
|
// device already reporting NED.
|
|
Mat3 nedFromReference(ImuReference r);
|
|
|
|
// Short name for logs and the Sensors view ("NED", "NWU", "ENU").
|
|
const char* referenceName(ImuReference r);
|
|
|
|
// The mount rotation: R = Rz(z) * Rx(x) * Ry(y).
|
|
//
|
|
// R carries the IMU frame onto the gimbal frame. Two transforms follow from it,
|
|
// and they are NOT the same matrix:
|
|
//
|
|
// - a sensor-frame vector's components in the gimbal frame: v_gimbal = R^T v_sensor
|
|
// - the gimbal's orientation: R_earth<-gimbal = R_earth<-sensor * R
|
|
Mat3 mountRotationZXY(const ImuMount& m);
|
|
|
|
// The MTi's own orientation output is the ordinary aerospace Z-Y-X sequence
|
|
// (yaw, then pitch, then roll), which is a DIFFERENT order from the mount above.
|
|
// These two helpers exist as a named pair so the two conventions cannot quietly
|
|
// swap places: R = Rz(yaw) * Ry(pitch) * Rx(roll).
|
|
Mat3 rotationFromEulerZYX(double roll_deg, double pitch_deg, double yaw_deg);
|
|
|
|
// Inverse of the above. Angles come back in the ranges atan2/asin produce, which
|
|
// are the MTi's own: roll and yaw in -180..180, pitch in -90..90. Degenerate at
|
|
// pitch = +/-90, where roll and yaw are no longer separable: roll is pinned to 0
|
|
// and the whole rotation is carried in yaw.
|
|
void eulerZYXFromRotation(const Mat3& R, double& roll_deg, double& pitch_deg, double& yaw_deg);
|
|
|
|
// The whole reading moved into the gimbal frame: orientation, acceleration,
|
|
// rate of turn and magnetic field. Temperature, sample counter and the valid
|
|
// flag are properties of the device rather than of a frame, and pass through.
|
|
//
|
|
// Returns the sample unchanged when the mount is the identity.
|
|
ImuSample applyMount(const ImuMount& m, const ImuSample& s);
|
|
|
|
// The full frame correction: re-reference the orientation from the device's earth
|
|
// frame to NED, then move the whole reading into the gimbal's body frame.
|
|
//
|
|
// R_ned<-gimbal = nedFromReference(ref) * R_local<-sensor * mountRotationZXY(m)
|
|
// \_____ earth _____/ \____ body ____/
|
|
//
|
|
// Only the orientation is re-referenced. acc, gyr and mag are BODY-frame
|
|
// quantities - they describe directions in the sensor, not relative to the earth
|
|
// - so the earth frame does not touch them and only the mount rotates them.
|
|
//
|
|
// Returns the sample unchanged when there is nothing to do (NED already, and an
|
|
// identity mount).
|
|
ImuSample applyFrames(ImuReference ref, const ImuMount& m, const ImuSample& s);
|
|
|
|
} // namespace fgc
|