Changing PID Values with a Pot

ApoC
Posts: 31
Joined: Fri Feb 10, 2012 2:16 pm

Changing PID Values with a Pot

Post by ApoC »

Hey

The Arducopter Guys have a nice feature, that makes it possible to tune a parameter with a Poti inflight.

Can we implement that for MWii?

So that we have a Checkbox in the GUI, there we say: Roll / Pitch P on Channel 7. From P = 0-10 maybe. On my TX ive a Poti on Channel 7 and can adjust it inflight.

Or would nobody use it?

Joachim08
Posts: 31
Joined: Sat Mar 17, 2012 10:10 am

Re: Changing PID Values with a Pot

Post by Joachim08 »

That would be a very handy and helpful thing.
Also the Mikrocopter guys have this kind of feature.

brandonhill
Posts: 1
Joined: Wed Feb 27, 2013 4:14 pm

Re: Changing PID Values with a Pot

Post by brandonhill »

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:

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
}


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();


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


... 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


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
*/


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.
Last edited by brandonhill on Sat Mar 23, 2013 11:34 am, edited 1 time in total.

User avatar
Rob
Posts: 77
Joined: Sun Apr 03, 2011 4:40 pm

Re: Changing PID Values with a Pot

Post by Rob »

One pot tuning for PID using Ziegler-Nichols calculation 100% result..... :


viewtopic.php?f=7&t=978

Rob keij

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Changing PID Values with a Pot

Post by Hamburger »

Rob wrote:One pot tuning for PID using Ziegler-Nichols calculation 100% result..... :
I am not sure I understand, do you mean '100% result' as in 100% perfectly usable result? The PID combo from this approach equals a hand tuned combo? Have you tried with different copters?

User avatar
Rob
Posts: 77
Joined: Sun Apr 03, 2011 4:40 pm

Re: Changing PID Values with a Pot

Post by Rob »

Yes,

I have tried with different copters.

With one parameter (tune) I have left pot for level PID en right pot for gyro PID.

Als I told.... A 100% Stable result.....

After landing I save the settings en disable the "Tune" parameter.

Rob

User avatar
Rob
Posts: 77
Joined: Sun Apr 03, 2011 4:40 pm

Re: Changing PID Values with a Pot

Post by Rob »

it's no problem for me to intergrate this software in the multiwii software, if there is interest.

Rob Keij

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Changing PID Values with a Pot

Post by Hamburger »

From what I understand you twist the pot until the copter feels rigbt?
Do you see a chance to replace this "feeling right " with a function?

User avatar
Rob
Posts: 77
Joined: Sun Apr 03, 2011 4:40 pm

Re: Changing PID Values with a Pot

Post by Rob »

@Hamburger

Yes I see that change, but first I have to implement my board to multiwii.
I use it in my own software written for a other C compiler.
but its now a reason to dive into the Multiwii software.
I have a board with ATMEGA644P to use a bootloader I use WIRING not Arduino.
But no problem to hold it compatible (I think).


Rob Keij

brm
Posts: 287
Joined: Mon Jun 25, 2012 12:00 pm

Re: Changing PID Values with a Pot

Post by brm »

Hamburger wrote:From what I understand you twist the pot until the copter feels rigbt?
Do you see a chance to replace this "feeling right " with a function?

de hamburger ....

you can define an oscilation level ... max whatever degrees you like.
now you need to define a slice of time where you measure the min max deviation from the stick input...

once you are able to do the measuring - then you very close to:
http://homepages.ihug.co.nz/~deblight/AUTResearch/papers/relay_autot.pdf

User avatar
Rob
Posts: 77
Joined: Sun Apr 03, 2011 4:40 pm

Re: Changing PID Values with a Pot

Post by Rob »

For my calculation I set the oscillation frequency on 3Hz if you use a Big and slow multicopter the oscillation is lower.
For most of my multicopters 3Hz is ok.
I always use a good fast ESC otherwise you can forget good tuning (specialy in bad situations strong wind etc..).


Freq = 3; // Frequency in Hz



