Add mag_align (#74)

Author: melehin

examples/00.HelloWorld/madflight_config.h

@@ -61,6 +61,7 @@ const char madflight_config[] = R""(
 
 //--- MAG --- Magnetometer
 //mag_gizmo      NONE  // options: NONE, QMC5883, QMC6309, RM3100
+//mag_align      CW0   // options: CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP
 //mag_i2c_adr    0
 //mag_i2c_bus   -1
 

examples/01.Quadcopter/madflight_config.h

@@ -61,6 +61,7 @@ const char madflight_config[] = R""(
 
 //--- MAG --- Magnetometer
 //mag_gizmo      NONE  // options: NONE, QMC5883, QMC6309, RM3100
+//mag_align      CW0   // options: CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP
 //mag_i2c_adr    0
 //mag_i2c_bus   -1
 

examples/02.QuadcopterAdvanced/madflight_config.h

@@ -61,6 +61,7 @@ const char madflight_config[] = R""(
 
 //--- MAG --- Magnetometer
 //mag_gizmo      NONE  // options: NONE, QMC5883, QMC6309, RM3100
+//mag_align      CW0   // options: CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP
 //mag_i2c_adr    0
 //mag_i2c_bus   -1
 

examples/03.Plane/madflight_config.h

@@ -61,6 +61,7 @@ const char madflight_config[] = R""(
 
 //--- MAG --- Magnetometer
 //mag_gizmo      NONE  // options: NONE, QMC5883, QMC6309, RM3100
+//mag_align      CW0   // options: CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP
 //mag_i2c_adr    0
 //mag_i2c_bus   -1
 

src/cfg/cfg.h

@@ -237,6 +237,9 @@ SOFTWARE.
   MF_PARAM( pin_rdr_trig,     -1, int32_t, 'p') \
   MF_PARAM( pin_rdr_echo,     -1, int32_t, 'p') \
   MF_PARAM( imu_bus_type,      0, int32_t, 'e', mf_SPI,mf_I2C) \
+\
+  /*v2.1.1 additions */ \
+  MF_PARAM( mag_align,         0, int32_t, 'e', mf_CW0,mf_CW90,mf_CW180,mf_CW270,mf_CW0FLIP,mf_CW90FLIP,mf_CW180FLIP,mf_CW270FLIP) \
 //end MF_PARAM_LIST
 
 

src/mag/mag.cpp

@@ -81,6 +81,34 @@ bool Mag::update() {
   if(!schedule.interval(_samplePeriod)) return false;
 
   if(!gizmo->update(&x, &y, &z)) return false;
+
+  //handle rotation for different mounting positions
+  switch((Cfg::mag_align_enum)cfg.mag_align) {
+    case Cfg::mag_align_enum::mf_CW0 :
+      break;
+    case Cfg::mag_align_enum::mf_CW90 :
+      { float tmp; tmp=x; x=-y; y=tmp; }
+      break;
+    case Cfg::mag_align_enum::mf_CW180 :
+      { x=-x; y=-y; }
+      break;
+    case Cfg::mag_align_enum::mf_CW270 :
+      { float tmp; tmp=x; x=y; y=-tmp; }
+      break;
+    case Cfg::mag_align_enum::mf_CW0FLIP :
+      { y=-y; z=-z; }
+      break;
+    case Cfg::mag_align_enum::mf_CW90FLIP :
+      { float tmp; tmp=x; x=y; y=tmp; z=-z; }
+      break;
+    case Cfg::mag_align_enum::mf_CW180FLIP :
+      { x=-x; z=-z; }
+      break;
+    case Cfg::mag_align_enum::mf_CW270FLIP :
+      { float tmp; tmp=x; x=-y; y=-tmp; z=-z; }
+      break;
+  }
+
   ts = micros();
   return true;
 }