28 lines
1.0 KiB
C++
28 lines
1.0 KiB
C++
 
|
#pragma once
|
||
|
|
#include <Arduino.h>
|
||
|
|
|
||
|
|
// Handset attitude from an MPU6050: same complementary filter as the robot's
|
||
|
|
// RobotIMU, plus a mounting-orientation remap, boot-time gyro bias capture,
|
||
|
|
// and a "level" reference so the operator's natural holding angle reads as 0.
|
||
|
|
class RemoteIMU {
|
||
|
|
public:
|
||
|
|
bool begin(); // false if the IMU didn't respond; hold the remote still during this
|
||
|
|
void update(float dt); // no-op if the IMU never came up
|
||
|
|
bool ok() const { return ok_; }
|
||
|
|
|
||
|
|
// Tilt relative to the zeroed level pose, degrees, handset frame
|
||
|
|
// (+roll = right side down, +pitch = far edge down).
|
||
|
|
float rollDeg() const { return roll_deg_ - zero_roll_; }
|
||
|
|
float pitchDeg() const { return pitch_deg_ - zero_pitch_; }
|
||
|
|
|
||
|
|
void zeroLevel() { zero_roll_ = roll_deg_; zero_pitch_ = pitch_deg_; }
|
||
|
|
|
||
|
|
private:
|
||
|
|
float roll_deg_ = 0, pitch_deg_ = 0;
|
||
|
|
float zero_roll_ = 0, zero_pitch_ = 0;
|
||
|
|
float gyro_bias_[3] = {};
|
||
|
|
bool ok_ = false;
|
||
|
|
|
||
|
|
// Reads and remaps into the handset frame. Accel in g, gyro in deg/s.
|
||
|
|
bool readRaw(float a[3], float g[3]);
|
||
|
|
};
|