I improved the barocode of dev_20120528

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
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

I improved the barocode of dev_20120528

Post by Crashpilot1000 »

Hello Alexinparis, hello to all!

I see you are very busy and successful implementing GPS in your outstanding MWII project.
My goal was to improve the baro/alt hold performance.
I have no experience (now 11 days) in C/Arduino programming - i did some MC68000 assembler on the "Amiga" 20 years ago.
So i have very limited abilities. I can not program a PID controller/kalmanfilter/moving average filter etc.
I found some logical issues in the current Baro code - and solutions. My solutions are definitely not perfect
but help the alt hold performance. The changes affect the Main/IMU/Sensors sections.

Example: dev_20120528

Main program issues:
====================

Original:

Code: Select all

....
  #if BARO
    if (baroMode) {
      if (abs(rcCommand[THROTTLE]-initialThrottleHold)>20) {
         baroMode = 0; // so that a new althold reference is defined
      }
      rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
    }
  #endif
.....

The code above has several problems:
The throttlecommand is not given to the motors and when the user moves the throttlestick out of range a new hight is locked.
So a random hight change is taken as a wanted new locked hight. This has absolutely no logic.
Possible solution: The user moves the ThrStick out of range (20 or so) AND the copter has changed the hight significantly (out of sensor noise)
THEN lock the new hight.


IMU/Sensors issues:
===================

Original:

Code: Select all

.....IMU.......

#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY      4000000  // 4 sec initialization delay
#define BARO_TAB_SIZE   40

void getEstimatedAltitude(){
  uint8_t index;
  static uint32_t deadLine = INIT_DELAY;

  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx;
  static int32_t BaroHigh,BaroLow;
  int32_t temp32;
  int16_t last;
 
  if (currentTime < deadLine) return;
  deadLine = currentTime + UPDATE_INTERVAL;

  //**** Alt. Set Point stabilization PID ****
  //calculate speed for D calculation
  last = BaroHistTab[BaroHistIdx];
  BaroHistTab[BaroHistIdx] = BaroAlt/10;
  BaroHigh += BaroHistTab[BaroHistIdx];
  index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
  BaroHigh -= BaroHistTab[index];
  BaroLow  += BaroHistTab[index];
  BaroLow  -= last;

  BaroHistIdx++;
  if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;

  BaroPID = 0;
  //D
  temp32 = conf.D8[PIDALT]*(BaroHigh - BaroLow) / 40;
  BaroPID-=temp32;

  EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
 
  temp32 = AltHold - EstAlt;
  if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small D parametr to reduce noise near zero position
 
  //P
  BaroPID += conf.P8[PIDALT]*constrain(temp32,(-2)*conf.P8[PIDALT],2*conf.P8[PIDALT])/100;   
  BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150

  //I
  errorAltitudeI += temp32*conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60
  BaroPID+=temp32;
}

..... SENSORS / BMP085

...
#define OSS 2 //we can get more uique samples and get better precision using average
....
    case 2:
      i2c_BMP085_UP_Start();
      bmp085_ctx.state++; bmp085_ctx.deadline += 14000;
      break;
.....


The Code above has several problems.
The getEstimatedAltitude() code waits on the first run magic 4s and then starts collecting new values from baro every 25ms not knowing if there are new values or not.
In the sensors part we see that the BMP is switched to mode 2 to deliver new data every 14 ms instead of every 26ms (sacrifice resolution 5cm).
So the getEstimatedAltitude() collects double barodata or looses barodata and takes no advantage of the higher barodatastream.
Solution: Take all values at full sensorspeed (MS/BMP 10/14ms), fill the 40 values buffer on the first runs (no 4s wait), and than do the pid controller at a fixed rate (i left it at 25ms - because i can not rewrite a PID controller).

My Changes:
===========

Main Variables:

Code: Select all

static uint8_t baroloopcounter=0;
static int32_t estaltsum=0;
static int32_t estaltmw=0;
static uint8_t newestalt=0;
static uint8_t newbaroalt=0;

Only "newbaroalt" is really necessary. By this the Sensors/Baro_update() indicates that it has a new "BaroAlt" by setting it to 1.
The other 4 variables are questionable - i use them for getting an 200ms average hight movement of the copter to judge a wanted hightchange by the user.
This might not be necessary anymore due to the improved datacollecting (later on).
newestalt is set by getEstimatedAltitude() to 1 if it has a new EstAlt.

Main code to detect a wanted hight change of the user:

Code: Select all


  #if BARO
    if (baroMode==1)
    {
     if (newestalt==1)                                                             //is there a new barovalue?
     {
      newestalt=0;
      if (baroloopcounter <8)                                                       //did i collect 8 values already?
      {
       estaltsum=estaltsum+EstAlt;                                                 //no, keep collecting
       baroloopcounter=baroloopcounter+1;
      }
      else                                                                           //yes, make new values
      {
       estaltmw=estaltsum>>3;                                                        //divide by 8
       baroloopcounter=0;                                                            //reset Values for next run
       estaltsum=0;
//       debug3=abs(estaltmw - AltHold);
//       debug4=abs(rcCommand[THROTTLE] - initialThrottleHold);
       if (abs(estaltmw - AltHold)>60 && abs(rcCommand[THROTTLE] - initialThrottleHold)>40) baroMode = 0; // so that a new althold reference is defined
      }
     }
     rcCommand[THROTTLE] = rcCommand[THROTTLE] + BaroPID;
     rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE],MINTHROTTLE,MAXTHROTTLE-50);
    }
  #endif



