MultiWii Quad! Alternativa ad Aeroquad/Baronpilot con sensori wii

[leoroby]

bene ragazzi visto che con la dx5e non era possibile armare la multiwii se non agendo sui trimm(cosa scomoda e pericolosa) ho comprato una dx6i e al primo colpo sono riuscito ad armare tutto! visto che ho disponibile sia il fw 1.7 che 1.9 per iniziare a capire multiwii che dite e meglio che lascio il 1.9 o 1.7 potrebbe essere più docile? tenete conto che devo imparare a capire i pid e come modificarli

[cronalex]

oggi ho provato head hold ma purtroppo senza nessun buon risultato... credo che questi disturbi mi danno problemi nel sensore acc vorrei provare la mediamobile sugli acc

ho provato queste funzioni Level... ho messo il quad in hovering e attivato la funzione level.. e fatto alcune trimmature il quad è stabile ma nn lotrovo per niente fermo... ho provato poi ad attivare sia level che mag e stesso risultato... infine ho provato ad attivare headhold e vedo che nn mi va per niente bene sale e scende ma nn è fermo... ho anche provato ad attivare sia headhold che mag ma sempre con il solito risultato

[leoroby]

bene ragazzi visto che con la dx5e non era possibile armare la multiwii se non agendo sui trimm(cosa scomoda e pericolosa) ho comprato una dx6i e al primo colpo sono riuscito ad armare tutto! visto che ho disponibile sia il fw 1.7 che 1.9 per iniziare a capire multiwii che dite e meglio che lascio il 1.9 o 1.7 potrebbe essere più docile? tenete conto che devo imparare a capire i pid e come modificarli

dopo un po di smadonnamento ho scoperto che il problema era il fw cioè con il fw di jacub(quadrame) che è la mia scheda non mi fa modificare pitch e roll ma gli altri si :S caricando il fw 1.9 normale mi fa modificare tutto... qualche consiglio?

[TermicOne]

E' uscita la 2.0 ufficiale.

Scaricata e provata la 2.0 "ufficiale" sia a vista che in FPV.
Tutto bene.
Non ho visto differenze rispetto alla preversion1 che stavo usando.

[kamikazeta]

Scaricata e provata la 2.0 "ufficiale" sia a vista che in FPV.
Tutto bene.
Non ho visto differenze rispetto alla preversion1 che stavo usando.

Provata anche io su un quadX con IMU di Fabio. Tutto bene anche il baro tiene la quota entro i 30 cm. Ottimo.
Peccato per il mag, mi soffre moltissimo le interferenze ellettromagnetiche, se lo attivo e do gas sulla gui il quad gira di 10 15 gradi da solo.
Anche i beep dei regolatori fanno sobbalzare il grafico. Impressionante.

Una cosa che ho notato: se si abilita la IMU in toto, 0.35 MS i sensori lavorano correttamente. Se invece attivo i sensori uno ad uno, c'è un macello. se inclino in avanti sulla GUI va a destra, insomma l'orientamento è completamente sballato.

occhio

[leoroby]

Ho provato a caric are il fw 2.0 ma oltre a definire i miei giro e accelerometri devo definire qualcos'altro? perché nella gui se inclino il quad a dx il quad della gui va a sinistra e viceversa... ???

[TermicOne]

Ho provato a caric are il fw 2.0 ma oltre a definire i miei giro e accelerometri devo definire qualcos'altro? perché nella gui se inclino il quad a dx il quad della gui va a sinistra e viceversa... ???

Con la 2.0 occorre defiinire xyz dei sensori. Per la Jakub vedi i miei post qualche pagina indietro.

[leoroby]

Con la 2.0 occorre defiinire xyz dei sensori. Per la Jakub vedi i miei post qualche pagina indietro.

ahh ecco... ok torno qualche pagina dietro!!!

ps:se non ci fossi bisognerebbe inventarti! XD

[leoroby]

Bene adesso che ho sottomano il fw sto vedendo che tra tutte le board manca quella di jacub ma c'e questa che ha i sensori uguali va bene? :

#if defined(MINIWII)
#define ITG3200
#define BMA180
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = -Y; accADC[YAW] = -Z;}
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = Y; gyroADC[PITCH] = -X; gyroADC[YAW] = -Z;}
#endif

e poi visto che sono già tutte senza le // le devo mettere a tutte tranne a quella che uso ?

[leoroby]

Bene adesso che ho sottomano il fw sto vedendo che tra tutte le board manca quella di jacub ma c'e questa che ha i sensori uguali va bene? :