Code: Select all

      if(PMTR_Tune){
         if(rcData[AUX3]>1700){ // Safe in eeprom
            Opslaan_PID_instellingen_in_Eeprom();
            PID_programmed=1;
         }   
         else if((rcData[AUX3]>1200)&&(PID_programmed==0)){
            PMTR_Acc_on_active; // anders kan deze uit staan!!!
            Freq = 3; // Frequentie in Hz
// P /10
// I /1000
// D   
/*

   Ziegler–Nichols method Control Type
          Kp           Ki        Kd
      P   0.50Ku    -              -
      PI   0.45Ku  1.2 Kp/Pu  -
      PID 0.60Ku  2 Kp/Pu     KpPu/8

Ku = input waarde 0...250
Kp = berekende P waarde (0,6 Ku)
Pu = de frequentie van oscillatie

*/
            PMTR.Gyro_Roll_Pitch_P   = (char)(((((constrain(rcData[AUX2],1000,2000))-1000)/4)*60)/100);
            tempp = PMTR.Gyro_Roll_Pitch_P; // waarde in een int, i.v.m. uitrekenen
            PMTR.Gyro_Roll_Pitch_I   = (char)((2*tempp)/Freq); //tempp hier niet nodig?
            PMTR.Gyro_Roll_Pitch_D    = (char)((tempp*Freq)/8); // tempp hier wel nodig
           
            PMTR.Acc_Roll_Pitch_P    = (char)(((((constrain(rcData[AUX1],1000,2000))-1000)/4)*45)/100);
            tempp = PMTR.Acc_Roll_Pitch_P; // waarde in een int, i.v.m. uitrekenen
            PMTR.Acc_Roll_Pitch_I    = (char)(((tempp*12)/10)/Freq);
         }
         else{
            PID_programmed=0;
            if(rcData[AUX1]>1500){   // ACC AAN
               PMTR_Acc_on_active;
            }
            else{               // ACC UIT
               PMTR_Acc_on_inactive;
            }     
         }
      }           
      else{

         if(rcData[AUX1]>1400){   // ACC AAN
            PMTR_Acc_on_active;
         }
         else{               // ACC UIT
            PMTR_Acc_on_inactive;
         }
         
         if(rcData[AUX3]>1200){
            OUT2_ON;
         }
         else{
            OUT2_OFF;
         }
      }


brm
Posts: 287
Joined: Mon Jun 25, 2012 12:00 pm

Re: Changing PID Values with a Pot

Post by brm »

hallo,
it makes sense to do this.
on smaller copters it would make sense to go higher - for example 6 hz.

as you already have done a few parts you are invited to present a protoype :)
im prepping my sw for fixed wing.
and as you can guess - for fixed wing the same principles do apply.

de groeten - rob
leuk weer eens andere taalen in code sections te leezen ...

obor
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by obor »

(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 :)
Last edited by obor on Thu Apr 04, 2013 1:27 pm, edited 2 times in total.

User avatar
Rob
Posts: 77
Joined: Sun Apr 03, 2011 4:40 pm

Re: Changing PID Values with a Pot

Post by Rob »

I know.... It works great.
Fine tuning not always needed.

Rob

4eprops
Posts: 7
Joined: Thu Apr 04, 2013 8:04 am

Re: Changing PID Values with a Pot

Post by 4eprops »

Hi.

Where in multiwii.ino your code must be inserted?

Thanks


obor wrote: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). Code below. Div_int is my own implementation of int division for efficiency.

