ahr_gizmo

Author: qqqlab

examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino

@@ -46,10 +46,10 @@ Copyright (c) 2023-2025 https://madflight.com
 
 //Vehicle specific madflight configuration
 #define VEH_TYPE VEH_TYPE_COPTER //set the vehicle type for logging and mavlink
-#define VEH_FLIGHTMODE_AP_IDS {AP_COPTER_FLIGHTMODE_STABILIZE, AP_COPTER_FLIGHTMODE_ACRO} //mapping of fightmode index to ArduPilot code for logging and mavlink
+#define VEH_FLIGHTMODE_AP_IDS {AP_COPTER_FLIGHTMODE_ACRO, AP_COPTER_FLIGHTMODE_STABILIZE} //mapping of fightmode index to ArduPilot code for logging and mavlink
 #define VEH_FLIGHTMODE_NAMES {"RATE", "ANGLE"} //fightmode names for telemetry
 enum flightmode_enum { RATE, ANGLE };  //the available flightmode indexes
-flightmode_enum rcin_to_flightmode_map[6] {RATE, RATE, RATE, ANGLE, ANGLE, ANGLE}; //flightmode mapping from 6 pos switch to flight mode (simulates a 2-pos switch: RATE/ANGLE)
+flightmode_enum rcin_to_flightmode_map[6] {RATE, RATE, RATE, RATE, ANGLE, ANGLE}; //flightmode mapping from 2/3/6 pos switch to flight mode (simulates a 2-pos switch: RATE/ANGLE)
 
 #include "madflight_config.h" //Edit this header file to setup the pins, hardware, radio, etc. for madflight
 #include <madflight.h>
