109 lines
4.2 KiB
C++
109 lines
4.2 KiB
C++
#pragma once
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
// Encode one native mode-0 IMU block. Inputs use the native-right body frame;
|
|
// quaternion wxyz rotates body vectors into the reference frame. Quaternion
|
|
// scale/sign do not matter. Finite acceleration saturates at signed Q28 limits.
|
|
// Invalid inputs leave output untouched; counter and elapsed must fit 12 bits.
|
|
bool probe_native_imu_pack(const float quaternion_wxyz[4], const float accel_g[3],
|
|
uint16_t counter_ticks, uint16_t elapsed_ticks,
|
|
int16_t temperature_raw, uint8_t output[30]);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
|
|
struct ProbeNativeMotionSample {
|
|
bool accel_valid = false;
|
|
bool gyro_valid = false;
|
|
uint32_t accel_sequence = 0;
|
|
uint32_t gyro_sequence = 0;
|
|
uint32_t accel_us = 0;
|
|
uint32_t gyro_us = 0;
|
|
float accel_g[3]{};
|
|
float gyro_dps[3]{};
|
|
};
|
|
|
|
enum class ProbeNativeMotionBias : uint8_t {
|
|
kAlreadyCalibrated,
|
|
kTrackStationary,
|
|
};
|
|
|
|
// Core 0 only. Call update even when sensors are unavailable, and consult ready
|
|
// before using the orientation. Sequence identities, not polling, admit samples;
|
|
// repeated sequences cannot refresh timestamps or contribute to calibration.
|
|
// All sources initialize from the first usable fresh sensor pair. Wii optionally
|
|
// refines residual bias in the background without resetting orientation. Steady
|
|
// rotation about gravity remains indistinguishable from bias without a reference.
|
|
// Fresh near-1g acceleration corrects tilt drift. Heading remains gyro-derived
|
|
// unless a reliable optical heading observation is supplied.
|
|
class ProbeNativeMotion {
|
|
public:
|
|
void reset();
|
|
void update(uint32_t now_us, uint32_t connection_generation,
|
|
const ProbeNativeMotionSample& sample,
|
|
ProbeNativeMotionBias bias_mode = ProbeNativeMotionBias::kAlreadyCalibrated);
|
|
// Call after update, using a full observed sensor-bar pair (never inferred).
|
|
// Positive optical yaw means aim-right, the negative reference-world turn.
|
|
// The first sample in each optical generation anchors the current heading;
|
|
// it supplies neither absolute world yaw nor an absolute console cursor.
|
|
void observe_optical_heading(uint32_t now_us, uint32_t reference_generation,
|
|
uint32_t sequence, uint32_t sample_us,
|
|
float yaw_radians, bool valid);
|
|
bool ready() const { return ready_; }
|
|
const float* quaternion() const { return quaternion_; }
|
|
const float* acceleration() const { return acceleration_; }
|
|
const float* bias() const { return bias_; }
|
|
|
|
private:
|
|
void invalidate();
|
|
void clear_candidate();
|
|
bool initialize_orientation();
|
|
bool integrate(const float gyro_dps[3], uint32_t elapsed_us);
|
|
bool correct_gravity(uint32_t elapsed_us);
|
|
bool normalize_orientation();
|
|
void track_bias(bool new_accel, bool new_gyro, uint32_t gyro_elapsed_us);
|
|
|
|
bool have_generation_ = false;
|
|
ProbeNativeMotionBias bias_mode_ = ProbeNativeMotionBias::kAlreadyCalibrated;
|
|
bool have_update_ = false;
|
|
bool seen_accel_sequence_ = false;
|
|
bool seen_gyro_sequence_ = false;
|
|
bool have_accel_ = false;
|
|
bool have_gyro_ = false;
|
|
bool candidate_ = false;
|
|
bool ready_ = false;
|
|
bool have_optical_generation_ = false;
|
|
bool seen_optical_sequence_ = false;
|
|
bool have_optical_reference_ = false;
|
|
bool have_optical_sample_ = false;
|
|
uint32_t connection_generation_ = 0;
|
|
uint32_t update_us_ = 0;
|
|
uint32_t accel_sequence_ = 0;
|
|
uint32_t gyro_sequence_ = 0;
|
|
uint32_t accel_us_ = 0;
|
|
uint32_t gyro_us_ = 0;
|
|
uint32_t candidate_us_ = 0;
|
|
uint32_t gyro_count_ = 0;
|
|
uint32_t accel_count_ = 0;
|
|
uint32_t optical_generation_ = 0;
|
|
uint32_t optical_sequence_ = 0;
|
|
uint32_t optical_sample_us_ = 0;
|
|
float optical_reference_radians_ = 0.0f;
|
|
float gyro_variation_ = 0.0f;
|
|
float accel_variation_ = 0.0f;
|
|
float quaternion_[4]{1.0f, 0.0f, 0.0f, 0.0f};
|
|
float acceleration_[3]{};
|
|
float gyro_dps_[3]{};
|
|
float bias_[3]{};
|
|
float bias_target_[3]{};
|
|
float mean_gyro_[3]{};
|
|
float mean_accel_[3]{};
|
|
float gravity_reference_[3]{};
|
|
};
|
|
#endif
|