Multiwii.ino

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] = div_int((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] = div_int((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 :)

obor
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by obor »

ah, yes i forgot this: at the end of the annexCode() function

4eprops
Posts: 7
Joined: Thu Apr 04, 2013 8:04 am

Re: Changing PID Values with a Pot

Post by 4eprops »

obor wrote:ah, yes i forgot this: at the end of the annexCode() function


Thank you very much Obor.

Anothe question:

which is the correct procedure to tune the quad with this new function?

4eprops
Posts: 7
Joined: Thu Apr 04, 2013 8:04 am

Re: Changing PID Values with a Pot

Post by 4eprops »

obor wrote:ah, yes i forgot this: at the end of the annexCode() function


When I try to compile the IDE returns

MultiWii.cpp: In function 'void annexCode()':
MultiWii:728: error: 'div_int' was not declared in this scope

What's wrong?

Sorry for my questions but I wish to try your tune function

obor
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by obor »

just replace all "div_int" by "/"

4eprops
Posts: 7
Joined: Thu Apr 04, 2013 8:04 am

Re: Changing PID Values with a Pot

Post by 4eprops »

obor wrote:just replace all "div_int" by "/"



Like this ((kp << 1)/FREQ_TU); ?

User avatar
alll
Posts: 220
Joined: Fri Dec 07, 2012 9:53 am

Re: Changing PID Values with a Pot

Post by 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
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by obor »

to 4eprops: yes <first post edited>

4eprops
Posts: 7
Joined: Thu Apr 04, 2013 8:04 am

Re: Changing PID Values with a Pot

Post by 4eprops »

obor wrote:to 4eprops: yes <first post edited>


All seems ok on the bench.

I'll try asap at the field

Thanks Obor

obor
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by 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
Posts: 7
Joined: Thu Apr 04, 2013 8:04 am

Re: Changing PID Values with a Pot

Post by 4eprops »

obor wrote: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
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by 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).

User avatar
alll
Posts: 220
Joined: Fri Dec 07, 2012 9:53 am

Re: Changing PID Values with a Pot

Post by alll »

obor wrote: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?
Image
Thanks,
manu
Attachments
multiwii22-max-gui-values.png
(20.85 KiB) Not downloaded yet

4eprops
Posts: 7
Joined: Thu Apr 04, 2013 8:04 am

Re: Changing PID Values with a Pot

Post by 4eprops »

obor wrote: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
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by obor »

alll wrote: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.

User avatar
alll
Posts: 220
Joined: Fri Dec 07, 2012 9:53 am

Re: Changing PID Values with a Pot

Post by 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
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by obor »


User avatar
alll
Posts: 220
Joined: Fri Dec 07, 2012 9:53 am

Re: Changing PID Values with a Pot

Post by 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
Posts: 26
Joined: Sun Mar 17, 2013 11:58 pm

Re: Changing PID Values with a Pot

Post by 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
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by sorg »

4eprops wrote:
obor wrote: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
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by sorg »

brandonhill wrote: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
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by sorg »

obor wrote:(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:)
Posts: 2
Joined: Tue Jun 18, 2013 6:17 am

Re: Changing PID Values with a Pot

Post by Hman:) »

obor wrote:(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
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by sorg »

Hman:) wrote:
obor wrote:(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
Posts: 244
Joined: Sat Mar 23, 2013 12:34 am
Location: Australia

Re: Changing PID Values with a Pot

Post by felixrising »

Do you have a video of this in action?

Hman:)
Posts: 2
Joined: Tue Jun 18, 2013 6:17 am

Re: Changing PID Values with a Pot

Post by Hman:) »

sorg wrote:
Hman:) wrote:
obor wrote:(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
Posts: 244
Joined: Sat Mar 23, 2013 12:34 am
Location: Australia

Re: Changing PID Values with a Pot

Post by 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
Posts: 244
Joined: Sat Mar 23, 2013 12:34 am
Location: Australia

Re: Changing PID Values with a Pot

Post by 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
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by 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
Posts: 244
Joined: Sat Mar 23, 2013 12:34 am
Location: Australia

Re: Changing PID Values with a Pot

Post by 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
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by 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
Posts: 244
Joined: Sat Mar 23, 2013 12:34 am
Location: Australia

Re: Changing PID Values with a Pot

Post by felixrising »

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

sorg
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by 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
Posts: 244
Joined: Sat Mar 23, 2013 12:34 am
Location: Australia

Re: Changing PID Values with a Pot

Post by 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
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by sorg »

felixrising wrote: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.

felixrising wrote:
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
Posts: 34
Joined: Mon Apr 08, 2013 2:49 pm

Re: Changing PID Values with a Pot

Post by 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.

Post Reply