OrientationEstimator.h

Go to the documentation of this file.

#ifndef ORIENTATION_ESTIMATOR_H
#define ORIENTATION_ESTIMATOR_H
 
#include "data_handling/DataPoint.h"
#include "state_estimation/StateEstimationTypes.h"
#include "state_estimation/States.h"

class OrientationEstimator {
public:
    OrientationEstimator(float gainPad = 0.05f, float gainFlight = 0.005f)
        : q0(1.0f), q1(0.0f), q2(0.0f), q3(0.0f),
          betaPad(gainPad), betaFlight(gainFlight),
          roll(0.0f), pitch(0.0f), yaw(0.0f), hasLaunched(false), lastUpdateTime(0) {}

    void update(AccelerationTriplet accel, GyroTriplet gyro, MagTriplet mag, uint32_t currentTime);
 
    Quaternion getQuaternion() const{
        Quaternion q = {q0, q1, q2, q3};
        return q;
    }
    void launchDetected() { hasLaunched = true;}
    float getRoll() const { return roll; }
    float getPitch() const { return pitch; }
    float getYaw() const { return yaw; }
    
private:
    float q0, q1, q2, q3;   // quaternion
    float betaPad;
    float betaFlight;
    float beta; // determines how much the algorithm relies on the accelerometer vs the gyroscope. Higher beta means more reliance on accel.
    float roll, pitch, yaw; // Euler angles in degrees
    bool hasLaunched;
    uint32_t lastUpdateTime;   // timestamp of the last update in milliseconds

    void updateFullAHRS(AccelerationTriplet accel, GyroTriplet gyro, MagTriplet mag, uint32_t currentTime);

    void updateIMU(AccelerationTriplet accel, GyroTriplet gyro, uint32_t currentTime);
 
    /*
    * @brief Compute Euler angles (roll, pitch, yaw) from the current quaternion estimate.
    * This is called every update to keep the Euler angles in sync with the quaternion.
    * The Euler angles are in degrees and follow the aerospace convention (roll around x-axis, pitch around y-axis, yaw around z-axis).
    */
    void getEuler();
};
 
 
#endif