Changing PID Values with a Pot

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

Re: Changing PID Values with a Pot

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

Re: Changing PID Values with a Pot

Post by 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
Posts: 6
Joined: Wed Apr 17, 2013 1:03 pm

Re: Changing PID Values with a Pot

Post by 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
Posts: 5
Joined: Tue Sep 17, 2013 9:32 am

Re: Changing PID Values with a Pot

Post by 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
Posts: 17
Joined: Sun Oct 20, 2013 1:40 pm
Location: Athens, Greece

Re: Changing PID Values with a Pot

Post by argotera »

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

MHefny
Posts: 18
Joined: Sun Jan 05, 2014 4:27 pm
Location: Cairo, Egypt
Contact:

Re: Changing PID Values with a Pot

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

User avatar
Benik3
Posts: 25
Joined: Mon Aug 26, 2013 1:06 pm
Contact:

Re: Changing PID Values with a Pot

Post by 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
Posts: 4
Joined: Sun Aug 03, 2014 8:59 pm

Re: Changing PID Values with a Pot

Post by 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
Posts: 4
Joined: Sun Aug 03, 2014 8:59 pm

Re: Changing PID Values with a Pot

Post by JonnyQuest »

I made it! It works in 2.3 )))
Attachments
multiwiiPID_POD.rar
(2.71 KiB) Downloaded 381 times

DEG
Posts: 1
Joined: Wed Sep 03, 2014 7:08 pm

Re: Changing PID Values with a Pot

Post by DEG »

JonnyQuest wrote: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
Posts: 2
Joined: Tue Sep 09, 2014 9:29 am

Re: Changing PID Values with a Pot

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

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Changing PID Values with a Pot

Post by 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
Posts: 16
Joined: Wed Oct 15, 2014 6:24 am

Re: Changing PID Values with a Pot

Post by Moraes »

Please, please, please :cry: 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.

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


brandonhill wrote: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?


brandonhill wrote: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:
Moraes wrote:// ******** 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.


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


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


brandonhill wrote: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
Posts: 2
Joined: Tue Sep 09, 2014 9:29 am

Re: Changing PID Values with a Pot

Post by 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
Posts: 16
Joined: Wed Oct 15, 2014 6:24 am

Re: Changing PID Values with a Pot

Post by Moraes »

Hello AstromarioDD,


Thank you so much, please see this:

AstromarioDD wrote: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:

Moraes wrote: #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;


AstromarioDD wrote:
// 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:

Moraes wrote: #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:

error.jpg


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
Posts: 16
Joined: Wed Oct 15, 2014 6:24 am

Re: Changing PID Values with a Pot

Post by 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
Posts: 16
Joined: Wed Oct 15, 2014 6:24 am

Re: Changing PID Values with a Pot

Post by Moraes »

Moraes wrote: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:

error.jpg


Damn, that thing hard ...

Thanks,

Moraes

Moraes
Posts: 16
Joined: Wed Oct 15, 2014 6:24 am

Re: Changing PID Values with a Pot

Post by Moraes »

AstromarioDD wrote: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 :cry:

Thanks,

JM

Moraes
Posts: 16
Joined: Wed Oct 15, 2014 6:24 am

Re: Changing PID Values with a Pot

Post by 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
Posts: 23
Joined: Tue Dec 09, 2014 6:40 pm

Re: Changing PID Values with a Pot

Post by pihlerm »

JonnyQuest wrote: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};
Attachments
multiwiiPID_POD.zip
(3.77 KiB) Downloaded 265 times

raviteja
Posts: 5
Joined: Sat Jul 18, 2015 9:58 am

Re: Changing PID Values with a Pot

Post by raviteja »

sorg 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 :)



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.

thanks for the code...,but when added the code and compiled arduino ide is showing "Arduino: 1.6.5 (Windows 8), Board: "Arduino Pro or Pro Mini, ATmega328 (5V, 16 MHz)"

MultiWii.cpp: In function 'void annexCode()':
MultiWii.cpp:633: error: 'struct conf_t' has no member named 'P8'
conf.P8[PIDROLL] = kp;
^
MultiWii.cpp:634: error: 'struct conf_t' has no member named 'I8'
conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
^
MultiWii.cpp:635: error: 'struct conf_t' has no member named 'D8'
conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;
^
MultiWii.cpp:637: error: 'struct conf_t' has no member named 'P8'
conf.P8[PIDPITCH] = conf.P8[PIDROLL];
^
MultiWii.cpp:637: error: 'struct conf_t' has no member named 'P8'
conf.P8[PIDPITCH] = conf.P8[PIDROLL];
^
MultiWii.cpp:638: error: 'struct conf_t' has no member named 'I8'
conf.I8[PIDPITCH] = conf.I8[PIDROLL];
^
MultiWii.cpp:638: error: 'struct conf_t' has no member named 'I8'
conf.I8[PIDPITCH] = conf.I8[PIDROLL];
^
MultiWii.cpp:639: error: 'struct conf_t' has no member named 'D8'
conf.D8[PIDPITCH] = conf.D8[PIDROLL];
^
MultiWii.cpp:639: error: 'struct conf_t' has no member named 'D8'
conf.D8[PIDPITCH] = conf.D8[PIDROLL];
^
MultiWii.cpp:643: error: 'struct conf_t' has no member named 'P8'
conf.P8[PIDLEVEL] = kp;
^
MultiWii.cpp:644: error: 'struct conf_t' has no member named 'I8'
conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
^
MultiWii.cpp:645: error: 'struct conf_t' has no member named 'D8'
conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;
^
'struct conf_t' has no member named 'P8'