#if defined(MINIWII)
#define ITG3200
#define BMA180
#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = -Y; accADC[YAW] = -Z;}
#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = Y; gyroADC[PITCH] = -X; gyroADC[YAW] = -Z;}
#endif

e poi visto che sono già tutte senza le // le devo mettere a tutte tranne a quella che uso ?

ok tutto risolto... cercavo nel def.pde invece che in config!

[cronalex]

Voglio provare ad implementare alla 2.0 la mediamobile sugli accelerometri... ho letto su multiwii.com che devo modificare queste parti:

config.h aggiungere e sostituire la parte gia esistente :

//################################################## ##################
// Moving Average Gyros by Magnetron1 (Michele Ardito) ########## beta
// if the I2C speed is 100kHz use MMGYROVECTORLENGHT from 2 to 6 best was 2 and MMACCVECTORLENGHT from 4 to 10 best was 6
// if the I2C speed is 400kHz use MMGYROVECTORLENGHT from 2 to 6 best was 4 and MMACCVECTORLENGHT from 4 to 12 best was 8
//#define MMGYRO // Active Moving Average Function for Gyros
#define MMGYROVECTORLENGHT 4 // Lenght of Moving Average Vector
// Moving Average Accelerometers by Magnetron1
//#define MMACC // Active Moving Average Function for Accelerometers
#define MMACCVECTORLENGHT 8 // Lenght of Moving Average Vector
// Moving Average ServoGimbal Signal Output
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal
#define MMSERVOGIMBALVECTORLENGHT 16 // Lenght of Moving Average Vector
/* Trusted AccZ Velocity Vector Magnetron1
/* This option should be uncommented if ACC Z is accurate enough when motors are running*/
#define TRUSTED_ACCZ_VEL


in sensor.pde sostituire gyro e acc cammon part:

// ****************
// GYRO common part
// ****************
void GYRO_Common() {
static int16_t previousGyroADC[3] = {0,0,0};
static int32_t g[3];
uint8_t axis;

#if defined MMGYRO
// Moving Average Gyros by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGHT];
static int32_t mediaMobileGyroADCSum[3];
static uint8_t mediaMobileGyroIDX;
//---------------------------------------------------
#endif
if (calibratingG>0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == 400) g[axis]=0;
// Sum up 400 readings
g[axis] +=gyroADC[axis];
// Clear global variables for next reading
gyroADC[axis]=0;
gyroZero[axis]=0;
if (calibratingG == 1) {
gyroZero[axis]=g[axis]/400;
blinkLED(10,15,1+3*nunchuk);
}
}
calibratingG--;
}
#ifdef MMGYRO
mediaMobileGyroIDX = ++mediaMobileGyroIDX % MMGYROVECTORLENGHT;
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
mediaMobileGyroADCSum[axis] -= mediaMobileGyroADC[axis][mediaMobileGyroIDX];
//anti gyro glitch, limit the variation between two consecutive readings
mediaMobileGyroADC[axis][mediaMobileGyroIDX] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
mediaMobileGyroADCSum[axis] += mediaMobileGyroADC[axis][mediaMobileGyroIDX];
gyroADC[axis] = mediaMobileGyroADCSum[axis] / MMGYROVECTORLENGHT;
#else
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
//anti gyro glitch, limit the variation between two consecutive readings
gyroADC[axis] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
#endif
previousGyroADC[axis] = gyroADC[axis];
}
}

