Hello i started this post because i think others like this
this is what i builded !
i use the arduino uno ic ,and height sensor to add some extra functions on the kk2.0 board quadcopters
i also use a APC220 module same like they use on arducopters for telemetry ,
and i use this for other stuff you can see bellow ,
i use the gimbal outputs on m7 and m8 on the kk board for reading horizontal and vertical level ,
transmited back to my GUI i am working on ,,just like the apm quadcopters ,,
so you wil need a kk2.0 quad copter ,,sonic height sensor exmpl HR-S04
and whit this system you can switch between normal mode or arduino bypas mode ,,
this wil bypas the throttle input ,
and use this for auto height control , auto liftoff , and some other stuff read bellow
i use this ping libary , because this works best on sonic sensors,
https://code.google.com/p/arduino-new-ping/
if you like to build a simular system let me know and maybe you like to use this for your own quadcopter
to ad some cool xtra's
think about what a arduino can do ,,add this to your quad copter
this wil be a cool machine
i posted diferent sketches so take the last one if you want the latest one !
and read everthing before destroying your vehicle ! 
did some new testing and i delay the down going number compesated with loss of drop rate , 
and have the going up rate react faster ,
this values are different for every quad copters weight and motor and props ,,
so i want to have this calculated a bit more ,
the other problem is the height sensor , 
it react different on different ground ,,
this can change between - 30 + 30 cm this wil not be compesated ,because the system wil take care of this
first i tested auto lift of and hang around 30 cm ,,
i did this because of the bouncing and when you bounce from one meter up and down LOL :* your quadcopter wil soon be destroyed ,,
when you start with this system you have to start low , and calibrate it till it wil stay hoover between -15 +15 centimeter
, then i adjusted the decent rate , and time < this wil work onley if the quadcopter = in reach of 1,5 meter ,
this is why
when the quadcopter is above the level you want it to be setup ,, it has to go down to reach let say 1 meter ,,
when doing so the quad throttle wil go lower by - 1 of 1000 steps ,,
so when the quad decent it wil decrease throttle til height is reached ,butt when 1 meter is not reached and the decrease of throttle wil go to almost 0 then the quadcopter wil drop fast because of no lift
if its in range of one meter and it drops it wil still have power to shoot back up because sensor wil see you below one meter whe have to go up ,,
this al combined is a hard task to debug in my brain !
i going to find out a method to this problem ,
maybe a safety function when your not in sensor range it wil not react ,,
this wil be the safest method to use with for auto decent to hoover state on height settings ,
wil post my video's later
major testing in progres 8)
small test near ground 
auto lift of and hoover ,
i finaly got this ]
copter tuned ,, this is wat i did
i tested the height sensor yesterday night on different surface , and also different settings and libary's
but the one i use newping 1.5 stil is the best ,
i also checked error rate of al lib's ,,
and this one had the most positive read outs ,
then i try to set constrain on the output
when the sensor see its below level it wil go up til certain pwm value ,, mine goes from 980 to 1880 on the remote the board wil block this to 1000-1725 ,, this is just enough power to lift my quad and rise ,,
and wil not shoot up in to air ,, but goes smooth ,,
then when level reached it does not has to over shoot the height level to much before decent ,
when decent it drops trhottle ,, when you have this in same speed as going up the quad wil have 0 throttle when back on ground ,, this is wrong so you need the decent delay to compesate this ,, this means how long does it take to come back below height set
and stil have power to go back up when bellow height set point ,,
also this rate is adjustable ,, i have stick scaling on the kk2.0 .,, i need to rise 4 points a time to have enough lift
you can see in sketch when below height + 4 and above -1 because this has to go slow ,,
it al hard work to get this working , but i am getting there ,,
this is a little part of things you have to tink off when building this kind of system
i checked the APM2.6 code and it also has this kind of code ,,
but not compatible like this ,, they use also a barometic sensor ,, for when quad is above sensor level ,,
i also tested the extra mem2 for onley positive readings to be used ,,
the problem is when my last reading is wrong it wil use this ,,
other problem is when i am above level and 0 is the value it wil rise ,,
i have to fix this also , butt stil i believe this al can be achived whit one sensor ,
its the code that needs to be harder !
about the sketch i now it looks lik trash ,, but this is the way other people not novice wil understad and can use pieces for other things ,,
i wil make it al better on the end ,
:%
// dexters KK2.0 automatic flight system //
// debug sketch 5-8-2014 //
#include <Servo.h>
#include <NewPing.h>
#define TRIGGER_PIN 3 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 2 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 600 // max sensor value 400 means 4 meter
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
// set height here //
const int height =5000; // is digital value for height
const int auxset =1800; // aux output to kk2.0 for setting auto level onboard on or off !
const int decent =400; // decent delay time this is the hard part depents on survace and height set //
// the above values need to be tuned //
int cm ; //incoming value of sensor 0-100000
int sensor;// sensor cm values in centimeter
int safety =1; // safety switch value//
int auxv ; // Here's where we'll keep our aux channel values 1400 < autolevel off 1400 > = on
int auxs ; // Here i store input aux to servo output numbers
int throtv ; // Here's where we'll keep our throttle channel values
int throts ; // Here i store input throttle to servo output numbers
int system ; // system for do stuf with throttle and height //
int mem1;
int sys;
int led = 13;
int piezo = 10;
// gimbal to horizontal and vertical pos readout //
// normal when start flying on the kk 2.0 the gimbal wil center its pos if used right //
int horim ; // using this for boxing the horizontal value from kk2.0
int horiz ;
Servo throttle; // make servo output to kk2.0 throttle input
Servo aux; // make servo output to kk2.0 aux input
int signal =0; // signal procces
void setup() {
Serial.begin(57600); // starting serial port for APC220 wireles tx rx baud rate 57600
aux.attach(7); // servo conection out pin 7 to kk2.0 aux input //
throttle.attach(8); // servo conection out pin 8 to kk2.0 throttle input //
aux.writeMicroseconds(auxset); // set aux output to autolevel on on kk2.0 //
pinMode(led, OUTPUT);
pinMode(piezo,OUTPUT);
pinMode(4, INPUT); // NEW HORIZ INPUT CHANNEL
pinMode(5, INPUT); // Set input channel for aux on pin 5 from rx //
pinMode(6, INPUT); // Set input channel for throttle on pin 6 from rx //
pinMode(9, INPUT); // Set input channel from gimbal horizontal pin 9 from rx for reading level //
pinMode(10, INPUT); // Set input channel from gimbal vertical pin 10 from rx for reading level //
// some indication light or piezo
digitalWrite(led, LOW); // turn the LED on
digitalWrite(piezo, LOW); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, HIGH); // turn the LED on
digitalWrite(piezo, HIGH); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, LOW);
//
system = 0;
throtv = pulseIn(6, HIGH, 20000); //checking throttle input from rx
auxv = pulseIn(5, HIGH, 20000); //checking throttle input from rx
if (throtv > 1025){ // if throttle is higher then 1025 then safety goes first //
throttle.writeMicroseconds(1000); // throttle on 0 before start
delay (1000);
safety =1; // safety wil be fase 1 = disable trhottle input , and output 0 throttle = disabled mode //
return;
}
else if (throtv < 1025)
{
safety =2; // if input throttle = near 0 then safety wil be fase 2 = working quadcopter going to safety fase 3 later = flight mode //
}
}
void loop() {
sensor = cm/ US_ROUNDTRIP_CM;
Serial.flush();
Serial.print((char)1);
Serial.print (throts);
Serial.print((char)2);
Serial.print (throtv);
Serial.print((char)3);
Serial.print (height);
Serial.print((char)4);
Serial.print (auxv);
Serial.print((char)5);
Serial.print (auxset);
Serial.print((char)6);
Serial.print (cm); // changed this to mem2 for testing
Serial.print((char)7);
Serial.print (sensor);
Serial.print((char)8);
Serial.print (safety);
Serial.print((char)9);
Serial.print (horiz);
Serial.print((char)10);
Serial.print (2);
Serial.print((char)11);
Serial.print (decent);
// vertm = pulseIn(10, HIGH, 20000); //checking gimbal vertical input
horim = pulseIn(4, HIGH, 20000); //checking gimbal horizontal input
throtv = pulseIn(6, HIGH, 20000); //checking throttle input
auxv = pulseIn(5, HIGH, 20000); //checking throttle input
horiz =map(horim, 1400, 1600,-25 ,25) ; //if reverse reverse gimbal on kk2.0
horiz =constrain(horiz ,-25,25);
// vertic =map(horim, 1600, 1400,-25,25);
cm = sonar.ping();
if (safety ==1){ // checking startup safety fase ,if fase = 1 then throttle wil be 0 and wil loop till reset button //
delay (3000);
return;
}
aux.writeMicroseconds(auxset); // aux ON for autolevel kk2.0 conected to aux input //
throtv = pulseIn(6, HIGH, 20000); //checking throttle input pwm
auxv = pulseIn(5, HIGH, 20000); // Read the pulse width of AUX input on pin 5 and reads input pwm//
if (auxv<1300){
digitalWrite(led, HIGH);
system =throtv ; // telling the system the value of the throtle input
throttle.writeMicroseconds(throtv); // write system speed to servo //
// if aux is off the input throttle = equal to input //
sys =throtv;
}
mem1 =sys;
if(height<cm){ // if sensor is lower then height value //
system = sys -1;// take speed DOWN by 1 digit of the system //
throts = mem1;
sys = constrain(system,1000,2000);
throttle.writeMicroseconds(sys); // write system speed to servo //
delay (decent);
}
if(height>cm){ // if sensor is lower then height value //
system = sys +4 ;// take speed up by 4 digit of the system //
throts = mem1;
sys = constrain(system,1000,1725);
throttle.writeMicroseconds(sys); // write system speed to servo //
}
}
i made a new video , and made a small drawing not a nice one , how i have this conected ,,
in the vid you wil see that when i switch back one time to manual the constrain from going down is stil there ]
strange thing it makes it shoot up and down because of the manual input is switching on and of the constrain
have to sort this out ,, stil thinking of different ways to act when there is no signal ,,
testing some ,
also orderd some new hbk lippo's ,, 
because they are running flat ,
the visual GUI i use , i use a delay so when i walk back in i can stil see some responses til it runs out of buffer ,,
but this is stuff for later on ,,
now i use a arduino uno chip lol ATmega328
i start to run out of pins on this big supernintendo cartridge , i use to put in and out my quadcopter for testing , 
i think its better to use a mini arduino ,,
i wil build a smd one later on when al work and i finish thinking what stuf i can put in there ,
need more pins for example gps ,, or some computer controled switches true the program ,, lights on lights of ,,
because when this project is nothing i always kan use this for other cool stuf ,,
but stil nothing is lost because i almost have what i want ,,
ok today i add some xtra code to the sketch ,,
i add a memory function , so i can store some values in the eeprom ,,
this wil keep your settings saved in the arduino ic when power down ,
you can change this al true GUI ,
and also made a button to change auto level on off on the kk2.0
also made a button to turn some pin on and off ,, for example cam on cam of ,
and i changed the GUI and the sketch serial tx rx ,,
i had some buffer overflow ,,on the GUI ,,
this is fixed ,, its realtime now !
in the video , you wil see throttle 2 x progresbar ,,
one is incoming other is out going ,,
you can see the height ,,
and al other inputs and outputs
i also tested the responses of the switches and throttle ,,
they are 100% ok
have to wait for better wheater !
but this is project is going the right way ,,
love the GUI for the kk2.0 looks cool when flying the quad copter and i have wirreless telemetry ,,
and i also have some more sensors , temp and humdity ,
i maybe gone use this also ,,
and rssi signal from the rc rx ,, sending this back to the GUI ,,
and also put a alarm on it when i almost loose signal my laptop wil sound a warning !
i wil keep up filling the code till the quad responses start to delay ,,
the last week i try to fix the GUI ,,
and the serial tx rx ,
this works nice now,,
also made buttons to change almost everthing needed for the system ,, true the GUI
so you can change this true air or true usb cable , or maybe a external arduino +lcd and some buttons to setup this thing
without a laptop XD
it wil save the settings it in eeprom on the arduino ic ,
also made some charts to read out throttle response and height ,
and one for auto height throttle response and height in one chart ,, to see the reactions in different decent settings ,,
i tested some new things and the responses the are still ok ,,
i put it on mediafire to download and gui til know ,,
you can use it for testing , or like me ,, but it needs to be tested a bit more ,,
first time it wil read the eeprom , and these settings need to be tuned first time and saved !
so height setting going up throttle max ,, and aux out ,, this last one you can change with the kk autolevel on off button
this wil set it right in eeprom values ,,
and then save ,then you are set to go !
in the GUI i have to change some MIN and MAX values , so its posible you exceed a value ,,
pressing continue wil proceed the GUI ,
have to fix those smal things ,,
the artificial horiz , i still have to tune that one , but i was not able to fly because of the bad wheater here ]
first the hard stuf then the easy parts ! 
and this is the GUI for now ,,
Every single post is by yourself.
Not one from another forum member.
Why?
i think this is to hard :%
i have 2000 readers in 2 weeks so there is a interest in subject ,, i ask the same ,why
maybe they want more movies , and proof of concept ,,
people like to take stuff , or learn for there own project ,
but not have the money to build ,, or the time ,,
i now for sure there are a lot of people using the sketch for other things ,,
addon for quadcopter , boats , and other rc stuff ,,
and i thinks the complete package looks hard to make ,
but the hardware and sketches i use are safe to use ,, 
maybe people are scarred ,, this is why a lot of people buy stuff , and not build it ,,
but yes you are the first one ,,
and yes i also removed some post , because if there is no interest why should i share my time and work ,
maybe you know the answer ,,
you read the post , the subject ,, is this hard to understand ??
let me know !
Dexterbot:
i think this is to hard :%
i have 2000 readers in 2 weeks so there is a interest in subject ,, i ask the same ,why
maybe they want more movies , and proof of concept ,,
people like to take stuff , or learn for there own project ,
but not have the money to build ,, or the time ,,
i now for sure there are a lot of people using the sketch for other things ,,
addon for quadcopter , boats , and other rc stuff ,,
and i thinks the complete package looks hard to make ,
but the hardware and sketches i use are safe to use ,,
maybe people are scarred ,, this is why a lot of people buy stuff , and not build it ,,
but yes you are the first one ,,
and yes i also removed some post , because if there is no interest why should i share my time and work ,
maybe you know the answer ,,
you read the post , the subject ,, is this hard to understand ??
let me know !
No it's not hard to read.
At least not for me
I have no use for a quadcopter but I still think the subject is intriguing.
Looking at how you solved it is fun reading so keep it up.
a update 8)
ok this is the progress ,
i made different sketches ,
some of them are build for my GUI made to see thing for debuging and research.
so it came to this ,,
i ended up with 3 different versions
version (1) uses the GUI to setup stuff and read out the data ,streamed true serial tx rx module ,,
this makes it capable to setup it on sight ,<with laptop or second arduino + lcd and a some buttons >
it read out horizontal vertical level true gimbal outputs for the GUI + artificial horizon
and added buttons to activate things ,example lights ,landing gears ,camera on an off ,
and safe settings on eeprom ,
it works like this
it uses the throttle input , when switched it wil ad a digit at the time til sensor
reaches height an then above it wil substract <this is why i use a descent delay to be sure when its bellow setpoint it wil not have 0 throttle ,
can't constrain this because when you switch back and forward it wil enter a constrained value and the quad wil start from the lowest value of this constrain
this wil be terrible when you are on the ground and you start from a mid stck pos example 1500-1800 .. 0 = 980 -1000 >
so you setup the quad height true GUI
auto level on of is for the kk2.0 auto level onboard , normaly switched by AUX ,
so if you want it on or of this is selectable ,
if your aux output = bellow 1300 its in normal mode
if your aux output = above 1300 its in bypas mode < the addon board wil take over throttle >
the decent delay is for delaying the decrease of the throttle value ..
and there is no manual overide .ONLEY WHEN AUX SWITCHED BACK ,,
VERSION 1
// dexters KK2.0 automatic flight system addon board software //
// debug sketch 15-8-2014 works ok //
// new rx from serial also used for using settings and safe in eeprom button //
// the one uses the GUI //
// responses OK //
/*
********************************************************************
x&x x&x
x. .x
. .
. .
. . CHECK KK2.0 RECIEVER INPUTS
. . *********************************
< > ****************** HEIGHT SENSOR PLACED UNDER QUAD
< > *********************************
. . USE THIS ONLEY WHEN NOVICE
. .
. .
. .
X x
x&x x&x
x x
********************************************************************
// ALL QUADS NEED TO BE TUNED //
//
*/
#include <Servo.h>
#include <NewPing.h>
#include <EEPROM.h>
#define TRIGGER_PIN 3 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 2 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 500 // max sensor value 400 means 4 meter
#define addr0 0
#define addr1 1
#define addr2 2
#define addr3 3
#define addr4 5
#define led 13
#define piezo 10
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int cm,sensor,auxv,auxs,throtv,throts,system,mem1,sys,val,height1,horim,horiz,vertm,verti,height,auxset,decent,up; //incoming value of sensor 0-100000
int safety =1; // safety switch value//
int signal =0; // signal procces
Servo throttle,aux; // make servo output to kk2.0 throttle input
void setup() {
Serial.begin(57600); // starting serial port for APC220 wireles tx rx baud rate 57600
aux.attach(7); // servo conection out pin 7 to kk2.0 aux input //
throttle.attach(8); // servo conection out pin 8 to kk2.0 throttle input //
aux.writeMicroseconds(auxset); // set aux output to autolevel on on kk2.0 //
pinMode(led, OUTPUT);
pinMode(piezo,OUTPUT);
pinMode(4, INPUT); // NEW HORIZ INPUT CHANNEL
pinMode(5, INPUT); // Set input channel for aux on pin 5 from rx //
pinMode(6, INPUT); // Set input channel for throttle on pin 6 from rx //
pinMode(9, INPUT); // Set input channel from gimbal horizontal pin 9 from rx for reading level //
digitalWrite(led, LOW); // turn the LED on
digitalWrite(piezo, LOW); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, HIGH); // turn the LED on
digitalWrite(piezo, HIGH); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, LOW);
height = EEPROM.read(addr0 )*100;
auxset = EEPROM.read(addr1 )*10;
decent = EEPROM.read(addr2 )*10;
up = EEPROM.read(addr3 )*10;
system = 0;
throtv = pulseIn(6, HIGH, 25000); //checking throttle input from rx
auxv = pulseIn(5, HIGH, 25000); //checking throttle input from rx
if (throtv > 1025||auxv > 1300) { // !! if throttle is higher then 1025 then safety goes first and when aux switch not in good pos it wil not turn on !! //
throttle.writeMicroseconds(1000); // throttle on 0 before start
delay (1000);
safety =1; // safety wil be fase 1 = disable trhottle input , and output 0 throttle = disabled mode //
return;
}
else if (throtv < 1025||auxv < 1300)
{
safety =2; // if input throttle = near 0 then safety wil be fase 2 = working quadcopter going to safety fase 3 later = flight mode //
}
}
void loop() {
up = constrain(up, 1500, 1760);
val = Serial.read() - '0';
delay (30);
if (val == 2) auxset =1000;
if (val == 3) auxset =1800;
if (val == 4) decent = decent +10;
if (val == 5) decent = decent -10;
if (val == 6) height = height +100;
if (val == 7) height = height -100;
if (val == 8) digitalWrite(led,LOW);
if (val == 9) digitalWrite(piezo,HIGH);
if (val == 69) up = up +10; //& char
if (val == 52) up = up -10; //% char
if (val == 1) {
EEPROM.write(addr0,height/100);
EEPROM.write(addr1,auxset/10);
EEPROM.write(addr2,decent/10);
EEPROM.write(addr3,up /10 );
height= EEPROM.read(addr0 )*100;
auxset = EEPROM.read(addr1 )*10;
decent = EEPROM.read(addr2 )*10;
up = EEPROM.read(addr3 )*10;
}
sensor = cm/ US_ROUNDTRIP_CM;
Serial.print((char)1);
Serial.print (throts);
Serial.print((char)2);
Serial.print (throtv);
Serial.print((char)3);
Serial.print (height);
Serial.print((char)4);
Serial.print (auxv);
Serial.print((char)5);
Serial.print (auxset);
Serial.print((char)6);
Serial.print (cm);
Serial.print((char)7);
Serial.print (sensor);
Serial.print((char)8);
Serial.print (safety);
Serial.print((char)9);
Serial.print (horiz);
Serial.print((char)10);
Serial.print (verti);
Serial.print((char)11);
Serial.print (decent);
Serial.print((char)12); // these rules are here to fil my buffer of GUI ,, not remove this make's things go faster on the other side
Serial.print (up); // this triggers a part that cleans out the input buffer on the GUI every second , so there is no major delays in the data because of buffer build up
Serial.print((char)13);
Serial.print (decent);
Serial.flush();
digitalWrite(piezo,LOW);
horim = pulseIn(4, HIGH, 25000); //checking gimbal horizontal input
auxv = pulseIn(5, HIGH, 25000); //checking aux input
throtv = pulseIn(6, HIGH, 25000); //checking throttle input
vertm = pulseIn(9, HIGH, 25000); //checking gimbal vertical input
horiz =map(horim, 1700, 1300,-20,100); // mapping value for GUI
verti =map(vertm, 1700, 1300,-20,100);
cm = sonar.ping();
aux.writeMicroseconds(auxset); // aux ON for autolevel kk2.0 conected to aux input //
throtv = pulseIn(6, HIGH, 25000); //checking throttle input pwm
auxv = pulseIn(5, HIGH, 25000); // Read the pulse width of AUX input on pin 5 and reads input pwm//
if(cm >0)height1 = cm;
if (auxv < 1300 && safety == 2){
throttle.writeMicroseconds(throtv);
digitalWrite(led, HIGH);
system =throtv ; // telling the system the value of the throtle input
sys =throtv;
}
else if (auxv > 1300 && safety == 2){
mem1 =sys;
if(height<height1){ // if sensor is lower then height value //
system = sys -1;// take speed DOWN by 1 digit of the system //
throts = mem1;
sys = constrain(system,1000,2000);
throttle.writeMicroseconds(sys); // write system speed to servo //
digitalWrite(led, HIGH);
delay (decent); // decent delay time
}
if(height>height1){ // if sensor is lower then height value //
system = sys +3 ;// take speed up by 4 digit of the system //
throts = mem1;
sys = constrain(system,1000,up); // THIS IS THE GOING UP THROTTLE CONSTRAIN YOU NEED TO TUNE THIS
throttle.writeMicroseconds(sys); // write system speed to servo //
digitalWrite(led, LOW);
digitalWrite(piezo,HIGH);
}
}
}
version (2)
this one uses same methods as version (1) but this wil use the height measured at flight
as setpoint for height , means you switch AUX and this height wil be the height it wil try to maintain ,
it also used a constrained output ,means when you switch at hoover state it wil use this value to ad some values and substract values
to maintain height , the nice thing is it wil not drop because of to much decrease of throttle , and wil not shoot up because of to much throttle,
you wil not have auto lift off ,
and there is no manual overide <this can be added true un comment one centence labeled // manual take over ,>
// dexters KK2.0 automatic flight system addon board software //
// debug sketch 15-8-2014 works ok //
// responses OK //
// height set when aux switched ,, without PID same results
// uses almost a diy PID - NOT MANUAL WHEN AUX SWITCHED ,,//
// un comment label manualy take over for manualy overides //
// instructions fly to height ,, hoover + aux = maintain height
// uses contsrained throttle input + 10 and - 5
// from original input before switched
/*
********************************************************************
x&x x&x
x. .x
. .
. .
. . CHECK KK2.0 RECIEVER INPUTS
. . *********************************
< > ****************** HEIGHT SENSOR PLACED UNDER QUAD
< > *********************************
. . USE THIS ONLEY WHEN NOVICE
. .
. .
. .
X x
x&x x&x
x x
********************************************************************
// ALL QUADS NEED TO BE TUNED //
//
*/
#include <Servo.h>
#include <NewPing.h>
#include <EEPROM.h>
#define TRIGGER_PIN 3 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 2 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 400 // max sensor value 400 means 4 meter
#define addr0 0
#define addr1 1
#define addr2 2
#define addr3 3
#define addr4 5
#define led 13
#define piezo 10
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int cm,sensor,auxv,auxs,throtv,throts,superx,superz,superg,system,mem1,sys,val,horim,horiz,vertm,verti,height,height1,htchk,auxset,decent,up; //incoming value of sensor 0-100000
int safety =1; // safety switch value//
int signal =0; // signal procces
Servo throttle,aux; // make servo output to kk2.0 throttle input
void setup() {
aux.attach(7); // servo conection out pin 7 to kk2.0 aux input //
throttle.attach(8); // servo conection out pin 8 to kk2.0 throttle input //
aux.writeMicroseconds(auxset); // set aux output to autolevel on on kk2.0 //
pinMode(led, OUTPUT);
pinMode(piezo,OUTPUT);
pinMode(4, INPUT); // NEW HORIZ INPUT CHANNEL
pinMode(5, INPUT); // Set input channel for aux on pin 5 from rx //
pinMode(6, INPUT); // Set input channel for throttle on pin 6 from rx //
pinMode(9, INPUT); // Set input channel from gimbal horizontal pin 9 from rx for reading level //
digitalWrite(led, LOW); // turn the LED on
digitalWrite(piezo, LOW); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, HIGH); // turn the LED on
digitalWrite(piezo, HIGH); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, LOW);
auxset = 1800;
system = 0;
throtv = pulseIn(6, HIGH, 25000); //checking throttle input from rx
auxv = pulseIn(5, HIGH, 25000); //checking throttle input from rx
if (throtv > 1025||auxv > 1300) { // !! if throttle is higher then 1025 then safety goes first and when aux switch not in good pos it wil not turn on !! //
throttle.writeMicroseconds(1000); // throttle on 0 before start
delay (1000);
safety =1; // safety wil be fase 1 = disable trhottle input , and output 0 throttle = disabled mode //
return;
}
else if (throtv < 1025||auxv < 1300)
{
safety =2; // if input throttle = near 0 then safety wil be fase 2 = working quadcopter going to safety fase 3 later = flight mode //
}
}
void loop() {
superx = constrain(superx,1000,2000);
digitalWrite(piezo,LOW);
horim = pulseIn(4, HIGH, 25000); //checking gimbal horizontal input
auxv = pulseIn(5, HIGH, 25000); //checking aux input
throtv = pulseIn(6, HIGH, 25000); //checking throttle input
vertm = pulseIn(9, HIGH, 25000); //checking gimbal vertical input
horiz =map(horim, 1700, 1300,-20,100); // mapping value for GUI
verti =map(vertm, 1700, 1300,-20,100);
cm = sonar.ping();
aux.writeMicroseconds(auxset); // aux ON for autolevel kk2.0 conected to aux input //
throtv = pulseIn(6, HIGH, 25000); //checking throttle input pwm
auxv = pulseIn(5, HIGH, 25000); // Read the pulse width of AUX input on pin 5 and reads input pwm//
// superx = throtv; // manual take over //
if(cm >0)height1 = cm;
if (auxv < 1300 && safety == 2){
throttle.writeMicroseconds(throtv);
digitalWrite(led, HIGH);
superx = throtv;
system =throtv ; // telling the system the value of the throtle input
sys =throtv;
height = height1;
}
else if (auxv > 1300 && safety == 2){
mem1 =sys;
superg = superx + 5;
superz = superx - 3;
if(height<height1){ // if sensor is lower then height value //
system = sys -1;// take speed DOWN by 1 digit of the system //
throts = mem1;
sys = constrain(system,superz,superg);
throttle.writeMicroseconds(sys); // write system speed to servo //
digitalWrite(led, HIGH);
delay (decent); // decent delay time
}
if(height>height1){ // if sensor is lower then height value //
system = sys +2 ;// take speed up by 2 digit of the system //
throts = mem1;
sys = constrain(system,superz,superg); // THIS IS THE GOING UP THROTTLE CONSTRAIN YOU NEED TO TUNE THIS
throttle.writeMicroseconds(sys); // write system speed to servo //
digitalWrite(led, LOW);
digitalWrite(piezo,HIGH);
}
}
}
version (3)
this one use same method as above but uses PID ,
if switched it wil use this height as reference , and try to maintain it by adding and substracting values from throttle input .
this is manualy overidable ,
it also uses a constrain when switched same method as above ,
but the constrain is overidable with throttle input also , so if you above switched setpoint and it wil fly away you can bring it back to setpoint
and try to hoover , and it wil use this hoover input throttle for the new constrain ,
the nice thing when using this way of overide is the battery problem ,
example when batt full you fly at halve throttle , this wil decrease in flight ,
so a fixed constrain like version (1) uses for not exceeding throttle ,meaning the quad wil shoot over the setpoint because to much power when not constraing the output max ,
this version wil not have this problem because it uses the value you use in mid flight ,, lower bat power
and build up a new constrain like version (2) every time you switch + manual overide throttle input .
they are al tested , and they can al use the GUI < i made different versions GUI but need some progress >
i also will make some easy setup GUI's to setup V1 V2 and V3 ,, true usb or serial rx tx module
,some xtra info
the sonic sensor , i use a buffer that onley will ad positive inputs for using the calculations ( if sonic sensor reads above zero it wil go to height buffer )
this buffer value is the one used ,zero reading wil not excist
what do i want to try ,,
i wil do a bypas for the gimbal also ,, so you can stil use the gimbal ,, and read out this movement true GUI
or move or overide it true GUI,< means also headtracking true APC220 >
,one new version combined the PID and the normal version + above ,
+, obstical avoidence ,, for when flying fpv it wil scan forwards and try to jump over object ,
so when i fly infront a fence it wil go back for brake , and wil rise above the fence til above , wil jump over
and give me back control , also manual overidable ,,
also stil thinking what wil be cool for the quadcopter to add ,
i can use all sensors for doing different tasking , and sends back data to my GUI ,,
example a temp sensor ,, voltage ,,
also gone modify my RC TX ,,aux switch from 2 switch to a 3 or four pos ,, and this wil make it capable to switch to diferent
aux outputs ,, and use them for different tasks ,
so there is stil a lot to do ,,
and it keeps me doing so
for using the GUI in al versions V2 and V3
i am building simplified GUI versions for V2 and V3 to setup these ,
and some other addons
VERSION 3
// dexters KK2.0 automatic flight system addon board software //
// debug sketch 15-8-2014 works ok //
// this uses PID to try to maintain on height //
// instructions < fly to level height + hoover + switch aux ,
// and it wil try to maintain height,
// height is set when switched//
// input is manual overidable
// responses OK //
/*
********************************************************************
x&x x&x
x. .x
. .
. .
. . CHECK KK2.0 RECIEVER INPUTS
. . *********************************
< > ****************** HEIGHT SENSOR PLACED UNDER QUAD
< > *********************************
. . USE THIS ONLEY WHEN NOVICE
. .
. .
. .
X x
x&x x&x
x x
********************************************************************
// ALL QUADS NEED TO BE TUNED //
//
*/
#include <Servo.h>
#include <NewPing.h>
#include <PID_v1.h>
double Setpoint,height1,throts,cm,throtv,trh1,trh2;
#define Kp 2
#define Ki 5
#define Kd 1
PID altPID(&height1, &throts, &Setpoint,Kp,Ki,Kd, DIRECT);
#define TRIGGER_PIN 3 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 2 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 400 // max sensor value 400 means 4 meter
#define addr0 0
#define addr1 1
#define addr2 2
#define addr3 3
#define addr4 5
#define led 13
#define piezo 10
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int sensor,auxv,auxs,system,mem1,sys,val,height,auxset; //incoming value of sensor 0-100000
int safety =1; // safety switch value//
int signal =0; // signal procces
Servo throttle,aux; // make servo output to kk2.0 throttle input
void setup() {
aux.attach(7); // servo conection out pin 7 to kk2.0 aux input //
throttle.attach(8); // servo conection out pin 8 to kk2.0 throttle input //
aux.writeMicroseconds(auxset); // set aux output to autolevel on on kk2.0 //
pinMode(led, OUTPUT);
pinMode(piezo,OUTPUT);
pinMode(4, INPUT); // NEW HORIZ INPUT CHANNEL
pinMode(5, INPUT); // Set input channel for aux on pin 5 from rx //
pinMode(6, INPUT); // Set input channel for throttle on pin 6 from rx //
pinMode(9, INPUT); // Set input channel from gimbal horizontal pin 9 from rx for reading level //
altPID.SetMode(AUTOMATIC);
digitalWrite(led, LOW); // turn the LED on
digitalWrite(piezo, LOW); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, HIGH); // turn the LED on
digitalWrite(piezo, HIGH); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, LOW);
auxset = 1800;
system = 0;
throtv = pulseIn(6, HIGH, 25000); //checking throttle input from rx
auxv = pulseIn(5, HIGH, 25000); //checking throttle input from rx
if (throtv > 1025||auxv > 1300) { // !! if throttle is higher then 1025 then safety goes first and when aux switch not in good pos it wil not turn on !! //
throttle.writeMicroseconds(1000); // throttle on 0 before start
delay (1000);
safety =1; // safety wil be fase 1 = disable trhottle input , and output 0 throttle = disabled mode //
return;
}
else if (throtv < 1025||auxv < 1300)
{
safety =2; // if input throttle = near 0 then safety wil be fase 2 = working quadcopter going to safety fase 3 later = flight mode //
}
}
void loop() {
trh1 = throtv - 2.5;
trh2 = throtv + 2.5;
altPID.SetOutputLimits(trh1,trh2);
digitalWrite(piezo,LOW);
auxv = pulseIn(5, HIGH, 25000); //checking aux input
throtv = pulseIn(6, HIGH, 25000); //checking throttle input
cm = sonar.ping();
aux.writeMicroseconds(auxset); // aux ON for autolevel kk2.0 conected to aux input //
if(cm >0)height1 = cm;
if (auxv < 1300 && safety == 2){
Setpoint= height1;
throttle.writeMicroseconds(throtv);
digitalWrite(led, HIGH);
system =throtv ; // telling the system the value of the throtle input
sys =throtv;
}
else if (auxv > 1300 && safety == 2){
cm = sonar.ping();
altPID.Compute();
throttle.writeMicroseconds(throts);
}
}
today , changed the int , to double's ,,
this makes things different ,
Now i can use fractions ,
i also wil make a smal readout tool for V1 V2 V3 ,
this is for checking your inputs and conections ,,
and testing the builded sketch , if its reacting the way we want it to ,
if aux input range from switch is ok ,, < is it on or off >
throttle responses ,,
and i wil make a better schematic drawing how things are connected,
there is to much i can change , and add to my quadcopter Arduino addon board ,,
the last sketches V2 and V3 i leave out the GUI ,,
i wil change this al according to my setup mini tool ,,
so you can change this without editing the sketches ,,
this makes things easy to use ,,
i hope some one like to help , maybe have some ideas ?
or seen my sketches , and you think this could be done better , please let me know !
Dexterbot:
i think this is to hard :%
i have 2000 readers in 2 weeks so there is a interest in subject ,, i ask the same ,why
maybe they want more movies , and proof of concept ,,
people like to take stuff , or learn for there own project ,
but not have the money to build ,, or the time ,,
i now for sure there are a lot of people using the sketch for other things ,,
addon for quadcopter , boats , and other rc stuff ,,
and i thinks the complete package looks hard to make ,
but the hardware and sketches i use are safe to use ,,
maybe people are scarred ,, this is why a lot of people buy stuff , and not build it ,,
but yes you are the first one ,,
and yes i also removed some post , because if there is no interest why should i share my time and work ,
maybe you know the answer ,,
you read the post , the subject ,, is this hard to understand ??
let me know !
@Dexterbot:
Here is what I know:
- You should be thankful that out of 2000 views, you had no questions! Maybe folks just understood your work.
- With no feedback, how do you know folks are "using your code for other things?" Are you clairvoyant? (Rhetorical.)
- Posting and removing is (IMO) just wrong, very wrong; it is selfish and childish.
- On one forum, I have 7981 reads of a post and not one single question - I must have done something right!
Posting on any forum, Arduino.cc in particular, is not going to start a parade down your street.
- Post because you are enthused about your hobby.
- Share because you want to further the art.
- Enlighten yourself from the works of others.
Ray
oke now i fixed al v1 v2 and v3 in one version ,
able to use decent delay ,, PID for control and adjustable true gui , or normal system i used before
auto heightset wil use the height you measure when switching from manual to auto mode
and it wil try to maintain height <sensor needs range !>
or manual true gui ,
when using pid mode ,you cant use a decent delay !
i use a automatic throttle constrain for min and max after you switch from manual to auto
,this wil use the input you are using before switching ,
example you hoover at throttle input 1730 ,,it wil build a constrain min 1700 max 1750 ,
this wil be the values it wil use to maintain height ,
i done this because it wil not have the quad copter shooting up ,, or loose to much throttle ,and drop ! ]
and alos battery voltage changes , wil make a fixed constrain unusable ,
this is manual overidable in PID mode ,
you can change the sketch the min and max values it uses for building constrains , and PID min and max .
this version use fractions , for more acurate responses ,
please be aware i tested al these things combined ,
i wil ad a extra button to be sure manual overide is on and of ,,, < on PID and other mode >
and some other things ,, wil explain later on ,,
i made the version i use now downloadable ,,
you can run the sketch on a Arduino for seeing it work ,,
sketch bellow
addon.bmp (2.93 MB)
// dexters KK2.0 automatic flight system addon board software //
// debug sketch 20-8-2014 works ok //
// last PID and state's saved in eeprom true gui button //
// responses OK //
/*
********************************************************************
x x
x&x x&x
X X
. .
. .
. . CHECK KK2.0 RECIEVER INPUTS
. . *********************************
< > ****************** HEIGHT SENSOR PLACED UNDER QUAD
< > *********************************
. . USE THIS ONLEY WHEN NOVICE
. .
. .
. .
X X
x&x x&x
x x
********************************************************************
// ALL QUADS NEED TO BE TUNED //
//
*/
#include <Servo.h>
#include <NewPing.h>
#include <EEPROM.h>
#include <PID_v1.h>
#define TRIGGER_PIN 3 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 2 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 500 // max sensor value 500 means 5 meter
#define addr0 0
#define addr1 1
#define addr2 2
#define addr3 3
#define addr4 4
#define addr5 5
#define addr6 6
#define addr7 7
#define addr8 8
#define led 13
#define piezo 10
int auxv,auxs,val,horim,horiz,vertm,verti,auxset,decent,Kp,Ki,Kd,pid,autoheigth,autodelay;
double cm,throtv,throts,system,mem1,sys,height1,height,up,sensor,trh1,trh2,trh3,trh4,Setpoint,trhm1,trhm2;
PID altPID(&height1, &throts, &Setpoint,Kp,Ki,Kd, DIRECT);
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
int safety =1; // safety switch value//
int signal =0;
Servo throttle,aux;
void setup() {
Serial.begin(57600); // starting serial port data for APC220 wireles tx rx baud rate 57600 for GUI //
aux.attach(7); // servo conection out pin 7 to kk2.0 aux input //
throttle.attach(8); // servo conection out pin 8 to kk2.0 throttle input //
aux.writeMicroseconds(auxset); // set aux output to autolevel on on kk2.0 //
pinMode(led, OUTPUT);
pinMode(piezo,OUTPUT);
pinMode(4, INPUT);
pinMode(5, INPUT);
pinMode(6, INPUT);
pinMode(9, INPUT);
altPID.SetMode(AUTOMATIC);
digitalWrite(led, LOW); // turn the LED on
digitalWrite(piezo, LOW); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, HIGH); // turn the LED on
digitalWrite(piezo, HIGH); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, LOW);
auxset = EEPROM.read(addr1 )*10;
decent = EEPROM.read(addr2 )*10;
autoheigth = EEPROM.read(addr3);
autodelay = EEPROM.read(addr4);
Kp = EEPROM.read(addr5 );
Ki = EEPROM.read(addr6 );
Kd = EEPROM.read(addr7 );
pid = EEPROM.read(addr8 );
if (auxset < 1000 || auxset > 1800 )auxset =1000;
if (Kp>20)Kp =2;
if (Ki>20)Ki =5;
if (Kd>20)Kd =1;
if (Kp<=0)Kp =2;
if (Ki<=0)Ki =5;
if (Kd<=0)Kd =1;
if (decent>500)decent =250;
trhm1 =1.25; // min for pid throttle output constrain you can change it ,this builds a pid output constrain from input throttle - this value
trhm2 =1.25; // max for pid throttle output constrain you can change it ,this builds a pid output constrain from input throttle + this value
system = 0;
throtv = pulseIn(6, HIGH, 25000); //checking throttle input from rx
auxv = pulseIn(5, HIGH, 25000); //checking throttle input from rx
if (throtv > 1025||auxv > 1300) { // !! if throttle is higher then 1025 then safety goes first and when aux switch not in good pos it wil not turn on !! //
throttle.writeMicroseconds(1000); // throttle on 0 before start
delay (1000);
safety =1; // safety wil be fase 1 = disable trhottle input , and output 0 throttle = disabled mode //
return;
}
else if (throtv < 1025||auxv < 1300)
{
safety =2; // if input throttle = near 0 then safety wil be fase 2 = working quadcopter going to safety fase 3 later = addon board armed mode //
}
}
void loop() {
val = Serial.read() - '0';
delay (35);
if (val == 55) autoheigth = autoheigth + 5 ;
if (val == 56) autodelay = autodelay + 5 ;
if (val == 17) pid = pid + 5 ;
if (val == 58) height = height +100 ;
if (val == 46) height = height -100;
if (val == 2) auxset =1000;
if (val == 3) auxset =1800;
if (val == 4) decent = decent +10;
if (val == 5) decent = decent -10;
if (val == 63) Kp = Kp -1;
if (val == 69) Kp = Kp +1;
if (val == 27) Ki = Ki -1;
if (val == 28) Ki = Ki +1;
if (val == 22) Kd = Kd -1;
if (val == 34) Kd = Kd +1;
if (val == 8) digitalWrite(led,LOW);
if (val == 9) digitalWrite(piezo,HIGH);
if (val == 1) {
EEPROM.write(addr1,auxset/10);
EEPROM.write(addr2,decent/10);
EEPROM.write(addr3,autoheigth);
EEPROM.write(addr4,autodelay);
EEPROM.write(addr5,Kp) ;
EEPROM.write(addr6,Ki) ;
EEPROM.write(addr7,Kd) ;
EEPROM.write(addr8,pid) ;
auxset = EEPROM.read(addr1 )*10;
decent = EEPROM.read(addr2 )*10;
}
Serial.flush();
sensor = cm/ US_ROUNDTRIP_CM;
Serial.print((char)1);
Serial.print (throts);
Serial.print((char)2);
Serial.print (throtv);
Serial.print((char)3);
Serial.print (height);
Serial.print((char)4);
Serial.print (auxv);
Serial.print((char)5);
Serial.print (auxset);
Serial.print((char)6);
Serial.print (cm);
Serial.print((char)7);
Serial.print (sensor);
Serial.print((char)8);
Serial.print (safety);
Serial.print((char)9);
Serial.print (horiz);
Serial.print((char)10);
Serial.print (verti);
Serial.print((char)11);
Serial.print (decent);
Serial.print((char)12);
Serial.print (trh1);
Serial.print((char)13);
Serial.print (trh2);
Serial.print((char)14);
Serial.print ("QCopter ADDON V1");
Serial.print((char)15);
Serial.print (autoheigth);
Serial.print((char)16);
Serial.print (autodelay);
Serial.print((char)17);
Serial.print (pid);
Serial.print((char)18);
Serial.print (Kp);
Serial.print((char)19);
Serial.print (Ki);
Serial.print((char)20);
Serial.print (Kd);
Serial.print((char)21);
trh1 = throtv - 2.5; // value - throttle for min throttle constrain you can change this //
trh2 = throtv + 2.5; // value + throttle for max throttle constrain you can change this //
if (autoheigth >5) autoheigth =0;
if (autodelay >5) autodelay =0;
if (pid >5) pid =0;
trh3 = throtv - trhm1;
trh4 = throtv + trhm2;
altPID.SetOutputLimits(trh3,trh4);
digitalWrite(piezo,LOW);
horim = pulseIn(4, HIGH, 25000); //checking gimbal horizontal input
auxv = pulseIn(5, HIGH, 25000); //checking aux input
throtv = pulseIn(6, HIGH, 25000); //checking throttle input
vertm = pulseIn(9, HIGH, 25000); //checking gimbal vertical input
horiz =map(horim, 1700, 1300,-20,100);
verti =map(vertm, 1700, 1300,-20,100);
cm = sonar.ping();
aux.writeMicroseconds(auxset);
throtv = pulseIn(6, HIGH, 25000);
auxv = pulseIn(5, HIGH, 25000);
if(cm >0)height1 = cm;
if (auxv < 1300 && safety == 2){
if (autoheigth ==0) height = height1;
Setpoint= height;
throttle.writeMicroseconds(throtv);
digitalWrite(led, HIGH);
system =throtv ; \
sys =throtv;
}
else if (auxv > 1300 && pid == 0){
cm = sonar.ping();
altPID.Compute();
throttle.writeMicroseconds(throts);
}
else if (auxv > 1300 && pid == 5){
mem1 =sys;
if(height<height1){
system = sys - 0.25;// take speed down by 0.25 digit of the system you can change this//
throts = mem1;
sys = constrain(system,trh1,trh2);
throttle.writeMicroseconds(sys);
digitalWrite(led, HIGH);
if (autodelay ==5) delay (decent);
}
if(height>height1){
system = sys + 0.25 ;// take speed up by 0.25 digit of the system you can change this //
throts = mem1;
sys = constrain(system,trh1,trh2);
throttle.writeMicroseconds(sys);
digitalWrite(led, LOW);
digitalWrite(piezo,HIGH);
}
}
}
i was a little bit sick ,, so i wil update one of these days ,, < this week > 
my latest sketch ,,
// dexters KK2.0 automatic flight system addon board software //
// debug sketch 24-8-2014 works ok //
// last PID and state's saved in eeprom true gui button //
// responses OK //
/*
********************************************************************
x x
x&x x&x
X X
. .
. .
. . CHECK KK2.0 RECIEVER INPUTS
. . *********************************
< > ****************** HEIGHT SENSOR PLACED UNDER QUAD
< > *********************************
. . USE THIS ONLEY WHEN NOVICE
. .
. .
. .
X X
x&x x&x
x x
********************************************************************
// ALL QUADS NEED TO BE TUNED //
//
*/
#include <Servo.h>
#include <NewPing.h>
#include <EEPROM.h>
#include <PID_v1.h>
#define TRIGGER_PIN 3 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 2 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 500 // max sensor value 500 means 5 meter
#define addr0 0
#define addr1 1
#define addr2 2
#define addr3 3
#define addr4 4
#define addr5 5
#define addr6 6
#define addr7 7
#define addr8 8
#define led 13
#define piezo 10
int auxv,auxs,val,horim,horiz,vertm,verti,auxset,decent,Kp,Ki,Kd,pid,autoheigth,autodelay;
double cm,throtv,throts,system,mem1,sys,height1,height,up,sensor,trh1,trh2,trh3,trh4,Setpoint,trhm1,trhm2;
PID altPID(&height1, &throts, &Setpoint,Kp,Ki,Kd, DIRECT);
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
int safety =1; // safety switch value//
int signal =0;
Servo throttle,aux;
void setup() {
Serial.begin(57600); // starting serial port data for APC220 wireles tx rx baud rate 57600 for GUI //
aux.attach(7); // servo conection out pin 7 to kk2.0 aux input //
throttle.attach(8); // servo conection out pin 8 to kk2.0 throttle input //
aux.writeMicroseconds(auxset); // set aux output to autolevel on on kk2.0 //
pinMode(led, OUTPUT);
pinMode(piezo,OUTPUT);
pinMode(4, INPUT);
pinMode(5, INPUT);
pinMode(6, INPUT);
pinMode(9, INPUT);
altPID.SetMode(AUTOMATIC);
digitalWrite(led, LOW); // turn the LED on
digitalWrite(piezo, LOW); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, HIGH); // turn the LED on
digitalWrite(piezo, HIGH); // turn the PIEZO on
delay(1000); // wait for a second
digitalWrite(led, LOW);
auxset = EEPROM.read(addr1 )*10;
decent = EEPROM.read(addr2 )*10;
autoheigth = EEPROM.read(addr3);
autodelay = EEPROM.read(addr4);
Kp = EEPROM.read(addr5 );
Ki = EEPROM.read(addr6 );
Kd = EEPROM.read(addr7 );
pid = EEPROM.read(addr8 );
if (auxset < 1000 || auxset > 1800 )auxset =1000;
if (Kp>20)Kp =2;
if (Ki>20)Ki =5;
if (Kd>20)Kd =1;
if (Kp<=0)Kp =2;
if (Ki<=0)Ki =5;
if (Kd<=0)Kd =1;
if (decent>500)decent =250;
trhm1 =1.25; // min for pid throttle output constrain you can change it ,this builds a pid output constrain from input throttle - this value
trhm2 =1.25; // max for pid throttle output constrain you can change it ,this builds a pid output constrain from input throttle + this value
system = 0;
throtv = pulseIn(6, HIGH, 25000); //checking throttle input from rx
auxv = pulseIn(5, HIGH, 25000); //checking throttle input from rx
if (throtv > 1025||auxv > 1300) { // !! if throttle is higher then 1025 then safety goes first and when aux switch not in good pos it wil not turn on !! //
throttle.writeMicroseconds(1000); // throttle on 0 before start
delay (1000);
safety =1; // safety wil be fase 1 = disable trhottle input , and output 0 throttle = disabled mode //
return;
}
else if (throtv < 1025||auxv < 1300)
{
safety =2; // if input throttle = near 0 then safety wil be fase 2 = working quadcopter going to safety fase 3 later = addon board armed mode //
}
}
void loop() {
val = Serial.read() - '0';
delay (35);
if (val == 55) autoheigth = autoheigth + 5 ;
if (val == 56) autodelay = autodelay + 5 ;
if (val == 17) pid = pid + 5 ;
if (val == 58) height = height +100 ;
if (val == 46) height = height -100;
if (val == 2) auxset =1000;
if (val == 3) auxset =1800;
if (val == 4) decent = decent +10;
if (val == 5) decent = decent -10;
if (val == 63) Kp = Kp -1;
if (val == 69) Kp = Kp +1;
if (val == 27) Ki = Ki -1;
if (val == 28) Ki = Ki +1;
if (val == 22) Kd = Kd -1;
if (val == 34) Kd = Kd +1;
if (val == 8) digitalWrite(led,LOW);
if (val == 9) digitalWrite(piezo,HIGH);
if (val == 1) {
EEPROM.write(addr1,auxset/10);
EEPROM.write(addr2,decent/10);
EEPROM.write(addr3,autoheigth);
EEPROM.write(addr4,autodelay);
EEPROM.write(addr5,Kp) ;
EEPROM.write(addr6,Ki) ;
EEPROM.write(addr7,Kd) ;
EEPROM.write(addr8,pid) ;
auxset = EEPROM.read(addr1 )*10;
decent = EEPROM.read(addr2 )*10;
}
Serial.flush();
sensor = cm/ US_ROUNDTRIP_CM;
Serial.print((char)1);
Serial.print (throts);
Serial.print((char)2);
Serial.print (throtv);
Serial.print((char)3);
Serial.print (height);
Serial.print((char)4);
Serial.print (auxv);
Serial.print((char)5);
Serial.print (auxset);
Serial.print((char)6);
Serial.print (cm);
Serial.print((char)7);
Serial.print (sensor);
Serial.print((char)8);
Serial.print (safety);
Serial.print((char)9);
Serial.print (horiz);
Serial.print((char)10);
Serial.print (verti);
Serial.print((char)11);
Serial.print (decent);
Serial.print((char)12);
Serial.print (trh1);
Serial.print((char)13);
Serial.print (trh2);
Serial.print((char)14);
Serial.print ("QCopter ADDON V1");
Serial.print((char)15);
Serial.print (autoheigth);
Serial.print((char)16);
Serial.print (autodelay);
Serial.print((char)17);
Serial.print (pid);
Serial.print((char)18);
Serial.print (Kp);
Serial.print((char)19);
Serial.print (Ki);
Serial.print((char)20);
Serial.print (Kd);
Serial.print((char)21);
trh1 = throtv - 2.5; // value - throttle for min throttle constrain you can change this //
trh2 = throtv + 2.5; // value + throttle for max throttle constrain you can change this //
if (autoheigth >5) autoheigth =0;
if (autodelay >5) autodelay =0;
if (pid >5) pid =0;
trh3 = throtv - trhm1;
trh4 = throtv + trhm2;
altPID.SetOutputLimits(trh3,trh4);
digitalWrite(piezo,LOW);
horim = pulseIn(4, HIGH, 25000); //checking gimbal horizontal input
auxv = pulseIn(5, HIGH, 25000); //checking aux input
throtv = pulseIn(6, HIGH, 25000); //checking throttle input
vertm = pulseIn(9, HIGH, 25000); //checking gimbal vertical input
horiz =map(horim, 1700, 1300,-20,100);
verti =map(vertm, 1700, 1300,-20,100);
cm = sonar.ping();
aux.writeMicroseconds(auxset);
throtv = pulseIn(6, HIGH, 25000);
auxv = pulseIn(5, HIGH, 25000);
if(cm >0)height1 = cm;
if (auxv < 1300 && safety == 2){
if (autoheigth ==0) height = height1;
Setpoint= height;
throttle.writeMicroseconds(throtv);
digitalWrite(led, HIGH);
system =throtv ; \
sys =throtv;
}
else if (auxv > 1300 && pid == 0){
cm = sonar.ping();
altPID.Compute();
throttle.writeMicroseconds(throts);
}
else if (auxv > 1300 && pid == 5){
mem1 =sys;
if(height<height1){
system = sys - 0.25;// take speed down by 0.25 digit of the system you can change this//
throts = mem1;
sys = constrain(system,trh1,trh2);
throttle.writeMicroseconds(sys);
digitalWrite(led, HIGH);
if (autodelay ==5) delay (decent);
}
if(height>height1){
system = sys + 0.25 ;// take speed up by 0.25 digit of the system you can change this //
throts = mem1;
sys = constrain(system,trh1,trh2);
throttle.writeMicroseconds(sys);
digitalWrite(led, LOW);
digitalWrite(piezo,HIGH);
}
}
}
- Ok , here i am again ,
i fixed a lot of isues , and i also made a other GUI ,
i wil upload it this weekend !
now i can change al things true tx 433 mhz transmiter onboar ,
i also fixed a digital i2C compas sensor , for heading and some other things maybe later ,
also i make use of Video in for the GUI so i can see video feed from a source ,
like a real drone station ,, but i stil figuring out a way to fix the way to switch back from auto lift up mode ,,
, i also make use of case's so you can ad your case if needed ,
somebody else also tested it on a other cc3d board , and he needed to change the incoming singal range from 1000 2000 , to 2000 to 4000 so you need to check your inputs before using ,!
before i post the new sketches i need to figure some small thing out , butt this is it for now ,
my sketch is to big 
wil find other source !