inflight ROLL + PITCH PID tuning.

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

inflight ROLL + PITCH PID tuning.

Post by jaysonragasa »

Some MultiWii coding weekend..

Here's my experimental inflight ROLL + PITCH PID tuning I wrote last weekend. I scrapped my last experimental PID tuning code which you can tune your PID while disarmed.

So this weekend, I had an idea how to tune the PIDs while inflight.

I am using CRIUS v2 AIOP and MultiWii 2.4. My code could probably support MW2.3 as well.
I want to have an individual axis PID tuning but unfortunately, I don't have the GUI for it and reflashing my CRIUS board is getting boring so instead I just combined ROLL and PITCH instead.

So here's the sample video ->> https://www.youtube.com/watch?v=lVwk3JNQx6g
(I can't post the youtube here... not sure why it's not loading.. click the image instead to redirect to youtube)
Image


The only drawback with this is, well I don't have any way to do it, the drawback is, after tuning, you have to reupload the firmware with #define INFLIGHT_PID_CALIBRATION commented out.

Let me know what you think? I'll post the code if needed.
Last edited by jaysonragasa on Fri Mar 27, 2015 2:49 pm, edited 4 times in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: inflight ROLL + PITCH PID tuning.

Post by jaysonragasa »

MultiWii.cpp

add this line

Code: Select all

uint8_t   isin_inflight_calib = 0;
uint16_t  old_AUX1_val = 0;

before

Code: Select all

// **************
// gyro+acc IMU
// **************


add this line

Code: Select all

#define AUX2_LO  (1<<(AUX2))
#define AUX2_CE  (3<<(AUX2))
#define AUX2_HI  (2<<(AUX2))

in

Code: Select all

// ******************
// rc functions
// ******************


add this line in loop()

Code: Select all

  /* --------------------------------------------------------------------------------- */
  /* ----------------- EXPERIMENTAL INFLIGHT ROLL + PITCH PID TUNING ----------------- */
  /* --------------------------------------------------------------------------------- */
#if defined(INFLIGHT_PID_CALIBRATION)
  //if (conf.activate[BOXMAG])
  {
    //if (f.ARMED && f.ANGLE_MODE)
    if (f.ARMED)
    {
      /*
        AUX3 - you must use your Tx's pot
        AUX2 - use your Tx's 3 way switch
      */
      uint16_t potauxval = rcData[AUX3];
      uint16_t pidsel    = rcData[AUX2];

      // flag that we're in calibration
      isin_inflight_calib = 1;

      // set the old AUX3 value once
      // MAKE SURE BEFORE ARMING, THE AUX3 IS AT THE LOWEST POINT!!
      if (old_AUX1_val == 0) old_AUX1_val = potauxval;

      /* this should not be in this place
         but it was placed here for quick implementation
      */
      uint16_t new_pid = 0;
      uint16_t new_i = 0;
      uint16_t aux_pos = 0;

      if (potauxval < old_AUX1_val) { // make sure new_i is at always 0
        new_i = 0;
      }
      else {
        // get the difference from the new AUX3 value and old AUX3 value;
        new_i = (potauxval - old_AUX1_val);
      }

      debug[0] = old_AUX1_val; //old_AUX1_val;
      debug[1] = potauxval; //new_p;
      debug[2] = new_i; //rcData[AUX1 + 2];

      // set safe values
      if (new_i < 0) {
        new_i = 0;
      }
      else if (new_i > 1000) {
        new_i = 1000;
      }

      debug[3] = new_i; //rcData[AUX1 + 2];

      // check for 3 way switch position
      aux_pos >>= 2;
      if (pidsel > MINCHECK) aux_pos |= 0x20;     // check for MIN
      if (pidsel < MAXCHECK) aux_pos |= 0x40;     // check for MAX

      new_pid = new_i;

      if (aux_pos == AUX2_LO)
      {
        new_pid = new_pid / 5;
        if (new_pid > 200) new_pid = 200;
        conf.pid[ROLL].P8 = new_pid + 1;
        conf.pid[PITCH].P8 = new_pid + 1;
      }
      else if (aux_pos == AUX2_CE)
      {
        new_pid = new_pid / 4;
        if (new_pid > 250) new_pid = 250;
        conf.pid[ROLL].I8 = new_pid + 1;
        conf.pid[PITCH].I8 = new_pid + 1;
      }
      else if (aux_pos == AUX2_HI)
      {
        new_pid = new_pid / 10;
        if (new_pid > 100) new_pid = 100;
        conf.pid[ROLL].D8 = new_pid + 1;
        conf.pid[PITCH].D8 = new_pid + 1;
      }
    }

    if (!f.ARMED && isin_inflight_calib == 1) {
      isin_inflight_calib = 0;
      writeParams(1);
    }
  }
