Changing PID Values with a Pot

[alll]

Suggestion,

To save Aux inputs and let Aux input behave different when armed / unarmed:

Unarmed => Aux1 to select axis (ROLL/PIICH or YAW) => Aux2 to select the 3 "ku"-PID parameters to set => Throttle stick to set param value (0..100%) => Pitch stick down to save param value to eeprom
When throttle stick equal current value, blink led or buzzer.

Armed: Aux inputs as defined in the GUI

Manu

[obor]

to 4eprops: yes <first post edited>

[4eprops]

to 4eprops: yes <first post edited>

All seems ok on the bench.

I'll try asap at the field

Thanks Obor

[obor]

I made a new version with 4 more pots in order to use 12 channels total. RC must be PPM (SBUS does not work for me) to get 12 channels. Now, I can tune almost all in flight (gyro R+P), level, alti, pos, posR, navrR (oy yaw). Your RC should have enough pots of course to do this.

In multiwii.ino, at the end of annexcode():

Code: Select all

 // ROLL + PITCH
  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
  int kp = ku * 6 ;
  conf.P8[PIDROLL] = kp;
  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;

  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
  conf.D8[PIDPITCH] = conf.D8[PIDROLL];

  // LEVEL
  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDLEVEL] = kp;
  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;

  // Alti
  ku = ((rcData[AUX5] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDALT] = kp;
  conf.I8[PIDALT] = (kp << 1)/FREQ_TU;
  conf.D8[PIDALT] = (kp * FREQ_TU) >> 3;

  // Pos
  ku = ((rcData[AUX6] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDPOS] = kp;
  conf.I8[PIDPOS] = (kp << 1)/FREQ_TU;

  // PosR
  ku = ((rcData[AUX7] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDPOSR] = kp;
  conf.I8[PIDPOSR] = (kp << 1)/FREQ_TU;
  conf.D8[PIDPOSR] = (kp * FREQ_TU) >> 3;

  // NavR
  ku = ((rcData[AUX8] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDNAVR] = kp;
  conf.I8[PIDNAVR] = (kp << 1)/FREQ_TU;
  conf.D8[PIDNAVR] = (kp * FREQ_TU) >> 3;
  #endif
}

and also change the enum rc at the begin of Multiwii.ino with:

Code: Select all

enum rc {
  ROLL,
  PITCH,
  YAW,
  THROTTLE,
  AUX1,
  AUX2,
  AUX3,
  AUX4,
  AUX5,
  AUX6,
  AUX7,
  AUX8
};

[4eprops]

I made a new version with 4 more pots in order to use 12 channels total. RC must be PPM (SBUS does not work for me) to get 12 channels. Now, I can tune almost all in flight (gyro R+P), level, alti, pos, posR, navrR (oy yaw). Your RC should have enough pots of course to do this.

In multiwii.ino, at the end of annexcode():

Code: Select all

 // ROLL + PITCH
  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
  int kp = ku * 6 ;
  conf.P8[PIDROLL] = kp;
  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;

  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
  conf.D8[PIDPITCH] = conf.D8[PIDROLL];

  // LEVEL
  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDLEVEL] = kp;
  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;

  // Alti
  ku = ((rcData[AUX5] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDALT] = kp;
  conf.I8[PIDALT] = (kp << 1)/FREQ_TU;
  conf.D8[PIDALT] = (kp * FREQ_TU) >> 3;

  // Pos
  ku = ((rcData[AUX6] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDPOS] = kp;
  conf.I8[PIDPOS] = (kp << 1)/FREQ_TU;

  // PosR
  ku = ((rcData[AUX7] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDPOSR] = kp;
  conf.I8[PIDPOSR] = (kp << 1)/FREQ_TU;
  conf.D8[PIDPOSR] = (kp * FREQ_TU) >> 3;

  // NavR
  ku = ((rcData[AUX8] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDNAVR] = kp;
  conf.I8[PIDNAVR] = (kp << 1)/FREQ_TU;
  conf.D8[PIDNAVR] = (kp * FREQ_TU) >> 3;
  #endif
}

and also change the enum rc at the begin of Multiwii.ino with:

Code: Select all

enum rc {
  ROLL,
  PITCH,
  YAW,
  THROTTLE,
  AUX1,
  AUX2,
  AUX3,
  AUX4,
  AUX5,
  AUX6,
  AUX7,
  AUX8
};

I'm trying to read 12ch with my Futaba 8FG (14chans) and ezUHF (rx with new firmware 1.41) but it seems that I was not able to read all the channels (only first 8)
I've tried MULT & MULT2 on the T8

Suggestions?

[obor]

By default PPM signal out on the 8FG is 8 channels. 12Ch out can be configured in the trainer menu, but I am not sure it works; at least it does not between two 8FG.
Also, the multiconf does not show chanels above 8, but if you affect pot like I did, you should see related config values changing in the conf (after pushing read button).

[alll]

I made a new version with 4 more pots in order to use 12 channels total. RC must be PPM (SBUS does not work for me) to get 12 channels. Now, I can tune almost all in flight (gyro R+P), level, alti, pos, posR, navrR (oy yaw). Your RC should have enough pots of course to do this.

In multiwii.ino, at the end of annexcode():

Code: Select all

 // ROLL + PITCH
 ...
}

and also change the enum rc at the begin of Multiwii.ino with:

Code: Select all

enum rc {  ROLL,  PITCH,  YAW,  THROTTLE,  AUX1,  AUX2,  AUX3,  AUX4,  AUX5,  AUX6,  AUX7,  AUX8};

Could these calculations also be done in the GUI, freq and ku instead of PID values? Are the GUI values = program config. values? I suspect not, the formulas with the osc. frequency of 2Hz, P and I should have the same value, this is not the case in the GUI (P=3.3 I=0.045 for example)! Very confusing! What units are these GUI variables?
Since PID are related based on osc. freq, why not have them in %, all settable from 0-100%.

Could you define the osc. frequency, is this the frequency where I=0 and D=0 and P>0 where constant overshoot occurs and never stabilizes?

Thanks,
manu

[4eprops]

By default PPM signal out on the 8FG is 8 channels. 12Ch out can be configured in the trainer menu, but I am not sure it works; at least it does not between two 8FG.
Also, the multiconf does not show chanels above 8, but if you affect pot like I did, you should see related config values changing in the conf (after pushing read button).

Changed transmitter from T8FG to Turnigy 9XR and all works fine!!

12channels working with ezUHF + Turnigy 9XR + MultiWii 2.2

[obor]

Could these calculations also be done in the GUI, freq and ku instead of PID values? Are the GUI values = program config. values? I suspect not, the formulas with the osc. frequency of 2Hz, P and I should have the same value, this is not the case in the GUI (P=3.3 I=0.045 for example)! Very confusing! What units are these GUI variables?

In the classical PID formula, I is 1/I, so 0.025 means a value of 4

Since PID are related based on osc. freq, why not have them in %, all settable from 0-100%.

could be. I am not sure that frequency need so much to be changed.

Could you define the osc. frequency, is this the frequency where I=0 and D=0 and P>0 where constant overshoot occurs and never stabilizes?

Just a guess: set a P value < overshoot with D and I =0, and see how quick it vibrates, that could be the frequency.

[alll]

You mentioned/used
PID: 0.60Ku  2 Kp/Pu     KpPu/8

I found this?:
PID: 0.6 Ku 2/Pu Pu/8

Why do you multiply I and D by Kp(=0.60Ku)?

manu

[obor]

I found it here:
http://en.wikipedia.org/wiki/Ziegler%E2 ... ols_method

[alll]

Why do the GUI PID values don't reflect the mwii-code conf.P8,conf.I8,conf.D8 and other PID values?
To apply the Ziegler–Nichols tuning and others, this would greatly help!

manu

[obor]

I understand that conf.P8/I8/D8 values are correct, but the way they are displayed is not. There should be a reason, but i don't know it. May be someone can explain ?

[sorg]

I made a new version with 4 more pots in order to use 12 channels total. RC must be PPM (SBUS does not work for me) to get 12 channels. Now, I can tune almost all in flight (gyro R+P), level, alti, pos, posR, navrR (oy yaw). Your RC should have enough pots of course to do this.

In multiwii.ino, at the end of annexcode():

Code: Select all

 // ROLL + PITCH
  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
  int kp = ku * 6 ;
  conf.P8[PIDROLL] = kp;
  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;

  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
  conf.D8[PIDPITCH] = conf.D8[PIDROLL];

  // LEVEL
  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDLEVEL] = kp;
  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;

  // Alti
  ku = ((rcData[AUX5] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDALT] = kp;
  conf.I8[PIDALT] = (kp << 1)/FREQ_TU;
  conf.D8[PIDALT] = (kp * FREQ_TU) >> 3;

  // Pos
  ku = ((rcData[AUX6] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDPOS] = kp;
  conf.I8[PIDPOS] = (kp << 1)/FREQ_TU;

  // PosR
  ku = ((rcData[AUX7] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDPOSR] = kp;
  conf.I8[PIDPOSR] = (kp << 1)/FREQ_TU;
  conf.D8[PIDPOSR] = (kp * FREQ_TU) >> 3;

  // NavR
  ku = ((rcData[AUX8] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDNAVR] = kp;
  conf.I8[PIDNAVR] = (kp << 1)/FREQ_TU;
  conf.D8[PIDNAVR] = (kp * FREQ_TU) >> 3;
  #endif
}

and also change the enum rc at the begin of Multiwii.ino with:

Code: Select all

enum rc {
  ROLL,
  PITCH,
  YAW,
  THROTTLE,
  AUX1,
  AUX2,
  AUX3,
  AUX4,
  AUX5,
  AUX6,
  AUX7,
  AUX8
};

I'm trying to read 12ch with my Futaba 8FG (14chans) and ezUHF (rx with new firmware 1.41) but it seems that I was not able to read all the channels (only first 8)
I've tried MULT & MULT2 on the T8

Suggestions?

Did anyone tested this helper function ?
Good results ?
I understand that assumption is made that the oscillating frequency is 3Hz for most of us. Is that confirmed ?

[sorg]

Hey guys,

I too missed the tuning capability of ArduPilot when I switched back to MultiWii, so I wrote one. It's not integrated into the GUI, so requires uploading inbetween every parameter you want to tune, but it still makes tuning way easier. I'm no Arduino pro, so there's probably lots of room for improvement! Works with MW v2.2 (probably older versions too, but I haven't tested beyond r1349). Demo video: http://youtu.be/6WnYLCQv1hs.

First, add a UserCode.ino page:

<snip>

Hope this is useful to someone! And of course, I'm not responsible for anything bad that happens to you or your machine.

I tried this patch yesterday.
It has worrked like a charm. Congratulation for this simple but efficient solution for tuning the PID .
This deserve to be in ne next multiwii release (with some helper function in the GUI in order to quickly change the PID parameter to be modified by the pot).

[sorg]

(post edited)Hi,
I just tried the PID tuning together with the Ziegler-Nichols method on my quadri. I am pretty impressed, so far it works great. Before I was trying to tune by hand but never get good results.
One pot (aux3) is for for gyro roll and pitch (same values for both), and one pot for level (aux4). My original code use Div_int, my own implementation of int division for efficiency.

Multiwii.ino, at the end of the annexCode() function

Code: Select all

#if defined(DIAL_TUNING)
  #define FREQ_TU 3
  // use aux3 and aux 4
  // see Ziegler Nichols method
  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
  int kp = ku * 6 ;
  conf.P8[PIDROLL] = kp;
  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;

  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
  conf.D8[PIDPITCH] = conf.D8[PIDROLL];

  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDLEVEL] = kp;
  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;

  #endif

config.h at the end:

Code: Select all

#define DIAL_TUNING

Note: this is my first post here

I tested this patch too, and I am very impressed by the result of the Ziegler-Nichols algorithm for the GYRO mode.
The Freq_Tu=3 seems perfect for my 450mm quadX : In a few second , i have been able to find a sweet point. My quad is as stable as possible.

However, the result for the ANGLE mode is not as good. My guess is that the freq_Tu is quite different . I had to increase manually the I in order to have a better behaviour.

[Hman:)]

(post edited)Hi,
I just tried the PID tuning together with the Ziegler-Nichols method on my quadri. I am pretty impressed, so far it works great. Before I was trying to tune by hand but never get good results.
One pot (aux3) is for for gyro roll and pitch (same values for both), and one pot for level (aux4). My original code use Div_int, my own implementation of int division for efficiency.

Multiwii.ino, at the end of the annexCode() function

Code: Select all

#if defined(DIAL_TUNING)
  #define FREQ_TU 3
  // use aux3 and aux 4
  // see Ziegler Nichols method
  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
  int kp = ku * 6 ;
  conf.P8[PIDROLL] = kp;
  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;

  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
  conf.D8[PIDPITCH] = conf.D8[PIDROLL];

  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDLEVEL] = kp;
  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;

  #endif

config.h at the end:

Code: Select all

#define DIAL_TUNING

Note: this is my first post here

Hi, Do you think you could please send me a full default Multiwii.ino code with the above implemented?
I tried to copy paste the code but keep getting all sorts of errors, I'm still learning (:

Herman

[sorg]

(post edited)Hi,
I just tried the PID tuning together with the Ziegler-Nichols method on my quadri. I am pretty impressed, so far it works great. Before I was trying to tune by hand but never get good results.
One pot (aux3) is for for gyro roll and pitch (same values for both), and one pot for level (aux4). My original code use Div_int, my own implementation of int division for efficiency.

Multiwii.ino, at the end of the annexCode() function

Code: Select all

#if defined(DIAL_TUNING)
  #define FREQ_TU 3
  // use aux3 and aux 4
  // see Ziegler Nichols method
  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
  int kp = ku * 6 ;
  conf.P8[PIDROLL] = kp;
  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;

  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
  conf.D8[PIDPITCH] = conf.D8[PIDROLL];

  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDLEVEL] = kp;
  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;

  #endif

config.h at the end:

Code: Select all

#define DIAL_TUNING

Note: this is my first post here

Hi, Do you think you could please send me a full default Multiwii.ino code with the above implemented?
I tried to copy paste the code but keep getting all sorts of errors, I'm still learning (:

Herman

Hi Hermann,

This is the diff file of the changes I applied (over Multiwii 2.2) :

Code: Select all

diff -u -N MultiWii_stock/MultiWii.ino MultiWii_ZieglerN/MultiWii.ino
--- MultiWii_stock/MultiWii.ino   Sat May 11 12:18:51 2013
+++ MultiWii_ZieglerN/MultiWii.ino   Wed Jun  5 19:14:53 2013
@@ -687,6 +687,28 @@
       #endif
     #endif
   }
+  
+  #if defined(DIAL_TUNING)
+  #define FREQ_TU 3
+  // use aux3 and aux 4
+  // see Ziegler Nichols method
+  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
+  int kp = ku * 6 ;
+  conf.P8[PIDROLL] = kp;
+  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
+  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;
+
+  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
+  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
+  conf.D8[PIDPITCH] = conf.D8[PIDROLL];
+
+  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.P8[PIDLEVEL] = kp;
+  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
+  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;
+
+  #endif
 }
 
 void setup() {
diff -u -N MultiWii_stock/config.h MultiWii_ZieglerN/config.h
--- MultiWii_stock/config.h   Sat Jun  1 13:24:18 2013
+++ MultiWii_ZieglerN/config.h   Wed Jun  5 19:15:33 2013
@@ -1053,3 +1053,5 @@
 /*************************************************************************************************/
 /****           END OF CONFIGURABLE PARAMETERS                                                ****/
 /*************************************************************************************************/
+
+#define DIAL_TUNING
\ No newline at end of file

Worked fine for me.

[felixrising]

Do you have a video of this in action?

[Hman:)]

(post edited)Hi,
I just tried the PID tuning together with the Ziegler-Nichols method on my quadri. I am pretty impressed, so far it works great. Before I was trying to tune by hand but never get good results.
One pot (aux3) is for for gyro roll and pitch (same values for both), and one pot for level (aux4). My original code use Div_int, my own implementation of int division for efficiency.

Multiwii.ino, at the end of the annexCode() function

Code: Select all

#if defined(DIAL_TUNING)
  #define FREQ_TU 3
  // use aux3 and aux 4
  // see Ziegler Nichols method
  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
  int kp = ku * 6 ;
  conf.P8[PIDROLL] = kp;
  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;

  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
  conf.D8[PIDPITCH] = conf.D8[PIDROLL];

  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
  kp = ku * 6 ;
  conf.P8[PIDLEVEL] = kp;
  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;

  #endif

config.h at the end:

Code: Select all

#define DIAL_TUNING

Note: this is my first post here

Hi, Do you think you could please send me a full default Multiwii.ino code with the above implemented?
I tried to copy paste the code but keep getting all sorts of errors, I'm still learning (:

Herman

Hi Hermann,

This is the diff file of the changes I applied (over Multiwii 2.2) :

Code: Select all

diff -u -N MultiWii_stock/MultiWii.ino MultiWii_ZieglerN/MultiWii.ino
--- MultiWii_stock/MultiWii.ino   Sat May 11 12:18:51 2013
+++ MultiWii_ZieglerN/MultiWii.ino   Wed Jun  5 19:14:53 2013
@@ -687,6 +687,28 @@
       #endif
     #endif
   }
