Altitude Hold met BMP085 en Arduino voor multicopters

Toon hier Uw afgeronde projecten aan anderen.
Berichten: 17
Geregistreerd: 06 Feb 2013, 19:30

Altitude Hold met BMP085 en Arduino voor multicopters

Berichtdoor retlawrobbe » 11 Jun 2013, 13:53

Altitude Hold met BMP085 en Arduino voor multicopters
zie ook http://arduinoforum.nl/viewtopic.php?f=20&t=408

Vooraf : deze SW is gemaakt om een Altitude-hold te maken voor in principe elke multicopter.
Door de SW te plaatsen op een arduino ( ik heb de Arduino nano gebruikt ) welke tussen de ontvanger en de multicopter bv KK2 ( HC ) bord is geplaatst .

De SW :
a.De Altitude hold wordt ingeschakeld met een driestanden schakelaar ( gear ) op de zender. in de eerste stand is de zelfleveling ("SelfLevel") van het KK2 bordje uitgeschakeld , in de tweede stand wordt de "SL" ingeschakeld, in de derde stand wordt altitude- hold ingeschakeld + "SL"
b. Wanneer FILTER_alt = op bv 5 wordt gezet kan men reeds verticale oscilaties krijgen , het beste resultaat keb ik verkregen door een FILTER_alt = 2 te nemen .
c. Om de veiligheid te verhogen wordt de Altitude-hold uitgeschakeld wanneer de gasknuppel ( Throt_tol: aanpassen ) over en beperkt gebied verplaatst wordt
( staat nu op +/- 100msec ) en kan eventueel vergroot of verkleind worden naar uw eigen inzicht.
d. sketch-grootte: 10.704 bytes


Code: Alles selecteren
 // ** BMP085 *******************************
 #include <Wire.h>     
 #include <BMP085.h>   
 #include <Servo.h> 
 BMP085 dps = BMP085();
 long Baro_Altitude = 0;     
 
 long newHeight;
// regulating parameters --------------------------------------
// Filter -------------------------------
 # define FILTER_alt 2
  int filterAltit;
  long filtered_Altit;

//regulator -----------------------------
 long setAltit,altitError,p_AltitTerm;
#define ALTIT_PGAIN 10   // (5 = 50% )  original setting  1.5
byte P_Regulator = ALTIT_PGAIN; // start setting 150%

//output -----------------------------------
 int altiCtrl;
#define ALTITMAXREG 150 //tolerance in PPM to be use while in altitude hold
//output -----------------------------------
#define Throt_tol 100 //Throttle_tolerance = if the pilot rise or fall the throttle more than it is going to be written by Althold
      // then exit from the alt hold mode and ignore the switch until it is set back to off position
      // it's like a kind of PANIC button
 // regulating parameters --------------------------------------
 
 // ** BMP085 *******************************

 Servo myThrottleChOut;

 int throttlePin1 = 6; //output rc throttle channel on flight controller
 int THROTTLE_CH3 = 7; //Reads channel 3 from RX Throttle    ( FUTABA THROTTLE )
 int GEAR_CH6     = 8; //Reads channel 5 from RX AUX channel ( FUTABA GEAR )
 int led = 13;         //LED for signaling the altitude hold mode (led gnd to arduino gnd)


 #define Alt_hold_No     0 // No ALTITUDE HOLD to prevent starting with Altitude Hold switch on
                           // switch OFF and ON to reactivate altitude hold
 #define Alt_hold_switch 1 // No ALTITUDE HOLD SET reset by switch
 #define Alt_hold_Yes    2 // Altitude hold YES
 byte Alt_hold = Alt_hold_No;

 int throttle_read;
 int throttle_althold;
 int alt_hold_read;
 int throttle_output;
 int throttle_min;
 boolean throt_min_set = false;
 
 char tchar[16];
 
 
void setup() {
  Serial.begin(38400);
  pinMode(led, OUTPUT);
  pinMode(THROTTLE_CH3, INPUT);
  pinMode(GEAR_CH6, INPUT);
  Wire.begin();  // ** BMP085
  delay(1000);   // ** BMP085
  dps.init();    // ** BMP085
 
  myThrottleChOut.attach(throttlePin1); //attach the "servo" to digital pin ,
                                        //this is the digital pin which is going to write to the KKboards throttle channel.
}

