From f7534cf5f97a6af7de6487600c76bca2d729703e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Fr=C3=A9d=C3=A9ric=20Bourgeois?= <bourgeoislab@gmail.com>
Date: Sat, 17 Jul 2021 19:38:34 +0200
Subject: [PATCH] Getter & setter for algorithm parameters (#23)

* Getter & setter for algorithm parameters

Getter and setter for Madgwick (beta) and Mahony (Kp & Ki).

* clang-format
---
 src/Adafruit_AHRS_Madgwick.h | 2 ++
 src/Adafruit_AHRS_Mahony.h   | 4 ++++
 2 files changed, 6 insertions(+)

diff --git a/src/Adafruit_AHRS_Madgwick.h b/src/Adafruit_AHRS_Madgwick.h
index c4c1cd5..5d3b8c9 100644
--- a/src/Adafruit_AHRS_Madgwick.h
+++ b/src/Adafruit_AHRS_Madgwick.h
@@ -49,6 +49,8 @@ public:
   // q0 + 2.0f * q3 * q3 - 1.0f);}; float getRoll(){return -1.0f * asinf(2.0f *
   // q1 * q3 + 2.0f * q0 * q2);}; float getYaw(){return atan2f(2.0f * q1 * q2
   // - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
+  float getBeta() { return beta; }
+  void setBeta(float beta) { this->beta = beta; }
   float getRoll() {
     if (!anglesComputed)
       computeAngles();
diff --git a/src/Adafruit_AHRS_Mahony.h b/src/Adafruit_AHRS_Mahony.h
index e25672f..61c60c3 100644
--- a/src/Adafruit_AHRS_Mahony.h
+++ b/src/Adafruit_AHRS_Mahony.h
@@ -42,6 +42,10 @@ public:
   void update(float gx, float gy, float gz, float ax, float ay, float az,
               float mx, float my, float mz);
   void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+  float getKp() { return twoKp / 2.0f; }
+  void setKp(float Kp) { twoKp = 2.0f * Kp; }
+  float getKi() { return twoKi / 2.0f; }
+  void setKi(float Ki) { twoKi = 2.0f * Ki; }
   float getRoll() {
     if (!anglesComputed)
       computeAngles();