+  
+  #if defined(DIAL_TUNING)
+  #define FREQ_TU 3
+  // use aux3 and aux 4
+  // see Ziegler Nichols method
+  int ku = ((rcData[AUX3] -1000) << 1) >> 6 ; // * 20/640;
+  int kp = ku * 6 ;
+  conf.P8[PIDROLL] = kp;
+  conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
+  conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;
+
+  conf.P8[PIDPITCH] = conf.P8[PIDROLL]; 
+  conf.I8[PIDPITCH] = conf.I8[PIDROLL];
+  conf.D8[PIDPITCH] = conf.D8[PIDROLL];
+
+  ku = ((rcData[AUX4] -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.P8[PIDLEVEL] = kp;
+  conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
+  conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;
+
+  #endif
 }
 
 void setup() {
diff -u -N MultiWii_stock/config.h MultiWii_ZieglerN/config.h
--- MultiWii_stock/config.h   Sat Jun  1 13:24:18 2013
+++ MultiWii_ZieglerN/config.h   Wed Jun  5 19:15:33 2013
@@ -1053,3 +1053,5 @@
 /*************************************************************************************************/
 /****           END OF CONFIGURABLE PARAMETERS                                                ****/
 /*************************************************************************************************/
+
+#define DIAL_TUNING
\ No newline at end of file

Worked fine for me.

Still have no clue what to do with the code you gave me, what is a diff file?
I'm new to multiwii coming from the KK2. All I know so far is how to change settings in config.h and Multiwiiconfig.exe hence why I was asking if someone could copy paste the full default Multiwii.ino code with this mod implemented. I've already tried multiple times but keep getting multiple errors because I have no idea what i'm doing.
Again i'm totally new to coding so it all looks another language to me. Can anyone help?
Thanks!

[felixrising]

If you are new to MultiWii, and new to coding, this isn't for you. If you just want some help with tuning, start with the defaults, then increase P in 0.5 increments (you can step smaller if you want) for Pitch/Roll until you see some wobbling, then decrease P about 0.5. so the wobbles go away. Next, increase I value for Pitch/Roll until it holds the angle well after attitude changes / resists you moving the attitude of the quad/hex... it should not ring, ie it shouldn't oscillate after strong stick inputs.. back it off a little. Generally defaults work well. More here: http://www.multiwii.com/wiki/index.php?title=PID

NB: tuning should be done in Acro mode..

Have fun flying!

[felixrising]

@sorg, I'll do up a diff which includes some documentation including warnings and caveats of the procedure and configurable AUX pots and tuning frequency details in config.h. Importantly, the Ziegler-Nichols method mentions different values/calculations for use in different control types, note that the MW2.2 PID loop was not a fully traditional Control Type, which has/is changing with the new "correct" implementation by Alex.khoroshko (alexk). Also, the alexk changes the Level I value to be used for horizon or something (i'll need to check that).

Could you can make a video of the procedure in action on your multi-rotor that would also be a good idea, especially if it can be demonstrated on more than one multi-rotor type. I'll try it out and make a video too.

EDIT: Looking at the values used, the method used is for classic PID. I'd like to try the "some overshoot" and the "no overshoot" methods Control Type too.. and of course, as all these articles warn, this gets the system within ballpark values, some manual tuning is always required to get it really good. http://www.mstarlabs.com/control/znrule.html

Cheers and happy flying!

[sorg]

Hi Felix.

I'll try to do the video, but i have only a phone to do that... Do not expect high quality. I will not fly for two weeks, but as soon as I can , I will do the video.


I have not tried the AlexK New PID implementation. Have you had good result with it ?
I read the Alexk topic, and as far as I understant, the Angle mode is a P-only regulator. So ZN algorithm is not needed. The Pot will modifiy directly P.

[felixrising]

I'm still unclear about the Tu component, isn't the idea that soley Kp is adjusted (with Ki and Kd set to 0) to obtain an oscillation period Tu, and then Ki and Kd are calculated from Kp and Tu... so without first obtaining Tu accurately, it's all pretty much a big guess?

What we need is some code to detect the period Tu, and a pot to raise P to a sufficient value to introduce a fixed period and amplitude oscillation, then the whole routine could be fairly accurately calculated. One pot, one switch and done... anyone know some simple code to work out oscillation period of a multirotor?

BTW, I found this also informative explanation of tuning methods: http://www.chem.mtu.edu/~tbco/cm416/zn.html

[sorg]

I'm still unclear about the Tu component, isn't the idea that soley Kp is adjusted (with Ki and Kd set to 0) to obtain an oscillation period Tu, and then Ki and Kd are calculated from Kp and Tu... so without first obtaining Tu accurately, it's all pretty much a big guess?

You are right. To be precise, It would need to be able to measure the Tu first. and then to adjust all the coefficient based on the "real" Tu.
What I have seen is that 3Hz is a pretty good match for my 450mm Quad in Gyro Mode . And I guess, I will be good enough for quad fromm 330mm to 600mm...

But, I see no reason for Tu to be the same for Angle, Baro and GPS PID loops... I feel that If I have disapointing results with the Angle loop tuning thanks to Z-N , is that the Tu is not the same a all...

[felixrising]

Whilst we're talking about autotune.. -->> http://www.rcgroups.com/forums/showthread.php?t=1922423

[sorg]

Hi!

I have updated the ziegler-nichols tuning with pot patch in order to work with the latest multiwii_shared (r1538).
I have also foreseen additional parameters in config.h in order to select witch RC channel will tune which PID loop.

The Level controller is also now handled differently in "oldschool" controller (PID) and in AlexK controller (P-only).

Code: Select all

Index: MultiWii.cpp
===================================================================
--- MultiWii.cpp   (revision 1538)
+++ MultiWii.cpp   (working copy)
@@ -554,6 +554,64 @@
       #endif
     #endif
   }