// ****************
// ACC common part
// ****************
void ACC_Common() {
static int32_t a[3];
uint8_t axis;
#if defined MMACC
// Moving Average Accelerometers by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileAccADC[3][MMACCVECTORLENGHT];
static int32_t mediaMobileAccADCSum[3];
static uint8_t mediaMobileAccIDX;
//---------------------------------------------------
#endif
if (calibratingA>0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == 400) a[axis]=0;
// Sum up 400 readings
a[axis] +=accADC[axis];
// Clear global variables for next reading
accADC[axis]=0;
accZero[axis]=0;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
accZero[ROLL] = a[ROLL]/400;
accZero[PITCH] = a[PITCH]/400;
accZero[YAW] = a[YAW]/400-acc_1G; // for nunchuk 200=1G
accTrim[ROLL] = 0;
accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
calibratingA--;
}
#if defined(InflightAccCalibration)
static int32_t b[3];
static int16_t accZero_saved[3] = {0,0,0};
static int16_t accTrim_saved[2] = {0, 0};
//Saving old zeropoints before measurement
if (InflightcalibratingA==50) {
accZero_saved[ROLL] = accZero[ROLL] ;
accZero_saved[PITCH] = accZero[PITCH];
accZero_saved[YAW] = accZero[YAW] ;
accTrim_saved[ROLL] = accTrim[ROLL] ;
accTrim_saved[PITCH] = accTrim[PITCH] ;
}
if (InflightcalibratingA>0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50) b[axis]=0;
// Sum up 50 readings
b[axis] +=accADC[axis];
// Clear global variables for next reading
accADC[axis]=0;
accZero[axis]=0;
}
//all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1;
blinkLED(10,10,2); //buzzer for indicatiing the start inflight
// recover saved values to maintain current flight behavior until new values are transferred
accZero[ROLL] = accZero_saved[ROLL] ;
accZero[PITCH] = accZero_saved[PITCH];
accZero[YAW] = accZero_saved[YAW] ;
accTrim[ROLL] = accTrim_saved[ROLL] ;
accTrim[PITCH] = accTrim_saved[PITCH] ;
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm == 1){ //the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = 0;
accZero[ROLL] = b[ROLL]/50;
accZero[PITCH] = b[PITCH]/50;
accZero[YAW] = b[YAW]/50-acc_1G; // for nunchuk 200=1G
accTrim[ROLL] = 0;
accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
#endif
#ifdef MMACC
//Moving Average Accelerometers by Magnetron1
mediaMobileAccIDX = ++mediaMobileAccIDX % MMACCVECTORLENGHT;
for (axis = 0; axis < 3; axis++) {
accADC[axis] -= accZero[axis];
//new implementation of Moving Average for fast response
mediaMobileAccADCSum[axis] -= mediaMobileAccADC[axis][mediaMobileAccIDX];
mediaMobileAccADC[axis][mediaMobileAccIDX] = accADC[axis];
mediaMobileAccADCSum[axis] += mediaMobileAccADC[axis][mediaMobileAccIDX];
accADC[axis] = mediaMobileAccADCSum[axis] / MMACCVECTORLENGHT;
}
#else
accADC[ROLL] -= accZero[ROLL] ;
accADC[PITCH] -= accZero[PITCH];
accADC[YAW] -= accZero[YAW] ;
#endif
}


ma non capisco dove devo inserire questa parte di codice:

BaroPID = 0;
//D
temp32 = D8[PIDALT]*(BaroHigh - BaroLow) / 40;
BaroPID-=temp32;
EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && BaroPID < 10) BaroPID = 0; //remove small D parametr to reduce noise near zoro position
// Magnetron1 (Michele Ardito) Trusted Z Acc
#if defined(TRUSTED_ACCZ_VEL)
BaroPID -= D8[PIDVEL]*(AccZHigh - AccZLow) / 30;
#endif

//P
BaroPID += P8[PIDALT]*constrain(temp32,(-2)*P8[PIDALT],2*P8[PIDALT])/100;
BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150

//I
errorAltitudeI += temp32*I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
temp32 = errorAltitudeI / 500; //I in range +/-60
BaroPID+=temp32;

dovrebbe risolvere i problemi del headhold questa parte???? come funziona dove si mette sta parte??!! scusate per le mille domande xD

[magnetron1]

Voglio provare ad implementare alla 2.0 la mediamobile sugli accelerometri... ho letto su multiwii.com che devo modificare queste parti:

config.h aggiungere e sostituire la parte gia esistente :

//################################################## ##################
// Moving Average Gyros by Magnetron1 (Michele Ardito) ########## beta
// if the I2C speed is 100kHz use MMGYROVECTORLENGHT from 2 to 6 best was 2 and MMACCVECTORLENGHT from 4 to 10 best was 6
// if the I2C speed is 400kHz use MMGYROVECTORLENGHT from 2 to 6 best was 4 and MMACCVECTORLENGHT from 4 to 12 best was 8
//#define MMGYRO // Active Moving Average Function for Gyros
#define MMGYROVECTORLENGHT 4 // Lenght of Moving Average Vector
// Moving Average Accelerometers by Magnetron1
//#define MMACC // Active Moving Average Function for Accelerometers
#define MMACCVECTORLENGHT 8 // Lenght of Moving Average Vector
// Moving Average ServoGimbal Signal Output
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal
#define MMSERVOGIMBALVECTORLENGHT 16 // Lenght of Moving Average Vector
/* Trusted AccZ Velocity Vector Magnetron1
/* This option should be uncommented if ACC Z is accurate enough when motors are running*/
#define TRUSTED_ACCZ_VEL


in sensor.pde sostituire gyro e acc cammon part:

// ****************
// GYRO common part
// ****************
void GYRO_Common() {
static int16_t previousGyroADC[3] = {0,0,0};
static int32_t g[3];
uint8_t axis;

#if defined MMGYRO
// Moving Average Gyros by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGHT];
static int32_t mediaMobileGyroADCSum[3];
static uint8_t mediaMobileGyroIDX;
//---------------------------------------------------
#endif
if (calibratingG>0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == 400) g[axis]=0;
// Sum up 400 readings
g[axis] +=gyroADC[axis];
// Clear global variables for next reading
gyroADC[axis]=0;
gyroZero[axis]=0;
if (calibratingG == 1) {
gyroZero[axis]=g[axis]/400;
blinkLED(10,15,1+3*nunchuk);
}
}
calibratingG--;
}
#ifdef MMGYRO
mediaMobileGyroIDX = ++mediaMobileGyroIDX % MMGYROVECTORLENGHT;
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
mediaMobileGyroADCSum[axis] -= mediaMobileGyroADC[axis][mediaMobileGyroIDX];
//anti gyro glitch, limit the variation between two consecutive readings
mediaMobileGyroADC[axis][mediaMobileGyroIDX] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
mediaMobileGyroADCSum[axis] += mediaMobileGyroADC[axis][mediaMobileGyroIDX];
gyroADC[axis] = mediaMobileGyroADCSum[axis] / MMGYROVECTORLENGHT;
#else
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
//anti gyro glitch, limit the variation between two consecutive readings
gyroADC[axis] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
#endif
previousGyroADC[axis] = gyroADC[axis];
}
}

// ****************
// ACC common part
// ****************
void ACC_Common() {
static int32_t a[3];
uint8_t axis;
#if defined MMACC
// Moving Average Accelerometers by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileAccADC[3][MMACCVECTORLENGHT];
static int32_t mediaMobileAccADCSum[3];
static uint8_t mediaMobileAccIDX;
//---------------------------------------------------
#endif
if (calibratingA>0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == 400) a[axis]=0;
// Sum up 400 readings
a[axis] +=accADC[axis];
// Clear global variables for next reading
accADC[axis]=0;
accZero[axis]=0;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
accZero[ROLL] = a[ROLL]/400;
accZero[PITCH] = a[PITCH]/400;
accZero[YAW] = a[YAW]/400-acc_1G; // for nunchuk 200=1G
accTrim[ROLL] = 0;
accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
calibratingA--;
}
#if defined(InflightAccCalibration)
static int32_t b[3];
static int16_t accZero_saved[3] = {0,0,0};
static int16_t accTrim_saved[2] = {0, 0};
//Saving old zeropoints before measurement
if (InflightcalibratingA==50) {
accZero_saved[ROLL] = accZero[ROLL] ;
accZero_saved[PITCH] = accZero[PITCH];
accZero_saved[YAW] = accZero[YAW] ;
accTrim_saved[ROLL] = accTrim[ROLL] ;
accTrim_saved[PITCH] = accTrim[PITCH] ;
}
if (InflightcalibratingA>0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50) b[axis]=0;
// Sum up 50 readings
b[axis] +=accADC[axis];
// Clear global variables for next reading
accADC[axis]=0;
accZero[axis]=0;
}
//all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1;
blinkLED(10,10,2); //buzzer for indicatiing the start inflight
// recover saved values to maintain current flight behavior until new values are transferred
accZero[ROLL] = accZero_saved[ROLL] ;
accZero[PITCH] = accZero_saved[PITCH];
accZero[YAW] = accZero_saved[YAW] ;
accTrim[ROLL] = accTrim_saved[ROLL] ;
accTrim[PITCH] = accTrim_saved[PITCH] ;
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm == 1){ //the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = 0;
accZero[ROLL] = b[ROLL]/50;
accZero[PITCH] = b[PITCH]/50;
accZero[YAW] = b[YAW]/50-acc_1G; // for nunchuk 200=1G
accTrim[ROLL] = 0;
accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
#endif
#ifdef MMACC
//Moving Average Accelerometers by Magnetron1
mediaMobileAccIDX = ++mediaMobileAccIDX % MMACCVECTORLENGHT;
for (axis = 0; axis < 3; axis++) {
accADC[axis] -= accZero[axis];
//new implementation of Moving Average for fast response
mediaMobileAccADCSum[axis] -= mediaMobileAccADC[axis][mediaMobileAccIDX];
mediaMobileAccADC[axis][mediaMobileAccIDX] = accADC[axis];
mediaMobileAccADCSum[axis] += mediaMobileAccADC[axis][mediaMobileAccIDX];
accADC[axis] = mediaMobileAccADCSum[axis] / MMACCVECTORLENGHT;
}
#else
accADC[ROLL] -= accZero[ROLL] ;
accADC[PITCH] -= accZero[PITCH];
accADC[YAW] -= accZero[YAW] ;
#endif
}