void loop(){
 
  throttle_read = pulseIn(THROTTLE_CH3, HIGH); //Reads the throttle level, returns 1150 - 1839 microseconds.
  throttle_output = throttle_read;             
  if ((throttle_read > 1100) && (throt_min_set == false )){ // (1100=>) read throttle when TX signal is ON
    throttle_min = throttle_read + 10;
    throt_min_set = true;
  }
 
  dps.getAltitude(&Baro_Altitude); // ** BMP085
  Baro_Altitude = filter_height(Baro_Altitude);
  alt_hold_read = pulseIn(GEAR_CH6, HIGH); //Reads the ch6 level 
  if (  throttle_read > throttle_min ){
    if( alt_hold_read > 1700){ //If the ch6 knob is turned almost to max, arduino enter altitude hold mode
                             // Initial start is made with Alt_hold_No. You have to switch OFF
                             // ( Alt_hold_switch ) before you can go to Alt_hold_Yes
      if ( Alt_hold == Alt_hold_switch) {
         Alt_hold = Alt_hold_Yes;  // ALTITUDE HOLD => YES
         digitalWrite(led, HIGH);  //fires up the led
      }     
    }else{
       Alt_hold = Alt_hold_switch; // ALTITUDE HOLD => NO
       digitalWrite(led, LOW);
    }
  }
 
  if (Alt_hold == Alt_hold_Yes){ 
    while ( Alt_hold == Alt_hold_Yes )  {  // arduino enters altitude control loop.
      BMP085_AltCtrl();               //BMP085
      myThrottleChOut.write(throttle_output);
      alt_hold_read = pulseIn(GEAR_CH6, HIGH);
      if ( alt_hold_read < 1700){  //Reads the ch6 level, this is needed to be able to exit the while-iteration
        Alt_hold = Alt_hold_switch;
        digitalWrite(led, LOW);
      }
     
      throttle_althold = pulseIn(THROTTLE_CH3, HIGH);
     
      if ((throttle_althold > (throttle_read + Throt_tol)) || (throttle_althold < (throttle_read - Throt_tol)) ){
        Alt_hold = Alt_hold_No;
        digitalWrite(led, LOW);
      }
    } // END WHILE
  } else { // Alt_hold <> Alt_hold_Yes NO ALTITUDE HOLD
     myThrottleChOut.write(throttle_read);
  }
} // END LOOP

//********** BMP085 **************************************************
  void BMP085_AltCtrl(){
 //  prev_newHeight = newHeight;
   dps.getAltitude(&newHeight); // ** BMP085
   
   // Filter ---------------------------
  filtered_Altit = filter_height(newHeight);
   //averagator -----------------------
  // averageAltit = average_height(newHeight); // call average function
 
 //regulator -----------------------------
    altitError = filtered_Altit - Baro_Altitude; //if altitError is negative, altiCtrl should be negative = above setPressure
    p_AltitTerm = (P_Regulator * altitError)/10;     //gain 2
   
 //output -----------------------------------
   altiCtrl = (int) p_AltitTerm; //convert to be safe
   
   if (altiCtrl > ALTITMAXREG) { altiCtrl = ALTITMAXREG; }    //maxreg is +-150us offset to motors
   if (altiCtrl < -ALTITMAXREG) { altiCtrl = -ALTITMAXREG; }
   // constrain :  is not so fast as above code
   //  altiCtrl = constrain(altiCtrl, -ALTITMAXREG, ALTITMAXREG); //maxreg is +-100us offset to motors
 // sprintf(tchar," C_alt:%005d",altiCtrl); Serial.println(tchar);
   throttle_output = throttle_read - altiCtrl;
   if (throttle_output > 2000) { throttle_output = 2000; }    //maxreg is +-150us offset to motors
   if (throttle_output < throttle_min) { throttle_output = throttle_min; }
   
}
//********** BMP085 **************************************************

//*** CALCULATE FILTER ************************************************
long filter_height ( long inp_height){
  long result;
  filterAltit += ((inp_height - filterAltit)/FILTER_alt);
  result = filterAltit + 300;
 // sprintf(tchar," F_alt:%005d",result); Serial.print(tchar);
 
  return result;
}
  //*** CALCULATE FILTER

Advertisement

Berichten: 38
Geregistreerd: 26 Nov 2013, 21:58

Re: Altitude Hold met BMP085 en Arduino voor multicopters

Berichtdoor phxrocketry » 24 Feb 2014, 19:24

Leuk project.

Ik ben nu met de BMP 180 bezig een hoogtemeter te maken voor in een modelraket

Terug naar Afgeronde projecten

Wie is er online?

Gebruikers in dit forum: Geen geregistreerde gebruikers en 5 gasten