+
+#if defined(DIAL_TUNING)
+
+  // see Ziegler Nichols method
+  #if defined(POT_ROLL)
+  int ku = ((rcData[POT_ROLL] -1000) << 1) >> 6 ; // * 20/640;
+  int kp = ku * 6 ;
+  conf.pid[PIDROLL].P8 = kp;
+  conf.pid[PIDROLL].I8 = (kp << 1)/FREQ_TU_ROLL;
+  conf.pid[PIDROLL].D8 = (kp * FREQ_TU_ROLL) >> 3;
+  #endif
+
+  #if defined(POT_PITCH)
+  ku = ((rcData[POT_PITCH] -1000) << 1) >> 6 ;
+  kp = ku * 6 ;
+  conf.pid[PIDPITCH].P8 = kp;
+  conf.pid[PIDPITCH].I8 = (kp << 1)/FREQ_TU_PITCH;
+  conf.pid[PIDPITCH].D8 = (kp * FREQ_TU_PITCH) >> 3;
+  #endif
+
+  #if defined(POT_LEVEL)
+   #if PID_CONTROLLER == 1 // odschool controller is PID
+  ku = ((rcData[POT_LEVEL] -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDLEVEL].P8 = kp;
+  conf.pid[PIDLEVEL].I8 = (kp << 1)/FREQ_TU_LEVEL;
+  conf.pid[PIDLEVEL].D8 = (kp * FREQ_TU_LEVEL) >> 3;
+   #elif PID_CONTROLLER == 2 //AlexK control is simple P
+  ku = ((rcData[POT_LEVEL] -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDLEVEL].P8 = kp;
+   #endif
+  #endif
+
+  #if defined(POT_BARO)
+  ku = ((rcData[POT_BARO] -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDALT].P8 = kp;
+  conf.pid[PIDALT].I8 = (kp << 1)/FREQ_TU_BARO;
+  conf.pid[PIDALT].D8 = (kp * FREQ_TU_BARO) >> 3;
+  #endif
+
+  #if defined(POT_MAG)      //P-only controller
+  ku = ((rcData[POT_MAG] -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDMAG].P8 = kp;
+  #endif
+
+  #if defined(POT_YAW)
+  ku = ((rcData[POT_YAW] -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDYAW].P8 = kp;
+  conf.pid[PIDYAW].I8 = (kp << 1)/FREQ_TU_YAW;
+  conf.pid[PIDYAW].D8 = (kp * FREQ_TU_YAW) >> 3;
+  #endif
+
+#endif
+
 }
 
 void setup() {
Index: config.h
===================================================================
--- config.h   (revision 1538)
+++ config.h   (working copy)

@@ -1079,6 +1080,24 @@
   /* disable use of the POWER PIN (allready done if the option RCAUXPIN12 is selected) */
   #define DISABLE_POWER_PIN
 
+  /**************************    Ziegler Nicholls PID Tuning     ******************************
+   * http://www.multiwii.com/forum/viewtopic.php?f=7&t=1701&start=30                          *
+   * http://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method                              *
+   * ******************************************************************************************/
+ #define DIAL_TUNING      // Activate PID tuning through Pots
+
+ #define FREQ_TU_ROLL 3      // PID proper frequency in Hz. 3hz is a good start for Roll and Pitch of medium size multi
+                     // Smaller multi : higer value and vice versa.
+ #define POT_ROLL AUX3      // Channel used to tune ROLL - Comment out to disable pot tuning for this PID
+ #define FREQ_TU_PITCH 3   // PID proper frequency in Hz: usually the same as ROLL
+ #define POT_PITCH AUX3      // Channel used to tune PITCH : usually the same as ROLL
+ #define FREQ_TU_LEVEL 3    // Need some experiment. This Frequency is not the same as ROLL & PITCH
+ #define POT_LEVEL AUX4     // Channel used to tune LEVEL
+ #define FREQ_TU_BARO 3       // Need some experiment. This Frequency is not the same as ROLL & PITCH
+ #define POT_BARO AUX5        // Channel used to tune BARO
+ #define POT_MAG AUX6        // Channel used to tune MAG
+ #define FREQ_TU_YAW 3       // Need some experiment. This Frequency is not the same as ROLL & PITCH
+ #define POT_YAW AUX7        // Channel used to tune YAW
 
 /*************************************************************************************************/
 /****           END OF CONFIGURABLE PARAMETERS                                                ****/

[felixrising]

Hi,

Nice job. i still have an issue with the frequency thing, without knowing the period of oscillation the I and D values are likely wrong... still need to get the system unstable enough to introduce an oscillation and measure it.

Have you looked at Bradquick's Auto PID Tuning stuff? http://www.rcgroups.com/forums/showthread.php?t=1922423

His BradWii implementation has it, worth trying it out..

[sorg]

Hi,

Nice job. i still have an issue with the frequency thing, without knowing the period of oscillation the I and D values are likely wrong... still need to get the system unstable enough to introduce an oscillation and measure it.

Same thing here... and I am a bit concerned about making my quad delibarately unstable while in the air...
However, my experience is that:
- for Pitch and roll, 3Hz is a good starting point for my HK450 frame.
- For Level PID, With Alek , there is no need to tune freq_tu as it is a simple P controller.
- For Mag PID, there is no need to tune freq_tu as it is a simple P controller.

So the main remaining parts are:
- Baro: I have not tested the tuning with ZN yet. But I will do this week end.
- GPS PIDs: I am not sure ZN will be great for this as the feedback period is probably quite long. Anyway, I am pretty sure that freq_tu is very low for these loop... maybe a fraction of Hertz... and I don't think the algortihm will work with that. Tha'ts why I have not included GPS tuning in the patch submitted above.

Have you looked at Bradquick's Auto PID Tuning stuff? http://www.rcgroups.com/forums/showthread.php?t=1922423

His BradWii implementation has it, worth trying it out..

Yes I saw it...
I know the theory of autotuning and used it for some PID tuning for temperature control... But when it comes to do the same with flying mechanics... hmmm I'm less confident...
However, I will have a look to bradwii code and see if ther is some way to port it back to Multiwii.




EDIT: hmmm , just saw its code. It's neat and well organized in Cpp/h files.
porting it to mutliwii is probably doable by someone knowing well the Multiwii code as the variables don't look the same in Bradwii and multiwii.

[sorg]

So,

I had a look to Brad wii code.
First general comment:
Its code is very well documented, very clean and welle organized. easy to understand, even for a poor programmer like me.
(to be honest, his code is a lot cleaner than Multiwii...)

A lot of thing have evolved between multiwii and bradwii, and i am not sure it is doable to port quickly his autotuning function:

Here is where i am :

Code: Select all

Index: src/MultiWii/lib_fp.h
===================================================================
--- src/MultiWii/lib_fp.h   (revision 0)
+++ src/MultiWii/lib_fp.h   (revision 0)
@@ -0,0 +1,77 @@
+#ifndef LIB_FP_H_
+#define LIB_FP_H_
+/*
+Copyright 2013 Brad Quick
+
+THIS LIBRARY IS DIRECTLY IMPORTED FROM BRADQUICK CODE:
+https://github.com/bradquick/bradwii_arduino/blob/master/bradwii/lib_fp.cpp
+revision date July 11, 2013
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+// a fixedpointnum is a real number that's stored in a long that's shifted left FIXEDPOINTSHIFT bits
+// FIXEDPOINTSHIFT is currently 16, so 16 bits are used for the integer part of the number and
+// the other 16 bits are used for the fractional part of the number.  I've tried using different
+// values, but the compliler likes shifting things by 16 bits (words at a time) so things run
+// faster with a FIXEDPOINTSHIFT of 16.
+
+// Therefore, the range of a fixedpointnum is -32768.0 to 32786.0 with an accuracy of 0.000015
+// There is no overflow or underflow protection, so it's up to the programmer to watch out.
+
+#define fixedpointnum long
+
+#define FIXEDPOINTSHIFT 16
+
+#define FIXEDPOINTONE (1L<<FIXEDPOINTSHIFT)
+
+#define FIXEDPOINTCONSTANT(number) ((fixedpointnum)(number*FIXEDPOINTONE))
+
+#define FIXEDPOINT45 (45L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT80 (80L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT90 (90L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT135 (135L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT180 (180L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT360 (360L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINTTWO (2L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINTTHREE (3L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINTONEOVERONE FIXEDPOINTONE
+#define FIXEDPOINTONEOVERONEHALF FIXEDPOINTTWO
+#define FIXEDPOINTONEOVERTWO (FIXEDPOINTONE>>1)
+#define FIXEDPOINTONEOVERFOUR (FIXEDPOINTONE>>2)
+#define FIXEDPOINTONEOVERONEFOURTH (FIXEDPOINTONE<<2)
+#define FIXEDPOINTONEOVERONESIXTEENTH (FIXEDPOINTONE<<4)
+#define FIXEDPOINTONEOVERONESIXTYITH (60L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINTONEFIFTIETH ((1L<<FIXEDPOINTSHIFT)/50)
+
+#define FIXEDPOINTPIOVER180 1144L // pi/180 for converting degrees to radians
+
+// since time slivers can be very small, we shift them an extra 8 bits to maintain accuracy
+#define TIMESLIVEREXTRASHIFT 8
+
+void lib_fp_constrain(fixedpointnum *lf,fixedpointnum low,fixedpointnum high);
+void lib_fp_constrain180(fixedpointnum *lf);
+fixedpointnum lib_fp_multiply(fixedpointnum x,fixedpointnum y);
+void lib_fp_lowpassfilter(fixedpointnum *variable,fixedpointnum newvalue,fixedpointnum timesliver,fixedpointnum oneoverperiod,int timesliverextrashift);
+fixedpointnum lib_fp_abs(fixedpointnum fp);
+fixedpointnum lib_fp_sine(fixedpointnum angle);
+fixedpointnum lib_fp_cosine(fixedpointnum angle);
+fixedpointnum lib_fp_atan2(fixedpointnum y, fixedpointnum x);
+fixedpointnum lib_fp_sqrt(fixedpointnum x);
+fixedpointnum lib_fp_stringtofixedpointnum(char *string);
+fixedpointnum lib_fp_invsqrt(fixedpointnum x);
+long lib_fp_stringtolong(char *string);
+
+#endif /* LIB_FP_H_ */
Index: src/MultiWii/config.h
===================================================================
--- src/MultiWii/config.h   (revision 1538)
+++ src/MultiWii/config.h   (working copy)
@@ -1079,6 +1079,14 @@
   /* disable use of the POWER PIN (allready done if the option RCAUXPIN12 is selected) */
   #define DISABLE_POWER_PIN
 
+// un-comment if you don't want to include autotune code
+//#define NO_AUTOTUNE
+// To adjust how agressive the tuning is, adjust the AUTOTUNEMAXOSCILLATION value.  A larger
+// value will result in more agressive tuning. A lower value will result in softer tuning.
+// It will rock back and forth between -AUTOTUNE_TARGET_ANGLE and AUTOTUNE_TARGET_ANGLE degrees
+#define AUTOTUNE_MAX_OSCILLATION 4.0
+#define AUTOTUNE_TARGET_ANGLE 20.0
+
 
 /*************************************************************************************************/
 /****           END OF CONFIGURABLE PARAMETERS                                                ****/
Index: src/MultiWii/autotune.cpp
===================================================================
--- src/MultiWii/autotune.cpp   (revision 0)
+++ src/MultiWii/autotune.cpp   (revision 0)
@@ -0,0 +1,195 @@
+/*
+Copyright 2013 Brad Quick
+
+THIS LIBRARY IS PORTED FROM BRADQUICK CODE:
+https://github.com/bradquick/bradwii_arduino/blob/master/bradwii/autotune.cpp
+revision date July 11, 2013
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+// The theory behind auto tuning is that we generate a step in the setpoint, then analyze how the system reacts.
+// First, we set the I term to zero and work on only the P and D terms.
+// The target angle alternates from negative to posive FPAUTOTUNETARGETANGLE degrees.  We set the target, then wait until
+// we cross the zero angle, just to make sure our angular velocity is in the right direction, then we look for the peak
+// which we recognize by observing the angular velocity switching direction.
+// Our goal is to get zero overshoot.  By requiring zero overshoot, we can assume that any subsequent oscillations are
+// caused by our D term, since at zero overshoot, the P term doesn't have any effect.  Once we see the first peak, we then
+// start a timer and look for a second peak in the negative direction.  The difference between the first and second peak will
+// tell us the effect the D term is having.  We will then try to keep this oscillation amplitude within a predeterrmined range.
+//
+// So, after we reach the target, then wait a little longer to record the peak oscillation, we have four choices:
+// if (overshoot)
+//      {
+//      if (too much oscillation) decrease P
+//      else increase D
+//      }
+//   else // undershoot
+//      {
+//      if (too much oscillation) decrease D
+//      else increase P
+//      }
+
+
+#include "config.h"
+//#include "bradwii.h"
+#include "eeprom.h"
+#include "autotune.h"
+
+//extern globalstruct global;
+//extern usersettingsstruct usersettings;
+
+#ifndef NO_AUTOTUNE
+
+#define FPAUTOTUNEMAXOSCILLATION FIXEDPOINTCONSTANT(AUTOTUNE_MAX_OSCILLATION)
+#define FPAUTOTUNETARGETANGLE FIXEDPOINTCONSTANT(AUTOTUNE_TARGET_ANGLE)
+
+unsigned char rising;
+unsigned char sawfirstpeak;
+int autotuneindex=ROLL;
+fixedpointnum autotunetime;
+fixedpointnum autotunepeak1;
+fixedpointnum autotunepeak2;
+fixedpointnum targetangle=0;
+fixedpointnum targetangleatpeak;
+fixedpointnum currentivalue;
+
+char cyclecount=1;
+
+void autotune(fixedpointnum *angleerror,unsigned char startingorstopping)
+   {
+   if (!f.ARMED)
+      {
+      // we aren't armed.  Don't do anything, but if autotuning is started and we have collected
+      // autotuning data, save our settings to eeprom
+      if (startingorstopping==AUTOTUNESTARTING && targetangle!=0)
+         writeusersettingstoeeprom(); // TODO: COMPLETE REWRITING....
+
+      return;
+      }
+
+   if (startingorstopping==AUTOTUNESTOPPING)
+      {
+     conf.pid[autotuneindex].I8=currentivalue;
+
+     conf.pid[YAW].I8=conf.pid[ROLL].I8;
+      conf.pid[YAW].D8=conf.pid[ROLL].D8;
+      conf.pid[YAW].P8=lib_fp_multiply(conf.pid[ROLL].P8,YAWGAINMULTIPLIER);
+
+      autotuneindex=!autotuneindex; // alternate between roll and pitch
+      return;
+      }
+
+   if (startingorstopping==AUTOTUNESTARTING)
+      {
+      currentivalue=conf.pid[autotuneindex].I8;
+      conf.pid[autotuneindex].I8=0;
+      cyclecount=1;
+      sawfirstpeak=0;
+      rising=0;
+      targetangle=-FPAUTOTUNETARGETANGLE;
+      }
+   else
+      {
+      if (!sawfirstpeak)
+         {
+         if ((rising && att.angle[autotuneindex]>0 && global.gyrorate[autotuneindex]<0) //TO CHECK : att.angle[autotuneindex] a value in degrees is expected... However not 100% sure it is the case. To be checked.
+            || (!rising && att.angle[autotuneindex]<0 && global.gyrorate[autotuneindex]>0)) // TODO : global.gyrorate[] : should be angular speed in degree/second
+            {
+            if (cyclecount==0) // we are checking the I value
+               {
+               autotunepeak1=att.angle[autotuneindex]-targetangle;
+               if (!rising) autotunepeak1=-autotunepeak1;
+
+               if (autotunepeak1<(FPAUTOTUNEMAXOSCILLATION>>1))
+                  currentivalue=lib_fp_multiply(currentivalue,AUTOTUNEINCREASEMULTIPLIER);
+               else
+                  {
+                  currentivalue=lib_fp_multiply(currentivalue,AUTOTUNEDECREASEMULTIPLIER);
+                  if (currentivalue<AUTOTUNEMINIMUMIVALUE) currentivalue=AUTOTUNEMINIMUMIVALUE;
+                  }
+
+               // go back to checking P and D
+               cyclecount=1;
+               rising=!rising;
+               }
+            else // we are checking P and D values
+               {
+               targetangleatpeak=targetangle;
+               autotunepeak2=autotunepeak1=att.angle[autotuneindex];
+
+               sawfirstpeak=1;
+               autotunetime=0;
+               conf.pid[autotuneindex].I8=0;
+               }
+            }
+         }
+      else // we saw the first peak. looking for the second
+         {
+         if ((rising && att.angle[autotuneindex]<autotunepeak2)
+            || (!rising && att.angle[autotuneindex]>autotunepeak2))
+            autotunepeak2=att.angle[autotuneindex];
+
+         autotunetime+=(cycleTime*4295L)>>(FIXEDPOINTSHIFT-TIMESLIVEREXTRASHIFT);
+
+         if (autotunetime>AUTOTUNESETTLINGTIME)
+            {
+            if (!rising)
+               {
+               autotunepeak1=-autotunepeak1;
+               autotunepeak2=-autotunepeak2;
+               targetangleatpeak=-targetangleatpeak;
+               }
+
+            fixedpointnum oscillationamplitude=autotunepeak1-autotunepeak2;
+
+            // analyze the data
+            // Our goal is to have zero overshoot and to have AUTOTUNEMAXOSCILLATION amplitude
+            if (autotunepeak1>targetangleatpeak) // overshoot
+               {
+               if (oscillationamplitude>FPAUTOTUNEMAXOSCILLATION) // we have too much oscillation, so we can't increase D, so decrease P
+                 conf.pid[autotuneindex].P8=lib_fp_multiply(conf.pid[autotuneindex].P8, AUTOTUNEDECREASEMULTIPLIER);
+               else // we don't have too much oscillation, so we can increase D
+                 conf.pid[autotuneindex].D8=lib_fp_multiply(conf.pid[autotuneindex].D8, AUTOTUNEINCREASEMULTIPLIER);
+               }
+            else // undershoot
+               {
+               if (oscillationamplitude>FPAUTOTUNEMAXOSCILLATION) // we have too much oscillation, so we should lower D
+                 conf.pid[autotuneindex].D8=lib_fp_multiply(conf.pid[autotuneindex].D8, AUTOTUNEDECREASEMULTIPLIER);
+               else // we don't have too much oscillation, so we increase P
+                 conf.pid[autotuneindex].P8=lib_fp_multiply(conf.pid[autotuneindex].P8, AUTOTUNEINCREASEMULTIPLIER);
+               }
+
+            // switch to the other direction and start a new cycle
+            rising=!rising;
+            sawfirstpeak=0;
+
+            if (++cyclecount==3)
+               { // switch to testing I value
+               cyclecount=0;
+
+               conf.pid[autotuneindex].I8=currentivalue;
+               }
+            }
+         }
+      }
+
+   if (rising) targetangle=global.rxvalues[autotuneindex]*20L+FPAUTOTUNETARGETANGLE; // TODO : global.rxvalues[autotuneindex] is The values of the RX inputs, ranging from -1.0 to 1.0
+   else targetangle=global.rxvalues[autotuneindex]*20L-FPAUTOTUNETARGETANGLE;
+
+   // override the angle error for the axis we are tuning
+   angleerror[autotuneindex]=targetangle-att.angle[autotuneindex];
+   }
+
+#endif
Index: src/MultiWii/autotune.h
===================================================================
--- src/MultiWii/autotune.h   (revision 0)
+++ src/MultiWii/autotune.h   (revision 0)
@@ -0,0 +1,48 @@
+#ifndef AUTOTUNE_H_
+#define AUTOTUNE_H_
+
+/*
+Copyright 2013 Brad Quick
+
+THIS LIBRARY IS PORTED FROM BRADQUICK CODE:
+https://github.com/bradquick/bradwii_arduino/blob/master/bradwii/autotune.h
+revision date July 11, 2013
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "lib_fp.h"
+
+
+#define AUTOTUNESTARTING 1
+#define AUTOTUNESTOPPING 2
+#define AUTOTUNETUNING 0
+
+#define AUTOTUNEGOINGTOWARDTARGET 0
+#define AUTOTUNEGOINGAWAYFROMTARGET 1
+
+#define AUTOTUNESETTLINGTIME (FIXEDPOINTCONSTANT(.25)<<TIMESLIVEREXTRASHIFT) // 1/4 second
+
+#define AUTOTUNEINCREASEMULTIPLIER FIXEDPOINTCONSTANT(1.05)
+#define AUTOTUNEDECREASEMULTIPLIER FIXEDPOINTCONSTANT(.95)
+#define AUTOTUNEMINIMUMIVALUE (1L<<3)
+
+#define YAWGAINMULTIPLIER FIXEDPOINTCONSTANT(2.0) // yaw p gain is simply set to a multiple of the roll p gain
+
+void autotune(fixedpointnum *angleerror,unsigned char startingorstopping);
+
+
+
+#endif /* AUTOTUNE_H_ */
Index: src/MultiWii/lib_fp.cpp
===================================================================
--- src/MultiWii/lib_fp.cpp   (revision 0)
+++ src/MultiWii/lib_fp.cpp   (revision 0)
@@ -0,0 +1,444 @@
+/*
+Copyright 2013 Brad Quick
+
+THIS LIBRARY IS DIRECTLY IMPORTED FROM BRADQUICK CODE:
+https://github.com/bradquick/bradwii_arduino/blob/master/bradwii/lib_fp.cpp
+revision date July 11, 2013
+
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "lib_fp.h"
+
+void lib_fp_constrain(fixedpointnum *lf,fixedpointnum low,fixedpointnum high)
+   {
+   if (*lf<low) *lf=low;
+   else if (*lf>high) *lf=high;
+   }
+
+// returns the equivilent angle within the range -180 and 180 degrees
+void lib_fp_constrain180(fixedpointnum *lf)
+   {
+   while (*lf<-FIXEDPOINT180) *lf+=FIXEDPOINT360;
+   while (*lf>FIXEDPOINT180) *lf-=FIXEDPOINT360;
+   }
+
+//             D1 C1 B1 A1 (x)
+//              x D2 C2 B2 A2 (y)
+// -----------------------
+// z0 y0 D0 C0 B0 A0 x0 w0
+
+
+// A2*A1 -> w0,x0 (skip)
+// A2*B1 -> x0,A0 (unsigned, unsigned)
+// A2*C1 -> A0,B0 (unsigned, unsigned) // do first
+// A2*D1 -> B0,C0 (unsigned, signed)
+// B2*A1 -> x0,A0 (unsigned, unsigned)
+// B2*B1 -> A0,B0 (unsigned, unsigned)
+// B2*C1 -> B0,C0 (unsigned, unsigned)
+// B2*D1 -> C0,D0 (unsigned, signed)
+// C2*A1 -> A0,B0 (unsigned, unsigned)
+// C2*B1   -> B0,C0 (unsigned, unsigned)
+// C2*C1 -> C0,D0 (unsigned, unsigned) // do second
+// C2*D1 -> D0,y0 (unsigned, signed)
+// D2*A1 -> B0,C0 (signed, unsigned)
+// D2*B1 -> C0,D0 (signed, unsigned)
+// D2*C1 -> D0,y0 (signed, unsigned)
+// D2*D1 -> y0,z0 (skip)
+
+
+// A2*C1 -> A0,B0 (unsigned, unsigned) // do first
+// C2*C1 -> C0,D0 (unsigned, unsigned)  // do second
+// B2*C1 -> B0,C0 (unsigned, unsigned)
+// D2*C1 -> D0,y0 (signed, unsigned)
+// B2*A1 -> x0,A0 (unsigned, unsigned)
+// C2*A1 -> A0,B0 (unsigned, unsigned)
+// D2*A1 -> B0,C0 (signed, unsigned)
+// A2*B1 -> x0,A0 (unsigned, unsigned)
+// B2*B1 -> A0,B0 (unsigned, unsigned)
+// C2*B1   -> B0,C0 (unsigned, unsigned)
+// D2*B1 -> C0,D0 (signed, unsigned)
+// A2*D1 -> B0,C0 (unsigned, signed)
+// B2*D1 -> C0,D0 (unsigned, signed)
+// C2*D1 -> D0,y0 (unsigned, signed)
+
+//#define FPPROCESSORDOESNTSUPPORTMULSU
+#ifndef FPPROCESSORDOESNTSUPPORTMULSU
+
+fixedpointnum lib_fp_multiply(fixedpointnum x,fixedpointnum y)
+   { // multiplies two fixed point numbers without overflowing and returns the result.
+   // doing this in assembly increased the speed tremendously.
+   fixedpointnum result;
+
+   asm volatile (
+      "clr r26 \n\t"
+      "mov r16,%C1 \n\t"
+      "mul %A2, r16 \n\t" // A2*C1 -> A0,B0 (unsigned, unsigned) // do first
+      "movw %A0, r0 \n\t"
+      "mul %C2, r16 \n\t" // C2*C1 -> C0,D0 (unsigned, unsigned)  // do second
+      "movw %C0, r0 \n\t"
+      "mul %B2, r16 \n\t" // B2*C1 -> B0,C0 (unsigned, unsigned)
+      "add %B0, r0 \n\t"
+      "adc %C0, r1 \n\t"
+      "adc %D0, r26 \n\t"
+      "mulsu %D2, r16 \n\t" // D2*C1 -> D0,y0 (signed, unsigned)
+      "add %D0, r0 \n\t"
+      "mov r16,%A1 \n\t"
+      "mul %B2, r16 \n\t"   // B2*A1 -> x0,A0 (unsigned, unsigned)
+      "add %A0, r1 \n\t"
+      "adc %B0, r26 \n\t"
+      "adc %C0, r26 \n\t"
+      "adc %D0, r26 \n\t"
+      "mul %C2, r16 \n\t"   // C2*A1 -> A0,B0 (unsigned, unsigned)
+      "add %A0, r0 \n\t"
+      "adc %B0, r1 \n\t"
+      "adc %C0, r26 \n\t"
+      "adc %D0, r26 \n\t"
+      "mulsu %D2, r16 \n\t"   // D2*A1 -> B0,C0 (signed, unsigned)
+      "sbc %D0, r26 \n\t"
+      "add %B0, r0 \n\t"
+      "adc %C0, r1 \n\t"
+      "adc %D0, r26 \n\t"
+      "mov r16,%B1 \n\t"
+      "mul %A2, r16 \n\t"   // A2*B1 -> x0,A0 (unsigned, unsigned)
+      "add %A0, r1 \n\t"
+      "adc %B0, r26 \n\t"
+      "adc %C0, r26 \n\t"
+      "adc %D0, r26 \n\t"
+      "mul %B2, r16 \n\t"   // B2*B1 -> A0,B0 (unsigned, unsigned)
+      "add %A0, r0 \n\t"
+      "adc %B0, r1 \n\t"
+      "adc %C0, r26 \n\t"
+      "adc %D0, r26 \n\t"
+      "mul %C2, r16 \n\t"   // C2*B1   -> B0,C0 (unsigned, unsigned)
+      "add %B0, r0 \n\t"
+      "adc %C0, r1 \n\t"
+      "adc %D0, r26 \n\t"
+      "mulsu %D2, r16 \n\t"   // D2*B1 -> C0,D0 (signed, unsigned)
+      "add %C0, r0 \n\t"
+      "adc %D0, r1 \n\t"
+      "mov r16,%D1 \n\t"
+      "mulsu r16, %A2 \n\t"   // A2*D1 -> B0,C0 (unsigned, signed)
+      "sbc %D0, r26 \n\t"
+      "add %B0, r0 \n\t"
+      "adc %C0, r1 \n\t"
+      "adc %D0, r26 \n\t"
+      "mulsu r16, %B2 \n\t"   // B2*D1 -> C0,D0 (unsigned, signed)
+      "add %C0, r0 \n\t"
+      "adc %D0, r1 \n\t"
+      "mulsu r16, %C2 \n\t"   // C2*D1 -> D0,y0 (unsigned, signed)
+      "add %D0, r0 \n\t"
+
+      "clr r1 \n\t"
+      :
+      "=&r" (result)
+      :
+      "r" (x),
+      "a" (y)
+      :
+      "r26","r16"
+      );
+
+   return(result);
+   }
+
+#else // this will work on all processors, but it's slower
+fixedpointnum lib_fp_multiply(fixedpointnum x,fixedpointnum y)
+   { // multiplies two fixed point numbers without overflowing and returns the result
+   long xh=x>>FIXEDPOINTSHIFT;
+   unsigned long xl=x & 0xffff;
+   long yh=y>>FIXEDPOINTSHIFT;
+   unsigned long yl=y & 0xffff;
+   return((xh*yh)<<FIXEDPOINTSHIFT)+xh*yl+yh*xl+((xl*yl)>>FIXEDPOINTSHIFT);
+   }
+#endif
+
+
+void lib_fp_lowpassfilter(fixedpointnum *variable,fixedpointnum newvalue,fixedpointnum timesliver,fixedpointnum oneoverperiod,int timesliverextrashift)
+   { // updates a low pass filter variable.  It uses timesliver.  oneoverperiod is one over the time period
+   // over which the filter is averaged.  We use oneoverperiod to avoid having to use division.
+   // this does the equivilent of:
+   //    float fraction=timesliver/period;
+   //    variable=fraction*newvalue+(1-fraction)*variable;
+   // except it does it using fixed point arithmatic
+   // If timesliver is very small, resolution can be gained by keeping timesliver shifted left by some extra bits.
+   // Make sure variable can be also shifted left this same number of bits or else the following will overflow!
+   fixedpointnum fraction=lib_fp_multiply(timesliver,oneoverperiod);
+
+   *variable=(lib_fp_multiply(fraction,newvalue)+lib_fp_multiply((FIXEDPOINTONE<<timesliverextrashift)-fraction,*variable))>>timesliverextrashift;
+
+   // the adder combines adding .5 for rounding error and adding .5 in the direction the newvalue is trying to pull us
+   // So we can zero in on the desired value.
+   if (newvalue>*variable) ++*variable;
+   }
+
+fixedpointnum lib_fp_abs(fixedpointnum fp)
+   {
+   if (fp<0) return(-fp);
+   else return(fp);
+   }
+
+// we may be able to make these unsigned ints and save space
+fixedpointnum biganglesinelookup[]={ // every 8 degrees
+                                    0, // sine(0)
+                                    9121, // sine(8)
+                                    18064, // sine(16)
+                                    26656, // sine(24)
+                                    34729, // sine(32)
+                                    42126,
+                                    48703,
+                                    54332,
+                                    58903,
+                                    62328,
+                                    64540,
+                                    65496,
+                                    };
+
+fixedpointnum biganglecosinelookup[]={ // every 8 degrees
+                                    65536, // cosine(0)
+                                    64898, // cosine(8)
+                                    62997, // cosine(16)
+                                    59870, // cosine(32)
+                                    55578,
+                                    50203,
+                                    43852,
+                                    36647,
+                                    28729,
+                                    20252,
+                                    11380,
+                                    2287
+                                    };
+
+fixedpointnum lib_fp_sine(fixedpointnum angle)
+   { // returns sine of angle where angle is in degrees
+   // manipulate so that we only have to work in a range from 0 to 90 degrees
+   char negate=0;
+
+   while (angle>FIXEDPOINT180)
+      angle-=FIXEDPOINT360;
+   while (angle<-FIXEDPOINT180)
+      angle+=FIXEDPOINT360;
+
+   if (angle<0)
+      {
+      angle=-angle;
+      negate=!negate;
+      }
+   if (angle>FIXEDPOINT90)
+      {
+      angle=FIXEDPOINT180-angle;
+      }
+
+   // below works from 0 to 90 degrees
+   int bigangle=angle>>(3+FIXEDPOINTSHIFT); // big angle is angle/8
+   angle-=((long)bigangle)<<(3+FIXEDPOINTSHIFT); // the small angle remainder
+
+   // sine(x+y)=cosine(x)*sine(y)+sine(x)*cosine(y);
+   // for small angles, sine(x)=x where x is in radians
+   // and cosine of x=1 for small angles
+   fixedpointnum result=lib_fp_multiply(biganglecosinelookup[bigangle],(angle*143)>>13)+biganglesinelookup[bigangle];
+   if (negate) result=-result;
+   return(result);
+   }
+
+fixedpointnum lib_fp_cosine(fixedpointnum angle)
+   {
+   return(lib_fp_sine(angle+FIXEDPOINT90));
+   }
+
+fixedpointnum atanlist[]=
+   {
+   2949120L, // atan(2^-i), in fixedpointnum
+   1740967L,
+   919879L,
+   466945L,
+   234379L,
+   117304L,
+   58666L,
+   29335L,
+   14668L,
+   7334L,
+   3667L,
+   1833L,
+   917L
+   };
+
+// cordic arctan2 using no division!
+// http://www.coranac.com/documents/arctangent/
+
+fixedpointnum lib_fp_atan2(fixedpointnum y, fixedpointnum x)
+   { // returns angle from -180 to 180 degrees
+   if (y==0) return (x>=0 ? 0 : FIXEDPOINT180);
+
+   fixedpointnum returnvalue=0;
+   fixedpointnum tmp;
+
+   if (y<  0)
+      {
+      x= -x;
+      y= -y;
+      returnvalue -= FIXEDPOINT180;
+      }
+   if (x<= 0)
+      {
+      tmp= x;
+      x=  y;
+      y= -tmp;
+      returnvalue += FIXEDPOINT90;
+      }
+   if (x<=y)
+      {
+      tmp=y-x;
+      x= x+y;
+      y=  tmp;
+      returnvalue += FIXEDPOINT45;
+      }
+
+    // Scale up a bit for greater accuracy.
+   if (x < 0x10000)
+      {
+      x *= 0x1000;
+      y *= 0x1000;
+      }
+
+   for (int i=1; i<11; i++) // increase the accuracy by increasing the number of cycles (was 12)
+      {
+      if   (y>=0)
+         {
+         tmp= x + (y>>i);
+         y  = y - (x>>i);
+         x  = tmp;
+         returnvalue += atanlist[i];
+         }
+      else
+         {
+         tmp= x - (y>>i);
+         y  = y + (x>>i);
+         x  = tmp;
+         returnvalue -= atanlist[i];
+         }
+      }
+   return (returnvalue);
+   }
+
+fixedpointnum lib_fp_sqrt(fixedpointnum x)
+   {
+   return(lib_fp_multiply(x, lib_fp_invsqrt(x)));
+   }
+
+long lib_fp_stringtolong(char *string)
+   { // converts a string to an integer.  Stops at any character other than 0-9 or '-'.  Stops at decimal places too.
+   long value=0;
+   char negative=0;
+   if (*string=='-')
+      {
+      negative=1;
+      ++string;
+      }
+   while (*string>='0' && *string<='9')
+      {
+      value=value*10+*string++-'0';
+      }
+   if (negative) return(-value);
+   else return(value);
+   }
+
+fixedpointnum lib_fp_stringtofixedpointnum(char *string)
+   {
+   char negative=0;
+   if (*string=='-')
+      {
+      negative=1;
+      ++string;
+      }
+
+   fixedpointnum value=(long)lib_fp_stringtolong(string)<<FIXEDPOINTSHIFT;
+
+   // find the decimal place
+   while (*string !='\0' && *string !='.') ++string;
+   if (*string=='\0') return(value);
+
+   // use six digits after the decimal
+   ++string;
+   int fractionallength=0;
+   while (string[fractionallength]>='0' && string[fractionallength]<='9') ++fractionallength;
+
+   if (fractionallength>6) string[6]='\0';
+   fixedpointnum fractional=lib_fp_stringtolong(string);
+   while (fractionallength++<6)
+      {
+      fractional*=10;
+      }
+   value=value+(lib_fp_multiply(fractional,137439L)>>5); // 137439L is .000001 * 2^16 * 2^16 * 2^5
+
+   if (negative) return(-value);
+   else return(value);
+   }
+
+fixedpointnum invsqrtstartingpoint[12]=
+   { // =(1/((x+4.5)/16)^0.5)
+   // add 4 because range of xysquared starts at .25.
+   // add .5 to get a value in the middle of the range.
+   // 16 is the number of steps we break down into (hence the shift by 4 when calling)
+   123575L,
+   111778L,
+   102821L,
+   95721L,
+   89914L,
+   85050L,
+   80899L,
+   77302L,
+   74145L,
+   71346L,
+   68842L,
+   66584L
+   };
+
+fixedpointnum lib_fp_invsqrt(fixedpointnum x)
+   {
+   if (x<=0) return(0);
+
+   fixedpointnum y=FIXEDPOINTONE;
+   fixedpointnum xysquared=x;
+
+   // find the starting point
+   while (xysquared<FIXEDPOINTONEOVERFOUR)
+      {
+      y=y<<1;
+      xysquared=xysquared<<2;
+      }
+   while (xysquared>=FIXEDPOINTONE)
+      {
+      y=y>>1;
+      xysquared=xysquared>>2;
+      }
+
+   // at this point .25 <= xysquared < 1
+   // the idea is to get xysquared as close to 1 as possible.  Start by using
+   // the lookup table.
+
+   y=lib_fp_multiply(y,invsqrtstartingpoint[(xysquared>>(FIXEDPOINTSHIFT-4))-4]);
+
+   // now use newton's method to increase accuracy
+   // increase iterations if you want more accuracy
+   // this one iteration should get us to within .5% error
+   xysquared=lib_fp_multiply(y,lib_fp_multiply(x,y));
+   y = lib_fp_multiply(y,(FIXEDPOINTTHREE - xysquared))>>1;
+//   xysquared=lib_fp_multiply(y,lib_fp_multiply(x,y));
+//   y = lib_fp_multiply(y,(FIXEDPOINTTHREE - xysquared))>>1;
+
+   return(y);
+   }

I met some difficulties not solved at this stage:
- All the Bradwii code is using variable type "fixedpointnum" . I am pretty sure, we ill meet some trouble by mixing with the conventionnal types used by multiwii.
- writeusersettingstoeeprom(); function, should probably be completely rewritten for multiwii... I have not loked at the details yet.
- global.gyrorate[autotuneindex] is a variable giving the angular speed in deg/s. I have not been able to find the equivalent variable in multiwii. Does it exist ?
- global.rxvalues[autotuneindex] is The values of the RX inputs, ranging from -1.0 to 1.0. As far as now, The RCvalues are stored in multiwii as 1000-2000 values. Some simple math to do.

[sorg]

Still working on the ZN patch (I want to test Alek PID and I need to tune my PID with ZN tomorrow).

The previous patch work well, but i have modified it to constrain the rc value to 1000/2000. When my pot was full left, It went beow 1000, and P jumped suddenly at a very high level.

Code: Select all

Index: src/MultiWii/config.h
===================================================================
--- src/MultiWii/config.h   (revision 1538)
+++ src/MultiWii/config.h   (working copy)
 
+  /**************************    Ziegler Nicholls PID Tuning     ******************************
+   * http://www.multiwii.com/forum/viewtopic.php?f=7&t=1701&start=30                          *
+   * http://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method                              *
+   *
+   * Allow to tune PID loop with RX pots. P, I and D are tuned with a single pots with ZN algo*
+   * ******************************************************************************************/
+ #define DIAL_TUNING      // Activate PID tuning through Pots
+
+ #define FREQ_TU_ROLL 3      // PID proper frequency in Hz. 3hz is a good start for Roll and Pitch of medium size multi
+                             // Smaller multi : higer value and vice versa.
+ #define POT_ROLL AUX5       // Channel used to tune ROLL - Comment out to disable pot tuning for this PID
+ #define FREQ_TU_PITCH 3     // PID proper frequency in Hz: usually the same as ROLL
+ #define POT_PITCH AUX5      // Channel used to tune PITCH : usually the same as ROLL
+ #define FREQ_TU_LEVEL 3     // Need some experiment. This Frequency is not the same as ROLL & PITCH
+ #define POT_LEVEL AUX6      // Channel used to tune LEVEL
+ #define FREQ_TU_BARO 3      // Need some experiment. This Frequency is not the same as ROLL & PITCH
+ //#define POT_BARO AUX7     // Channel used to tune BARO
+ //#define POT_MAG AUX6      // Channel used to tune MAG
+ #define FREQ_TU_YAW 3       // Need some experiment. This Frequency is not the same as ROLL & PITCH
+ #define POT_YAW AUX7        // Channel used to tune YAW
 
 /*************************************************************************************************/
 /****           END OF CONFIGURABLE PARAMETERS                                                ****/
Index: src/MultiWii/MultiWii.cpp
===================================================================
--- src/MultiWii/MultiWii.cpp   (revision 1538)
+++ src/MultiWii/MultiWii.cpp   (working copy)
@@ -554,6 +554,64 @@
       #endif
     #endif
   }
+
+#if defined(DIAL_TUNING)
+
+  // see Ziegler Nichols method
+  #if defined(POT_ROLL)
+  int ku = ((constrain(rcData[POT_ROLL],1000,2000) -1000) << 1) >> 6 ; // * 20/640;
+  int kp = ku * 6 ;
+  conf.pid[PIDROLL].P8 = kp;
+  conf.pid[PIDROLL].I8 = (kp << 1)/FREQ_TU_ROLL;
+  conf.pid[PIDROLL].D8 = (kp * FREQ_TU_ROLL) >> 3;
+  #endif
+
+  #if defined(POT_PITCH)
+  ku = ((constrain(rcData[POT_PITCH],1000,2000) -1000) << 1) >> 6 ;
+  kp = ku * 6 ;
+  conf.pid[PIDPITCH].P8 = kp;
+  conf.pid[PIDPITCH].I8 = (kp << 1)/FREQ_TU_PITCH;
+  conf.pid[PIDPITCH].D8 = (kp * FREQ_TU_PITCH) >> 3;
+  #endif
+
+  #if defined(POT_LEVEL)
+   #if PID_CONTROLLER == 1 // odschool controller is PID
+  ku = ((constrain(rcData[POT_LEVEL],1000,2000) -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDLEVEL].P8 = kp;
+  conf.pid[PIDLEVEL].I8 = (kp << 1)/FREQ_TU_LEVEL;
+  conf.pid[PIDLEVEL].D8 = (kp * FREQ_TU_LEVEL) >> 3;
+   #elif PID_CONTROLLER == 2 //AlexK control is simple P
+  ku = ((constrain(rcData[POT_LEVEL],1000,2000) -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDLEVEL].P8 = kp;
+   #endif
+  #endif
+
+  #if defined(POT_BARO)
+  ku = ((constrain(rcData[POT_BARO],1000,2000) -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDALT].P8 = kp;
+  conf.pid[PIDALT].I8 = (kp << 1)/FREQ_TU_BARO;
+  conf.pid[PIDALT].D8 = (kp * FREQ_TU_BARO) >> 3;
+  #endif
+
+  #if defined(POT_MAG)      //P-only controller
+  ku = ((constrain(rcData[POT_MAG],1000,2000) -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDMAG].P8 = kp;
+  #endif
+
+  #if defined(POT_YAW)
+  ku = ((constrain(rcData[POT_YAW],1000,2000) -1000) << 1) >> 6 ; // * 20/640;
+  kp = ku * 6 ;
+  conf.pid[PIDYAW].P8 = kp;
+  conf.pid[PIDYAW].I8 = (kp << 1)/FREQ_TU_YAW;
+  conf.pid[PIDYAW].D8 = (kp * FREQ_TU_YAW) >> 3;
+  #endif
+
+#endif
+
 }
 
 void setup() {

This patch compile fine with all coptertest on eclipse.

[sorg]

Ok, so I flyed today to tune Alexk PID with ZN pot.

I started with gyro :
I found in a few seconds a sweet point at mid-range of the pot. (too low : i can't take off... too high: the altitude is uncontrollable).

At the same time, I raised immediatly the yaw Ku, as the copter was not flyable (spinning on itself).

Once able to fly, I lowerd a little bit my settings (wobble at fast bank) and i found a good setting for roll and pitch at 7.8 / 0.052 / 29 .
It is a little bit less than what i used to set with the olschool PID.

Also Roll and Pitch stick were very sensitive (a lot more than before) , and I made at some point I made a mistake ... and crashed... and broke two arms of my frame...
I glued them with epoxy glue... hope this will hold all the part together to fly tomorrow.

Yaw has been set at 6.6 / 0.044 / 24 , but this was not perfect yet.

I have not flyed long enough to tune level.

[BradQuick2]

Here's a link to the theory behind my autotune code. If you understand how it works, it may be easier to port to Multi-Wii.

One thing that will make it difficult to port is that to the best of my knowledge Multi-Wii uses nested loops for PID control. I believe it uses one PID loop for angular velocity and another for position (level mode). Mine uses a single PID loop for position. So my technique will probably only work on the position loop in Multi-Wii.

http://www.rcgroups.com/forums/showthread.php?t=1922423

[doehme]

Hi Brandon

I'm just looking at implementing your code for POT tuning of PID's from March 23rd.

I may be missing something basic (probably) but in the third section you say to select the following code and put it inwo "MultiWii.ino. This bit is around line 690".

I don't have a Multiwii.ino tab (V2.2) just Multiwii.ccp and Multiwii.h. Also how do I get Arduino to dsiplay my line numbers so I can tell where to insert it.

Thanks for your help. As you can tell - I'm a bit new to this but learning fast.

Yours,

Dene

[argotera]

You probably have different version of Multiwii.You have MW2.3 while Brandon refers to 2.2 probably.

[MHefny]

Hi I developed a method called ZERO-PID
viewtopic.php?f=8&t=5190
It flies your quadcopter with starting values ZERO for PID of Yaw,Roll & Pitch together.

[Benik3]

MHefny:
It looks, that it's only automatic mode for now, where we don't need pot.
But it doesn't tune D parameter right?

[JonnyQuest]

Hi! I port almost all from post at March 23rd for MultiWii 2.3 sources but i didn't know where i must put next:

Code: Select all

Then put the hook calls in the main setup and loop functions in MultiWii.ino. This bit is around line 690 (MW v2.2) - at the end of the loop function (I've set the USER_LOOP_FAST to 100 Hz for sake of very short LED flashes - again not related to PID tuning - you can set this to 50 Hz):

// USER LOOPS (BH)
#if defined(USER_LOOP_FAST)
   static uint32_t userLoopFastTime;
   if (currentTime > userLoopFastTime) {
      //userLoopFastTime = currentTime + 20000; // 50 Hz
      userLoopFastTime = currentTime + 10000; // 100 Hz
      USER_LOOP_FAST;
   }
#endif
#if defined(USER_LOOP_SLOW)
   static uint32_t userLoopSlowTime;
   if (currentTime > userLoopSlowTime) {
      userLoopSlowTime = currentTime + 100000; // 10 Hz
      USER_LOOP_SLOW;
   }
#endif
#if defined(USER_LOOP_1HZ)
   static uint32_t userLoop1HzTime;
   if (currentTime > userLoop1HzTime) {
      userLoop1HzTime = currentTime + 1000000; // 1 Hz
      USER_LOOP_1HZ;
   }
#endif


... and this is around line 822 (MW v2.2) near the end of the setup function:


SELECT ALL
// USER SETUP (BH)
#ifdef USER_SETUP
   USER_SETUP
#endif

Anyone do this for 2.3 ?

[JonnyQuest]

I made it! It works in 2.3 )))

[DEG]

I made it! It works in 2.3 )))

I am pretty new at this and have the 2.3 version. I would like to try to use your code but I am unsure where to place it. Do you create a separate tab within or do you place it into the preexisting code? Any help would be greatly appreciated.

Thanks

[AstromarioDD]

hello,

so it looks to be easy to extend the code from "multiwiiPID_POD" with some lines of code for the calculation of I and D regarding to Ziegler-Nichols.

Then I could try to find a point near the stability limit by changing P inflight and then set I and D automaticaly,
or to change P inflight (starting from a default value) and calculate I and D immediately.

Mario

[Crashpilot1000]

The bradwii approach has been ported in cleanflight here: https://github.com/hydra/cleanflight/bl ... autotune.c (and called here https://github.com/hydra/cleanflight/bl ... /main/mw.c).
Mhefny has done his zero pid tuner here: viewtopic.php?f=8&t=5190 and his repo is here: https://github.com/HefnySco/Multiwii_GTune
Based on Mhefnys' findings I also did an acro P only tuner here: https://github.com/Crashpilot1000/TestC ... /mw.c#L921 (note gyrovalues are not div by 4 there).
Since it's all directly mwii related the chances are 100% that there is something usable for you at play.

[Moraes]

Please, please, please Brandonhill, or another colleague who can help me?

Im tryng to apply the code, but it is so difficult to identify the place where I put the codes.

I'd love to be able to do this mod on a dev firmware that I'm using (with autoland), but the line numbers do not match, and I have trouble finding the right places.

This MOD is a gold! Able to mwc a feature as same the naza, and give a chance to anyone tune pids more easy.

Sorry for my poor English. I am Brazilian and I learned little English was self-taught due to liking for computers and programming. I read a lot better than I write, it is still difficult to understand that is not my native language, and here unfortunately learn English is weak in public schools.

First, add a UserCode.ino page:

Code: Select all

// USER CODE HOOKS (BH)

void userSetup() {
   #ifdef BH_TUNE
      bhTune_setup();
   #endif
}

//void userLoopFast() {}

void userLoopSlow() {
   #ifdef BH_TUNE
      bhTune_loopSlow();
   #endif
}

void userLoop1Hz() {
   #ifdef BH_TUNE
      bhTune_save();
   #endif
}

Ok, This step is easy, Just creating a tab and copy and paste, done.

Then define the hooks in config.h (I am using the USER_LOOP_FAST, just not for PID tuning):

Code: Select all

/********************************************************************/
/****           User code callbacks (BH)                         ****/
/********************************************************************/

// uncomment any hooks you're using in UserCode.ino
#define USER_SETUP userSetup();
//#define USER_LOOP_FAST userLoopFast();
#define USER_LOOP_SLOW userLoopSlow();
#define USER_LOOP_1HZ userLoop1Hz();

Ok easy too, Its anywhere in Config.h, right?

I put inside SECTION 7 - TUNING & DEVELOPER. It's Right?

Other question, you reccomend leave loopfast commented and leave slow and 1hz uncommented? Its is?

Then put the hook calls in the main setup and loop functions in MultiWii.ino. This bit is around line 690 (MW v2.2) - at the end of the loop function (I've set the USER_LOOP_FAST to 100 Hz for sake of very short LED flashes - again not related to PID tuning - you can set this to 50 Hz):

Code: Select all

// USER LOOPS (BH)
#if defined(USER_LOOP_FAST)
   static uint32_t userLoopFastTime;
   if (currentTime > userLoopFastTime) {
      //userLoopFastTime = currentTime + 20000; // 50 Hz
      userLoopFastTime = currentTime + 10000; // 100 Hz
      USER_LOOP_FAST;
   }
#endif
#if defined(USER_LOOP_SLOW)
   static uint32_t userLoopSlowTime;
   if (currentTime > userLoopSlowTime) {
      userLoopSlowTime = currentTime + 100000; // 10 Hz
      USER_LOOP_SLOW;
   }
#endif
#if defined(USER_LOOP_1HZ)
   static uint32_t userLoop1HzTime;
   if (currentTime > userLoop1HzTime) {
      userLoop1HzTime = currentTime + 1000000; // 1 Hz
      USER_LOOP_1HZ;
   }
#endif

This step I do not know if I'm able to do correctly. Can not guide me by the number line because the code I use is changed.

I think I found the "Main Loop", but did not quite understand where the end is, and where should I paste the code.

You could copy and paste here where should I put the code?

Thus, for example:

// ******** Main Loop *********
void loop () {
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
uint8_t axis,i;
int16_t error,errorAngle;
int16_t delta,deltaSum;
int16_t PTerm,ITerm,DTerm;
int16_t PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0;
static int16_t lastGyro[3] = {0,0,0};
static int16_t delta1[3],delta2[3];
static int16_t errorGyroI[3] = {0,0,0};
static int16_t errorAngleI[2] = {0,0};
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
static uint32_t timestamp_fixated = 0;
//----------PASTE THE CODE HERE--------//

#if defined(SPEKTRUM)
if (spekFrameFlags == 0x01) readSpektrum();
#endif

#if defined(OPENLRSv2MULTI)
Read_OpenLRS_RC();
#endif

if (currentTime > rcTime ) { // 50Hz
rcTime = currentTime + 20000;
computeRC();
// Failsafe routine - added by MIS
#if defined(FAILSAFE)
.......
.....

If you so indicate me where I put the code, I think it is easy to find.

... and this is around line 822 (MW v2.2) near the end of the setup function:

Code: Select all

// USER SETUP (BH)
#ifdef USER_SETUP
   USER_SETUP
#endif

Like the previous step, I am not able to find where to put this code snippet.

If you can point me in the same way as above, would be great.

Okay, the hooks are in place, now add a BHTuning.ino page (I prefix everything BH not for vanity but as a habit of conflict avoidance):

Code: Select all

/**
 * PID POTENTIOMETER TUNING (BH)
 * tune PID values using TX potentiometer
 *
   Maximum PID values (for reference when setting your range)
   -------------------------------------
   Param       P       I        D
   -------------------------------------
   ROLL        20.0    0.250    100
   PITCH       20.0    0.250    100
   YAW         20.0    0.250    100
   PIDALT      20.0    0.250    100
   PIDPOS       2.5    2.5
   PIDPOSR     25.0    2.50       0.250
   PIDNAVR     25.0    2.50       0.250
   PIDLEVEL    20.0    0.250    100
   PIDMAG      20.0
 */

// enable/disable
#define BH_TUNE

#ifdef BH_TUNE
   
   enum bhTune_params {P, I, D};
   
   // ****************************************************************
   // ***************** START OF CONFIGURABLE PARAMS *****************
   
   // PID loop to tune - ROLL, PITCH, YAW, PIDALT, PIDPOS, PIDPOSR, PIDNAVR, PIDLEVEL or PIDMAG
   const uint8_t bhTune_iKey = PITCH;
   
   // PID parameter to tune - P, I or D
   const uint8_t bhTune_iParam = P;
   
   // parameter value range - see maximums above
   const float bhTune_iValRange[] = {2, 4};
   
   // RC ch. of TX potentiometer input (e.g. AUX1)
   const uint8_t bhTune_iInputCh = AUX3;
   
   // lock pitch & roll values
   const uint8_t bhTune_bLockPitchRoll = 1;
   
   // ****************** END OF CONFIGURABLE PARAMS ******************
   // ****************************************************************
   
   // check if setting exists
   const uint8_t bhTune_bSettingInvalid =
      (bhTune_iKey > PIDMAG) ||
      (bhTune_iParam > D) ||
      (bhTune_iKey == PIDPOS && bhTune_iParam == D) ||
      (bhTune_iKey == PIDMAG && bhTune_iParam != P)
      ? 1 : 0;
   
   uint8_t bhTune_bSaved = 0;
   uint8_t bhTune_iParamRangeDelta;
   uint8_t bhTune_iParamRangeMin;
   
   // SETUP
   void bhTune_setup() {
      if (!bhTune_bSettingInvalid) {
         bhTune_iParamRangeMin = bhTune_clampUserVal(bhTune_iValRange[0]);
         bhTune_iParamRangeDelta = bhTune_clampUserVal(bhTune_iValRange[1]) - bhTune_iParamRangeMin;
      }
   }
   
   // LOOP
   void bhTune_loopSlow() {
      if (!bhTune_bSettingInvalid) {
         
         static uint8_t iParamVal;
         static uint8_t iParamValOld;
         
         // get param val (apply TX input to range)
         iParamVal = bhTune_iParamRangeDelta * ((min(max(rcData[bhTune_iInputCh], 1000), 2000) - 1000) * 0.001) + bhTune_iParamRangeMin;
         
         // check if it's changed
         if (iParamVal != iParamValOld) {
            
            // update state vars
            bhTune_bSaved = 0;
            iParamValOld = iParamVal;
            
            // pitch & roll locked
            if ((bhTune_iKey == PITCH || bhTune_iKey == ROLL) && bhTune_bLockPitchRoll) {
               bhTune_setConfVal(iParamVal, PITCH);
               bhTune_setConfVal(iParamVal, ROLL);
            } else {
               bhTune_setConfVal(iParamVal, bhTune_iKey);
            }
         }
      }
   }
   
   // APPLY MULTIPLIER, LIMIT
   uint8_t bhTune_clampUserVal(float fParamVal) {
      
      // max vals (multipliers applied)
      uint8_t aPMax[] = {200, 200, 200, 200, 250, 250, 250, 200, 200};
      uint8_t aIMax[] = {250, 250, 250, 250, 250, 250, 250, 250};
      uint8_t aDMax[] = {100, 100, 100, 100, 0, 250, 250, 100};
      
      // multipliers
      uint8_t aPMult[] = {10, 10, 10, 10, 100, 10, 10, 10, 10};
      uint16_t aIMult[] = {1000, 1000, 1000, 1000, 100, 100, 100, 1000};
      uint16_t aDMult[] = {1, 1, 1, 1, 0, 1000, 1000, 1};
      
      // multiply and limit
      switch (bhTune_iParam) {
      case P:
         return min(max(0, fParamVal) * aPMult[bhTune_iKey], aPMax[bhTune_iKey]);
         break;
      case I:
         return min(max(0, fParamVal) * aIMult[bhTune_iKey], aIMax[bhTune_iKey]);
         break;
      case D:
         return min(max(0, fParamVal) * aDMult[bhTune_iKey], aDMax[bhTune_iKey]);
         break;
      }
   }
   
   // SAVE
   void bhTune_save() {
      if (!f.ARMED && !bhTune_bSettingInvalid && !bhTune_bSaved) {
         bhTune_bSaved = 1;
         writeParams(0);
      }
   }
   
   // UPDATE PARAM
   void bhTune_setConfVal(uint8_t iParamVal, uint8_t iTuneKey) {
      switch (bhTune_iParam) {
      case P:
         conf.P8[iTuneKey] = iParamVal;
         break;
      case I:
         conf.I8[iTuneKey] = iParamVal;
         break;
      case D:
         conf.D8[iTuneKey] = iParamVal;
         break;
      }
   }
   
#endif

/*
MULTIPLIERS
              P       I       D
ROLL         10    1000       1
PITCH        10    1000       1
YAW          10    1000       1
PIDALT       10    1000       1
PIDPOS      100       100       0
PIDPOSR      10       100    1000
PIDNAVR      10       100    1000
PIDLEVEL     10    1000       1
PIDMAG       10
*/

Ok, as the first step, this step is easy, Just creating a tab and copy and paste, done.

The configurable bits are near the top, and should be fairly easy to understand, but here's an overview anyhow:

- BH_TUNE - comment/uncomment this to disable/enable
- bhTune_iKey - PID loop you want to tune - ROLL, PITCH, YAW, PIDALT, PIDPOS, PIDPOSR, PIDNAVR, PIDLEVEL or PIDMAG.
- bhTune_iParam - PID value you want to tune - P, I or D.
- bhTune_iValRange - the range you want to control with your TX pot. This is automatically limited to what MultiWii will allow, but it might behave erratically if you're being silly with huge numbers. Refer to the table of maximum values provided at the top.
- bhTune_iInputCh - RC channel to which you've assigned the TX pot - AUX1, AUX2, AUX3 or AUX4.
- bhTune_bLockPitchRoll - indicates whether you want to have PITCH and ROLL values the same - 1 = yes, 0 = no.

Hope this is useful to someone! And of course, I'm not responsible for anything bad that happens to you or your machine.

And this, i think is a guide to config, I believe to have understood this part.


Anyway, I apologize for giving that work for you to help me, but I believe that just as there will be many other people who have some facility to change codes in an optical user, but has no deep knowledge as you, and have had these same difficulties. By helping me, you are helping all these people at once.

Thank you in advance.

Moraes

[AstromarioDD]

Hello Moares,

#ifdef USER_SETUP
USER_SETUP
#endif

I merged this part at the end of the setup-function in MultiWii.cpp
void setup() {
# a lot of
# other code

#ifdef LOG_PERMANENT_SHOW_AT_STARTUP
dumpPLog(0);
#endif
plog.armed_time = 0; // lifetime in seconds
//plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut
#endif

/********************************************************************/
/**** User code callbacks (BH) ****/
/********************************************************************/
// USER SETUP (BH)
#ifdef USER_SETUP
USER_SETUP
#endif
/********************************************************************/
debugmsg_append_str("initialization completed\n");
}

// USER LOOPS (BH)
#if defined(USER_LOOP_FAST)
static uint32_t userLoopFastTime;
if (currentTime > userLoopFastTime) {
//userLoopFastTime = currentTime + 20000; // 50 Hz
userLoopFastTime = currentTime + 10000; // 100 Hz
USER_LOOP_FAST;
}
#endif
.....

I merged this part into the main loop in MultiWii.cpp before computeIMU():

<----------------

computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;

[Moraes]

Hello AstromarioDD,


Thank you so much, please see this:

I merged this part at the end of the setup-function in MultiWii.cpp
void setup() {
# a lot of
# other code

#ifdef LOG_PERMANENT_SHOW_AT_STARTUP
dumpPLog(0);
#endif
plog.armed_time = 0; // lifetime in seconds
//plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut
#endif

/********************************************************************/
/**** User code callbacks (BH) ****/
/********************************************************************/
// USER SETUP (BH)
#ifdef USER_SETUP
USER_SETUP
#endif
/********************************************************************/
debugmsg_append_str("initialization completed\n");
}

The firmware I'm using has no multiwii.cpp, but I found the location indicated by you in multiwii.ino:

#ifdef LOG_PERMANENT_SHOW_AT_STARTUP
dumpPLog(0);
#endif
plog.armed_time = 0; // lifetime in seconds
//plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut
#endif
/********************************************************************/
/**** User code callbacks (BH) ****/
/********************************************************************/
// USER SETUP (BH)
#ifdef USER_SETUP
USER_SETUP
#endif
/********************************************************************/
debugmsg_append_str("initialization completed\n");
}

#if BARO
void resetAltHold() {
errorAltitudeI = 0; // clear all ALT_HOLD code values to default of OFF
BaroPID = 0;

// USER LOOPS (BH)
#if defined(USER_LOOP_FAST)
static uint32_t userLoopFastTime;
if (currentTime > userLoopFastTime) {
//userLoopFastTime = currentTime + 20000; // 50 Hz
userLoopFastTime = currentTime + 10000; // 100 Hz
USER_LOOP_FAST;
}
#endif
.....

I merged this part into the main loop in MultiWii.cpp before computeIMU():

<----------------

computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;

I Merge too:

#endif
#ifdef VARIOMETER
if (f.VARIO_MODE) vario_signaling();
#endif
break;
}
}
// USER LOOPS (BH)
#if defined(USER_LOOP_FAST)
static uint32_t userLoopFastTime;
if (currentTime > userLoopFastTime) {
//userLoopFastTime = currentTime + 20000; // 50 Hz
userLoopFastTime = currentTime + 10000; // 100 Hz
USER_LOOP_FAST;
}
#endif
#if defined(USER_LOOP_SLOW)
static uint32_t userLoopSlowTime;
if (currentTime > userLoopSlowTime) {
userLoopSlowTime = currentTime + 100000; // 10 Hz
USER_LOOP_SLOW;
}
#endif
#if defined(USER_LOOP_1HZ)
static uint32_t userLoop1HzTime;
if (currentTime > userLoop1HzTime) {
userLoop1HzTime = currentTime + 1000000; // 1 Hz
USER_LOOP_1HZ;
}
#endif
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;

//***********************************
//**** Experimental FlightModes *****
//***********************************
#if defined(ACROTRAINER_MODE)

And did the other necessary steps, but unfortunately it did not work. When compiled, resulting in this error:

What could have gone wrong?

I'm trying to put the pot pid control, in two different firmwares. If I put the link of firmware I'm using, it can be trying to give me more help?

This, based in MWC 2.2:
download/file.php?id=2157

And This, based in MWC 2,3:
download/file.php?id=3268 (I still have not tested in this firmware, I will test now)

Thank you so much!

Moraes

[Moraes]

Hey, just above, the post of JonnyQuest, said he could port the MWC 2.3.

I downloaded the zip, and inside has a .patch file. how to use this patch file?

If I could see the code, at least it was port in 2.3.


Hugs,

Moraes

[Moraes]

And This, based in MWC 2,3:
download/file.php?id=3268 (I still have not tested in this firmware, I will test now)

Have tested in 2.3 version, dont work too

My complete Multiwii.cpp tab(this version is cpp)

Code: Select all

/*
MultiWiiCopter by Alexandre Dubus
www.multiwii.com
November  2013     V2.3
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 any later version. see <http://www.gnu.org/licenses/>
*/

#include <avr/io.h>

#include "Arduino.h"
#include "config.h"
#include "def.h"
#include "types.h"
#include "MultiWii.h"
#include "Alarms.h"
#include "EEPROM.h"
#include "IMU.h"
#include "LCD.h"
#include "Output.h"
#include "RX.h"
#include "Sensors.h"
#include "Serial.h"
#include "GPS.h"
#include "Protocol.h"

#include <avr/pgmspace.h>

/*********** RC alias *****************/

const char pidnames[] PROGMEM =
  "ROLL;"
  "PITCH;"
  "YAW;"
  "ALT;"
  "Pos;"
  "PosR;"
  "NavR;"
  "LEVEL;"
  "MAG;"
  "VEL;"
;

const char boxnames[] PROGMEM = // names for dynamic generation of config GUI
  "ARM;"
  #if ACC
    "ANGLE;"
    "HORIZON;"
  #endif
  #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
    "BARO;"
  #endif
  #ifdef VARIOMETER
    "VARIO;"
  #endif
  #if MAG
    "MAG;"
    #if defined(HEADFREE)
      "HEADFREE;"
      "HEADADJ;"
    #endif
  #endif
  #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
    "CAMSTAB;"
  #endif
  #if defined(CAMTRIG)
    "CAMTRIG;"
  #endif
  #if GPS
    "GPS HOME;"
    "GPS HOLD;"
  #endif
  #if defined(FIXEDWING) || defined(HELICOPTER)
    "PASSTHRU;"
  #endif
  #if defined(BUZZER)
    "BEEPER;"
  #endif
  #if defined(LED_FLASHER)
    "LEDMAX;"
    "LEDLOW;"
  #endif
  #if defined(LANDING_LIGHTS_DDR)
    "LLIGHTS;"
  #endif
  #ifdef INFLIGHT_ACC_CALIBRATION
    "CALIB;"
  #endif
  #ifdef GOVERNOR_P
    "GOVERNOR;"
  #endif
  #ifdef OSD_SWITCH
    "OSD SW;"
  #endif
;

const uint8_t boxids[] PROGMEM = {// permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function.
  0, //"ARM;"
  #if ACC
    1, //"ANGLE;"
    2, //"HORIZON;"
  #endif
  #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
    3, //"BARO;"
  #endif
  #ifdef VARIOMETER
    4, //"VARIO;"
  #endif
  #if MAG
    5, //"MAG;"
    #if defined(HEADFREE)
      6, //"HEADFREE;"
      7, //"HEADADJ;"
    #endif
  #endif
  #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
    8, //"CAMSTAB;"
  #endif
  #if defined(CAMTRIG)
    9, //"CAMTRIG;"
  #endif
  #if GPS
    10, //"GPS HOME;"
    11, //"GPS HOLD;"
  #endif
  #if defined(FIXEDWING) || defined(HELICOPTER)
    12, //"PASSTHRU;"
  #endif
  #if defined(BUZZER)
    13, //"BEEPER;"
  #endif
  #if defined(LED_FLASHER)
    14, //"LEDMAX;"
    15, //"LEDLOW;"
  #endif
  #if defined(LANDING_LIGHTS_DDR)
    16, //"LLIGHTS;"
  #endif
  #ifdef INFLIGHT_ACC_CALIBRATION
    17, //"CALIB;"
  #endif
  #ifdef GOVERNOR_P
    18, //"GOVERNOR;"
  #endif
  #ifdef OSD_SWITCH
    19, //"OSD_SWITCH;"
  #endif
};


uint32_t currentTime = 0;
uint16_t previousTime = 0;
uint16_t cycleTime = 0;     // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
uint16_t calibratingA = 0;  // the calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
uint16_t calibratingB = 0;  // baro calibration = get new ground pressure value
uint16_t calibratingG;
int16_t  magHold,headFreeModeHold; // [-180;+180]
uint8_t  vbatMin = VBATNOMINAL;  // lowest battery voltage in 0.1V steps
uint8_t  rcOptions[CHECKBOXITEMS];
int32_t  AltHold; // in cm
int16_t  sonarAlt;
int16_t  BaroPID = 0;
int16_t  errorAltitudeI = 0;

// **************
// gyro+acc IMU
// **************
int16_t gyroZero[3] = {0,0,0};

imu_t imu;

analog_t analog;

alt_t alt;

att_t att;

#if defined(ARMEDTIMEWARNING)
  uint32_t  ArmedTimeWarningMicroSeconds = 0;
#endif

int16_t  debug[4];

flags_struct_t f;

//for log
#if defined(LOG_VALUES) || defined(LCD_TELEMETRY)
  uint16_t cycleTimeMax = 0;       // highest ever cycle timen
  uint16_t cycleTimeMin = 65535;   // lowest ever cycle timen
  int32_t  BAROaltMax;             // maximum value
  uint16_t GPS_speedMax = 0;       // maximum speed from gps
  uint16_t powerValueMaxMAH = 0;
#endif
#if defined(LOG_VALUES) || defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT)
  uint32_t armedTime = 0;
#endif

int16_t  i2c_errors_count = 0;
int16_t  annex650_overrun_count = 0;



#if defined(THROTTLE_ANGLE_CORRECTION)
  int16_t throttleAngleCorrection = 0;   // correction of throttle in lateral wind,
  int8_t  cosZ = 100;               // cos(angleZ)*100
#endif



// **********************
//Automatic ACC Offset Calibration
// **********************
#if defined(INFLIGHT_ACC_CALIBRATION)
  uint16_t InflightcalibratingA = 0;
  int16_t AccInflightCalibrationArmed;
  uint16_t AccInflightCalibrationMeasurementDone = 0;
  uint16_t AccInflightCalibrationSavetoEEProm = 0;
  uint16_t AccInflightCalibrationActive = 0;
#endif

// **********************
// power meter
// **********************
#if defined(POWERMETER) || ( defined(LOG_VALUES) && (LOG_VALUES >= 3) )
  uint32_t pMeter[PMOTOR_SUM + 1];  // we use [0:7] for eight motors,one extra for sum
  uint8_t pMeterV;                  // dummy to satisfy the paramStruct logic in ConfigurationLoop()
  uint32_t pAlarm;                  // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
  uint16_t powerValue = 0;          // last known current
#endif
uint16_t intPowerTrigger1;

// **********************
// telemetry
// **********************
#if defined(LCD_TELEMETRY)
  uint8_t telemetry = 0;
  uint8_t telemetry_auto = 0;
#endif
#ifdef LCD_TELEMETRY_STEP
  char telemetryStepSequence []  = LCD_TELEMETRY_STEP;
  uint8_t telemetryStepIndex = 0;
#endif

// ******************
// rc functions
// ******************
#define ROL_LO  (1<<(2*ROLL))
#define ROL_CE  (3<<(2*ROLL))
#define ROL_HI  (2<<(2*ROLL))
#define PIT_LO  (1<<(2*PITCH))
#define PIT_CE  (3<<(2*PITCH))
#define PIT_HI  (2<<(2*PITCH))
#define YAW_LO  (1<<(2*YAW))
#define YAW_CE  (3<<(2*YAW))
#define YAW_HI  (2<<(2*YAW))
#define THR_LO  (1<<(2*THROTTLE))
#define THR_CE  (3<<(2*THROTTLE))
#define THR_HI  (2<<(2*THROTTLE))

int16_t failsafeEvents = 0;
volatile int16_t failsafeCnt = 0;

int16_t rcData[RC_CHANS];    // interval [1000;2000]
int16_t rcSerial[8];         // interval [1000;2000] - is rcData coming from MSP
int16_t rcCommand[4];        // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
uint8_t rcSerialCount = 0;   // a counter to select legacy RX when there is no more MSP rc serial data
int16_t lookupPitchRollRC[5];// lookup table for expo & RC rate PITCH+ROLL
int16_t lookupThrottleRC[11];// lookup table for expo & mid THROTTLE

#if defined(SPEKTRUM) || defined(SBUS)
  volatile uint8_t  spekFrameFlags;
  volatile uint32_t spekTimeLast;
  uint8_t  spekFrameDone;
#endif

#if defined(OPENLRSv2MULTI)
  uint8_t pot_P,pot_I; // OpenLRS onboard potentiometers for P and I trim or other usages
#endif


// *************************
// motor and servo functions
// *************************
int16_t axisPID[3];
int16_t motor[8];
int16_t servo[8] = {1500,1500,1500,1500,1500,1500,1500,1000};

// ************************
// EEPROM Layout definition
// ************************
static uint8_t dynP8[2], dynD8[2];

global_conf_t global_conf;

conf_t conf;

#ifdef LOG_PERMANENT
  plog_t plog;
#endif

// **********************
// GPS common variables
// **********************
  int16_t  GPS_angle[2] = { 0, 0};                      // the angles that must be applied for GPS correction
  int32_t  GPS_coord[2];
  int32_t  GPS_home[2];
  int32_t  GPS_hold[2];
  uint8_t  GPS_numSat;
  uint16_t GPS_distanceToHome;                          // distance to home  - unit: meter
  int16_t  GPS_directionToHome;                         // direction to home - unit: degree
  uint16_t GPS_altitude;                                // GPS altitude      - unit: meter
  uint16_t GPS_speed;                                   // GPS speed         - unit: cm/s
  uint8_t  GPS_update = 0;                              // a binary toogle to distinct a GPS position update
  uint16_t GPS_ground_course = 0;                       //                   - unit: degree*10
  uint8_t  GPS_Present = 0;                             // Checksum from Gps serial
  uint8_t  GPS_Enable  = 0;
  uint8_t  GPS_Frame   = 0;

  // The desired bank towards North (Positive) or South (Negative) : latitude
  // The desired bank towards East (Positive) or West (Negative)   : longitude
  int16_t  nav[2];
  int16_t  nav_rated[2];    //Adding a rate controller to the navigation to make it smoother

  uint8_t nav_mode = NAV_MODE_NONE; // Navigation mode

  uint8_t alarmArray[16];           // array
 
#if BARO
  int32_t baroPressure;
  int16_t baroTemperature;
  int32_t baroPressureSum;
#endif

void annexCode() { // this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
  static uint32_t calibratedAccTime;
  uint16_t tmp,tmp2;
  uint8_t axis,prop1,prop2;

  // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
  prop2 = 128; // prop2 was 100, is 128 now
  if (rcData[THROTTLE]>1500) { // breakpoint is fix: 1500
    if (rcData[THROTTLE]<2000) {
      prop2 -=  ((uint16_t)conf.dynThrPID*(rcData[THROTTLE]-1500)>>9); //  /512 instead of /500
    } else {
      prop2 -=  conf.dynThrPID;
    }
  }

  for(axis=0;axis<3;axis++) {
    tmp = min(abs(rcData[axis]-MIDRC),500);
    #if defined(DEADBAND)
      if (tmp>DEADBAND) { tmp -= DEADBAND; }
      else { tmp=0; }
    #endif
    if(axis!=2) { //ROLL & PITCH
      tmp2 = tmp>>7; // 500/128 = 3.9  => range [0;3]
      rcCommand[axis] = lookupPitchRollRC[tmp2] + ((tmp-(tmp2<<7)) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2])>>7);
      prop1 = 128-((uint16_t)conf.rollPitchRate*tmp>>9); // prop1 was 100, is 128 now -- and /512 instead of /500
      prop1 = (uint16_t)prop1*prop2>>7; // prop1: max is 128   prop2: max is 128   result prop1: max is 128
      dynP8[axis] = (uint16_t)conf.pid[axis].P8*prop1>>7; // was /100, is /128 now
      dynD8[axis] = (uint16_t)conf.pid[axis].D8*prop1>>7; // was /100, is /128 now
    } else {      // YAW
      rcCommand[axis] = tmp;
    }
    if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis];
  }
  tmp = constrain(rcData[THROTTLE],MINCHECK,2000);
  tmp = (uint32_t)(tmp-MINCHECK)*2559/(2000-MINCHECK); // [MINCHECK;2000] -> [0;2559]
  tmp2 = tmp/256; // range [0;9]
  rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*256) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 256; // [0;2559] -> expo -> [conf.minthrottle;MAXTHROTTLE]

  #if defined(HEADFREE)
    if(f.HEADFREE_MODE) { //to optimize
      float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
      float cosDiff = cos(radDiff);
      float sinDiff = sin(radDiff);
      int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff;
      rcCommand[ROLL] =  rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff; 
      rcCommand[PITCH] = rcCommand_PITCH;
    }
  #endif

  // query at most one multiplexed analog channel per MWii cycle
  static uint8_t analogReader =0;
  switch (analogReader++%3) {
  #if defined(POWERMETER_HARD)
  case 0:
  {
    uint16_t pMeterRaw; // used for current reading
    static uint32_t lastRead = currentTime;
    static uint8_t ind = 0;
    static uint16_t pvec[PSENSOR_SMOOTH], psum;
    uint16_t p =  analogRead(PSENSORPIN);
    //LCDprintInt16(p); LCDcrlf();
    //debug[0] = p;
    #if PSENSOR_SMOOTH != 1
      psum += p;
      psum -= pvec[ind];
      pvec[ind++] = p;
      ind %= PSENSOR_SMOOTH;
      p = psum / PSENSOR_SMOOTH;
    #endif
    powerValue = ( conf.psensornull > p ? conf.psensornull - p : p - conf.psensornull); // do not use abs(), it would induce implicit cast to uint and overrun
    analog.amperage = powerValue * conf.pint2ma;
    pMeter[PMOTOR_SUM] += ((currentTime-lastRead) * (uint32_t)((uint32_t)powerValue*conf.pint2ma))/100000; // [10 mA * msec]
    lastRead = currentTime;
    break;
  }
  #endif // POWERMETER_HARD

  #if defined(VBAT)
  case 1:
  {
      static uint8_t ind = 0;
      static uint16_t vvec[VBAT_SMOOTH], vsum;
      uint16_t v = analogRead(V_BATPIN);
      //debug[1] = v;
      #if VBAT_SMOOTH == 1
        analog.vbat = (v<<4) / conf.vbatscale; // result is Vbatt in 0.1V steps
      #else
        vsum += v;
        vsum -= vvec[ind];
        vvec[ind++] = v;
        ind %= VBAT_SMOOTH;
        #if VBAT_SMOOTH == 16
          analog.vbat = vsum / conf.vbatscale; // result is Vbatt in 0.1V steps
        #elif VBAT_SMOOTH < 16
          analog.vbat = (vsum * (16/VBAT_SMOOTH)) / conf.vbatscale; // result is Vbatt in 0.1V steps
        #else
          analog.vbat = ((vsum /VBAT_SMOOTH) * 16) / conf.vbatscale; // result is Vbatt in 0.1V steps
        #endif
      #endif
      break;
  }
  #endif // VBAT
  #if defined(RX_RSSI)
  case 2:
  {
    static uint8_t ind = 0;
    static uint16_t rvec[RSSI_SMOOTH], rsum;
    uint16_t r = analogRead(RX_RSSI_PIN);
    #if RSSI_SMOOTH == 1
      analog.rssi = r;
    #else
      rsum += r;
      rsum -= rvec[ind];
      rvec[ind++] = r;
      ind %= RSSI_SMOOTH;
      r = rsum / RSSI_SMOOTH;
      analog.rssi = r;
    #endif
    break;
  }
  #endif
  } // end of switch()

  #if defined(BUZZER)
    alarmHandler(); // external buzzer routine that handles buzzer events globally now
  #endif


  if ( (calibratingA>0 && ACC ) || (calibratingG>0) ) { // Calibration phasis
    LEDPIN_TOGGLE;
  } else {
    if (f.ACC_CALIBRATED) {LEDPIN_OFF;}
    if (f.ARMED) {LEDPIN_ON;}
  }

  #if defined(LED_RING)
    static uint32_t LEDTime;
    if ( currentTime > LEDTime ) {
      LEDTime = currentTime + 50000;
      i2CLedRingState();
    }
  #endif

  #if defined(LED_FLASHER)
    auto_switch_led_flasher();
  #endif

  if ( currentTime > calibratedAccTime ) {
    if (! f.SMALL_ANGLES_25) {
      // the multi uses ACC and is not calibrated or is too much inclinated
      f.ACC_CALIBRATED = 0;
      LEDPIN_TOGGLE;
      calibratedAccTime = currentTime + 100000;
    } else {
      f.ACC_CALIBRATED = 1;
    }
  }

  #if !(defined(SPEKTRUM) && defined(PROMINI))  //Only one serial port on ProMini.  Skip serial com if Spektrum Sat in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0.
    #if defined(GPS_PROMINI)
      if(GPS_Enable == 0) {serialCom();}
    #else
      serialCom();
    #endif
  #endif

  #if defined(POWERMETER)
    analog.intPowerMeterSum = (pMeter[PMOTOR_SUM]/PLEVELDIV);
    intPowerTrigger1 = conf.powerTrigger1 * PLEVELSCALE; 
  #endif

  #ifdef LCD_TELEMETRY_AUTO
    static char telemetryAutoSequence []  = LCD_TELEMETRY_AUTO;
    static uint8_t telemetryAutoIndex = 0;
    static uint16_t telemetryAutoTimer = 0;
    if ( (telemetry_auto) && (! (++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ) )  ){
      telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)];
      LCDclear(); // make sure to clear away remnants
    }
  #endif  
  #ifdef LCD_TELEMETRY
    static uint16_t telemetryTimer = 0;
    if (! (++telemetryTimer % LCD_TELEMETRY_FREQ)) {
      #if (LCD_TELEMETRY_DEBUG+0 > 0)
        telemetry = LCD_TELEMETRY_DEBUG;
      #endif
      if (telemetry) lcd_telemetry();
    }
  #endif

  #if GPS & defined(GPS_LED_INDICATOR)       // modified by MIS to use STABLEPIN LED for number of sattelites indication
    static uint32_t GPSLEDTime;              // - No GPS FIX -> LED blink at speed of incoming GPS frames
    static uint8_t blcnt;                    // - Fix and sat no. bellow 5 -> LED off
    if(currentTime > GPSLEDTime) {           // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ...
      if(f.GPS_FIX && GPS_numSat >= 5) {
        if(++blcnt > 2*GPS_numSat) blcnt = 0;
        GPSLEDTime = currentTime + 150000;
        if(blcnt >= 10 && ((blcnt%2) == 0)) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
      }else{
        if((GPS_update == 1) && !f.GPS_FIX) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
        blcnt = 0;
      }
    }
  #endif

  #if defined(LOG_VALUES) && (LOG_VALUES >= 2)
    if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // remember highscore
    if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // remember lowscore
  #endif
  if (f.ARMED)  {
    #if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT)
      armedTime += (uint32_t)cycleTime;
    #endif
    #if defined(VBAT)
      if ( (analog.vbat > NO_VBAT) && (analog.vbat < vbatMin) ) vbatMin = analog.vbat;
    #endif
    #ifdef LCD_TELEMETRY
      #if BARO
        if ( (alt.EstAlt > BAROaltMax) ) BAROaltMax = alt.EstAlt;
      #endif
      #if GPS
        if ( (GPS_speed > GPS_speedMax) ) GPS_speedMax = GPS_speed;
      #endif
    #endif
  }
}

