Altitude Hold met hc-sr04 en Arduino voor multicopters

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

Altitude Hold met hc-sr04 en Arduino voor multicopters

Berichtdoor retlawrobbe » 26 Mei 2013, 13:28

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 is het mogelijk om op de Throttle-puls in te werken en aldus een regeling van de hoogte te verkrijgen.
opmerkingen :
Ik zelf ben niet zo onder de indruk (tevreden) over de de hoogte (afstand ) meting van HC-SR04
a. de maximum afstand welke ik kunnen meten heb is 325cm in ideale ( ideaal = meten t.o.v. zeer effen oppervlak en werkelijk loodrecht op de sensor)
b. werkt bijna niet boven een gras oppervlakte ( +/- tot 15 a 20cm hoogte )
c. werkt boven een betonnen oppervlakte tot ongeveer 1meter
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 de meting door de HC-SR04 niet kan gedaan worden ( afstand te groot of de hoek waaronder gemeten wordt groter dan 30° ) wordt geleidelijk
de throttle vermeerderd of verminderd. om de meethoek steeds zo klein mogelijk te houden heb ik mijn sonar op de camera gymbal geplaatst .
c. Er wordt gebruikt gemaakt van een versnellings meting om de oscillaties op te vangen.
d. Om de veiligheid te verhogen wordt de Altitude-hold uitgeschakeld wanneer de gasknuppel over en beperkt gebied verplaatst wordt
( staat nu op +/- 50msec ) en kan eventueel vergroot of verkleind worden naar uw eigen inzicht.

Ik heb ook reeds een programma gemaakt met de combinatie van een sonar ( LOW-altitude-hold ) en een baromete ( HIGH-altitude-hold ),
maar aangezien de sonar niet zo schitterend werkt wordt mijn volgende projectje een Altitude-hold met enkel de barometer BMP085 pressure sensor.
ref : http://blog.giuseppeurso.net/low-altitude-hold-auto-takeoff-autolanding-with-hk-v2-1-arduino-sonar/index.html
Code: Alles selecteren

Latest (2012-12-06):
Retlawrobbe ( 2013-05-09 )
*/

/*
 http://blog.giuseppeurso.net/low-altitude-hold-auto-takeoff-autolanding-with-hk-v2-1-arduino-sonar/index.html
Hi, i'm glad you downloaded my arducode, please remember to take a look to my website
http://giuseppeurso.net
where you will find updates and troubleshooting tips.


 */

 #include <NewPing.h>
 #include <Servo.h>
 #include <mat.h>
 //#include

 #define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters).
                         // Maximum sensor distance is rated at 400-500cm.
 Servo myThrottleChOut;

 #define TRIGGER_PIN  12  // Arduino pin tied to trigger pin on the ultrasonic sensor.
 #define ECHO_PIN     11  // Arduino pin tied to echo pin on the ultrasonic sensor.
 //Arduino 5V -> Sonar vcc (you can use any 5v source as all these components have actually common ground)
 //Arduino GND -> Sonar Gnd (as previous)

 NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
 int throttlePin1 = 6; //output rc throttle channel on flight controller
 int pinReadCh6   = 8; //Reads channel 5 from RX AUX channel ( FUTABA GEAR )
 int pinReadCh3   = 7; //Reads channel 3 from RX Throttle    ( FUTABA THROTTLE )
 int led = 13;       //LED for signaling the altitude hold mode (led gnd to arduino gnd)
 boolean takeoff = false;
 int distance,newDistance,prev_newDistance;;

 long accel_act_time,accel_prev_time;

 #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_takeoff;
 int throttle_min;
 boolean throt_min_set = false;
 // regulating parameters --------------------------------------
 #define TX_THRESHOLD 150 //tollerance in PPM to be use while in altitude hold
 #define throt_perc 20/100  // for           TX_THRESHOLD  =150  or =200
                            // regulating add_trot 10% => 0 to 38cm   
                            //                     20% => 0 to 27cm    31cm
                            //                     30% => 0 to 22cm    25cm
                            //                     40% => 0 to 19cm    22cm
                            //                     50% => 0 to 17cm    20cm
                            //                     40% => 0 to 15cm    18cm
 #define alt_interval 0  //+ 10 will give an intervall between desired altitude +- 10 cm
                                            // in the upper and lower bounds.     
 #define accel_influ  4  //               
 // regulating parameters ----------------------------------------
 char tchar[16]; // used for debug printing to PC

