Altitude Hold met hc-sr04 en Arduino voor multicopters
1 bericht
• Pagina 1 van 1
- retlawrobbe
- Berichten: 17
- Geregistreerd: 06 Feb 2013, 19:30
Altitude Hold met hc-sr04 en Arduino voor multicopters
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
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
1 bericht
• Pagina 1 van 1
Terug naar Afgeronde projecten
Wie is er online?
Gebruikers in dit forum: Geen geregistreerde gebruikers en 20 gasten