void setup() {
  #if !defined(GPS_PROMINI)
    SerialOpen(0,SERIAL0_COM_SPEED);
    #if defined(PROMICRO)
      SerialOpen(1,SERIAL1_COM_SPEED);
    #endif
    #if defined(MEGA)
      SerialOpen(1,SERIAL1_COM_SPEED);
      SerialOpen(2,SERIAL2_COM_SPEED);
      SerialOpen(3,SERIAL3_COM_SPEED);
    #endif
  #endif
  LEDPIN_PINMODE;
  POWERPIN_PINMODE;
  BUZZERPIN_PINMODE;
  STABLEPIN_PINMODE;
  POWERPIN_OFF;
  initOutput();
  readGlobalSet();
  #ifndef NO_FLASH_CHECK
    #if defined(MEGA)
      uint16_t i = 65000;                             // only first ~64K for mega board due to pgm_read_byte limitation
    #else
      uint16_t i = 32000;
    #endif
    uint16_t flashsum = 0;
    uint8_t pbyt;
    while(i--) {
      pbyt =  pgm_read_byte(i);        // calculate flash checksum
      flashsum += pbyt;
      flashsum ^= (pbyt<<8);
    }
  #endif
  #ifdef MULTIPLE_CONFIGURATION_PROFILES
    global_conf.currentSet=2;
  #else
    global_conf.currentSet=0;
  #endif
  while(1) {                                                    // check settings integrity
  #ifndef NO_FLASH_CHECK
    if(readEEPROM()) {                                          // check current setting integrity
      if(flashsum != global_conf.flashsum) update_constants();  // update constants if firmware is changed and integrity is OK
    }
  #else
    readEEPROM();                                               // check current setting integrity
  #endif  
    if(global_conf.currentSet == 0) break;                      // all checks is done
    global_conf.currentSet--;                                   // next setting for check
  }
  readGlobalSet();                              // reload global settings for get last profile number
  #ifndef NO_FLASH_CHECK
    if(flashsum != global_conf.flashsum) {
      global_conf.flashsum = flashsum;          // new flash sum
      writeGlobalSet(1);                        // update flash sum in global config
    }
  #endif
  readEEPROM();                                 // load setting data from last used profile
  blinkLED(2,40,global_conf.currentSet+1);          
  configureReceiver();
  #if defined (PILOTLAMP) 
    PL_INIT;
  #endif
  #if defined(OPENLRSv2MULTI)
    initOpenLRS();
  #endif
  initSensors();
  #if GPS
    GPS_set_pids();
  #endif
  previousTime = micros();
  #if defined(GIMBAL)
   calibratingA = 512;
  #endif
  calibratingG = 512;
  calibratingB = 200;  // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
  #if defined(POWERMETER)
    for(uint8_t j=0; j<=PMOTOR_SUM; j++) pMeter[j]=0;
  #endif
  /************************************/
  #if GPS
    #if defined(GPS_SERIAL)
      GPS_SerialInit();
      for(uint8_t j=0;j<=5;j++){
        GPS_NewData(); 
        LEDPIN_ON
        delay(20);
        LEDPIN_OFF
        delay(80);
      }
      #if defined(GPS_PROMINI)
        if(!GPS_Present){
          SerialEnd(GPS_SERIAL);
          SerialOpen(0,SERIAL0_COM_SPEED);
        }
      #else
        GPS_Present = 1;
      #endif
      GPS_Enable = GPS_Present;    
    #else
      GPS_Enable = 1;
    #endif
  #endif
  
  #if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) || defined(OLED_DIGOLE) || defined(LCD_TELEMETRY_STEP)
    initLCD();
  #endif
  #ifdef LCD_TELEMETRY_DEBUG
    telemetry_auto = 1;
  #endif
  #ifdef LCD_CONF_DEBUG
    configurationLoop();
  #endif
  #ifdef LANDING_LIGHTS_DDR
    init_landing_lights();
  #endif
  #ifdef FASTER_ANALOG_READS
    ADCSRA |= _BV(ADPS2) ; ADCSRA &= ~_BV(ADPS1); ADCSRA &= ~_BV(ADPS0); // this speeds up analogRead without loosing too much resolution: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1208715493/11
  #endif
  #if defined(LED_FLASHER)
    init_led_flasher();
    led_flasher_set_sequence(LED_FLASHER_SEQUENCE);
  #endif
  f.SMALL_ANGLES_25=1; // important for gyro only conf
  #ifdef LOG_PERMANENT
    // read last stored set
    readPLog();
    plog.lifetime += plog.armed_time / 1000000;
    plog.start++;         // #powercycle/reset/initialize events
    // dump plog data to terminal
    #ifdef LOG_PERMANENT_SHOW_AT_STARTUP
      dumpPLog(0);
    #endif
    plog.armed_time = 0;   // lifetime in seconds
    //plog.running = 0;       // toggle on arm & disarm to monitor for clean shutdown vs. powercut
  #endif
  /********************************************************************/
/**** User code callbacks (BH) ****/
/********************************************************************/
// USER SETUP (BH)
#ifdef USER_SETUP
USER_SETUP
#endif 
/********************************************************************/
  debugmsg_append_str("initialization completed\n");
}