void setup() {
  Serial.begin(38400); // used for debug printing to PC || DO NOT USE DEBUG PRINT IN THE ALTITUDE HOLD LOOP=> TIMING !!!
  pinMode(led, OUTPUT);
  pinMode(pinReadCh3, INPUT);
  pinMode(pinReadCh6, INPUT);
 
  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(pinReadCh3, HIGH); //Reads the throttle level, returns 1150 - 1839 microseconds.
  throttle_output = throttle_read;          // 1 millisecond = 1000 microseconds -> throttle_read/1000: ex 1,736ms     
  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;
  }
  unsigned int uS = sonar.ping();    // Send ping, get ping time in microseconds (uS).
  distance = (uS / US_ROUNDTRIP_CM); //Gets the altitude before entering altitude hold mode, this will be a reference
  //  sprintf(tchar," DIST :%0000005d",distance); Serial.println(tchar);
  //save to tell us how much throttle has been used for take off
  if (distance>20 && distance <40 && takeoff==false){
    takeoff = true;
    throttle_takeoff = throttle_read;
  } else {   
     if ( distance < 10 ) { takeoff = false; }
  } 
  alt_hold_read = pulseIn(pinReadCh6, HIGH); //Reads the ch6 level 
  if ( (distance < MAX_DISTANCE-50 ) && ( 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{ // ALTITUDE HOLD => NO
       Alt_hold = Alt_hold_switch;
       digitalWrite(led, LOW);
    }
  }
  if (Alt_hold == Alt_hold_Yes){ 
    while ( Alt_hold == Alt_hold_Yes )  {  // Enters altitude control loop.
      prev_newDistance = newDistance;
      unsigned int uS2 = sonar.ping(); // Send ping, get ping time in microseconds (uS).
      newDistance = ((uS2 / US_ROUNDTRIP_CM)); //Gets the current altitude,
                                               // this will be compared to the reference altitude
                                               //switch to while
      if ( newDistance == 0 ){ // if sonar angle > 30° there is a posibility that the reading result is equal zero
                               // The sonar works best above a concrete surface and even then the distance
                               // which can be measured is limited to about a max of 100cm,
                               // it does certainly not above grass
        if ( prev_newDistance  > distance ){ newDistance = prev_newDistance-2;
        }else {                              newDistance = prev_newDistance+2;}
      }
      accel_prev_time = accel_act_time;  // measure time between each distance check
      accel_act_time = millis();
      if (newDistance > distance + alt_interval){  // going UP       
      throttle_output = throttle_read - add_throt((newDistance-distance));
        if ( throttle_output < throttle_takeoff ) { throttle_output = throttle_takeoff;}       
      } else { // going DOWN                                       
        if(newDistance < distance - alt_interval){
          throttle_output =  throttle_read + add_throt((distance-newDistance));;
        }
      }
      myThrottleChOut.write(throttle_output);
      alt_hold_read = pulseIn(pinReadCh6, HIGH);
      if ( alt_hold_read < 1700){  //Reads the ch6 (Gear) level, this is needed to be able to exit the while-iteration
        Alt_hold = Alt_hold_switch;
        digitalWrite(led, LOW);
      }
     
      throttle_althold = pulseIn(pinReadCh3, HIGH);
      //if the pilot rise the throttle more than it is going to be written by arduino
      // then exit from the alt hold mode and ignore the switch until is set back to off position
      if ((throttle_althold > (throttle_read + 50)) || (throttle_althold < (throttle_read - 50)) ){
        Alt_hold = Alt_hold_No;
        digitalWrite(led, LOW);
      }
    } // END WHILE
   
  } else { // Alt_hold <> Alt_hold_Yes NO ALTITUDE HOLD
     myThrottleChOut.write(throttle_read);
  } // END  Alt_hold == Alt_hold_Yes
} // END LOOP

//----- Calculate throttle correction -----------------------------------
int add_throt ( int dist){
   int result;
   
   result = pow(dist,2);
   result = result * throt_perc;
   
    int accel_dist = prev_newDistance -newDistance;
    if ( accel_dist < 0 ){ accel_dist = accel_dist *-1;} // make sure that it is positive number
    accel_dist = accel_dist * 100;
    // accel_dist * 100: to become a nice number above zero that can be devided by the accel_time
   int accel = accel_dist / ( accel_act_time - accel_prev_time) ;
   // The time variation is 44 msec <> 46 msec between two measurements without => delay(MAIN_CLOCK
   accel = accel * accel_influ ; // *4 so you have more influence from the accelleration
   // When the actual altitude ( newDistance ) is near the set altitude ( distance )
   // the result will be near zero, the acceleration could be greather than the result
   // therefore check if result is > then accel before subtracting the acceleration from the result
   // to a avoid negative result
   if ( result > accel){  // the result should not go below zero 
     result = result - accel;
   }
   result = (int) result; //convert to be safe
   if ( result > TX_THRESHOLD ) {
    result = TX_THRESHOLD;
   }
   return result;
}
//----- Calculate throttle correction -----------------------------------

Advertisement

Terug naar Afgeronde projecten

Wie is er online?

Gebruikers in dit forum: Geen geregistreerde gebruikers en 4 gasten