HEADHOLD and HEADFREE without a magnetometer

giiba · Post by **giiba** » Tue May 13, 2014 5:55 pm

Headhold and Headfree without a magnetometer is correct! That being said I've only the faintest idea how it works, but it does. The Headhold drifts a few degrees (maybe 10) over the course of a battery, but the yaw hold in Headhold mode is really good in a breeze.

I found this feature in a build by user Benedikt @ http://www.rcgroups.com/forums/showpost.php?p=28206278 in his build for the Hobbyking Micro MWC board. After noticing that the dev build of MultiWii includes the brushed motor support, I gave it a try and quickly started missing the Headhold feature. After a few attempts to copy+paste this feature to the dev build, I have to admit that I'm not much of a programmer (I work guiding wilderness trips and ski patrolling in fact). That has led me here as I suspect someone around here knows how to make this work. Myself I cannot get past the compiles errors, and do not understand the few points where stock code has been commented out.

Ultimately I think this feature should be baked right in to MultiWii. As far as I can tell this code is quite simple so it seems a shame if it couldn't be included (and improved!) as Headfree is a great beginner feature. I would love to have access to the HEADADJ function I see in the 'if MAG' stuff, as at the moment this code sets the heading at arming and you must land and disarm - re-arm to change the heading.

The only catch is I have no idea who to credit with this, Benedikt did as I did and copy/pasted from somewhere he doesn't remember. If this is your work thanks!

To this end I have thoroughly compared the build that works (..._PIDtune_stripped) with the stock 2.3 code and below you will find what I've got, and diff files. I have stripped everything from the _PIDtune_stripped version that does not pertain to this code so you still have to set config.h options to fly this.

Included in the zip file below are:

-modified files:
config.h,IMU.cpp,MultiWii.cpp,MultiWii.h,Protocol.cpp,Sensors.cpp,types.h