void go_arm() {
  if(calibratingG == 0
  #if defined(ONLYARMWHENFLAT)
    && f.ACC_CALIBRATED 
  #endif
  #if defined(FAILSAFE)
    && failsafeCnt < 2
  #endif
    ) {
    if(!f.ARMED && !f.BARO_MODE) { // arm now!
      f.ARMED = 1;
      #if defined(HEADFREE)
        headFreeModeHold = att.heading;
      #endif
      magHold = att.heading;
      #if defined(VBAT)
        if (analog.vbat > NO_VBAT) vbatMin = analog.vbat;
      #endif
      #ifdef LCD_TELEMETRY // reset some values when arming
        #if BARO
          BAROaltMax = alt.EstAlt;
        #endif
        #if GPS
          GPS_speedMax = 0;
        #endif
        #ifdef POWERMETER_HARD
          powerValueMaxMAH = 0;
        #endif
      #endif
      #ifdef LOG_PERMANENT
        plog.arm++;           // #arm events
        plog.running = 1;       // toggle on arm & disarm to monitor for clean shutdown vs. powercut
        // write now.
        writePLog();
      #endif
    }
  } else if(!f.ARMED) { 
    blinkLED(2,255,1);
    alarmArray[8] = 1;
  }
}
void go_disarm() {
  if (f.ARMED) {
    f.ARMED = 0;
    #ifdef LOG_PERMANENT
      plog.disarm++;        // #disarm events
      plog.armed_time = armedTime ;   // lifetime in seconds
      if (failsafeEvents) plog.failsafe++;      // #acitve failsafe @ disarm
      if (i2c_errors_count > 10) plog.i2c++;           // #i2c errs @ disarm
      plog.running = 0;       // toggle @ arm & disarm to monitor for clean shutdown vs. powercut
      // write now.
      writePLog();
    #endif
  }
}