MultiWii.cpp: In function 'void annexCode()':
MultiWii.cpp:633: error: 'struct conf_t' has no member named 'P8'
conf.P8[PIDROLL] = kp;
^
MultiWii.cpp:634: error: 'struct conf_t' has no member named 'I8'
conf.I8[PIDROLL] = (kp << 1)/FREQ_TU;
^
MultiWii.cpp:635: error: 'struct conf_t' has no member named 'D8'
conf.D8[PIDROLL] = (kp * FREQ_TU) >> 3;
^
MultiWii.cpp:637: error: 'struct conf_t' has no member named 'P8'
conf.P8[PIDPITCH] = conf.P8[PIDROLL];
^
MultiWii.cpp:637: error: 'struct conf_t' has no member named 'P8'
conf.P8[PIDPITCH] = conf.P8[PIDROLL];
^
MultiWii.cpp:638: error: 'struct conf_t' has no member named 'I8'
conf.I8[PIDPITCH] = conf.I8[PIDROLL];
^
MultiWii.cpp:638: error: 'struct conf_t' has no member named 'I8'
conf.I8[PIDPITCH] = conf.I8[PIDROLL];
^
MultiWii.cpp:639: error: 'struct conf_t' has no member named 'D8'
conf.D8[PIDPITCH] = conf.D8[PIDROLL];
^
MultiWii.cpp:639: error: 'struct conf_t' has no member named 'D8'
conf.D8[PIDPITCH] = conf.D8[PIDROLL];
^
MultiWii.cpp:643: error: 'struct conf_t' has no member named 'P8'
conf.P8[PIDLEVEL] = kp;
^
MultiWii.cpp:644: error: 'struct conf_t' has no member named 'I8'
conf.I8[PIDLEVEL] = (kp << 1)/FREQ_TU;
^
MultiWii.cpp:645: error: 'struct conf_t' has no member named 'D8'
conf.D8[PIDLEVEL] = (kp * FREQ_TU) >> 3;
^
'struct conf_t' has no member named 'P8'

This report would have more information with
"Show verbose output during compilation"
enabled in File > Preferences.

".can u plz xplain in detail how to add the code to arduino ide and compile succesfully.... :cry: :roll: :? :? :? :( :( :(

raviteja
Posts: 5
Joined: Sat Jul 18, 2015 9:58 am

Re: Changing PID Values with a Pot

Post by raviteja »

pihlerm wrote:
JonnyQuest wrote: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};
,
thanks for the code,,,,.but when i compile this on arduino ide i am getting this error:::"Arduino: 1.6.5 (Windows 8), Board: "Arduino Pro or Pro Mini, ATmega328 (5V, 16 MHz)"

BHTuning.cpp: In function 'void bhTune_loopSlow()':
BHTuning.cpp:132: error: 'bhTune_bSettingInvalid' was not declared in this scope
if (!bhTune_bSettingInvalid(i)) {
^
'bhTune_bSettingInvalid' was not declared in this scope

This report would have more information with
"Show verbose output during compilation"
enabled in File > Preferences.
".can u plz rectify this,i am unable to find out... :( :? :? :?

Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

Re: Changing PID Values with a Pot

Post by Dennis D »

Hey All,

Thanks for the this good idea.
I have Tarot TL250 (quad) with Crius SE v2.5 & MW 2.4. I added an abbreviated version of this to my code for roll, pitch, level, altitude. I use PID_Controller 2

I have 5 chan tx so I used sticks to select which PID (used multiple profile selection code) and AUX1 (pot) to dial in the PID values.
Works great for roll and pitch (acro - gyro). My initial PID settings were almost stock. Dialed in new settings in only 2-3 mins (fast - good). PID was 2.8, 0.01, 7. New PID is 6.8, 0.045, 25. Quad feels great, like on rails!
Not much different for level (acc). Will try again.
Haven't tried altitude (baro) yet.
I disabled my mag because of susceptibility to interference. (One less PID to tune)

Dennis

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

Re: Changing PID Values with a Pot

Post by alll »

Denis, can you create a "patch" file for MW 2.4 ?
Thanks,
manu

Post Reply