Multiwii + SRF08 Sonar PID Controller Question

[iDaniel]

Hi,

i have some questions about programing an PID Controller in Multiwii. So far so good, I have an Multiwii 2.4 on Arduino pro mini connected to an SRF08 Sensor. It works, measures pretty well, but my PID Controller for Altitude Controll is the Problem.
for now ist generates from the Sonar Error P, I, and D but witch int you can´t do multiplikation as i want to do...or maybe I cant do it because of my programing skills The estimated PID could be P:0.25 I:0.001 and D:1.3. The Controller adds an rC Command vallue from -100/100 to the Stick Value.

Here the existing Code: Imu.cpp

Code: Select all

unsigned long lastTime;
int16_t  lastSonarAlt;
int16_t  lastSonarError;

         int16_t kp=20; //x*100 wegen Kommazahl
         int16_t ki=1;  //x*1000 wegen Kommazahl
         int16_t kd=126;   //x*100 wegen Kommazahl
         
         int16_t SampleTime=100;

      void SonarPID(){
                unsigned long now = millis();
                int16_t timeChange = (now-lastTime);
                if(timeChange>=SampleTime)
                {
                  int16_t error = SonarSetpoint - sonarAlt;
                  ITermSonar+=(ki*timeChange*error/1000000);  
                  constrain(ITermSonar,-10,+10);
                  int16_t dInput = (error - lastSonarError);
                  
                  //SonarPidOutput = kp*error + ITermSonar -kd*dInput/timeChange/1000;
                  SonarPidOutput = kp*error/100 + ITermSonar +kd*dInput/timeChange/100000;
                  constrain(SonarPidOutput,-100,+100);
                  
                  lastSonarError=error;
                  lastSonarAlt = sonarAlt;
                  lastTime = now;
                }
               }

in Multiwii.cpp

Code: Select all

#if SONAR

                                  
           if(f.SONAR_MODE) {
              
              
              SonarSetpoint= 150;
              
              SonarPID();
                                     
              rcCommand[THROTTLE]+= SonarPidOutput;
              constrain(rcCommand[THROTTLE],1150,1850);
              }
            if(!f.SONAR_MODE) {
              SonarPidOutput=0;
              ITermSonar=0;
              }
              
  #endif

[Kbev5709]

The SRF08 is currently in the stock code as a landing lights sensor. Is it your intention to fully integrate that sensor for Alt Hold?
Why not just use the HC SR04 generic sonar code and modify it to reflect the SRF08? They already have the HC SR04 code working for that. If you have a baro, the code is already merged to work with one. Or better yet, just get yourself an HC SR04 and use the code that is already working for that. You can get 5 for less than a buck each on Ebay.
If it is your goal to get that expensive sonar working for this purpose, I wish you good luck and forget my suggestion.

[iDaniel]

Thanks for the advice, but i want to use the SRF08... they where free for me and they are a little bit better.
The last time i read the thread with HC SR04 they used an Baro/sonar fusion which doesn´t work. With the Sonar you have the hight from the ground and with the Baro you have the absolut hight.
I want to have the alt hold only with the sonar. For indoor youse you could fly up stairs without throttle correction. with more sensors you could avoid obstacles...etc.

[iDaniel]

Ok for now i fixed the problem a little bit

the code now looks like below:

Code: Select all

unsigned long lastTime;
int16_t  lastSonarAlt;
int16_t  lastSonarError;

         float kp=0.25; //x*100 wegen Kommazahl
         float ki=0.001;  //x*1000 wegen Kommazahl
         float kd=1.3;   //x*100 wegen Kommazahl
         
         int16_t SampleTime=100;

      void SonarPID(){ 
                unsigned long now = millis();
                int16_t timeChange = (now-lastTime);
                if(timeChange>=SampleTime)
                {
                  int16_t error = SonarSetpoint - sonarAlt;
                  //ITermSonar+=(ki*timeChange*error/1000000);
                  ITermSonar+= ki*(float)error*(float)timeChange;  
                  //constrain(ITermSonar,-10,+10);
                  int16_t dInput = (error - lastSonarError);
                  
                  //SonarPidOutput = kp*error + ITermSonar -kd*dInput/timeChange/1000;
                  //SonarPidOutput = kp*error/100 + ITermSonar +kd*dInput/timeChange/100000;
                  SonarPidOutput = kp * (float)error + ITermSonar/1000.0 + kd*(float)dInput;
                  constrain(SonarPidOutput,-100,+100);
                  
                  lastSonarError=error;
                  lastSonarAlt = sonarAlt;
                  lastTime = now;
                }
               }

I made the P,I,D values float and convert the calculations in float. then they´r stored in int ... yes the value behind the .xyz is deleted but the esc won´t need such correct value.
I tested the Copter outside and it hold the hight pretty goog. At stronger wind the copter goes down but recovers ... without hightcorrection ist won´t