// ******** Main Loop *********
void loop () {
  static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
  static uint8_t rcSticks;       // this hold sticks position for command combos
  uint8_t axis,i;
  int16_t error,errorAngle;
  int16_t delta;
  int16_t PTerm = 0,ITerm = 0,DTerm, PTermACC, ITermACC;
  static int16_t lastGyro[2] = {0,0};
  static int16_t errorAngleI[2] = {0,0};
#if PID_CONTROLLER == 1
  static int32_t errorGyroI_YAW;
  static int16_t delta1[2],delta2[2];
  static int16_t errorGyroI[2] = {0,0};
#elif PID_CONTROLLER == 2
  static int16_t delta1[3],delta2[3];
  static int32_t errorGyroI[3] = {0,0,0};
  static int16_t lastError[3] = {0,0,0};
  int16_t deltaSum;
  int16_t AngleRateTmp, RateError;
#endif
  static uint16_t rcTime  = 0;
  static int16_t initialThrottleHold;
  int16_t rc;
  int32_t prop = 0;

  #if defined(SPEKTRUM)
    if (spekFrameFlags == 0x01) readSpektrum();
  #endif

  #if defined(SBUS)
    if (spekFrameFlags == 0x01) readSBus();
  #endif

  #if defined(OPENLRSv2MULTI) 
    Read_OpenLRS_RC();
  #endif 

  #if defined(SPEKTRUM) || defined(SBUS)
  if ((spekFrameDone == 0x01) || ((int16_t)(currentTime-rcTime) >0 )) { 
    spekFrameDone = 0x00;
  #else
  if ((int16_t)(currentTime-rcTime) >0 ) { // 50Hz
  #endif
    rcTime = currentTime + 20000;
    computeRC();
    // Failsafe routine - added by MIS
    #if defined(FAILSAFE)
      if ( f.FAILSAFE_MODE && f.ARMED) {                 // Stabilize, and set Throttle to specified level
        for(i=0; i<3; i++) rcData[i] = MIDRC;            // after specified guard time after RC signal is lost (in 0.1sec)
          if(!f.FAILSAFE_RTH_ENABLE && !f.ADV_RTH_THRO_BLOCK )
     {
       rcData[THROTTLE] = conf.failsafe_throttle;
            if ((failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY))) {  // Turn OFF motors after specified Time (in 0.1sec)
              go_disarm();     // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
              f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
            }
     }
        failsafeEvents++;
      }
      if ( f.FAILSAFE_MODE && !f.ARMED) {  //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
          go_disarm();     // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
          f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
      }
      if(!f.FAILSAFE_RTH_ENABLE && !f.ADV_RTH_THRO_BLOCK ) failsafeCnt++;
    #endif
    // end of failsafe routine - next change is made with RcOptions setting

    // ------------------ STICKS COMMAND HANDLER --------------------
    // checking sticks positions
    uint8_t stTmp = 0;
    for(i=0;i<4;i++) {
      stTmp >>= 2;
      if(rcData[i] > MINCHECK) stTmp |= 0x80;      // check for MIN
      if(rcData[i] < MAXCHECK) stTmp |= 0x40;      // check for MAX
    }
    if(stTmp == rcSticks) {
      if(rcDelayCommand<250) rcDelayCommand++;
    } else rcDelayCommand = 0;
    rcSticks = stTmp;
    
    // perform actions    
    if (rcData[THROTTLE] <= MINCHECK) {            // THROTTLE at minimum
      #if !defined(FIXEDWING)
        errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
        #if PID_CONTROLLER == 1
          errorGyroI_YAW = 0;
        #elif PID_CONTROLLER == 2
          errorGyroI[YAW] = 0;
        #endif
        errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
      #endif
      if (conf.activate[BOXARM] > 0) {             // Arming/Disarming via ARM BOX
        if ( rcOptions[BOXARM] && f.OK_TO_ARM ) go_arm(); else if (f.ARMED) go_disarm();
      }
    }
    if(rcDelayCommand == 20) {
      if(f.ARMED) {                   // actions during armed
        #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW
          if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm();    // Disarm via YAW
        #endif
        #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
          if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm();    // Disarm via ROLL
        #endif
      } else {                        // actions during not armed
        i=0;
        if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {    // GYRO calibration
          calibratingG=512;
          #if GPS 
            GPS_reset_home_position();
          #endif
          #if BARO
            calibratingB=10;  // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
          #endif
        }
        #if defined(INFLIGHT_ACC_CALIBRATION)  
         else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI) {    // Inflight ACC calibration START/STOP
            if (AccInflightCalibrationMeasurementDone){                // trigger saving into eeprom after landing
              AccInflightCalibrationMeasurementDone = 0;
              AccInflightCalibrationSavetoEEProm = 1;
            }else{ 
              AccInflightCalibrationArmed = !AccInflightCalibrationArmed; 
              #if defined(BUZZER)
               if (AccInflightCalibrationArmed) alarmArray[0]=2; else   alarmArray[0]=3;
              #endif
            }
         } 
        #endif
        #ifdef MULTIPLE_CONFIGURATION_PROFILES
          if      (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1;    // ROLL left  -> Profile 1
          else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2;    // PITCH up   -> Profile 2
          else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3;    // ROLL right -> Profile 3
          if(i) {
            global_conf.currentSet = i-1;
            writeGlobalSet(0);
            readEEPROM();
            blinkLED(2,40,i);
            alarmArray[0] = i;
          }
        #endif
        if (rcSticks == THR_LO + YAW_HI + PIT_HI + ROL_CE) {            // Enter LCD config
          #if defined(LCD_CONF)
            configurationLoop(); // beginning LCD configuration
          #endif
          previousTime = micros();
        }
        #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW
          else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm();      // Arm via YAW
        #endif
        #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
          else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI) go_arm();      // Arm via ROLL
        #endif
        #ifdef LCD_TELEMETRY_AUTO
          else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {              // Auto telemetry ON/OFF
            if (telemetry_auto) {
              telemetry_auto = 0;
              telemetry = 0;
            } else
              telemetry_auto = 1;
          }
        #endif
        #ifdef LCD_TELEMETRY_STEP
          else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {              // Telemetry next step
            telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)];
            #if defined( OLED_I2C_128x64)
              if (telemetry != 0) i2c_OLED_init();
            #elif defined(OLED_DIGOLE)
              if (telemetry != 0) i2c_OLED_DIGOLE_init();
            #endif
            LCDclear();
          }
        #endif
        #if ACC
          else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512;     // throttle=max, yaw=left, pitch=min
        #endif
        #if MAG
          else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1;  // throttle=max, yaw=right, pitch=min
        #endif
        i=0;
        if      (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {conf.angleTrim[PITCH]+=2; i=1;}
        else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {conf.angleTrim[PITCH]-=2; i=1;}
        else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {conf.angleTrim[ROLL] +=2; i=1;}
        else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {conf.angleTrim[ROLL] -=2; i=1;}
        if (i) {
          writeParams(1);
          rcDelayCommand = 0;    // allow autorepetition
          #if defined(LED_RING)
            blinkLedRing();
          #endif
        }
      }
    }
    #if defined(LED_FLASHER)
      led_flasher_autoselect_sequence();
    #endif
    
    #if defined(INFLIGHT_ACC_CALIBRATION)
      if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement
        InflightcalibratingA = 50;
        AccInflightCalibrationArmed = 0;
      }  
      if (rcOptions[BOXCALIB]) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
        if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){
          InflightcalibratingA = 50;
        }
      }else if(AccInflightCalibrationMeasurementDone && !f.ARMED){
        AccInflightCalibrationMeasurementDone = 0;
        AccInflightCalibrationSavetoEEProm = 1;
      }
    #endif

    uint16_t auxState = 0;
    for(i=0;i<4;i++)
      auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
    for(i=0;i<CHECKBOXITEMS;i++)
      rcOptions[i] = (auxState & conf.activate[i])>0;

    // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false
    #if ACC
      if ( rcOptions[BOXANGLE] || f.FAILSAFE_MODE ) {
        // bumpless transfer to Level mode
        if (!f.ANGLE_MODE) 
        {
          f.ANGLE_MODE = 1;
          errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
        }
      } else {
        // failsafe support
        f.ANGLE_MODE = 0;
      }
      if ( rcOptions[BOXHORIZON] ) {
        f.ANGLE_MODE = 0;
        if (!f.HORIZON_MODE) {
          errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
          f.HORIZON_MODE = 1;
        }
      } else {
        f.HORIZON_MODE = 0;
      }
    #endif

    if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
    #if !defined(GPS_LED_INDICATOR)
      if (f.ANGLE_MODE || f.HORIZON_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
    #endif

    #if BARO
      #if (!defined(SUPPRESS_BARO_ALTHOLD))
        if (rcOptions[BOXBARO]||(f.FAILSAFE_RTH_ENABLE )) {
          if (!f.BARO_MODE) {
            f.BARO_MODE = 1;
            AltHold = alt.EstAlt;
            #if defined(ALT_HOLD_THROTTLE_MIDPOINT)
              initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT;
            #else
              initialThrottleHold = rcCommand[THROTTLE];
            #endif
            errorAltitudeI = 0;
            BaroPID=0;
          }
        } else {
          f.BARO_MODE = 0;
        }
      #endif
      #ifdef VARIOMETER
        if (rcOptions[BOXVARIO]) {
          if (!f.VARIO_MODE) {
            f.VARIO_MODE = 1;
          }
        } else {
          f.VARIO_MODE = 0;
        }
      #endif
    #endif
    #if MAG
      if (rcOptions[BOXMAG]||f.FAILSAFE_RTH_ENABLE) {
        if (!f.MAG_MODE) {
          f.MAG_MODE = 1;
          magHold = att.heading;
        }
      } else {
        f.MAG_MODE = 0;
      }
      #if defined(HEADFREE)
        if (rcOptions[BOXHEADFREE]) {
          if (!f.HEADFREE_MODE) {
            f.HEADFREE_MODE = 1;
          }
          #if defined(ADVANCED_HEADFREE)
            if ((f.GPS_FIX && GPS_numSat >= 5) && (GPS_distanceToHome > ADV_HEADFREE_RANGE) ) {
              if (GPS_directionToHome < 180)  {headFreeModeHold = GPS_directionToHome + 180;} else {headFreeModeHold = GPS_directionToHome - 180;}
            }
          #endif
        } else {
          f.HEADFREE_MODE = 0;
        }
        if (rcOptions[BOXHEADADJ]) {
          headFreeModeHold = att.heading; // acquire new heading
        }
      #endif
    #endif
                   
#if defined(FAILSAFE)
  if ( failsafeCnt > (5*FAILSAFE_DELAY) ){
    f.FAILSAFE_MODE =1;
    #if ACC
      f.ANGLE_MODE = 1;
    #endif
    for(i=0; i<3; i++) rcData[i] = MIDRC;
    rcData[THROTTLE] = conf.failsafe_throttle;
  #if defined(FIXEDWING)
    // No GPS  Failsafe and Force a soft left turn.
    f.PASSTHRU_MODE = 0;
    rcData[ROLL]=MIDRC-50;
  #else
    if(ADV_RTH){
      f.FAILSAFE_RTH_ENABLE = 1;
    }
  #endif
  }else{
    f.FAILSAFE_MODE =0;
    
    if(f.FAILSAFE_RTH_ENABLE){ //PatrikE
      f.BARO_MODE  = 0;
      f.MAG_MODE   = 0;
      GPS_reset_nav();
      f.FAILSAFE_RTH_ENABLE = 0;
      f.ADV_RTH_THRO_BLOCK = 0;
    }
  }
#endif
    #if GPS  //  TODO ADV RTH
      static uint8_t GPSNavReset = 1;
      if (f.GPS_FIX && GPS_numSat >= 5 ) {
        if (rcOptions[BOXGPSHOME] || f.FAILSAFE_RTH_ENABLE ) {  // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
          if (!f.GPS_HOME_MODE){
             // Start Rth in poshold and climb to safe Alt
             if( ADV_RTH && AltHold < SAFE_RTH_ALT*100 && GPS_distanceToHome >= 5 && f.BARO_MODE)
             {
                AltHold = SAFE_RTH_ALT*100;     
                f.GPS_HOME_MODE = 0;
                f.GPS_HOLD_MODE = 1;
                GPSNavReset = 0;
                GPS_hold[LAT] = GPS_coord[LAT];
                GPS_hold[LON] = GPS_coord[LON];
                GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
                nav_mode = NAV_MODE_POSHOLD;  
             }
             
             // Normal RTH or Switch to RTH at safe Alt
             if( !ADV_RTH || ( ADV_RTH && (alt.EstAlt >= (SAFE_RTH_ALT -1)*100 || GPS_distanceToHome <= 5 )))
             {
               f.GPS_HOME_MODE = 1;
               f.GPS_HOLD_MODE = 0;
               GPSNavReset = 0;
               GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
               nav_mode = NAV_MODE_WP;
             }
           }
          if(ADV_RTH )
          {
            f.ADV_RTH_THRO_BLOCK=1;
            if(GPS_distanceToHome <= 5 && f.BARO_MODE && f.GPS_HOME_MODE){
              f.ADV_RTH_THRO_BLOCK=0;
              
              if (abs(AltHold - alt.EstAlt)<100)AltHold-=100;
              
              static uint16_t FS_StopCounter=0;  // Test DISARM counter
              if (f.FAILSAFE_RTH_ENABLE && AltHold <= 0 &&  alt.EstAlt < 50) {FS_StopCounter ++;}else{FS_StopCounter=0;}
              if (FS_StopCounter > 500) go_disarm();
            }
          }
        } else {
          f.GPS_HOME_MODE = 0;
          f.ADV_RTH_THRO_BLOCK = 0;
          if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL])< AP_MODE && abs(rcCommand[PITCH]) < AP_MODE) {
            if (!f.GPS_HOLD_MODE) {
              f.GPS_HOLD_MODE = 1;
              GPSNavReset = 0;
              GPS_hold[LAT] = GPS_coord[LAT];
              GPS_hold[LON] = GPS_coord[LON];
              GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
              nav_mode = NAV_MODE_POSHOLD;
            }
          } else {
            f.GPS_HOLD_MODE = 0;
            f.GPS_HOME_MODE = 0;
            // both boxes are unselected here, nav is reset if not already done
            if (GPSNavReset == 0 ) {
              GPSNavReset = 1;
              GPS_reset_nav();
            }
          }
        }
      } else {
        f.GPS_HOME_MODE = 0;
        f.GPS_HOLD_MODE = 0;
        nav_mode = NAV_MODE_NONE;
      }
    #endif
    
    #if defined(FIXEDWING) || defined(HELICOPTER)
      if (rcOptions[BOXPASSTHRU] && !f.FAILSAFE_RTH_ENABLE ) {f.PASSTHRU_MODE = 1;}
      else {f.PASSTHRU_MODE = 0;}
    #endif
 
  } else { // not in rc loop
    static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes
    switch (taskOrder) {
      case 0:
        taskOrder++;
        #if MAG
          if (Mag_getADC() != 0) break; // 320 µs
        #endif
      case 1:
        taskOrder++;
        #if BARO
          if (Baro_update() != 0) break; // for MS baro: I2C set and get: 220 us  -  presure and temperature computation 160 us
        #endif
      case 2:
        taskOrder++;
        #if BARO
          if (getEstimatedAltitude() != 0) break; // 280 us
        #endif    
      case 3:
        taskOrder++;
        #if GPS
          if(GPS_Enable) {  
            if (GPS_Compute() != 0) break;  // performs computation on new frame only if present
            if (GPS_NewData() != 0) break;  // SERIAL: try to detect a new nav frame based on the current received buffer --- I2C: 160 us with no new data / 1250us! with new data 
          }
        #endif
      case 4:
        taskOrder=0;
        #if SONAR
          Sonar_update(); //debug[2] = sonarAlt;
        #endif
        #ifdef LANDING_LIGHTS_DDR
          auto_switch_landing_lights();
        #endif
        #ifdef VARIOMETER
          if (f.VARIO_MODE) vario_signaling();
        #endif
        break;
    }
  }
 // USER LOOPS (BH)