The changed IMU / getEstimatedAltitude() code:
Crucial point: sensors/Baro_update() indicates a new barovalue by setting "newbaroalt" to 1.
So the new code skips datacollecting if new no new barovalues are available.
It indicates a new EstAlt value by setting newestalt to 1;
The fstrun byte is set to 0 when the Barotab is filled for the first time with valid data.
The PID controller has still his 25ms turn. This can be improved (sorry i cant).

Code: Select all

#define BARO_TAB_SIZE   40
#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)

void getEstimatedAltitude(){
  uint8_t index;
  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx;
  static int8_t fstrun=1;
  static int32_t BaroHigh,BaroLow;
  static uint32_t deadLine = 0;
  int32_t temp32;
  int16_t last;

  if (newbaroalt==1)                                        // Update only, if new values are available
   {
    newbaroalt=0;                                           // Reset Boolean
    //**** Alt. Set Point stabilization PID ****
    //calculate speed for D calculation
    last = BaroHistTab[BaroHistIdx];                        // Do the magic
    BaroHistTab[BaroHistIdx] = BaroAlt/10;
    BaroHigh += BaroHistTab[BaroHistIdx];
    index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
    BaroHigh -= BaroHistTab[index];
    BaroLow  += BaroHistTab[index];
    BaroLow  -= last;
    BaroHistIdx++;
    if (BaroHistIdx == BARO_TAB_SIZE)
     {
      BaroHistIdx = 0;
      fstrun = 0;
     }
   }
  if (currentTime<deadLine || fstrun==1) return;            // Maintain old timing (40Hz)for PID calculation
  deadLine = currentTime + UPDATE_INTERVAL;
  EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
  newestalt=1;                                              // Indicate, new EstAlt RDY
 
  BaroPID = 0;                                              // Calculate new PIDs
   //D
  temp32 = conf.D8[PIDALT]*(BaroHigh - BaroLow) / 40;
  BaroPID-=temp32;
  temp32 = AltHold - EstAlt;
  if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small D parametr to reduce noise near zero position
  //P
  BaroPID += conf.P8[PIDALT]*constrain(temp32,(-2)*conf.P8[PIDALT],2*conf.P8[PIDALT])/100;   
  BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150
  //I
  errorAltitudeI += temp32*conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60
  BaroPID+=temp32;
}



Now the minor changes in the Sensors Tab:

The Sensors/Baro_update code now sets the byte newbaroalt to 1 when it has a new BaroAlt.

Change for BMP085:

Code: Select all

...
    case 3:
      i2c_BMP085_UP_Read();
      i2c_BMP085_Calculate();
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
      bmp085_ctx.state = 0; bmp085_ctx.deadline += 5000;
      newbaroalt=1;
      break;
....



Change for MS561101:

Code: Select all

....
   case 3:
      i2c_MS561101BA_UP_Read();
      i2c_MS561101BA_Calculate();
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
      ms561101ba_ctx.state = 0; ms561101ba_ctx.deadline += 4000;
      newbaroalt=1;
      break;
.....


That was it. My changes are not substantial but they help the Barocode.
I think that some of my ideas should be taken into account. No doubt my code could be better (learning).
The attachment is the MultiWii_dev_20120528 altered with the code above.
The developement of the code was done here: http://fpv-community.de/showthread.php? ... rn%FCnftig
And the new, smooth barocurve in the gui can be seen here:
http://www.youtube.com/watch?v=tYam_n6IBaw

http://www.youtube.com/watch?v=CZ4GZg3oABg

So long

Crashpilot1000
Attachments
MultiWii_dev_20120528BaroModNR5b.zip
(98.75 KiB) Downloaded 124 times

User avatar
kos
Posts: 286
Joined: Thu Feb 16, 2012 4:51 am
Location: Fr

Re: I improved the barocode of dev_20120528

Post by kos »

Crashpilot1000 wrote:So a random hight change is taken as a wanted new locked hight. This has absolutely no logic.

if you want +/-10 cm altitude increment you can have a look at alexmos code ;)

i did some similar changes for the "Failsafe - Autolanding with baro ".. variable was named "setBaroMode".

Code: Select all

if (setBaroMode){
baroMode=1
setBaroMode=0
// record new allt (from the previous mesurement , 10/20ms lag from the tx , so we do not care too much about precision here)
}

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

Re: I improved the barocode of dev_20120528

Post by Crashpilot1000 »

I looked at the alexmos code - but there were too many changes i did not understand. Like the calculated ACC Z vector & integration - very important - but over my head. I am very interested in your Baroautolanding feature - because this is also on my wish list. Do you have a link to your project?

So long

Crashpilot1000

User avatar
kos
Posts: 286
Joined: Thu Feb 16, 2012 4:51 am
Location: Fr

Re: I improved the barocode of dev_20120528

Post by kos »


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

Re: I improved the barocode of dev_20120528

Post by Crashpilot1000 »

Thank you Kos! I will try something like you.

Post Reply