copterrichie wrote:I think if you were to try making the P = 1 and D as you have it, it may just work as expected. This is just a guess but well worth a try.
shikra wrote:Firstly I think we should recognise the contributions of everyone to get us where we are today.
Without doubt, ACRO is simply amazing on Multiwii.
Personally I would love to see improvements to BARO rather than new functionality / bling. But I know its so much more fun / interesting to work on new things.....
Are we out of idea share? Is there anything worth gaining from the other Open Source multi solutions?
mahowik wrote:Hi,
... - double integrated ACC data and errors accumulations...
copterrichie wrote:In my opinion, a Baro on a Rotor copter is going to have problems because of the pressure differences created by the spinning props. Not to speak of vibration issues. ......
pm1 wrote:mahowik wrote:Hi,
... - double integrated ACC data and errors accumulations...
exactly my experience too.
I did then concentrate on baro only, I denoise the input in a different way. I now have a D term, which prevents fast vertical velocity. In hover I it is as good as the baro delivers (sometimes I see jumps of 1-2m, which are not noise, but the baro changes mind for 10-20 seconds). The only problem I still have is when fast corrections are neccessary, like going from hover to fast forward flight. Here the copter drops 1-2m and gains height slowly after 1-2s later
dramida wrote:copterrichie wrote:In my opinion, a Baro on a Rotor copter is going to have problems because of the pressure differences created by the spinning props. Not to speak of vibration issues. ......
Guys, let's have a positive attitude about the Alt Hold. If anyone ask themself if it's possible, you should see DJI Naze how amazing holds its altitude based on same sensors as MWC.
Please look at the graph and see the spectrum of the noise for accelerommeter and barometer. Filter out (LPF) those frequencies and you will get a clean signal.
Mihai.
dramida wrote:Filter out (LPF) those frequencies and you will get a clean signal.
dramida wrote:If we have an 5-10 seconds averaging vector for altitude, all the noise is gone. So mixing the resul of those two control loops will do a far better job than actual P controller for Acc.
mahowik wrote:could you share your code on this?
pm1 wrote:
My alt pids: 1.0 0.010 20
Crashpilot1000 wrote:@pm1: ...the Accz part is really crude and far from perfect...
copterrichie wrote:The GPS Altitude readings are more accurate for reference then the Baro, in my opinion.
Alexinparis wrote:copterrichie wrote:The GPS Altitude readings are more accurate for reference then the Baro, in my opinion.
That's not true.
A GPS has a very poor alt resolution (some 10 meters), much more worse than its ground 2D accuracy.
A MS baro have a real 10cm alt resolution on the desk.
int16_t invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G
#define K_ACC_1G 200 // scale for calculated acceleration
int16_t scaledAccZ = accZ * K_ACC_1G / acc_1G;
float InvSqrt (float x){
union{
int32_t i;
float f;
} conv;
conv.f = x;
conv.i = 0x5f3759df - (conv.i >> 1);
return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
int32_t isq(int32_t x){return x * x;}
// acc_1G ~= 1/invG
int16_t invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG;
#define ACC_Z_DEADBAND 10
if(abs(accZ) < ACC_Z_DEADBAND) {
accZ = 0;
} else if(accZ > 0){
accZ -= ACC_Z_DEADBAND;
} else if(accZ < 0){
accZ += ACC_Z_DEADBAND;
}
pm1 wrote:And her a vido showing PH+Alt hold: https://vimeo.com/48646152
mahowik wrote:It's not quite correct to test algoritms based on baro data near the ground. Because if altitude less than 60-100cm you get -1..2m in measuring because of the air cushion effect there...
jevermeister wrote:I would prefer altitude hold without gps. as stated before:serial gps even brings the mega 2560 to it's knees.I will build myself a i2c gps in winter.
nils
ps.: I am out of popcorn
Crashpilot1000 wrote:I sum it up:
2 people (dramida & nhadrian) hold the holy grail of alt hold in their hands but haven't been able to disclose any concise details on the last 4 pages. Just some general ideas, you could get from anyone in the street.
......
So long
Kraut ROB
vpb wrote:Hi dramida, about your idea - the ALT hold solution and the ALT PID 3, 0.15, 7 what is your tuning procedure? I want to test your idea. Tks!
dramida wrote:I just checked and the actual values Alt PID are 3, 0.15, 7, i played a lot and i forgot the changes since it works satisfactory even in forward flight. Please see video and hear the engines when alt hold and gps pos hold are activated
http://www.youtube.com/watch?v=B4D2CRxx7zk
#define ACC_LPF_FOR_VELOCITY 10
static float accLPFVel[3];
static t_fp_vector EstG;
void getEstimatedAttitude(){
.....................................
.....................................
#if defined(ACC_LPF_FACTOR)
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f/ACC_LPF_FOR_VELOCITY)) + accADC[axis] * (1.0f/ACC_LPF_FOR_VELOCITY);
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
#define ACC_Z_DEADBAND (acc_1G/50)
void getEstimatedAltitude(){
uint8_t index;
static uint32_t deadLine = INIT_DELAY;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static int8_t BaroHistIdx;
static int32_t BaroHigh;
if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
uint16_t dTime = currentTime - deadLine;
deadLine = currentTime;
//**** Alt. Set Point stabilization PID ****
BaroHistTab[BaroHistIdx] = BaroAlt/10;
BaroHigh += BaroHistTab[BaroHistIdx];
index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
BaroHistIdx++;
if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;
//EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
EstAlt = EstAlt*0.6f + (BaroHigh*10.0f/(BARO_TAB_SIZE/2))*0.4f; // additional LPF to reduce baro noise
//P
int16_t error = applyDeadband16(AltHold - EstAlt, 10); //remove small P parametr to reduce noise near zero position
BaroPID = constrain((conf.P8[PIDALT] * error / 100), -100, +100);
//I
errorAltitudeI += error * conf.I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
BaroPID += (errorAltitudeI / 500); //I in range +/-60
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
//int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G;
//int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG;
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
debug[0] = accZ;
static float vel = 0.0f;
static float accVelScale = 9.80665f / acc_1G / 10000.0f;
// Integrator - velocity, cm/sec
vel+= accZ * accVelScale * dTime;
static int32_t lastBaroAlt = EstAlt;
float baroVel = (EstAlt - lastBaroAlt) / (dTime/1000000.0f);
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero
lastBaroAlt = EstAlt;
debug[1] = baroVel;
// apply Complimentary Filter to keep near zero caluculated velocity based on baro velocity
vel = vel * 0.985f + baroVel * 0.015f;
//vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
debug[2] = vel;
//debug[3] = applyDeadbandFloat(vel, 5);
//D
BaroPID -= constrain(conf.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
debug[3] = BaroPID;
}
int16_t applyDeadband16(int16_t value, int16_t deadband) {
if(abs(value) < deadband) {
value = 0;
} else if(value > 0){
value -= deadband;
} else if(value < 0){
value += deadband;
}
return value;
}
float applyDeadbandFloat(float value, int16_t deadband) {
if(abs(value) < deadband) {
value = 0;
} else if(value > 0){
value -= deadband;
} else if(value < 0){
value += deadband;
}
return value;
}
float InvSqrt (float x){
union{
int32_t i;
float f;
} conv;
conv.f = x;
conv.i = 0x5f3759df - (conv.i >> 1);
return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
int32_t isq(int32_t x){return x * x;}
Return to Software development
Users browsing this forum: No registered users and 0 guests