#if defined(USER_LOOP_FAST)
   static uint32_t userLoopFastTime;
   if (currentTime > userLoopFastTime) {
      //userLoopFastTime = currentTime + 20000; // 50 Hz
      userLoopFastTime = currentTime + 10000; // 100 Hz
      USER_LOOP_FAST;
   }
#endif
#if defined(USER_LOOP_SLOW)
   static uint32_t userLoopSlowTime;
   if (currentTime > userLoopSlowTime) {
      userLoopSlowTime = currentTime + 100000; // 10 Hz
      USER_LOOP_SLOW;
   }
#endif
#if defined(USER_LOOP_1HZ)
   static uint32_t userLoop1HzTime;
   if (currentTime > userLoop1HzTime) {
      userLoop1HzTime = currentTime + 1000000; // 1 Hz
      USER_LOOP_1HZ;
   }
#endif
  computeIMU();
  // Measure loop rate just afer reading the sensors
  currentTime = micros();
  cycleTime = currentTime - previousTime;
  previousTime = currentTime;

  //***********************************
  //**** Experimental FlightModes *****
  //***********************************
  #if defined(ACROTRAINER_MODE)
    if(f.ANGLE_MODE){
      if (abs(rcCommand[ROLL]) + abs(rcCommand[PITCH]) >= ACROTRAINER_MODE ) {
        f.ANGLE_MODE=0;
        f.HORIZON_MODE=0;
        f.MAG_MODE=0;
        f.BARO_MODE=0;
        f.GPS_HOME_MODE=0;
        f.GPS_HOLD_MODE=0;
      }
    }
  #endif

 //*********************************** 
 
  #if MAG
    if (abs(rcCommand[YAW]) <70 && f.MAG_MODE) {
      int16_t dif = att.heading - magHold;
      if (dif <= - 180) dif += 360;
      if (dif >= + 180) dif -= 360;
      if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.pid[PIDMAG].P8>>5;
    } else magHold = att.heading;
  #endif

  #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
    /* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease 
     * altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)
     */
    if (f.BARO_MODE) {
      static uint8_t isAltHoldChanged = 0;
      static int16_t AltHoldCorr = 0;
      if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE && !f.ADV_RTH_THRO_BLOCK) {
        // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
        AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold;
        if(abs(AltHoldCorr) > 512) {
          AltHold += AltHoldCorr/512;
          AltHoldCorr %= 512;
        }
        isAltHoldChanged = 1;
      } else if (isAltHoldChanged) {
        AltHold = alt.EstAlt;
        isAltHoldChanged = 0;
      }
      rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
    }
  #endif

  #if defined(THROTTLE_ANGLE_CORRECTION)
    if(f.ANGLE_MODE || f.HORIZON_MODE) {
       rcCommand[THROTTLE]+= throttleAngleCorrection;
    }
  #endif
  
  #if GPS
    if ( (f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME ) {
      float sin_yaw_y = sin(att.heading*0.0174532925f);
      float cos_yaw_x = cos(att.heading*0.0174532925f);
      #if defined(NAV_SLEW_RATE)     
        nav_rated[LON]   += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
        nav_rated[LAT]   += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
        GPS_angle[ROLL]   = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
        GPS_angle[PITCH]  = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
      #else 
        GPS_angle[ROLL]   = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
        GPS_angle[PITCH]  = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
      #endif
    } else {
      GPS_angle[ROLL]  = 0;
      GPS_angle[PITCH] = 0;
    }
  #endif

  //**** PITCH & ROLL & YAW PID ****
#if PID_CONTROLLER == 1 // evolved oldschool
  if ( f.HORIZON_MODE ) prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),512);

  // PITCH & ROLL
  for(axis=0;axis<2;axis++) {
    rc = rcCommand[axis]<<1;
    error = rc - imu.gyroData[axis];
    errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);       // WindUp   16 bits is ok here
    if (abs(imu.gyroData[axis])>640) errorGyroI[axis] = 0;

    ITerm = (errorGyroI[axis]>>7)*conf.pid[axis].I8>>6;                        // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000

    PTerm = mul(rc,conf.pid[axis].P8)>>6;
    
    if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC
      // 50 degrees max inclination
      errorAngle         = constrain(rc + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
      errorAngleI[axis]  = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);                                                // WindUp     //16 bits is ok here

      PTermACC           = mul(errorAngle,conf.pid[PIDLEVEL].P8)>>7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768   16 bits is ok for result

      int16_t limit      = conf.pid[PIDLEVEL].D8*5;
      PTermACC           = constrain(PTermACC,-limit,+limit);

      ITermACC           = mul(errorAngleI[axis],conf.pid[PIDLEVEL].I8)>>12;   // 32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result

      ITerm              = ITermACC + ((ITerm-ITermACC)*prop>>9);
      PTerm              = PTermACC + ((PTerm-PTermACC)*prop>>9);
    }

    PTerm -= mul(imu.gyroData[axis],dynP8[axis])>>6; // 32 bits is needed for calculation   

    delta          = imu.gyroData[axis] - lastGyro[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
    lastGyro[axis] = imu.gyroData[axis];
    DTerm          = delta1[axis]+delta2[axis]+delta;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
 
    DTerm = mul(DTerm,dynD8[axis])>>5;        // 32 bits is needed for calculation

    axisPID[axis] =  PTerm + ITerm - DTerm;
  }

  //YAW
  #define GYRO_P_MAX 300
  #define GYRO_I_MAX 250

  rc = mul(rcCommand[YAW] , (2*conf.yawRate + 30))  >> 5;

  error = rc - imu.gyroData[YAW];
  errorGyroI_YAW  += mul(error,conf.pid[YAW].I8);
  errorGyroI_YAW  = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28));
  if (abs(rc) > 50) errorGyroI_YAW = 0;
  
  PTerm = mul(error,conf.pid[YAW].P8)>>6;
  #ifndef COPTER_WITH_SERVO
    int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8;
    PTerm = constrain(PTerm,-limit,+limit);
  #endif
  
  ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX);
  
  axisPID[YAW] =  PTerm + ITerm;
  