#endif

after this

Code: Select all

  //Used to communicate back nav angles to the GPS simulator (for HIL testing)
#if defined(GPS_SIMULATOR)
  SerialWrite(2, 0xa5);
  SerialWrite16(2, nav[LAT] + rcCommand[PITCH]);
  SerialWrite16(2, nav[LON] + rcCommand[ROLL]);
  SerialWrite16(2, (nav[LAT] + rcCommand[PITCH]) - (nav[LON] + rcCommand[ROLL])); //check
#endif

#endif //GPS


now in config.h
add this new paramater

Code: Select all

/*************************    INFLIGHT ROLL & PITCH PID Calibration    *****************************/
#define INFLIGHT_PID_CALIBRATION
Last edited by jaysonragasa on Tue Mar 24, 2015 1:11 am, edited 3 times in total.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: inflight ROLL + PITCH PID tuning.

Post by jaysonragasa »

I'm am updating the code to have a way to enable/disable the inflight tuning without having to reflash the firmware.. I'll post the code soon. for the mean time, you can test the code here

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: inflight ROLL + PITCH PID tuning.

Post by jaysonragasa »

just a quick update to the code.

I commented out this line!!

Code: Select all

//if (conf.activate[BOXMAG])

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: inflight ROLL + PITCH PID tuning.

Post by jaysonragasa »

now I can enable/disable PID tuning using box(aux)

should be "PID TUNE"
2015-03-24_1015.png

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: inflight ROLL + PITCH PID tuning.

Post by jaysonragasa »

latest update: March 25, 2015
-- updated code
-- added new box option called "PID TUNE" so you can enable/disable the tuning with your AUX channel.

let's start with headers first
config.h - add a new config definition

Code: Select all

/*************************    INFLIGHT ROLL & PITCH PID Calibration    *****************************/
#define INFLIGHT_PID_TUNING

before this line

Code: Select all

#endif /* CONFIG_H_ */


types.h - add new box for the GUI

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
  BOXPIDTUNE,
#endif

after this line

Code: Select all

#if GPS
  BOXGPSNAV,
  BOXLAND,
#endif

note: it's important to have the new BOX before the CHECKBOXITEMS

add this line

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
  uint8_t PIDTUNE_MODE :1;
#endif

after this line

Code: Select all

#if GPS
  uint8_t GPS_FIX :1 ;
  uint8_t GPS_FIX_HOME :1 ;
  uint8_t GPS_BARO_MODE : 1;         // This flag is used when GPS controls baro mode instead of user (it will replace rcOptions[BARO]
  uint8_t GPS_head_set: 1;           // it is 1 if the navigation engine got commands to control heading (SET_POI or SET_HEAD) CLEAR_HEAD will zero it
  uint8_t LAND_COMPLETED: 1;
  uint8_t LAND_IN_PROGRESS: 1;
#endif


then the CPP files. let's start with
Protocol.cpp
add this line

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
      if (f.PIDTUNE_MODE) tmp |= 1 << BOXPIDTUNE;
#endif

after this code

Code: Select all

#if defined(OSD_SWITCH)
      if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif


then MultiWii.cpp
add this line - that should define the box name

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
  "PID TUNE;"
#endif

after this code

Code: Select all