ma non capisco dove devo inserire questa parte di codice:

BaroPID = 0;
//D
temp32 = D8[PIDALT]*(BaroHigh - BaroLow) / 40;
BaroPID-=temp32;
EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && BaroPID < 10) BaroPID = 0; //remove small D parametr to reduce noise near zoro position
// Magnetron1 (Michele Ardito) Trusted Z Acc
#if defined(TRUSTED_ACCZ_VEL)
BaroPID -= D8[PIDVEL]*(AccZHigh - AccZLow) / 30;
#endif

//P
BaroPID += P8[PIDALT]*constrain(temp32,(-2)*P8[PIDALT],2*P8[PIDALT])/100;
BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150

//I
errorAltitudeI += temp32*I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
temp32 = errorAltitudeI / 500; //I in range +/-60
BaroPID+=temp32;

dovrebbe risolvere i problemi del headhold questa parte???? come funziona dove si mette sta parte??!! scusate per le mille domande xD

Si, ottimo!
La parte della media mobile è proprio quella.
La parte che non sai dove si inserisce va nel file IMU.PDE in coda trovi tutto.
Se usi anche la funzione TRUSTED_ACCZ_VEL ricordati di settare il PID del VEL, nella routine è usato solo il valore D del PID quindi metti D almeno a 30-40 e prova, occhio alla reazione. La funzione serve per mantenere l'altitudine in abbinamento al barometro non centra con l'headhold.

[cronalex]

messo tutto ma quando compilo mi da questi errori:


MultiWii_2_0.cpp: In function 'void getEstimatedAltitude()':
MultiWii_2_0:1337: error: 'AccZHigh' was not declared in this scope
MultiWii_2_0:1337: error: 'AccZLow' was not declared in this scope

[magnetron1]

messo tutto ma quando compilo mi da questi errori:


MultiWii_2_0.cpp: In function 'void getEstimatedAltitude()':
MultiWii_2_0:1337: error: 'AccZHigh' was not declared in this scope
MultiWii_2_0:1337: error: 'AccZLow' was not declared in this scope

asp che controllo nel codice...

[magnetron1]

Ecco la routine completa che dovrebbe risolvere qualsiasi problema di compilazione:

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,BaroLow;
int32_t temp32;
int16_t last;

// Magnetron1 (Michele Ardito) Trusted Z Acc
#if defined(TRUSTED_ACCZ_VEL)
#define ACCZ_TAB_SIZE 40
uint8_t AccZindex;
static int16_t AccZHistTab[ACCZ_TAB_SIZE];
static int8_t AccZHistIdx;
static int32_t AccZHigh,AccZLow;
int16_t AccZlast;

AccZlast = AccZHistTab[AccZHistIdx];
AccZHistTab[AccZHistIdx] = accADC[YAW]; // <-- Acc Z axis value
AccZHigh += AccZHistTab[AccZHistIdx];
AccZindex = (AccZHistIdx + (ACCZ_TAB_SIZE/2))%ACCZ_TAB_SIZE;
AccZHigh -= AccZHistTab[AccZindex];
AccZLow += AccZHistTab[AccZindex];
AccZLow -= AccZlast;
AccZHistIdx++;
if (AccZHistIdx == ACCZ_TAB_SIZE) AccZHistIdx = 0;
#endif

if (currentTime < deadLine) return;
deadLine = currentTime + UPDATE_INTERVAL;

//**** Alt. Set Point stabilization PID ****
//calculate speed for D calculation
last = BaroHistTab[BaroHistIdx];
BaroHistTab[BaroHistIdx] = BaroAlt/10;
BaroHigh += BaroHistTab[BaroHistIdx];
index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
BaroLow += BaroHistTab[index];
BaroLow -= last;
BaroHistIdx++;
if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;

BaroPID = 0;
//D
temp32 = D8[PIDALT]*(BaroHigh - BaroLow) / 40;
BaroPID-=temp32;
EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && BaroPID < 10) BaroPID = 0; //remove small D parametr to reduce noise near zoro position
// Magnetron1 (Michele Ardito) Trusted Z Acc
#if defined(TRUSTED_ACCZ_VEL)
BaroPID -= D8[PIDVEL]*(AccZHigh - AccZLow) / 30;
#endif

//P
BaroPID += P8[PIDALT]*constrain(temp32,(-2)*P8[PIDALT],2*P8[PIDALT])/100;
BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150

//I
errorAltitudeI += temp32*I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
temp32 = errorAltitudeI / 500; //I in range +/-60
BaroPID+=temp32;
}