#elif PID_CONTROLLER == 2 // alexK
  #define GYRO_I_MAX 256
  #define ACC_I_MAX 256
  prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500]

  //----------PID controller----------
  for(axis=0;axis<3;axis++) {
    //-----Get the desired angle rate depending on flight mode
    if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC
      // calculate error and limit the angle to 50 degrees max inclination
      errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
    }
    if (axis == 2) {//YAW is always gyro-controlled (MAG correction is applied to rcCommand)
      AngleRateTmp = (((int32_t) (conf.yawRate + 27) * rcCommand[2]) >> 5);
    } else {
      if (!f.ANGLE_MODE) {//control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
        AngleRateTmp = ((int32_t) (conf.rollPitchRate + 27) * rcCommand[axis]) >> 4;
        if (f.HORIZON_MODE) {
          //mix up angle error to desired AngleRateTmp to add a little auto-level feel
          AngleRateTmp += ((int32_t) errorAngle * conf.pid[PIDLEVEL].I8)>>8;
        }
      } else {//it's the ANGLE mode - control is angle based, so control loop is needed
        AngleRateTmp = ((int32_t) errorAngle * conf.pid[PIDLEVEL].P8)>>4;
      }
    }

    //--------low-level gyro-based PID. ----------
    //Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
    //-----calculate scaled error.AngleRates
    //multiplication of rcCommand corresponds to changing the sticks scaling here
    RateError = AngleRateTmp  - imu.gyroData[axis];

    //-----calculate P component
    PTerm = ((int32_t) RateError * conf.pid[axis].P8)>>7;

    //-----calculate I component
    //there should be no division before accumulating the error to integrator, because the precision would be reduced.
    //Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
    //Time correction (to avoid different I scaling for different builds based on average cycle time)
    //is normalized to cycle time = 2048.
    errorGyroI[axis]  += (((int32_t) RateError * cycleTime)>>11) * conf.pid[axis].I8;
    //limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
    //I coefficient (I8) moved before integration to make limiting independent from PID settings
    errorGyroI[axis]  = constrain(errorGyroI[axis], (int32_t) -GYRO_I_MAX<<13, (int32_t) +GYRO_I_MAX<<13);
    ITerm = errorGyroI[axis]>>13;

    //-----calculate D-term
    delta          = RateError - lastError[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
    lastError[axis] = RateError;

    //Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
    // would be scaled by different dt each time. Division by dT fixes that.
    delta = ((int32_t) delta * ((uint16_t)0xFFFF / (cycleTime>>4)))>>6;
    //add moving average here to reduce noise
    deltaSum       = delta1[axis]+delta2[axis]+delta;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;

    DTerm = (deltaSum*conf.pid[axis].D8)>>8;

    //-----calculate total PID output
    axisPID[axis] =  PTerm + ITerm + DTerm;
  }
#else
  #error "*** you must set PID_CONTROLLER to one existing implementation"
#endif
  mixTable();
  // do not update servos during unarmed calibration of sensors which are sensitive to vibration
  if ( (f.ARMED) || ((!calibratingG) && (!calibratingA)) ) writeServos();
  writeMotors();
}

Resulting this error:

Damn, that thing hard ...

Thanks,

Moraes

[Moraes]

Hello Moares,

#ifdef USER_SETUP
USER_SETUP
#endif

I merged this part at the end of the setup-function in MultiWii.cpp
void setup() {
# a lot of
# other code

#ifdef LOG_PERMANENT_SHOW_AT_STARTUP
dumpPLog(0);
#endif
plog.armed_time = 0; // lifetime in seconds
//plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut
#endif

/********************************************************************/
/**** User code callbacks (BH) ****/
/********************************************************************/
// USER SETUP (BH)
#ifdef USER_SETUP
USER_SETUP
#endif
/********************************************************************/
debugmsg_append_str("initialization completed\n");
}

// USER LOOPS (BH)
#if defined(USER_LOOP_FAST)
static uint32_t userLoopFastTime;
if (currentTime > userLoopFastTime) {
//userLoopFastTime = currentTime + 20000; // 50 Hz
userLoopFastTime = currentTime + 10000; // 100 Hz
USER_LOOP_FAST;
}
#endif
.....

I merged this part into the main loop in MultiWii.cpp before computeIMU():

<----------------

computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;

Finally, tested in many versions, today tested in clean 2.3 version, and when I click "verify", ever results in same error: 'userSetup' was not declared in this scope.

If possible, post your working version. I cannot ableto work

Thanks,

JM

[Moraes]

Anyone help? I test a clear multiwii 2.2 following stepsby the book, line 690 and 822, and still resulting errors when Verify compilation.

Anyone could make changes in pids with potentiometer as in the brandonhill video?

I would love to be able to do ...

hugs

[pihlerm]

I made it! It works in 2.3 )))

Based on JohhnyQuest's post i've made a slight change, so that I can use all three pots at the same time (Turnigy 9xR).
Only tested on my quad so far, so if anyone is willing to risk it, here is the code.

-> Works with Multiwii 3.2
-> Add UserCode.cpp, UserCode.h, BHTuning.cpp and BHTuning.h to the project.
-> apply patch to other files (either manually or with Tortoise)

How to configure multiple potentiometers?

In BHTuning.cpp there is a line

const uint8_t bhTune_iNumCh = 3;

Set this number to the number of AUX channels you can use for pot tuning. Then extend/shrink the rest of the config for each channel separately:

// PID loop to tune - ROLL, PITCH, YAW, PIDALT, PIDPOS, PIDPOSR, PIDNAVR, IDLEVEL or PIDMAG
const uint8_t bhTune_iKey[bhTune_iNumCh] = {PITCH,PITCH,PITCH};

// PID parameter to tune - P, I or D
const uint8_t bhTune_iParam[bhTune_iNumCh] = {P,I,D};

// parameter value range - see maximums above
const float bhTune_iValRange[bhTune_iNumCh][2] = {{1.5, 3.5},{0.01, 0.07},{15, 30}};

// RC ch. of TX potentiometer input (e.g. AUX1)
const uint8_t bhTune_iInputCh[bhTune_iNumCh] = {AUX2,AUX3,AUX4};