#if GPS
  "MISSION;"
  "LAND;"
#endif


add this line

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
  22, //"PID CALIB;"
#endif

after this code

Code: Select all

#if GPS
  20, //"MISSION;"
  21, //"LAND;"
#endif


then this line

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
uint8_t      isin_inflight_calib = 0;
uint16_t   old_AUX1_val = 0;

uint16_t   new_pid = 0;
uint16_t   new_i = 0;
uint16_t   aux_pos = 0;
#endif

after this line

Code: Select all

int32_t  AltHold; // in cm
int16_t  sonarAlt;
int16_t  BaroPID = 0;
int16_t  errorAltitudeI = 0;


then let's add new method

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
/* --------------------------------------------------------------------------------- */
/* ----------------- EXPERIMENTAL INFLIGHT ROLL + PITCH PID TUNING ----------------- */
/* --------------------------------------------------------------------------------- */
void go_inflight_pid_tuning() {
   debug[3] = conf.activate[BOXARM];

   if (f.PIDTUNE_MODE)
   {
      //if (f.ARMED && f.ANGLE_MODE)
      if (f.ARMED)
      {
         /*
         AUX3 - you must use your Tx's pot
         AUX2 - use your Tx's 3 way switch
         */
         uint16_t potauxval = rcData[AUX3];
         uint16_t pidsel = rcData[AUX2];

         // flag that we're in calibration
         isin_inflight_calib = 1;

         // set the old AUX3 value once
         // MAKE SURE BEFORE ARMING, THE AUX3 IS AT THE LOWEST POINT!!
         if (old_AUX1_val == 0) old_AUX1_val = potauxval;

         new_pid = 0;
         new_i = 0;
         aux_pos = 0;

         if (potauxval < old_AUX1_val) { // make sure new_i is at always 0
            new_i = 0;
         }
         else {
            // get the difference from the new AUX3 value and old AUX3 value;
            new_i = (potauxval - old_AUX1_val);
         }

         debug[0] = old_AUX1_val; //old_AUX1_val;
         debug[1] = potauxval; //new_p;
         debug[2] = new_i; //rcData[AUX1 + 2];

         // set safe values
         if (new_i < 0) {
            new_i = 0;
         }
         else if (new_i > 1000) {
            new_i = 1000;
         }

         debug[3] = new_i; //rcData[AUX1 + 2];

         // check for 3 way switch position
         aux_pos >>= 2;
         if (pidsel > MINCHECK) aux_pos |= 0x20;     // check for MIN
         if (pidsel < MAXCHECK) aux_pos |= 0x40;     // check for MAX

         new_pid = new_i;

         if (aux_pos == AUX2_LO)
         {
            new_pid = new_pid / 5;
            if (new_pid > 200) new_pid = 200;
            conf.pid[ROLL].P8 = new_pid + 1;
            conf.pid[PITCH].P8 = new_pid + 1;
         }
         else if (aux_pos == AUX2_CE)
         {
            new_pid = new_pid / 4;
            if (new_pid > 250) new_pid = 250;
            conf.pid[ROLL].I8 = new_pid + 1;
            conf.pid[PITCH].I8 = new_pid + 1;
         }
         else if (aux_pos == AUX2_HI)
         {
            new_pid = new_pid / 10;
            if (new_pid > 100) new_pid = 100;
            conf.pid[ROLL].D8 = new_pid + 1;
            conf.pid[PITCH].D8 = new_pid + 1;
         }
      }
   }

   if (!f.ARMED && !f.PIDTUNE_MODE) {
      if (isin_inflight_calib == 1) {
         isin_inflight_calib = 0;
         writeParams(1);
      }
   }
}
#endif

before the go_disarm() method

Code: Select all

void go_disarm() {
   .
   .
   .
}


then this line

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
      if (rcOptions[BOXPIDTUNE]) {
         f.PIDTUNE_MODE = 1;
      }
      else {
         f.PIDTUNE_MODE = 0;
      }
#endif

after this code

Code: Select all