-original files
same 7 above (source: https://code.google.com/p/multiwii/downloads/detail?name=MultiWii_2_3.zip&can=2&q=)

-WinMerge patch file:
patch_stock_PIDtune_stripped.txt

-edited patch file for visual clarity:
patch_visual edit - stock_PIDtune_stripped.txt

I'm going to put the edited patch file up in the next post so that it can be viewed without a download.

giiba · Post by **giiba** » Tue May 13, 2014 5:56 pm

Code: Select all

*********************************************************
*** config.h   MultiWii_2_3_PIDtune_stripped
--- config.h   MultiWii
*********************************************************

*** 212,213 ****
-    #define HEADFREE
-    #define HEADHOLD
--- 211 ----
***************


*********************************************************
*** IMU.cpp   MultiWii_2_3_PIDtune_stripped
--- IMU.cpp   MultiWii
*********************************************************

*** 13,16 ****
!   //static int16_t gyroADCprevious[3] = {0,0,0};
!   //int16_t gyroADCp[3];
!   //int16_t gyroADCinter[3];
!   //static uint32_t timeInterleave = 0;
--- 13,16 ----
!   static int16_t gyroADCprevious[3] = {0,0,0};
!   int16_t gyroADCp[3];
!   int16_t gyroADCinter[3];
!   static uint32_t timeInterleave = 0;
***************

*** 46,47 ****
- 
-     annexCode();
--- 45 ----
***************

*** 49,50 ****
!       imu.gyroData[axis] =  imu.gyroADC[axis];
!     /*
--- 47 ----
!       gyroADCp[axis] =  imu.gyroADC[axis];
***************

*** 66 ****
-     */
--- 62 ----
***************

*** 185,187 ****
- static t_fp_vector EstN = { 1000.0, 0.0, 0.0 };
- static t_int32_t_vector EstN32;
- 
--- 180 ----
***************

*** 211,212 ****
-   rotateV(&EstN.V,deltaGyroAngle);
- 
--- 203 ----
***************

*** 226,228 ****
- 
-     EstN32.A[axis] = EstN.A[axis];
- 
--- 216 ----
***************

*** 254,258 ****
-   act_heading =  _atan2(
-   EstN32.V.Z * EstG32.V.X - EstN32.V.X * EstG32.V.Z,
-   EstN32.V.Y * invG * sqGX_sqGZ  - (EstN32.V.X * EstG32.V.X + EstN32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y );//_atan2(EstN32.V.Y, EstN32.V.X);
-   act_heading = act_heading / 10.0;   
- 
--- 241 ----
***************


*********************************************************
*** MultiWii.cpp   MultiWii_2_3_PIDtune_stripped
--- MultiWii.cpp   MultiWii
*********************************************************

*** 58,65 ****
-   
-   #ifdef HEADFREE
-    "HEADFREE;"
-   #endif
-   #ifdef HEADHOLD
-    "HEADHOLD;"
-   #endif
-   
--- 57 ----
***************

*** 117,119 ****
-   
-   5,6, // HEADFREE, HEADHOLD
-   
--- 108 ----
***************

*** 166 ****
! int16_t  act_heading,magHold,headFreeModeHold; // [-180;+180]
--- 155 ----
! int16_t  magHold,headFreeModeHold; // [-180;+180]
***************

*** 380 ****
!     float radDiff = (act_heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
--- 369 ----
!     float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
***************

*** 716,717 ****
!       headFreeModeHold = act_heading;
!       magHold = act_heading;
--- 705,706 ----
!       headFreeModeHold = att.heading;
!       magHold = att.heading;
***************

*** 988,994 ****
-    #ifdef HEADFREE
-       if (rcOptions[BOXHEADFREE] == 1) f.HEADFREE_MODE = 1; else f.HEADFREE_MODE = 0;
-    #endif
-    #ifdef HEADHOLD
-       if (rcOptions[BOXHEADHOLD] == 1) f.HEADHOLD_MODE = 1; else f.HEADHOLD_MODE = 0;
-    #endif
- 
--- 976 ----
***************

*** 1169,1179 ****
-    #ifdef HEADHOLD
-    if(f.HEADHOLD_MODE)
-    { //to optimize
-       errorAngle = headFreeModeHold - act_heading;
-       if (errorAngle >  180) errorAngle -= 360;
-       if (errorAngle < -180) errorAngle += 360;
-       errorAngle = constrain(errorAngle,-100,100);
-       rcCommand[YAW] += (errorAngle*conf.pid[PIDMAG].P8) >> 4;
-    }
-    #endif
- 
--- 1150 ----
***************


*********************************************************
*** MultiWii.h   MultiWii_2_3_PIDtune_stripped
--- MultiWii.h   MultiWii
*********************************************************

*** 46 ****
- extern int16_t  act_heading;
--- 45 ----
***************


*********************************************************
*** Protocol.cpp   MultiWii_2_3_PIDtune_stripped
--- Protocol.cpp   MultiWii
*********************************************************

*** 378,385 ****
- 
-      #if defined(HEADFREE)
-       if(f.HEADFREE_MODE) tmp |= 1<<BOXHEADFREE;
-      #endif
-      #if defined(HEADHOLD)
-       if(f.HEADHOLD_MODE) tmp |= 1<<BOXHEADHOLD;
-      #endif
- 
--- 377 ----
***************

*** 476 ****
-      att.heading = act_heading;
--- 467 ----
***************


*********************************************************
*** Sensors.cpp   MultiWii_2_3_PIDtune_stripped
--- Sensors.cpp   MultiWii
*********************************************************

*** 370 ****
!     //imu.gyroADC[axis] = constrain(imu.gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
--- 370 ----
!     imu.gyroADC[axis] = constrain(imu.gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
***************

*** 372 ****
!     //previousGyroADC[axis] = imu.gyroADC[axis];
--- 372 ----
!     previousGyroADC[axis] = imu.gyroADC[axis];
***************


*********************************************************
*** types.h   MultiWii_2_3_PIDtune_stripped
--- types.h   MultiWii
*********************************************************

*** 45,52 ****
-   
-   #ifdef HEADFREE
-    BOXHEADFREE,
-   #endif
-   #ifdef HEADHOLD
-    BOXHEADHOLD,
-   #endif
-   
--- 44 ----
***************

*** 131 ****
-   uint8_t HEADHOLD_MODE :1 ;
--- 122 ----
***************

Plüschi · Post by **Plüschi** » Tue May 13, 2014 8:30 pm

This originates from my pocketquad firmware and made it into baseflight (corrected by TC) but not into mwii. It works best with full gyro resolution, but full-res is not used anywhere except in my pocket-code (i think). Original mwii gyro resolution will give substantial drift, and since there is no mag to correct the drift the N vector will go outside the horizontal plane and .... only use this for short flights.

I would love to have someone fix that N vector to stay in a horizontal plane but i could not find anyone to help. I have a N vector (x,y,z) pointing north, and i want to scale this vector to stay 90 deg to the G (gravity) vector (x,y,z). Vector length doesent matter. In other words i want to project the N vector onto a plane defined by (perpendicular to) the G vector. Myself i suck at math, especially at linear algebra.

Plüschi · Post by **Plüschi** » Sat May 17, 2014 9:26 pm

I had a look at the benedikt source. Why does the imu part break the mag functionality? Sorry but the guy doesent understand shit about how stuff works.

giiba · Post by **giiba** » Sat May 17, 2014 9:37 pm

I couldn't tell you, except he isn't the originator. The origin of the code I posted is unknown.

As to the lines commented out in imu.cpp, I can only confirm that if you uncomment them it breaks the compass. My uneducated guess is that the commented code stops both mag and not-mag compasses working at the same time, and against each other as the gui would show.

The code does compile and boot fine with the code uncommented.

goebish · Post by **goebish** » Sun May 18, 2014 9:52 am

I reimplemented magless headfree & headhold over Plüschi's code with high resolution gyro/acc a few months ago but it doesn't work really well, I probably missed something.
http://www.rcgroups.com/forums/showthread.php?t=2115410

giiba · Post by **giiba** » Wed May 21, 2014 3:41 am

Thanks to your code Goebish I've had a modicum of success; comparing yours to Benedikt's to stock 2.3 I've narrowed the changes down. Now the code compiles, boots, and talks to the GUI as expected. Better the sensor readouts all look rational enough and the compass readout sems to indicate success.

Only trouble now is when I arm the copter the LED turns on for an instant then turns off. In other words it'll arm and then disarm itself within 1/2 second. With throttle on while I arm, I get a blip of power just after the LED turns on and then they stop.

I'll post code before the night is through.

giiba · Post by **giiba** » Wed May 21, 2014 8:45 am

Success! I've been pouring over this too much the past few days, but I just took my first flight on a working firmware.

My problem earlier was placing an annexCode() function before:

Code: Select all

    #else
      annexCode();
      for (axis = 0; axis < 3; axis++)
        imu.gyroData[axis] =  imu.gyroADC[axis];
    #endif

instead of after:

Code: Select all

    #else
      for (axis = 0; axis < 3; axis++)
        imu.gyroData[axis] =  imu.gyroADC[axis];
      annexCode();
    #endif

in IMU.cpp, grrrrrrrrrr

If only that didn't take so long to figure out... my not-a-programmers-eye finally noticed the slight syntax change, ha! Below is what I think is the minimum of changes possible to implement this code. For now it's working, HEADHOLD, HEADFREE, and yes HEADADJ! tested in the air.

Attached is a zip containing:
- modified: IMU.cpp, MultiWii.cpp, Protocol.cpp, types.h
- WinMerge patch file
- Text file of changes (below)

only other thing needed is to add:
#define HEADFREE
#define HEADHOLD
to your config.h as you configure

Code: Select all

*********************************************************
*** IMU.cpp
*********************************************************


*** 13 ****
-   #if MAG
--- 12 ----
***************


*** 18 ****
-   #endif
--- 16 ----
***************


*** 48 ****
-     #if MAG
--- 45 ----
***************


*** 66,70 ****
-     #else
-       for (axis = 0; axis < 3; axis++)
-         imu.gyroData[axis] =  imu.gyroADC[axis];
-       annexCode();
-     #endif
--- 62 ----
***************


*** 187,189 ****
- #else
-   static t_fp_vector EstN = { 1000.0, 0.0, 0.0 };
-   static t_int32_t_vector EstN32;
--- 178 ----
***************


*** 217,218 ****
-   #else
-     rotateV(&EstN.V,deltaGyroAngle);
--- 205 ----
***************


*** 233,234 ****
-     #else
-       EstN32.A[axis] = EstN.A[axis];
--- 219 ----
***************


*** 255,260 ****
-   #else
-     att.heading = _atan2(
-       EstN32.V.Z * EstG32.V.X - EstN32.V.X * EstG32.V.Z,
-       EstN32.V.Y * invG * sqGX_sqGZ  - (EstN32.V.X * EstG32.V.X + EstN32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y
-       ); 
-     att.heading /= 10;
--- 239 ----




*********************************************************
*** MultiWii.cpp
*********************************************************


*** 58,66 ****
-   
-   #ifdef HEADFREE
-    "HEADFREE;"
-   #endif
-   #ifdef HEADHOLD
-    "HEADHOLD;"
-    "HEADADJ;"
-   #endif
- 
--- 57 ----
***************


*** 986,995 ****
-     #ifdef HEADFREE
-      if (rcOptions[BOXHEADFREE] == 1) f.HEADFREE_MODE = 1; else f.HEADFREE_MODE = 0;
-     #endif
-     #ifdef HEADHOLD
-      if (rcOptions[BOXHEADHOLD] == 1) f.HEADHOLD_MODE = 1; else f.HEADHOLD_MODE = 0;
-       if (rcOptions[BOXHEADADJ]) {
-         headFreeModeHold = att.heading; // acquire new heading
-         }
-     #endif
- 
--- 976 ----
***************


*** 1171,1179 ****
-   #ifdef HEADHOLD
-    if(f.HEADHOLD_MODE)
-      { errorAngle = headFreeModeHold - att.heading;
-      if (errorAngle >  180) errorAngle -= 360;
-      if (errorAngle < -180) errorAngle += 360;
-      errorAngle = constrain(errorAngle,-100,100);
-      rcCommand[YAW] += (errorAngle*conf.pid[PIDMAG].P8) >> 4;
-         }
-   #endif
--- 1149 ----




*********************************************************
*** Protocol.cpp
*********************************************************


*** 378,386 ****
-     
-     #if defined(HEADFREE)
-       if(f.HEADFREE_MODE) tmp |= 1<<BOXHEADFREE;
-     #endif
-     #if defined(HEADHOLD)
-       if(f.HEADHOLD_MODE) tmp |= 1<<BOXHEADHOLD;
-         if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
-     #endif
-     
--- 377 ----
***************




*********************************************************
*** types.h
*********************************************************


*** 45,53 ****
- 
-   #ifdef HEADFREE
-    BOXHEADFREE,
-   #endif
-   #ifdef HEADHOLD
-    BOXHEADHOLD,
-       BOXHEADADJ, // acquire heading for HEADFREE mode
-   #endif
-   
--- 44 ----
***************


*** 132 ****
-   uint8_t HEADHOLD_MODE :1 ;
--- 122 ----
***************

giiba · Post by **giiba** » Fri May 23, 2014 9:15 am

Tonight's progress involved cleaning up a bit and removing the need for define statements in config.h. Better yet the odd #if MAG around early parts of IMU.cpp have tested to be unnecessary. This is tested in the air and working for me.

Code: Select all

*********************************************
*** IMU.cpp ***HHHFHA_nMAG_diff_v1_0
--- IMU.cpp
*********************************************


*** 179,181 ****
- #else
-   static t_fp_vector EstN = { 1000.0, 0.0, 0.0 };
-   static t_int32_t_vector EstN32;
--- 178 ----
***************


*** 209,210 ****
-   #else
-     rotateV(&EstN.V,deltaGyroAngle);
--- 205 ----
***************


*** 225,226 ****
-     #else
-       EstN32.A[axis] = EstN.A[axis];
--- 219 ----
***************


*** 247,252 ****
-     #else
-       att.heading = _atan2(
-       EstN32.V.Z * EstG32.V.X - EstN32.V.X * EstG32.V.Z,
-       EstN32.V.Y * invG * sqGX_sqGZ  - (EstN32.V.X * EstG32.V.X + EstN32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y
-       ); 
-       att.heading /= 10;
--- 239 ----




*********************************************
*** MultiWii.cpp ***HHHFHA_nMAG_diff_v1_0
--- MultiWii.cpp
*********************************************


*** 62,65 ****
-   #else
-     "HEADHOLD;"
-     "HEADFREE;"
-     "HEADADJ;"
--- 61 ----
***************


*** 117,120 ****
-   #else
-     5, //"HEADHOLD;"
-     6, //"HEADFREE;"
-     7, //"HEADADJ;"
--- 112 ----
***************


*** 1042,1045 ****
-     #else
-      if (rcOptions[BOXHEADFREE] == 1) f.HEADFREE_MODE = 1; else f.HEADFREE_MODE = 0;
-      if (rcOptions[BOXHEADHOLD] == 1) f.HEADHOLD_MODE = 1; else f.HEADHOLD_MODE = 0;
-      if (rcOptions[BOXHEADADJ]) {headFreeModeHold = att.heading;}
--- 1033 ----
***************


*** 1163,1170 ****
- 
-   if(f.HEADHOLD_MODE)
-      { errorAngle = headFreeModeHold - att.heading;
-       if (errorAngle >  180) errorAngle -= 360;
-       if (errorAngle < -180) errorAngle += 360;
-       errorAngle = constrain(errorAngle,-100,100);
-       rcCommand[YAW] += (errorAngle*conf.pid[PIDMAG].P8) >> 4;
-         }
--- 1150 ----




*********************************************
*** Protocol.cpp ***HHHFHA_nMAG_diff_v1_0
--- Protocol.cpp
*********************************************


*** 387,390 ****
-      #else
-     if(f.HEADFREE_MODE) tmp       |= 1<<BOXHEADFREE;
-     if(f.HEADHOLD_MODE) tmp       |= 1<<BOXHEADHOLD;
-        if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
--- 386 ----
***************




*********************************************
*** types.h ***HHHFHA_nMAG_diff_v1_0
--- types.h
*********************************************


*** 49,52 ****
-   #else
-     BOXHEADHOLD,
-     BOXHEADFREE,
-     BOXHEADADJ,
--- 48 ----
***************


*** 127 ****
-   uint8_t HEADHOLD_MODE :1 ;
--- 122 ----
***************

e_lm_70 · Post by **e_lm_70** » Fri May 23, 2014 10:46 am

Good job.

Does this version now also include full resolution gyro all around ? Or only in the Z axis ?

I know that the original version was done by Cesco and then develop further from Benedikt ... but originally Cesco took the base from Baseflight ...

giiba · Post by **giiba** » Fri May 23, 2014 5:04 pm

Does this version now also include full resolution gyro all around ? Or only in the Z axis?

I think it has full resolution... I really do not understand the math but it sure looks to use X, Y, and Z variables in the formula. As far as I can see, the Benedikt code and the Goebish code are the the same just implemented differently. And the formulas are almost identical to the current mag hold formulas. Makes me wonder if I can't integrate this further and use the maghold mode instead of creating a headhold mode (types.h). For another time

I think the thing that needs doing is what Plüschi said:

In other words i want to project the N vector onto a plane defined by (perpendicular to) the G vector.

Though the math does seem to use the Gravity vector, EstG, or am I wrong on what that is? This I most certainly did not do myself; I wouldn't know how.

Sure it drifts, but on a micro quad it sure seems pretty good to me.

giiba · Post by **giiba** » Sat May 24, 2014 8:25 am

Another update. By integrating this code further I have simplified it further. I have redone "#if MAG" code to use the mag code as much as possible, only changing the GUI box name (HEADHOLD instead of MAG). On my way to this version I tested one that just changed the math with "#else" statements but otherwise just removed "#if MAG" tags.

Code: Select all

*********************************************
+++ IMU.cpp -- HHHF_nMAG_v3_1
--- IMU.cpp
*********************************************


*** 179,181 ****
+ #else
+   static t_fp_vector EstN = { 1000.0, 0.0, 0.0 };
+   static t_int32_t_vector EstN32;
--- 178 ----
***************


*** 209,210 ****
+   #else
+     rotateV(&EstN.V,deltaGyroAngle);
--- 205 ----
***************


*** 225,226 ****
+     #else
+       EstN32.A[axis] = EstN.A[axis];
--- 219 ----
***************


*** 247,252 ****
+   #else
+       att.heading = _atan2(
+       EstN32.V.Z * EstG32.V.X - EstN32.V.X * EstG32.V.Z,
+       EstN32.V.Y * invG * sqGX_sqGZ  - (EstN32.V.X * EstG32.V.X + EstN32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y
+       ); 
+       att.heading /= 10;
--- 239 ----
***************




*********************************************
+++ MultiWii.cpp -- HHHF_nMAG_v3_1
--- MultiWii.cpp
*********************************************


*** 60,62 ****
+   #else
+     "HEADHOLD;"
+   #endif
--- 59 ----
***************


*** 65 ****
+ 
--- 62 ----
-   #endif
***************


*** 114,116 ****
+   #else
+     5, //"HEADHOLD"
+   #endif
--- 110 ----
***************


*** 118 ****
--- 113 ----
-   #endif
***************


*** 1024,1027 ****
+     #else
+       if (rcOptions[BOXHEADHOLD]) {if (!f.MAG_MODE) {f.MAG_MODE = 1; magHold = att.heading;}}
+       else {f.MAG_MODE = 0;}
+     #endif
--- 1018 ----
***************


*** 1042 ****
--- 1034,1035 ----
-     #endif
-     
***************


*** 1168,1175 ****
+   #else
+     if(f.MAG_MODE) {
+       int16_t dif = att.heading - magHold;
+       if (dif <= - 180) dif += 360;
+       if (dif >= + 180) dif -= 360;
+       dif = constrain(dif,-100,100);
+       rcCommand[YAW] -= (dif*conf.pid[PIDMAG].P8) >> 5;
+      } else magHold = att.heading;
--- 1160 ----
***************




*********************************************
+++ Protocol.cpp -- HHHF_nMAG_v3_1
--- Protocol.cpp
*********************************************


*** 383,385 ****
+      #else
+        if(f.MAG_MODE) tmp |= 1<<BOXHEADHOLD;
+      #endif
--- 382 ----
***************


*** 389 ****
--- 387 ----
-      #endif
***************




*********************************************
+++ types.h -- HHHF_nMAG_v3_1
--- types.h
*********************************************


*** 47,49 ****
+   #else
+     BOXHEADHOLD,
+   #endif
--- 46 ----
***************


*** 51 ****
--- 49 ----
-   #endif
***************

e_lm_70 · Post by **e_lm_70** » Sat May 24, 2014 9:12 am

It sounds a very good job

I'm running MultiWii 2.3 on KK2.1 ... as a normal multiwii with just a 6050 on it.

I think I will adapt my port to you version of MultiWii ... it should be quite cool on KK2.1 board

Having more precise math, I do expect also the horizon mode should work better, that means easy loops for everybody ...

giiba · Post by **giiba** » Sat May 24, 2014 10:48 pm

I've now tried to merge this change into the 'dev_2014_01_14__r1648' build of MultiWii.

Most of it was easy enough, however the changes to IMU.cpp put this over my head I think. I have corrected the syntax of the equations to compile properly, and the build does fly fine; but I get no heading readout in the GUI and no headhold when selected.

I need someone out there who understands these changes to IMU.cpp to help at this point. I figure that I'm just missing some easy change but I have no idea what it is. I cleaned it up slightly but it's basically the same code as my last post so I'm baffled as to why there is no heading

for now this is what I've got:

Code: Select all

*********************************************
*** IMU.cpp -- dev_2014_01_14__r1648
+++ IMU.cpp -- v3_1_dev_merge
*********************************************


*** 218 ****
!   static t_int32_t_vector EstG,EstM;
--- 218 ----
!   static t_int32_t_vector EstG,EstM,EstN;
***************


*** 248 ****
--- 249,250 ----
+   #else
+     rotateV32(&EstN,deltaGyroAngle16);
***************


*** 259 ****
--- 262,263 ----
+     #else
+       EstN.A32[axis] = EstN.A16[axis];
***************


*** 280 ****
--- 285,289 ----
+   #else
+       att.heading = _atan2(
+       mul(EstN.V16.Z , EstG.V16.X) - mul(EstN.V16.X , EstG.V16.Z),
+       (EstN.V16.Y * invG * sqGX_sqGZ  - (mul(EstN.V16.X , EstG.V16.X)
+       + mul(EstN.V16.Z , EstG.V16.Z)) * invG)* EstG.V16.Y ); att.heading /= 10;
***************




*********************************************
*** MultiWii.cpp -- dev_2014_01_14__r1648
+++ MultiWii.cpp -- v3_1_dev_merge
*********************************************


*** 59 ****
--- 60,62 ----
+   #else
+     "HEADHOLD;"
+   #endif
***************


*** 62 ****
-   #endif
--- 64 ----
***************


*** 110 ****
--- 113,115 ----
+   #else
+     5, //"HEADHOLD"
+   #endif
***************


*** 113 ****
-   #endif
--- 117 ----
***************


*** 1027 ****
--- 1032,1035 ----
+     #else
+       if (rcOptions[BOXHEADHOLD]) {if (!f.MAG_MODE) {f.MAG_MODE = 1; magHold = att.heading;}}
+       else {f.MAG_MODE = 0;}
+     #endif
***************


*** 1043,1044 ****
-     #endif
-     
--- 1050 ----
***************


*** 1168 ****
--- 1175,1182 ----
+   #else
+     if(f.MAG_MODE)
+     { int16_t dif = att.heading - magHold;
+      if (dif <= - 180) dif += 360;
+      if (dif >= + 180) dif -= 360;
+      dif = constrain(dif,-100,100);
+      rcCommand[YAW] -= (dif*conf.pid[PIDMAG].P8) >> 5;
+       } else magHold = att.heading;
***************




*********************************************
*** Protocol.cpp -- dev_2014_01_14__r1648
+++ Protocol.cpp -- v3_1_dev_merge
*********************************************


*** 382 ****
--- 383,385 ----
+      #else
+        if(f.MAG_MODE) tmp |= 1<<BOXHEADHOLD;
+      #endif
***************


*** 387 ****
-      #endif
--- 389 ----
***************




*********************************************
*** types.h -- dev_2014_01_14__r1648
+++ types.h -- v3_1_dev_merge
*********************************************


*** 46 ****
--- 47,49 ----
+   #else
+     BOXHEADHOLD,
+   #endif
***************


*** 49 ****
-   #endif
--- 51 ----
***************

giiba · Post by **giiba** » Sat May 24, 2014 10:50 pm

A tangential question for anyone watching: are these files I'm uploading helpful for understanding what I'm doing? Is there a better way of presenting this info?

koenkooi · Post by **koenkooi** » Mon May 26, 2014 6:45 am

It would be best to base it against the latest multiwii: https://github.com/multiwii/multiwii-firmware
Using github will also make it a lot easier to do diffs and upload your changes. Zips files attached to forum posts is something for the 1990s

timecop · Post by **timecop** » Mon May 26, 2014 6:59 am

You're preaching to the choir.
We've already been through this whole "zip files in a forum" thing.

e_lm_70 · Post by **e_lm_70** » Mon May 26, 2014 8:05 am

giiba wrote:A tangential question for anyone watching: are these files I'm uploading helpful for understanding what I'm doing? Is there a better way of presenting this info?

I appreciate your sharing and your finding.

It is looking an excellent job.

My target is to put this on a KK2.1 ... on a normal multiwii board the 5883 chips is so cheap that is always better to use this chip for Yaw support.
So, I just download your file, and I will "process" them when I will have time to further on the KK2.1 project.

Even if ... due to possible MAG interference have have this more precise yaw control it could help for a more advanced check in case.

Anyhow ... other advantage is having a more precise gyro computation ... that can be useful in the acro world.

I think baseflight use already most of these functionalities, but it is on STM32 vs ATMega 8 bit

Plüschi · Post by **Plüschi** » Mon May 26, 2014 10:10 am

giiba wrote:are these files I'm uploading helpful

Your long text "code" inserts are annoying.

giiba wrote:this over my head I think

I agree. Removing some #ifdef's and thereby needlessly wasting ram aint cutting it.
What do you try to do what has not been done before?

giiba wrote:help at this point

An easy way starting from actual r1614 is:

- init "EstM.A16[3] = 1000;" the first time getEstimatedAttitude runs and there is NO MAG.
- always do the "rotateV32(&EstM,deltaGyroAngle16);" regardless of mag or not
- always do the "att.heading = _atan2( ... and following lines" regardless of mag or not

This way you get rid of some #ifdefs, do not increase ram-memory usage, and it works (i am running this).

o_lampe · Post by **o_lampe** » Tue May 27, 2014 8:55 am

...I have corrected the syntax of the equations to compile properly, and the build does fly fine; but I get no heading readout in the GUI and no headhold when selected.

You can't get a real heading readout without a mag-sensor. You could get something like a "delta"-heading value with some additional programming.

e_lm_70 · Post by **e_lm_70** » Tue May 27, 2014 9:06 am

o_lampe wrote:
...I have corrected the syntax of the equations to compile properly, and the build does fly fine; but I get no heading readout in the GUI and no headhold when selected.

You can't get a real heading readout without a mag-sensor. You could get something like a "delta"-heading value with some additional programming.

Unless you have a GPS, and do some further tricks in the firmware

Plüschi · Post by **Plüschi** » Tue May 27, 2014 3:19 pm

e_lm_70 wrote:
o_lampe wrote:You can't get a real heading readout without a mag-sensor

Unless you have a GPS, and do some further tricks in the firmware

Which i did. And i did test fly it. And it did work. And then it started to behave weirdly. The problem is described above, N vector drifts out of the horizontal plane. Someone has to invent the proper vector math for this. I wont put time in it unless this fundamental flaw gets attention.

giiba · Post by **giiba** » Thu May 29, 2014 12:20 am

Plüschi, I'm wondering if what you speak of about rotating has already been done with the changes to IMU.cpp there is the new variable accZ that seems to be used to correct the gyro axis. The dev code seems to track the heading in the GUI perfectly when the board is pitched/rolled 90° and then rotated around Gravity's axis; the 2.3 code starts giving glitchy readings at angles > 50° or so. I'm going to test this side by side with the old code soon.

I see what you mean about just using the current EstM variable and formulas. I find I still need to section off some of the MAG code, specifically:

Code: Select all

    #if MAG
       EstM.A32[axis]  += (int32_t)(imu.magADC[axis] - EstM.A16[2*axis+1])<<(16-GYR_CMPFM_FACTOR);
    #else
       EstM.A32[axis]  += (int32_t)(deltaGyroAngle16);
    #endif

and

Code: Select all

      #if (MAG)
      (att.heading += conf.mag_declination); // Set from GUI
      #endif

for the code to compile and work. But simpler still.

giiba · Post by **giiba** » Fri May 30, 2014 12:27 am

For those interested, I have figured out Github and created a fork to better share.

https://github.com/giiba/MultiWii--HH-H ... gnetometer

Code is in the air and flying.

edit: I changed the URL as I started a new fork to do the changes properly and fix the end of line change issues I was having. This time round it is as clean as possible

midelic · Post by **midelic** » Sat Jun 07, 2014 3:34 am

Don't think is correct to activate in def.h file
EXT_MOTOR_RANGE
and
EXT_MOTOR_32KHZ
Or one or the other..anyone can be used for brushed ,,different freq.

#if defined(HK_MICRO_MWC)
//
#define EXT_MOTOR_RANGE
#define EXT_MOTOR_32KHZ
//
#endif

Alexinparis · Post by **Alexinparis** » Mon Jun 09, 2014 10:53 pm

giiba wrote:I see what you mean about just using the current EstM variable and formulas. I find I still need to section off some of the MAG code, specifically:[code] #if MAG
EstM.A32[axis] += (int32_t)(imu.magADC[axis] - EstM.A16[2*axis+1])<<(16-GYR_CMPFM_FACTOR);
#else
EstM.A32[axis] += (int32_t)(deltaGyroAngle16);
#endif

Hi,

This portion of code is not valid.
This whole part should be removed in case of no mag config.
And EstM must be initialize with non null value.
For instance: EstM = {0,(int32_t)1<<24,0} => north is pointed first.

giiba · Post by **giiba** » Wed Jun 11, 2014 5:59 am

Alexinparis wrote:
giiba wrote:...

Hi,

This portion of code is not valid.
This whole part should be removed in case of no mag config.
And EstM must be initialize with non null value.
For instance: EstM = {0,(int32_t)1<<24,0} => north is pointed first.

I assume you mean this piece of code is invalid?

EstM.A32[axis] += (int32_t)(deltaGyroAngle16)

This was the change I stumbled upon. The MAG code is removed and though I do not understand what this is doing, with this line headhold features work. If I just remove the MAG code (EstM.A32[axis] += (int32_t)(imu.magADC[axis] - ...) the build compiles and the heading features do nothing when activated. Valid or not it works.

Midelic, I see what you mean. It seems EXT_MOTOR_RANGE just over rides other settings. And either one works... with testing I really see no differences between that and 32khz.

MultiWii

HEADHOLD and HEADFREE without a magnetometer

HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer

Re: HEADHOLD and HEADFREE without a magnetometer