@@ -131,7 +131,11 @@ void loop() {
 
   mag.update();
 
-  if(gps.update()) {bbx.log_gps(); bbx.log_att();} //update gps (and log GPS and ATT for plot.ardupilot.org visualization)
+  //update gps (and log GPS and ATT for plot.ardupilot.org visualization)
+  if(gps.update()) {
+    bbx.log_gps(); 
+    bbx.log_att();
+  } 
 
   //logging
   static uint32_t log_ts = 0;
@@ -157,7 +161,9 @@ void imu_loop() {
 
   //Get radio commands - Note: don't do this in loop() because loop() is a lower priority task than imu_loop(), so in worst case loop() will not get any processor time.
   rcl.update();
-  veh.setFlightmode( rcin_to_flightmode_map[rcl.flightmode] ); //map rcl.flightmode (0 to 5) to vehicle flightmode
+  if(rcl.connected() && veh.setFlightmode( rcin_to_flightmode_map[rcl.flightmode] )) { //map rcl.flightmode (0 to 5) to vehicle flightmode
+    Serial.printf("Flightmode:%s\n",veh.flightmode_name());
+  }
 
   //PID Controller
   switch( veh.getFlightmode() ) {

src/madflight.h

@@ -26,30 +26,21 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 ===========================================================================================*/
 
-//#pragma once //don't use here, we want to get an error if included twice
-
-// madflight config string
-#ifndef MADFLIGHT_BOARD
-  #define MADFLIGHT_BOARD ""
-#endif
-#ifndef MADFLIGHT_CONFIG
-  #define MADFLIGHT_CONFIG ""
-#endif
-const char* madflight_config = MADFLIGHT_BOARD MADFLIGHT_CONFIG;
+//#pragma once //don't use here, we want to get an error if this file is included twice
 
 // bus abstraction
 #include "madflight/hal/MF_Serial.h"
 #include "madflight/hal/MF_I2C.h"
 
 // include all "_cpp.h" modules which have compile time config options
 #define MF_ALLOW_INCLUDE_CCP_H
-#include "madflight/ahr/ahr_cpp.h" //TODO - convert to use gizmos
 #include "madflight/alt/alt_cpp.h" //TODO - convert to use gizmos
 #include "madflight/hal/hal_cpp.h"
 #include "madflight/imu/imu_cpp.h" //for IMU_EXEC
 #undef MF_ALLOW_INCLUDE_CCP_H
 
 // include all other modules without compile time config options
+#include "madflight/ahr/ahr.h"
 #include "madflight/cfg/cfg.h"
 #include "madflight/cli/cli.h"
 #include "madflight/bar/bar.h"
@@ -76,6 +67,29 @@ void madflight_warn_or_die(String msg, bool die);
 // madflight_setup()
 //===============================================================================================
 
+// madflight config string by defines MADFLIGHT_BOARD, MADFLIGHT_CONFIG
+#ifndef MADFLIGHT_BOARD
+  #define MADFLIGHT_BOARD ""
+#endif
+#ifndef MADFLIGHT_CONFIG
+  #define MADFLIGHT_CONFIG ""
+#endif
+const char* madflight_config = MADFLIGHT_BOARD MADFLIGHT_CONFIG;
+
+// vehicle setup by defines VEH_TYPE, VEH_FLIGHTMODE_AP_IDS, VEH_FLIGHTMODE_NAMES
+#ifndef VEH_TYPE
+  #define VEH_TYPE VEH_TYPE_GENERIC
+#endif
+#ifndef VEH_FLIGHTMODE_AP_IDS
+  #define VEH_FLIGHTMODE_AP_IDS {0,1,2,3,4,5}
+#endif
+#ifndef VEH_FLIGHTMODE_NAMES
+  #define VEH_FLIGHTMODE_NAMES {"FM0","FM1","FM2","FM3","FM4","FM5"}
+#endif
+const uint8_t Veh::mav_type = VEH_TYPE; //mavlink vehicle type
+const uint8_t Veh::flightmode_ap_ids[6] = VEH_FLIGHTMODE_AP_IDS; //mapping from flightmode to ArduPilot flight mode id
+const char* Veh::flightmode_names[6] = VEH_FLIGHTMODE_NAMES; //[6] define flightmode name strings for telemetry
+
 void madflight_setup() {
   Serial.begin(115200); //start console serial
 
@@ -176,7 +190,6 @@ void madflight_setup() {
   rdr.config.baud = cfg.rdr_baud; //baud rate
   rdr.setup();
 
-
   // GPS
   gps.config.gizmo = (Cfg::gps_gizmo_enum)cfg.gps_gizmo; //the gizmo to use
   gps.config.ser_bus_id = cfg.gps_ser_bus; //serial bus id
@@ -194,9 +207,19 @@ void madflight_setup() {
 
   // ALT - Altitude Estimator
   alt.setup(bar.alt); 
-  
+
   // AHR - setup low pass filters for AHRS filters
-  ahr.setup(cfg.imu_gyr_lp, cfg.imu_acc_lp, cfg.mag_lp);
+  ahr.config.gizmo = (Cfg::ahr_gizmo_enum)cfg.ahr_gizmo; //the gizmo to use
+  ahr.config.gyrLpFreq = cfg.imu_gyr_lp; //gyro low pass filter freq [Hz]
+  ahr.config.accLpFreq = cfg.imu_acc_lp; //accelerometer low pass filter freq [Hz]
+  ahr.config.magLpFreq = cfg.mag_lp; //magnetometer low pass filter freq [Hz]
+  ahr.config.pimu = &imu; //pointer to Imu to use
+  ahr.config.pmag = &mag; //pointer to Mag to use
+  ahr.config.gyr_offset = &(cfg.imu_cal_gx); //gyro offset[3] [deg/sec]
+  ahr.config.acc_offset = &(cfg.imu_cal_ax); //acc offset[3] [G]
+  ahr.config.mag_offset = &(cfg.mag_cal_x); //mag offset[3] [adc_lsb]
+  ahr.config.mag_scale = &(cfg.mag_cal_sx); //mag scale[3] [uT/adc_lsb]
+  ahr.setup();
 
   // IMU - Intertial Measurement Unit (gyro/acc/mag)
   imu.config.sampleRate = cfg.imu_rate; //sample rate [Hz]

src/madflight/ahr/AhrGizmoMadgwick.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "ahr.h"
+#include "Madgwick/Madgwick.h"
+
+class AhrGizmoMadgwick : public AhrGizmo {
+private:
+  AhrConfig *config;
+  AhrState *state;
+  Madgwick ahrs;
+
+public:
+  AhrGizmoMadgwick(Ahr *ahr) {
+    this->config = &(ahr->config);
+    this->state = (AhrState*)ahr;
+  }
+
+  void setInitalOrientation(float *qnew) {
+    ahrs.q0 = qnew[0];
+    ahrs.q1 = qnew[1];
+    ahrs.q2 = qnew[2];
+    ahrs.q3 = qnew[3];
+  }
+
+  bool update() {
+    ahrs.update(
+      state->gx * Ahr::deg_to_rad, state->gy * Ahr::deg_to_rad, state->gz * Ahr::deg_to_rad,
+      state->ax, state->ay, state->az,
+      state->mx, state->my, state->mz,
+      state->dt
+    );
+    state->q[0] = ahrs.q0;
+    state->q[1] = ahrs.q1;
+    state->q[2] = ahrs.q2;
+    state->q[3] = ahrs.q3;
+
+    return true;
+  }
+};

src/madflight/ahr/AhrGizmoMahony.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "ahr.h"
+#include "Mahony/Mahony.h"
+
+class AhrGizmoMahony : public AhrGizmo {
+private:
+  AhrConfig *config;
+  AhrState *state;
+  Mahony ahrs;
+
+public:
+  AhrGizmoMahony(Ahr *ahr, bool update_1g_only) {
+    this->config = &(ahr->config);
+    this->state = (AhrState*)ahr;
+
+    //Betaflight flavored version, only update if acc is between 0.9 and 1.1 G
+    if(update_1g_only) {
+      ahrs.config_alen2_min = 0.81;
+      ahrs.config_alen2_max = 1.21;
+    }
+  }
+
+  void setInitalOrientation(float *qnew) {
+    ahrs.q0 = qnew[0];
+    ahrs.q1 = qnew[1];
+    ahrs.q2 = qnew[2];
+    ahrs.q3 = qnew[3];
+  }
+
+  bool update() {
+    ahrs.update(
+      state->gx * Ahr::deg_to_rad, state->gy * Ahr::deg_to_rad, state->gz * Ahr::deg_to_rad,
+      state->ax, state->ay, state->az,
+      state->mx, state->my, state->mz,
+      state->dt
+    );
+    state->q[0] = ahrs.q0;
+    state->q[1] = ahrs.q1;
+    state->q[2] = ahrs.q2;
+    state->q[3] = ahrs.q3;
+
+    return true;
+  }
+};

src/madflight/ahr/AhrGizmoVqf.h

@@ -0,0 +1,66 @@
+//=================================================================================================
+// VQF - see https://github.com/dlaidig/vqf
+//=================================================================================================
+
+//NOTE: only one AhrGizmoVqf instance is allowed
+
+//NOTE: do not use filter for VQF, feed the data directly into the filter, i.e. set gyrLpFreq, accLpFreq, magLpFreq to 1e10
+
+#pragma once
+
+#include "ahr.h"
+#include "vqf/vqf.h"
+
+class AhrGizmoVqf : public AhrGizmo {
+private:
+  AhrConfig *config;
+  AhrState *state;
+
+public:
+  AhrGizmoVqf(Ahr *ahr) {
+    this->config = &(ahr->config);
+    this->state = (AhrState*)ahr;
+
+    initVqf(1.0/config->pimu->getSampleRate(), 1.0/config->pimu->getSampleRate(), 1.0/config->pimu->getSampleRate());
+  }
+
+  void setInitalOrientation(float *qnew) {
+    //TODO (?)
+  }
+
+  bool update() {
+    //convert to ENU from NED: xE = yE, yN = xE, zU = -zD 
+    float gyr[3] = {state->gy * Ahr::deg_to_rad,  state->gx * Ahr::deg_to_rad, -state->gz * Ahr::deg_to_rad}; //gyr in rad/sec
+    float acc[3] = {state->ay * 9.81f,            state->ax * 9.81f,           -state->az * 9.81f};           //acc in m/s/s -  acc[] horizontal: [0, 0, -9.81], nose down:[0, 9.81, 0], right down: [9.81, 0, 0]
+    float mag[3] = {state->mx,                   -state->my,                    state->mz};                   //mag no unit - mag[] horizontal pointing north: [15, 0, 40], east: [0,15,40]
+
+    //get fused q_enu in ENU frame
+    float q_enu[4];
+    if (state->mx==0 && state->my==0 && state->mz==0) {
+      updateGyr(gyr);
+      updateAcc(acc);
+      getQuat6D(q_enu);
+    }else{
+      updateGyr(gyr);
+      updateAcc(acc);
+      updateMag(mag);
+      getQuat9D(q_enu);
+    }
+    
+    //convert q_enu to NED reference frame
+    float q_trans[4] = {0,M_SQRT2/2,M_SQRT2/2,0}; //ENU to NED
+    quatMultiply(q_enu, q_trans, state->q);
+
+    return true;
+  }
+
+  static void quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4])
+  {
+      vqf_real_t w = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
+      vqf_real_t x = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
+      vqf_real_t y = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
+      vqf_real_t z = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
+      out[0] = w; out[1] = x; out[2] = y; out[3] = z;
+  }
+
+};