#if ACC
      if (rcOptions[BOXANGLE] || (failsafeCnt > 5 * FAILSAFE_DELAY)) {
         // bumpless transfer to Level mode
         if (!f.ANGLE_MODE) {
            errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
            f.ANGLE_MODE = 1;
         }
      }
      else {
         if (f.ANGLE_MODE) {
            errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
         }
         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 {
         if (f.HORIZON_MODE) {
            errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
         }
         f.HORIZON_MODE = 0;
      }
#endif


and finally this line

Code: Select all

#if defined(INFLIGHT_PID_TUNING)
   // call inflight pid tuning
   go_inflight_pid_tuning();
#endif

after this code

Code: Select all

#if defined(GPS_SIMULATOR)
   SerialWrite(2, 0xa5);
   SerialWrite16(2, nav[LAT] + rcCommand[PITCH]);
   SerialWrite16(2, nav[LON] + rcCommand[ROLL]);
   SerialWrite16(2, (nav[LAT] + rcCommand[PITCH]) - (nav[LON] + rcCommand[ROLL])); //check
#endif

#endif //GPS


let me know if you have some trouble implementing it.

User avatar
jaysonragasa
Posts: 53
Joined: Wed Jan 28, 2015 6:40 am
Location: Philippines
Contact:

Re: inflight ROLL + PITCH PID tuning.

Post by jaysonragasa »

just an update. just in case you want to tune your ALT as well

in config.h file, just below #define INFLIGHT_PID_TUNING, add this

Code: Select all

#define INFLIGHT_PID_TUNING_TYPE 0 // 0 for PITCH and ROLL
                           // 1 for PITCH
                           // 2 for ROLL
                                   // 3 for PIDALT (Altitude)


then in method go_inflight_pid_tuning() in MultiWii.cpp file, look for this line

Code: Select all

if (aux_pos == AUX2_LO)
{
   new_pid = new_pid / 5;
   if (new_pid > 200) new_pid = 200;
   conf.pid[ROLL].P8 = new_pid + 1;
   conf.pid[PITCH].P8 = new_pid + 1;
}
else if (aux_pos == AUX2_CE)
{
   new_pid = new_pid / 4;
   if (new_pid > 250) new_pid = 250;
   conf.pid[ROLL].I8 = new_pid + 1;
   conf.pid[PITCH].I8 = new_pid + 1;
}
else if (aux_pos == AUX2_HI)
{
   new_pid = new_pid / 10;
   if (new_pid > 100) new_pid = 100;
   conf.pid[ROLL].D8 = new_pid + 1;
   conf.pid[PITCH].D8 = new_pid + 1;
}


and change it to

Code: Select all

if (aux_pos == AUX2_LO)
{
   new_pid = new_pid / 5;
   if (new_pid > 200) new_pid = 200;

   switch (INFLIGHT_PID_TUNING_TYPE) {
   case 0:
      conf.pid[ROLL].P8 = new_pid + 1;
      conf.pid[PITCH].P8 = new_pid + 1;
      break;
   case 1:
      conf.pid[PITCH].P8 = new_pid + 1;
      break;
   case 2:
      conf.pid[ROLL].P8 = new_pid + 1;
      break;
   case 3:
      conf.pid[PIDALT].P8 = new_pid + 1;
      break;
   }
}
else if (aux_pos == AUX2_CE)
{
   new_pid = new_pid / 4;
   if (new_pid > 250) new_pid = 250;
   switch (INFLIGHT_PID_TUNING_TYPE) {
   case 0:
      conf.pid[ROLL].I8 = new_pid + 1;
      conf.pid[PITCH].I8 = new_pid + 1;
      break;
   case 1:
      conf.pid[PITCH].I8 = new_pid + 1;
      break;
   case 2:
      conf.pid[ROLL].I8 = new_pid + 1;
      break;
   case 3:
      conf.pid[PIDALT].I8 = new_pid + 1;
      break;
   }
}
else if (aux_pos == AUX2_HI)
{
   new_pid = new_pid / 10;
   if (new_pid > 100) new_pid = 100;
   switch (INFLIGHT_PID_TUNING_TYPE) {
   case 0:
      conf.pid[ROLL].D8 = new_pid + 1;
      conf.pid[PITCH].D8 = new_pid + 1;
      break;
   case 1:
      conf.pid[PITCH].D8 = new_pid + 1;
      break;
   case 2:
      conf.pid[ROLL].D8 = new_pid + 1;
      break;
   case 3:
      conf.pid[PIDALT].D8 = new_pid + 1;
      break;
   }
}


I just got the Sonar HC-SR04 working today with MultiWii 2.4 and I have to tune the ALT as well so I thought of updating my code.

Maine_Guy
Posts: 27
Joined: Fri Mar 06, 2015 7:07 pm

Re: inflight ROLL + PITCH PID tuning.

Post by Maine_Guy »

Thanks for sharing, got this implemented and functioning. I added some functionality.

I added a few lines of code to extract the PID levels and send them back via telemetry.

The PID levels are stored in variables (like PIDtelemetryP) - then spit out via SBUS telemetry back to my radio.

I used these telemetry channels to display the PID values:
Temperature1
Temperature2
Current (10x)

This allows you to check the levels from the radio and also log them as flight is occurring.


Code: Select all

         if (aux_pos == AUX2_LO)
          {
             new_pid = new_pid / 5;
             if (new_pid > 200) new_pid = 200;
         
             switch (INFLIGHT_PID_TUNING_TYPE) {
             case 0:
                conf.pid[ROLL].P8 = new_pid + 1;
                conf.pid[PITCH].P8 = new_pid + 1;
                PIDtelemetryP = conf.pid[PITCH].P8;
                break;
             case 1:
                conf.pid[PITCH].P8 = new_pid + 1;
                PIDtelemetryP = conf.pid[PITCH].P8;
                break;
             case 2:
                conf.pid[ROLL].P8 = new_pid + 1;
                PIDtelemetryP = conf.pid[ROLL].P8;
                break;
             case 3:
                conf.pid[PIDALT].P8 = new_pid + 1;
                PIDtelemetryP = conf.pid[PIDALT].P8;
                break;
             }
          }
          else if (aux_pos == AUX2_CE)
          {
             new_pid = new_pid / 4;
             if (new_pid > 250) new_pid = 250;
             switch (INFLIGHT_PID_TUNING_TYPE) {
             case 0:
                conf.pid[ROLL].I8 = new_pid + 1;
                conf.pid[PITCH].I8 = new_pid + 1;
                PIDtelemetryI = conf.pid[PITCH].I8;
                break;
             case 1:
                conf.pid[PITCH].I8 = new_pid + 1;
                PIDtelemetryI = conf.pid[PITCH].I8;
                break;
             case 2:
                conf.pid[ROLL].I8 = new_pid + 1;
                PIDtelemetryI = conf.pid[ROLL].I8;
                break;
             case 3:
                conf.pid[PIDALT].I8 = new_pid + 1;
                PIDtelemetryI = conf.pid[PIDALT].I8;
                break;
             }
          }
          else if (aux_pos == AUX2_HI)
          {
             new_pid = new_pid / 10;
             if (new_pid > 100) new_pid = 100;
             switch (INFLIGHT_PID_TUNING_TYPE) {
             case 0:
                conf.pid[ROLL].D8 = new_pid + 1;
                conf.pid[PITCH].D8 = new_pid + 1;
                PIDtelemetryD = conf.pid[PITCH].D8;
                break;
             case 1:
                conf.pid[PITCH].D8 = new_pid + 1;
                PIDtelemetryD = conf.pid[PITCH].D8;
                break;
             case 2:
                conf.pid[ROLL].D8 = new_pid + 1;
                PIDtelemetryD = conf.pid[ROLL].D8;
                break;
             case 3:
                conf.pid[PIDALT].D8 = new_pid + 1;
                PIDtelemetryD = conf.pid[PIDALT].D8;
                break;
             }
          }
      }

Post Reply