Funktionieren bei Euch Kompass und Baro vernünftig ?
- Themenstarter Joachim08
- Beginndatum
@Roberto, Deacon und die anderen erfreulich engagierten Coder
Ich würde mich freuen, wenn ihr Eure Codeschnippsel zu einem eigenen Release oder Fork zusammenbauen würdet. Falls Euch das zu viel Aufwand ist, gibt es noch die Jungs im MultiWii Channel auf Freenode. Die haben ein eigenes Repository am Start wo ihr eure Codeschnippsel einliefern könnt. Eventuell hat ja jemand Lust, ein eigenes FPV-Community Repository zu warten. Vor allem in dem Chat sind einige fähige Leute, die Eure Verbesserungen im Detail prüfen und wertvolle Ratschläge geben können.
Ich würde mich freuen, wenn ihr Eure Codeschnippsel zu einem eigenen Release oder Fork zusammenbauen würdet. Falls Euch das zu viel Aufwand ist, gibt es noch die Jungs im MultiWii Channel auf Freenode. Die haben ein eigenes Repository am Start wo ihr eure Codeschnippsel einliefern könnt. Eventuell hat ja jemand Lust, ein eigenes FPV-Community Repository zu warten. Vor allem in dem Chat sind einige fähige Leute, die Eure Verbesserungen im Detail prüfen und wertvolle Ratschläge geben können.
Hi!
Ich bin momentan leider ziemlich mit anderen Dingen beschäftigt, so dass ich mich leider nicht um unser Projekt hier weiter kümmern konnte.
Endlich steigst Du, DeaconBlues hier mit ein! Gottseidank, hast Du wenigstens Ahnung von der Materie!
Ich habe mir Deine getEstimatedAltitude auch mit grossem Interesse angeschaut und eingebaut. Leider kompiliert es sich nicht auf Anhieb. Sobald es hier Flugwetter gibt wird getestet!
Ich habe Deinen Code etwas umstricken müssen, damit er sich kompilieren lässt. Ich habe Sonar und den Debugcode entfernt. Kannst Du schauen, ob es so in Ordnung ist:
Aus mangelnder C Kenntnis und daher aus Angst vor Überläufen, habe ich die Bytevariablen "PIDALT" auf 32 Bit für die Berechnung aufgeblasen.
Ich frage mich immer, ob es nicht sinnvoller ist, nur neue Barowerte zu verarbeiten. In Deiner, wie auch in der Originalversion, wird Baroalt immer ausgelesen, egal ob es nun ein neuer oder alter Wert ist.
LG
Rob
EDIT:
P.s.:
@TomW
Das ist der Originalcode am Ende von "IMU", den Du rausschmeissen musst:
Ich bin momentan leider ziemlich mit anderen Dingen beschäftigt, so dass ich mich leider nicht um unser Projekt hier weiter kümmern konnte.
Endlich steigst Du, DeaconBlues hier mit ein! Gottseidank, hast Du wenigstens Ahnung von der Materie!
Ich habe mir Deine getEstimatedAltitude auch mit grossem Interesse angeschaut und eingebaut. Leider kompiliert es sich nicht auf Anhieb. Sobald es hier Flugwetter gibt wird getestet!
Ich habe Deinen Code etwas umstricken müssen, damit er sich kompilieren lässt. Ich habe Sonar und den Debugcode entfernt. Kannst Du schauen, ob es so in Ordnung ist:
Code:
typedef struct avg_var16 {
int32_t buf; // internal bufer to store non-rounded average value
int16_t res; // result (rounded to int)
} t_avg_var16;
/* n=(1..16) */
void average16(struct avg_var16 *avg, int16_t cur, int8_t n) {
avg->buf+= cur - avg->res;
avg->res = avg->buf >> n;
}
#define UPDATE_INTERVAL 25000 // 25000 = 40hz update rate (20hz LPF on acc)
#define UPDATE_INTERVAL2 250000 // 250000 = 4hz Rate für den Regler
#define INIT_DELAY 4000000 // 4 sec initialization delay
void getEstimatedAltitude(){
static uint32_t deadLine = INIT_DELAY,deadLine2 = INIT_DELAY;
static t_avg_var16 avgAlt = {0,0};
static int32_t DeltaAlt, DeltaAltOld; // Regelabweichung alt für PID-D
static int32_t velocity; // Geschwindigkeit Höhenänderung
int16_t PidP = 0, PidI = 0, PidD;
if (currentTime < deadLine) return;
deadLine = currentTime + UPDATE_INTERVAL;
average16(&avgAlt, BaroAlt, 5); // / 4 = 16 = 0,375 sec 5 = 32 = 0,75 sec 6 = 64 = 1,5 sek // Mittelwert der Höhe
EstAlt = avgAlt.res;
if (currentTime < deadLine2) return;
deadLine2 = currentTime + UPDATE_INTERVAL2;
// Dieser Teil der Routine wird nur 4 mal in der Sekunde ausgeführt
DeltaAltOld = DeltaAlt;
DeltaAlt = AltHold - EstAlt; // ist positiv bei zu niedrig
velocity = DeltaAlt - DeltaAltOld; // ist positiv bei sinken, negativ beim Steigen
//D
PidD = ((int32_t)conf.D8[PIDALT] * (velocity)) / 16; // ist positiv bei sinken // soll der schnellen Bewegung engegenwirken
BaroPID = PidD;
//P
PidP = ((int32_t)conf.P8[PIDALT] * constrain(DeltaAlt,-100,100))/100;
BaroPID += PidP;
BaroPID = constrain(BaroPID,-150,+150);
//I
errorAltitudeI = errorAltitudeI + (((int32_t)conf.I8[PIDALT] * DeltaAlt)/ 16);
errorAltitudeI = constrain(errorAltitudeI,-3000,3000); // war 30000 erhöht auf 3750 max range dann +/- 75
PidI = errorAltitudeI / 75; //500 I in range +/-40
BaroPID += PidI;
}Ich frage mich immer, ob es nicht sinnvoller ist, nur neue Barowerte zu verarbeiten. In Deiner, wie auch in der Originalversion, wird Baroalt immer ausgelesen, egal ob es nun ein neuer oder alter Wert ist.
LG
Rob
EDIT:
P.s.:
@TomW
Das ist der Originalcode am Ende von "IMU", den Du rausschmeissen musst:
Code:
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
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;
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 = conf.D8[PIDALT]*(BaroHigh - BaroLow) / 40;
BaroPID-=temp32;
EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0; //remove small D parametr to reduce noise near zero position
//P
BaroPID += conf.P8[PIDALT]*constrain(temp32,(-2)*conf.P8[PIDALT],2*conf.P8[PIDALT])/100;
BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150
//I
errorAltitudeI += temp32*conf.I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
temp32 = errorAltitudeI / 500; //I in range +/-60
BaroPID+=temp32;
}
Hi Paraglider,
ich habe mal alles überflüssige rausgeschmissen und gerade kompiliert. Version war die letzte DEV. Leider habe ich jetzt vor der Arbeit keine Zeit mir einen Platz zum umploaden zu suchen.
Dieses ist der Inhalt der IMU.ino. Hoffe das es so klappt.
Gruß Deacon Blues
// ab hier der Inhalt der Datei IMU.ino
void computeIMU () {
uint8_t axis;
static int16_t gyroADCprevious[3] = {0,0,0};
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0;
//we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
//gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
//gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
#if defined(NUNCHUCK)
annexCode();
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
ACC_getADC();
getEstimatedAttitude(); // computation time must last less than one interleaving delay
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
f.NUNCHUKDATA = 1;
while(f.NUNCHUKDATA) ACC_getADC(); // For this interleaving reading, we must have a gyro update at this point (less delay)
for (axis = 0; axis < 3; axis++) {
// empirical, we take a weighted value of the current and the previous values
// /4 is to average 4 values, note: overflow is not possible for WMP gyro here
gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis])/4;
gyroADCprevious[axis] = gyroADC[axis];
}
#else
#if ACC
ACC_getADC();
getEstimatedAttitude();
#endif
#if GYRO
Gyro_getADC();
#endif
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave=micros();
annexCode();
if ((micros()-timeInterleave)>650) {
annex650_overrun_count++;
} else {
while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
}
#if GYRO
Gyro_getADC();
#endif
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis]+gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3;
gyroADCprevious[axis] = gyroADCinter[axis]/2;
if (!ACC) accADC[axis]=0;
}
#endif
#if defined(GYRO_SMOOTHING)
static int16_t gyroSmooth[3] = {0,0,0};
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+gyroData[axis]+1 ) / conf.Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
#elif defined(TRI)
static int16_t gyroYawSmooth = 0;
gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW])/3;
gyroYawSmooth = gyroData[YAW];
#endif
}
// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// **************************************************
//****** advanced users settings *******************
/* Set the Low Pass Filter factor for ACC */
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/
/* Comment this if you do not want filter at all.*/
#ifndef ACC_LPF_FACTOR
#define ACC_LPF_FACTOR 100
#endif
/* Set the Low Pass Filter factor for Magnetometer */
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
/* Comment this if you do not want filter at all.*/
/* Default WMC value: n/a*/
#ifndef MG_LPF_FACTOR
//#define MG_LPF_FACTOR 4
#endif
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
/* Default WMC value: 300*/
#ifndef GYR_CMPF_FACTOR
#define GYR_CMPF_FACTOR 400.0f
#endif
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
#ifndef GYR_CMPFM_FACTOR
#define GYR_CMPFM_FACTOR 200.0f
#endif
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#if GYRO
#define GYRO_SCALE ((2380 * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result
// +-2000/sec deg scale
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
// +- 200/sec deg scale
// 1.5 is emperical, not sure what it means
// should be in rad/sec
#else
#define GYRO_SCALE (1.0f/200e6f)
// empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility
// !!!!should be adjusted to the rad/sec
#endif
// Small angle approximation
#define ssin(val) (val)
#define scos(val) 1.0f
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
int16_t _atan2(float y, float x){
#define fp_is_neg(val) ((((uint8_t*)&val)[3] & 0x80) != 0)
float z = y / x;
int16_t zi = abs(int16_t(z * 100));
int8_t y_neg = fp_is_neg
;
if ( zi < 100 ){
if (zi > 10)
z = z / (1.0f + 0.28f * z * z);
if (fp_is_neg(x)) {
if (y_neg) z -= PI;
else z += PI;
}
} else {
z = (PI / 2.0f) - z / (z * z + 0.28f);
if (y_neg) z -= PI;
}
z *= (180.0f / PI * 10);
return z;
}
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v,float* delta) {
fp_vector v_tmp = *v;
v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
}
void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
static t_fp_vector EstG;
#if MAG
static t_fp_vector EstM;
#endif
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
#if defined(ACC_LPF_FACTOR)
static float accLPF[3];
#endif
static uint16_t previousT;
uint16_t currentT = micros();
float scale, deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale;
#if defined(ACC_LPF_FACTOR)
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
accSmooth[axis] = accLPF[axis];
#define ACC_VALUE accSmooth[axis]
#else
accSmooth[axis] = accADC[axis];
#define ACC_VALUE accADC[axis]
#endif
// accMag += (ACC_VALUE * 10 / (int16_t)acc_1G) * (ACC_VALUE * 10 / (int16_t)acc_1G);
accMag += (int32_t)ACC_VALUE*ACC_VALUE ;
#if MAG
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
#endif
}
accMag = accMag*100/((int32_t)acc_1G*acc_1G);
rotateV(&EstG.V,deltaGyroAngle);
#if MAG
rotateV(&EstM.V,deltaGyroAngle);
#endif
if ( abs(accSmooth[ROLL])<acc_25deg && abs(accSmooth[PITCH])<acc_25deg && accSmooth[YAW]>0) {
f.SMALL_ANGLES_25 = 1;
} else {
f.SMALL_ANGLES_25 = 0;
}
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if ( ( 36 < accMag && accMag < 196 ) || f.SMALL_ANGLES_25 )
for (axis = 0; axis < 3; axis++) {
int16_t acc = ACC_VALUE;
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;
}
#if MAG
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
#endif
// Attitude of the estimated vector
angle[ROLL] = _atan2(EstG.V.X , EstG.V.Z) ;
angle[PITCH] = _atan2(EstG.V.Y , EstG.V.Z) ;
#if MAG
// Attitude of the cross product vector GxM
heading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
if ( heading > 180) heading = heading - 360;
else if (heading < -180) heading = heading + 360;
#endif
}
typedef struct avg_var16 {
int32_t buf; // internal bufer to store non-rounded average value
int16_t res; // result (rounded to int)
} t_avg_var16;
/* n=(1..16) */
void average16(struct avg_var16 *avg, int16_t cur, int8_t n) {
avg->buf+= cur - avg->res;
avg->res = avg->buf >> n;
}
#define UPDATE_INTERVAL 25000 // 25000 = 40hz update rate (20hz LPF on acc)
#define UPDATE_INTERVAL2 250000 // 250000 = 4hz Rate f�r den Regler
#define INIT_DELAY 4000000 // 4 sec initialization delay
void getEstimatedAltitude(){
static uint32_t deadLine = INIT_DELAY,deadLine2 = INIT_DELAY;
static t_avg_var16 avgAlt = {0,0};
static int32_t DeltaAlt, DeltaAltOld; // Regelabweichung alt f�r PID-D
static int32_t velocity; // Geschwindigkeit H�hen�nderung
int16_t PidP = 0, PidI = 0, PidD;
if (currentTime < deadLine) return;
deadLine = currentTime + UPDATE_INTERVAL;
average16(&avgAlt, BaroAlt, 5); // / 4 = 16 = 0,375 sec 5 = 32 = 0,75 sec 6 = 64 = 1,5 sek // Mittelwert der H�he
EstAlt = avgAlt.res;
if (currentTime < deadLine2) return;
deadLine2 = currentTime + UPDATE_INTERVAL2;
// Dieser Teil der Routine wird nur 4 mal in der Sekunde ausgef�hrt
DeltaAltOld = DeltaAlt;
DeltaAlt = AltHold - EstAlt; // ist positiv bei zu niedrig
velocity = DeltaAlt - DeltaAltOld; // ist positiv bei sinken, negativ beim Steigen
//D
PidD = conf.D8[PIDALT] * (velocity) / 16; // ist positiv bei sinken // soll der schnellen Bewegung engegenwirken
BaroPID = PidD;
//P
PidP = conf.P8[PIDALT] * constrain(DeltaAlt,-100,100)/100;
BaroPID += PidP;
BaroPID = constrain(BaroPID,-150,+150);
//I
errorAltitudeI += DeltaAlt * conf.I8[PIDALT] / 16;
errorAltitudeI = constrain(errorAltitudeI,-3000,3000); // war 30000
PidI = errorAltitudeI / 75; // I in range +/-40
BaroPID += PidI;
}
ich habe mal alles überflüssige rausgeschmissen und gerade kompiliert. Version war die letzte DEV. Leider habe ich jetzt vor der Arbeit keine Zeit mir einen Platz zum umploaden zu suchen.
Dieses ist der Inhalt der IMU.ino. Hoffe das es so klappt.
Gruß Deacon Blues
// ab hier der Inhalt der Datei IMU.ino
void computeIMU () {
uint8_t axis;
static int16_t gyroADCprevious[3] = {0,0,0};
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0;
//we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
//gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
//gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
#if defined(NUNCHUCK)
annexCode();
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
ACC_getADC();
getEstimatedAttitude(); // computation time must last less than one interleaving delay
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
f.NUNCHUKDATA = 1;
while(f.NUNCHUKDATA) ACC_getADC(); // For this interleaving reading, we must have a gyro update at this point (less delay)
for (axis = 0; axis < 3; axis++) {
// empirical, we take a weighted value of the current and the previous values
// /4 is to average 4 values, note: overflow is not possible for WMP gyro here
gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis])/4;
gyroADCprevious[axis] = gyroADC[axis];
}
#else
#if ACC
ACC_getADC();
getEstimatedAttitude();
#endif
#if GYRO
Gyro_getADC();
#endif
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave=micros();
annexCode();
if ((micros()-timeInterleave)>650) {
annex650_overrun_count++;
} else {
while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
}
#if GYRO
Gyro_getADC();
#endif
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis]+gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3;
gyroADCprevious[axis] = gyroADCinter[axis]/2;
if (!ACC) accADC[axis]=0;
}
#endif
#if defined(GYRO_SMOOTHING)
static int16_t gyroSmooth[3] = {0,0,0};
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+gyroData[axis]+1 ) / conf.Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
#elif defined(TRI)
static int16_t gyroYawSmooth = 0;
gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW])/3;
gyroYawSmooth = gyroData[YAW];
#endif
}
// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// **************************************************
//****** advanced users settings *******************
/* Set the Low Pass Filter factor for ACC */
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/
/* Comment this if you do not want filter at all.*/
#ifndef ACC_LPF_FACTOR
#define ACC_LPF_FACTOR 100
#endif
/* Set the Low Pass Filter factor for Magnetometer */
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
/* Comment this if you do not want filter at all.*/
/* Default WMC value: n/a*/
#ifndef MG_LPF_FACTOR
//#define MG_LPF_FACTOR 4
#endif
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
/* Default WMC value: 300*/
#ifndef GYR_CMPF_FACTOR
#define GYR_CMPF_FACTOR 400.0f
#endif
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
#ifndef GYR_CMPFM_FACTOR
#define GYR_CMPFM_FACTOR 200.0f
#endif
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#if GYRO
#define GYRO_SCALE ((2380 * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result
// +-2000/sec deg scale
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
// +- 200/sec deg scale
// 1.5 is emperical, not sure what it means
// should be in rad/sec
#else
#define GYRO_SCALE (1.0f/200e6f)
// empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility
// !!!!should be adjusted to the rad/sec
#endif
// Small angle approximation
#define ssin(val) (val)
#define scos(val) 1.0f
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
int16_t _atan2(float y, float x){
#define fp_is_neg(val) ((((uint8_t*)&val)[3] & 0x80) != 0)
float z = y / x;
int16_t zi = abs(int16_t(z * 100));
int8_t y_neg = fp_is_neg
;if ( zi < 100 ){
if (zi > 10)
z = z / (1.0f + 0.28f * z * z);
if (fp_is_neg(x)) {
if (y_neg) z -= PI;
else z += PI;
}
} else {
z = (PI / 2.0f) - z / (z * z + 0.28f);
if (y_neg) z -= PI;
}
z *= (180.0f / PI * 10);
return z;
}
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v,float* delta) {
fp_vector v_tmp = *v;
v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
}
void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
static t_fp_vector EstG;
#if MAG
static t_fp_vector EstM;
#endif
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
#if defined(ACC_LPF_FACTOR)
static float accLPF[3];
#endif
static uint16_t previousT;
uint16_t currentT = micros();
float scale, deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale;
#if defined(ACC_LPF_FACTOR)
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
accSmooth[axis] = accLPF[axis];
#define ACC_VALUE accSmooth[axis]
#else
accSmooth[axis] = accADC[axis];
#define ACC_VALUE accADC[axis]
#endif
// accMag += (ACC_VALUE * 10 / (int16_t)acc_1G) * (ACC_VALUE * 10 / (int16_t)acc_1G);
accMag += (int32_t)ACC_VALUE*ACC_VALUE ;
#if MAG
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
#endif
}
accMag = accMag*100/((int32_t)acc_1G*acc_1G);
rotateV(&EstG.V,deltaGyroAngle);
#if MAG
rotateV(&EstM.V,deltaGyroAngle);
#endif
if ( abs(accSmooth[ROLL])<acc_25deg && abs(accSmooth[PITCH])<acc_25deg && accSmooth[YAW]>0) {
f.SMALL_ANGLES_25 = 1;
} else {
f.SMALL_ANGLES_25 = 0;
}
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if ( ( 36 < accMag && accMag < 196 ) || f.SMALL_ANGLES_25 )
for (axis = 0; axis < 3; axis++) {
int16_t acc = ACC_VALUE;
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;
}
#if MAG
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
#endif
// Attitude of the estimated vector
angle[ROLL] = _atan2(EstG.V.X , EstG.V.Z) ;
angle[PITCH] = _atan2(EstG.V.Y , EstG.V.Z) ;
#if MAG
// Attitude of the cross product vector GxM
heading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
if ( heading > 180) heading = heading - 360;
else if (heading < -180) heading = heading + 360;
#endif
}
typedef struct avg_var16 {
int32_t buf; // internal bufer to store non-rounded average value
int16_t res; // result (rounded to int)
} t_avg_var16;
/* n=(1..16) */
void average16(struct avg_var16 *avg, int16_t cur, int8_t n) {
avg->buf+= cur - avg->res;
avg->res = avg->buf >> n;
}
#define UPDATE_INTERVAL 25000 // 25000 = 40hz update rate (20hz LPF on acc)
#define UPDATE_INTERVAL2 250000 // 250000 = 4hz Rate f�r den Regler
#define INIT_DELAY 4000000 // 4 sec initialization delay
void getEstimatedAltitude(){
static uint32_t deadLine = INIT_DELAY,deadLine2 = INIT_DELAY;
static t_avg_var16 avgAlt = {0,0};
static int32_t DeltaAlt, DeltaAltOld; // Regelabweichung alt f�r PID-D
static int32_t velocity; // Geschwindigkeit H�hen�nderung
int16_t PidP = 0, PidI = 0, PidD;
if (currentTime < deadLine) return;
deadLine = currentTime + UPDATE_INTERVAL;
average16(&avgAlt, BaroAlt, 5); // / 4 = 16 = 0,375 sec 5 = 32 = 0,75 sec 6 = 64 = 1,5 sek // Mittelwert der H�he
EstAlt = avgAlt.res;
if (currentTime < deadLine2) return;
deadLine2 = currentTime + UPDATE_INTERVAL2;
// Dieser Teil der Routine wird nur 4 mal in der Sekunde ausgef�hrt
DeltaAltOld = DeltaAlt;
DeltaAlt = AltHold - EstAlt; // ist positiv bei zu niedrig
velocity = DeltaAlt - DeltaAltOld; // ist positiv bei sinken, negativ beim Steigen
//D
PidD = conf.D8[PIDALT] * (velocity) / 16; // ist positiv bei sinken // soll der schnellen Bewegung engegenwirken
BaroPID = PidD;
//P
PidP = conf.P8[PIDALT] * constrain(DeltaAlt,-100,100)/100;
BaroPID += PidP;
BaroPID = constrain(BaroPID,-150,+150);
//I
errorAltitudeI += DeltaAlt * conf.I8[PIDALT] / 16;
errorAltitudeI = constrain(errorAltitudeI,-3000,3000); // war 30000
PidI = errorAltitudeI / 75; // I in range +/-40
BaroPID += PidI;
}
Hallo Deacon Blues,
ich habe es so gemacht wie du geschrieben hast. Also eine neue Imu gebaut, deinen Text reingeschrieben und in den kompl. Sketch eingefügt. Beim verifizieren tauchen diese Fehler auf:
MultiWii_dev_20120622:203: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void writeAllMotors(int16_t)':
Output:292: error: 'NUMBER_MOTOR' was not declared in this scope
Output:293: error: 'motor' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void initOutput()':
Output:304: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void mixTable()':
Output:1073: error: 'motor' was not declared in this scope
Output:1074: error: 'NUMBER_MOTOR' was not declared in this scope
Output:1076: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void evaluateCommand()':
Serial:202: error: 'MULTITYPE' was not declared in this scope
Serial:234: error: 'NUMBER_MOTOR' was not declared in this scope
Serial:234: error: 'motor' was not declared in this scope
Gruß Paraglider58
ich habe es so gemacht wie du geschrieben hast. Also eine neue Imu gebaut, deinen Text reingeschrieben und in den kompl. Sketch eingefügt. Beim verifizieren tauchen diese Fehler auf:
MultiWii_dev_20120622:203: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void writeAllMotors(int16_t)':
Output:292: error: 'NUMBER_MOTOR' was not declared in this scope
Output:293: error: 'motor' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void initOutput()':
Output:304: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void mixTable()':
Output:1073: error: 'motor' was not declared in this scope
Output:1074: error: 'NUMBER_MOTOR' was not declared in this scope
Output:1076: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void evaluateCommand()':
Serial:202: error: 'MULTITYPE' was not declared in this scope
Serial:234: error: 'NUMBER_MOTOR' was not declared in this scope
Serial:234: error: 'motor' was not declared in this scope
Gruß Paraglider58
Hallo Parglider
wenn ich den Sketch mit #define QUADX in der config.h kompiliere, dann kompiliere ich ohne Fehler. Wenn ich in der config.h meinen copter auskommentiere (//#define QUADX) kann ich folgendes erzeugen.... siehe unten. Das kenne ich vom testen heute morgen. Nebenbei ist mir aufgefallen das die debug1 -debug4 variablen auch fehlermeldungen erzeugen, weiß der Teufel wieso diese wieder wegoptimiert worden sind.
Gruß DeaconBlues - Ladegerät hat sich geschrottet, bis zum Neukauf kann ich selber nur noch 2 accus leerfliegen
MultiWii_dev_20120622:203: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void writeAllMotors(int16_t)':
Output:292: error: 'NUMBER_MOTOR' was not declared in this scope
Output:293: error: 'motor' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void initOutput()':
Output:304: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void mixTable()':
Output:1073: error: 'motor' was not declared in this scope
Output:1074: error: 'NUMBER_MOTOR' was not declared in this scope
Output:1076: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void evaluateCommand()':
Serial:202: error: 'MULTITYPE' was not declared in this scope
Serial:234: error: 'NUMBER_MOTOR' was not declared in this scope
Serial:234: error: 'motor' was not declared in this scope
wenn ich den Sketch mit #define QUADX in der config.h kompiliere, dann kompiliere ich ohne Fehler. Wenn ich in der config.h meinen copter auskommentiere (//#define QUADX) kann ich folgendes erzeugen.... siehe unten. Das kenne ich vom testen heute morgen. Nebenbei ist mir aufgefallen das die debug1 -debug4 variablen auch fehlermeldungen erzeugen, weiß der Teufel wieso diese wieder wegoptimiert worden sind.
Gruß DeaconBlues - Ladegerät hat sich geschrottet, bis zum Neukauf kann ich selber nur noch 2 accus leerfliegen
MultiWii_dev_20120622:203: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void writeAllMotors(int16_t)':
Output:292: error: 'NUMBER_MOTOR' was not declared in this scope
Output:293: error: 'motor' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void initOutput()':
Output:304: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void mixTable()':
Output:1073: error: 'motor' was not declared in this scope
Output:1074: error: 'NUMBER_MOTOR' was not declared in this scope
Output:1076: error: 'NUMBER_MOTOR' was not declared in this scope
MultiWii_dev_20120622.cpp: In function 'void evaluateCommand()':
Serial:202: error: 'MULTITYPE' was not declared in this scope
Serial:234: error: 'NUMBER_MOTOR' was not declared in this scope
Serial:234: error: 'motor' was not declared in this scope
Hi DeaconBlues,
irgend etwas habe ich wohl falsch gemacht. Nun ist es gegangen und der erste Probeflug ist auch gemacht.
Ohne irgend etwas an den PID-Werte gemacht zu haben ist der Kopter nach Drehzahlerhöhung, Einschalten von Level, MAG u. Baro u. weiterer Drehzahlerhöhung auf ca. 2mtr. Höhe gestiegen. Plötzlich sagte er durch bis auf den Boden. Ich habe diesen Test 5x durchgeführt, immer das gleiche. Durchsacken bis auf den Boden. Nur durch mehr Gas geben konnte ich einen Aufprall vermeiden.
Kann man da irgendwo etwas durch einstellung ect. verbessern?
Gruß Paraglider58
irgend etwas habe ich wohl falsch gemacht. Nun ist es gegangen und der erste Probeflug ist auch gemacht.
Ohne irgend etwas an den PID-Werte gemacht zu haben ist der Kopter nach Drehzahlerhöhung, Einschalten von Level, MAG u. Baro u. weiterer Drehzahlerhöhung auf ca. 2mtr. Höhe gestiegen. Plötzlich sagte er durch bis auf den Boden. Ich habe diesen Test 5x durchgeführt, immer das gleiche. Durchsacken bis auf den Boden. Nur durch mehr Gas geben konnte ich einen Aufprall vermeiden.
Kann man da irgendwo etwas durch einstellung ect. verbessern?
Gruß Paraglider58
Hi Paraglider,
PID erfliegen. Als wichtigstes P und I. Wie am besten vorgehen? Am besten alles auf 0
P: Den Copter im langsamen Sinkflug bringen und die Höhenregelung einschalten. Dann P (am besten per Fernbedienung) schrittweise erhöhen so das er gerade über der Sollhöhe oziliert, aber sich dabei nicht weiter aufschaukelt, d.h. das die Auf und Abbewegungen sich nicht verstärken. Diesen Wert merken und erst mal so 60% davon als P-Wert eintragen. Danach regelt er die Höhe aus aber erreicht die Sollhöhe nicht mehr.
I: So weit erhöhen das die Sollhöhe erreicht wird aber kein starkes Pendeln um die Sollhöhe stattfindet, ggf dann etwas reduzieren.
D: Da habe ich mir noch keine Gedanken gemacht, damit habe ich auch noch nicht experimentiert. Aber als Gedankenstütze sollte folgendes passieren. Bis zu einem gewissen Level wird ein niedriges D starkes Auf und Ab abremsen, also die Geschwindigkeit rausnehmen. Bei zu viel kann aber auch eine Beschleunigung in die falsche Richtung passieren. Dort muss ich mich auch noch rantasten. Aber eh erst nach D und I.
Am besten alles bei nicht zu heftigen Wetter, wer innen testen kann ist natürlich klar im Vorteil. (So ein Mist, ich muss immer warten bis die Kinder vom Spielplatz verschwunden sind oder die Deppen vom Jobcenter wieder einmal vergessen ihren Parkplatz abzusperren. Im Innenhof werden sich wohl bald die Nachbar beschwerden weil die Pflanzen von meinen Rotoren geköpft werden)
Hilfsmittel:
1. Ich habe einen Umschalter der Ferndienung so belegt, das ich damit einen Wert (p i oder d) während des Fluges erhöhen oder erniedrigen kann. Mit RCOptionbeep bekommen ich dann sogar einen Quittierungston vom Copter. Mitzählen ist manchmal mühsam, doch meistens kann ich mir die Werte merken.
2. 3 Leds an freien Pins. Eine zeigt mir an das der Copter stark steigen oder steigen soll, eine ist für die Sollhöhe, eine fürs Sinken. Damit läßt sich abschätzen ob der Copter auch das richtige tut. Blinken Dauerein etc können dann auch mehr Zustände anzeigen
Gruß DeaconBlues
PID erfliegen. Als wichtigstes P und I. Wie am besten vorgehen? Am besten alles auf 0
P: Den Copter im langsamen Sinkflug bringen und die Höhenregelung einschalten. Dann P (am besten per Fernbedienung) schrittweise erhöhen so das er gerade über der Sollhöhe oziliert, aber sich dabei nicht weiter aufschaukelt, d.h. das die Auf und Abbewegungen sich nicht verstärken. Diesen Wert merken und erst mal so 60% davon als P-Wert eintragen. Danach regelt er die Höhe aus aber erreicht die Sollhöhe nicht mehr.
I: So weit erhöhen das die Sollhöhe erreicht wird aber kein starkes Pendeln um die Sollhöhe stattfindet, ggf dann etwas reduzieren.
D: Da habe ich mir noch keine Gedanken gemacht, damit habe ich auch noch nicht experimentiert. Aber als Gedankenstütze sollte folgendes passieren. Bis zu einem gewissen Level wird ein niedriges D starkes Auf und Ab abremsen, also die Geschwindigkeit rausnehmen. Bei zu viel kann aber auch eine Beschleunigung in die falsche Richtung passieren. Dort muss ich mich auch noch rantasten. Aber eh erst nach D und I.
Am besten alles bei nicht zu heftigen Wetter, wer innen testen kann ist natürlich klar im Vorteil. (So ein Mist, ich muss immer warten bis die Kinder vom Spielplatz verschwunden sind oder die Deppen vom Jobcenter wieder einmal vergessen ihren Parkplatz abzusperren. Im Innenhof werden sich wohl bald die Nachbar beschwerden weil die Pflanzen von meinen Rotoren geköpft werden)
Hilfsmittel:
1. Ich habe einen Umschalter der Ferndienung so belegt, das ich damit einen Wert (p i oder d) während des Fluges erhöhen oder erniedrigen kann. Mit RCOptionbeep bekommen ich dann sogar einen Quittierungston vom Copter. Mitzählen ist manchmal mühsam, doch meistens kann ich mir die Werte merken.
2. 3 Leds an freien Pins. Eine zeigt mir an das der Copter stark steigen oder steigen soll, eine ist für die Sollhöhe, eine fürs Sinken. Damit läßt sich abschätzen ob der Copter auch das richtige tut. Blinken Dauerein etc können dann auch mehr Zustände anzeigen
Gruß DeaconBlues
Hi Tom,
nen Drehsteller hätte ich gerne gehabt, gibt meine Fernsteuerung aber nicht her. Behelf ist, mit einem Schalter hochzählen, mit dem anderen runter. In MultiWiiWinGui habe ich den Taster auf die ungenutzte Funktion BOXCAMSTAB und BOXCAMTRIG gelegt. Die Werte sind beim Ausschalten verschwunden, also mitzählen. Einzufügen in MultiWii_dev_2012xxxx.ino. Achtung bei alten Versionen heissen die Variablen anders, auch der Beeper wird anders angesprochen!! Ich arbeite gerade mit der 20120528.
Gruß DeaconBlues
#if BARO
if (rcOptions[BOXBARO]) {
if (baroMode == 0) {
baroMode = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else baroMode = 0;
#endif
static uint32_t switchtime;
static int8_t switchflag;
// der gehörte hier nicht hin ]
//#define PIDOPT conf.P8[PIDALT]
//#define PIDOPT conf.I8[PIDALT]
#define PIDOPT conf.D8[PIDALT]
if (rcOptions[BOXCAMSTAB]) {
if (switchflag == 0)
{ switchtime = currentTime + 1000000;
switchflag = 1;}
{
if (currentTime > switchtime )
{
PIDOPT += 1;
if (PIDOPT > 150) PIDOPT = 150; //p bis 150 d bis 100
switchflag = 0;
toggleBeep = 1;}
} }
if (rcOptions[BOXCAMTRIG]) {
if (switchflag == 0)
{
switchtime = currentTime + 1000000;
switchflag = 1; }
{
if (currentTime > switchtime )
{
PIDOPT -= 1;
if (PIDOPT < 1) PIDOPT = 1;
switchflag = 0;
toggleBeep = 1;}
}}
#if MAG
if (rcOptions[BOXMAG]) {
if (magMode == 0) {
magMode = 1;
magHold = heading;
}
} else magMode = 0;
if (rcOptions[BOXHEADFREE]) {
if (headFreeMode == 0) {
headFreeMode = 1;
}
} else headFreeMode = 0;
#endif
nen Drehsteller hätte ich gerne gehabt, gibt meine Fernsteuerung aber nicht her. Behelf ist, mit einem Schalter hochzählen, mit dem anderen runter. In MultiWiiWinGui habe ich den Taster auf die ungenutzte Funktion BOXCAMSTAB und BOXCAMTRIG gelegt. Die Werte sind beim Ausschalten verschwunden, also mitzählen. Einzufügen in MultiWii_dev_2012xxxx.ino. Achtung bei alten Versionen heissen die Variablen anders, auch der Beeper wird anders angesprochen!! Ich arbeite gerade mit der 20120528.
Gruß DeaconBlues
#if BARO
if (rcOptions[BOXBARO]) {
if (baroMode == 0) {
baroMode = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else baroMode = 0;
#endif
static uint32_t switchtime;
static int8_t switchflag;
// der gehörte hier nicht hin ]
//#define PIDOPT conf.P8[PIDALT]
//#define PIDOPT conf.I8[PIDALT]
#define PIDOPT conf.D8[PIDALT]
if (rcOptions[BOXCAMSTAB]) {
if (switchflag == 0)
{ switchtime = currentTime + 1000000;
switchflag = 1;}
{
if (currentTime > switchtime )
{
PIDOPT += 1;
if (PIDOPT > 150) PIDOPT = 150; //p bis 150 d bis 100
switchflag = 0;
toggleBeep = 1;}
} }
if (rcOptions[BOXCAMTRIG]) {
if (switchflag == 0)
{
switchtime = currentTime + 1000000;
switchflag = 1; }
{
if (currentTime > switchtime )
{
PIDOPT -= 1;
if (PIDOPT < 1) PIDOPT = 1;
switchflag = 0;
toggleBeep = 1;}
}}
#if MAG
if (rcOptions[BOXMAG]) {
if (magMode == 0) {
magMode = 1;
magHold = heading;
}
} else magMode = 0;
if (rcOptions[BOXHEADFREE]) {
if (headFreeMode == 0) {
headFreeMode = 1;
}
} else headFreeMode = 0;
#endif
Hallo DeaconBlues,
sowie ich deinen Code (grüner Text) einpflege erscheint eine Fehlermeldung. Diese hier:
MultiWii_dev_20120622_Deacon_Blues.cpp: In function 'void loop()':
MultiWii_dev_20120622_Deacon_Blues:809: error: expected primary-expression before ']' token
MultiWii_dev_20120622_Deacon_Blues:809: error: expected `;' before ']' token
Serial:509: error: expected `}' at end of input
Serial:509: error: expected `}' at end of input
Was mir dabei aufgefallen ist, deine Zeilen vor dem grünen Text sind etwas anders als bei mir. Hier der Vergleich:
DeinText:
#if BARO
if (rcOptions[BOXBARO]) {
if (baroMode == 0) {
baroMode = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else baroMode = 0;
#endif
Mein Text:
#if BARO
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else {
f.BARO_MODE = 0;
}
#endif
Zeile 3 und Zeile 4 sind da anders.
Gruß Paraglider58
sowie ich deinen Code (grüner Text) einpflege erscheint eine Fehlermeldung. Diese hier:
MultiWii_dev_20120622_Deacon_Blues.cpp: In function 'void loop()':
MultiWii_dev_20120622_Deacon_Blues:809: error: expected primary-expression before ']' token
MultiWii_dev_20120622_Deacon_Blues:809: error: expected `;' before ']' token
Serial:509: error: expected `}' at end of input
Serial:509: error: expected `}' at end of input
Was mir dabei aufgefallen ist, deine Zeilen vor dem grünen Text sind etwas anders als bei mir. Hier der Vergleich:
DeinText:
#if BARO
if (rcOptions[BOXBARO]) {
if (baroMode == 0) {
baroMode = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else baroMode = 0;
#endif
Mein Text:
#if BARO
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else {
f.BARO_MODE = 0;
}
#endif
Zeile 3 und Zeile 4 sind da anders.
Gruß Paraglider58
@DeaconBlues @Alle
Hi!
Ich finde Dein Einsteigen in das Projekt hier ausserordentlich gut! Nach meinen Erfahrungen mit der Höhenregelung, ist die Latenz des reinen Höhenreglers einfach zu gross. Du wirst nicht an der Implementation des ACC herumkommen. Wenn man mit dem dem Baro einen Öltanker (blöder Verlgleich) steueren wollte wäre das ok. Der Copter ist einfach zu spritzig für eine reine Baro Steuerung. Ich weiss, die Microcopter Piloten sind eine besondere Spezies, aber schau mal hier: http://www.mikrokopter.de/ucwiki/MK-Parameter/Altitude. Dort werden noch verschiedene Parameter mit einbezogen, die ich teilweise für schlichtweg unpraktisch halte. z.B
Zitat: "Schwebe-Gas +/- Begrenzt die Reglerausgaben auf die Umgebung des Hoverpunktes,"
Hier muss ein Hoverpunkt angegeben werden. Dieser ist natürlich abhängig von der Zuladung. Ich fliege z.B nicht immer mit meinem Videozeugs, oder verwende verschiedene Propeller, Lipos etc d.h. ich muss den Hoverpunkt immer neu einstellen.
Zitat: "Verstärkung/Rate Ermöglicht größere Flughöhen, wenn dieser Wert vergrößert wird." Was soll dass denn? Muss ich mir vorher überlegen in welcher Höhe die Höhenregelung überhaupt greifen soll?
Interessant ist aber, dass Z-ACC und GPS-Z und zur Dämpfung des Höhenreglers eingesetzt werden. Davon ist der offizielle MW Code noch meilenweit entfernt. Diese Werte sind jedoch offensichtlich erforderlich, um den trägen und ungenauen Barosensor zu unterstützen.
Zitat: "Min Gas Unter diesen Wert wird das Gas nie gestellt, wenn die Höhe überschritten wurde". Ebenfalls blöd einzustellen, da die Copterconfig eben wechseln kann (s.o). Das ist aber ebenfalls interessant, und entspricht quasi meinem "I". D.h. Abwärtsbewegungen werden abgeschwächt oder limitiert.
Aktuell bin ich mit der Version aus dem Anhang unterwegs. Die Regelung ist gegenüber meinen älteren Versionen eher etwas schlechter, aber sie hat den Vorteil nicht mehr so sprunghaft zu sein und der ACC knallt nicht mehr so rein. D.h. ESC und Motoren werden nicht mehr so belastet. Schau Dir mal meinen simplen "moving average" code "For Dummies" an. Der (6-Fach) filtert gut, ist zeitnah und frisst wenig Speicher. Die Division durch die Sensorauflösung habe ich so eliminieren können (d.h. auch der MS dürfte jetzt seine 10 cm ausspielen können). Das "P" habe ich schärfer gestellt. Aktuell fliege ich mit "P" 3.0-4.0 "I" 0.014 - 0.018 und "D" ca. 70 (BMA020). Als nächstes probiere ich den 6-Fach Filter zu reduzieren auf 5 oder 4 Fach - mal schauen, ob das die Regelung verbessert, ohne den Antriebsstrang zu sehr zu belasten. Die 6-Fach Version ist geflogen und funktioniert. Die 4-Fach Version habe ich zum Testen hier auch noch angehängt (noch nicht geflogen). Die config.h ist nicht geändert. Viel Spass beim Testen!
LG
Rob
Hi!
Ich finde Dein Einsteigen in das Projekt hier ausserordentlich gut! Nach meinen Erfahrungen mit der Höhenregelung, ist die Latenz des reinen Höhenreglers einfach zu gross. Du wirst nicht an der Implementation des ACC herumkommen. Wenn man mit dem dem Baro einen Öltanker (blöder Verlgleich) steueren wollte wäre das ok. Der Copter ist einfach zu spritzig für eine reine Baro Steuerung. Ich weiss, die Microcopter Piloten sind eine besondere Spezies, aber schau mal hier: http://www.mikrokopter.de/ucwiki/MK-Parameter/Altitude. Dort werden noch verschiedene Parameter mit einbezogen, die ich teilweise für schlichtweg unpraktisch halte. z.B
Zitat: "Schwebe-Gas +/- Begrenzt die Reglerausgaben auf die Umgebung des Hoverpunktes,"
Hier muss ein Hoverpunkt angegeben werden. Dieser ist natürlich abhängig von der Zuladung. Ich fliege z.B nicht immer mit meinem Videozeugs, oder verwende verschiedene Propeller, Lipos etc d.h. ich muss den Hoverpunkt immer neu einstellen.
Zitat: "Verstärkung/Rate Ermöglicht größere Flughöhen, wenn dieser Wert vergrößert wird." Was soll dass denn? Muss ich mir vorher überlegen in welcher Höhe die Höhenregelung überhaupt greifen soll?
Interessant ist aber, dass Z-ACC und GPS-Z und zur Dämpfung des Höhenreglers eingesetzt werden. Davon ist der offizielle MW Code noch meilenweit entfernt. Diese Werte sind jedoch offensichtlich erforderlich, um den trägen und ungenauen Barosensor zu unterstützen.
Zitat: "Min Gas Unter diesen Wert wird das Gas nie gestellt, wenn die Höhe überschritten wurde". Ebenfalls blöd einzustellen, da die Copterconfig eben wechseln kann (s.o). Das ist aber ebenfalls interessant, und entspricht quasi meinem "I". D.h. Abwärtsbewegungen werden abgeschwächt oder limitiert.
Aktuell bin ich mit der Version aus dem Anhang unterwegs. Die Regelung ist gegenüber meinen älteren Versionen eher etwas schlechter, aber sie hat den Vorteil nicht mehr so sprunghaft zu sein und der ACC knallt nicht mehr so rein. D.h. ESC und Motoren werden nicht mehr so belastet. Schau Dir mal meinen simplen "moving average" code "For Dummies" an. Der (6-Fach) filtert gut, ist zeitnah und frisst wenig Speicher. Die Division durch die Sensorauflösung habe ich so eliminieren können (d.h. auch der MS dürfte jetzt seine 10 cm ausspielen können). Das "P" habe ich schärfer gestellt. Aktuell fliege ich mit "P" 3.0-4.0 "I" 0.014 - 0.018 und "D" ca. 70 (BMA020). Als nächstes probiere ich den 6-Fach Filter zu reduzieren auf 5 oder 4 Fach - mal schauen, ob das die Regelung verbessert, ohne den Antriebsstrang zu sehr zu belasten. Die 6-Fach Version ist geflogen und funktioniert. Die 4-Fach Version habe ich zum Testen hier auch noch angehängt (noch nicht geflogen). Die config.h ist nicht geändert. Viel Spass beim Testen!
LG
Rob
