1,15 → 1,735 |
/* |
Copyright 2008, by Michael Walter |
|
All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
License along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
unless it is stated otherwise. |
*/ |
|
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "kafi.h" |
#include "analog.h" |
#include "main.h" |
#include "FlightControl.h" |
#include "mm3.h" |
|
|
/***************************************************************************** |
(SYMBOLIC) CONSTANTS |
*****************************************************************************/ |
|
/***************************************************************************** |
VARIABLES |
*****************************************************************************/ |
volatile int KompassValue = 0; |
|
/* Kalman filter */ |
f32_t Phi, Theta, sinPhi, cosPhi, tanTheta, secTheta; |
f32_t sinPhi, cosPhi, sinTheta, cosTheta; |
f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P; |
|
f32_t **matPs; /* Predicted system Noise */ |
f32_t **matP; /* Predicted system Noise */ |
f32_t **matP_tmp; |
f32_t **matQ; /* system noise covariance */ |
f32_t **matR; /* measurement noise covariance */ |
f32_t **matXd; /* estimated state */ |
f32_t **matXs; /* predicted state */ |
f32_t **matX_tmp1; |
f32_t **matX_tmp2; |
|
f32_t **matPhi; /* transition matrix to predict new state from current state */ |
f32_t **matB ; /* control matrix to predict new state from ext. control vector U */ |
f32_t **matU; /* control vector */ |
f32_t **matC; /* Jacobi matrix */ |
f32_t **matY; /* real measurements */ |
f32_t **matYs; /* predicted measurements */ |
f32_t **matdY; /* innovation */ |
f32_t **temp_meas_meas; |
f32_t **temp_meas_state; |
f32_t **temp_state_meas; |
f32_t **temp_meas_2; |
f32_t **temp_state_state; |
f32_t **matK; /* the Kalman gain matrix */ |
|
char fYValid[KAFI_DIM_Y]; /* measurement validity flag */ |
|
|
|
/* **************************************************************************** |
Functionname: AttitudeEstimation */ /*! |
Description: Main Kalman Filter Loop |
|
@param[in] |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void FlightAttitudeEstimation() |
{ |
/* Predict the Measurent */ |
trPredictMeasurement(); |
|
/* Update the Jacobi Matrix */ |
trUpdateJacobiMatrix(); |
|
/* Extract measurements and store them in vector matY. |
Measurement validity is indicated by fYValid[] */ |
trMeasure(); |
|
/* Innovation: calculate Xd, and Pd */ |
trInnovate(); |
|
/* Update transition matrix Phi */ |
//trUpdatePhi(); // Is an Identity matrix which will not change |
|
/* Update control matrix B */ |
trUpdateBU(); |
|
/* Predict new state Xs and Ps */ |
KAFIPrediction(); |
|
/* store innovated state vector in current state */ |
matGetFull(matXs, _Phi , 0, &status.Phi); |
matGetFull(matXs, _Theta , 0, &status.Theta); |
matGetFull(matXs, _Psi , 0, &status.Psi); |
|
/* Limit Angles to [-2Pi;...;+2Pi] */ |
trLimitAngles(); |
} |
|
/* **************************************************************************** |
Functionname: Kafi_Init */ /*! |
Description: Generate Kalman Matrizes |
|
@param[in] |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void Kafi_Init() |
{ |
/* create kalman filter and associated matrizes */ |
ui32_t i, j; |
matPs = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
matQ = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
matR = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_Y); |
matXd = matrix( 1, KAFI_DIM_X, 1,1); /* estimated state */ |
matXs = matrix( 1, KAFI_DIM_X, 1,1); /* predicted state */ |
matX_tmp1 = matrix( 1, KAFI_DIM_X,1, 1); |
matX_tmp2 = matrix( 1, KAFI_DIM_X, 1,1); |
matPhi = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X);/* transition matrix to predict new state from current state */ |
matB = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_U); /* control matrix to predict new state from ext. control vector U */ |
matU = matrix( 1, KAFI_DIM_U, 1,1); /* control vector */ |
matC = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_X); /* Jacobi matrix */ |
matY = matrix( 1, KAFI_DIM_Y, 1,1); /* real measurements */ |
matdY = matrix( 1, KAFI_DIM_Y, 1,1); /* innovation */ |
matYs = matrix( 1, KAFI_DIM_Y, 1,1); /* predicted measurements */ |
matP = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
matK = matrix(1, KAFI_DIM_X, 1, KAFI_DIM_Y); |
temp_state_state = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
temp_meas_state = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_X ); |
temp_state_meas = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_Y ); |
temp_meas_meas = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
temp_meas_2 = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
matP_tmp = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X); |
|
/* set phi to identity matrix */ |
for ( j = 0; j < KAFI_DIM_X; j++ ) |
{ |
for ( i = 0; i < KAFI_DIM_X; i++ ) |
{ |
matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F); |
} |
} |
|
/* set diagonal of measurement noise covariance R */ |
matSetDiagonal(matR, _ax, _ax, 10.0F); |
matSetDiagonal(matR, _ay, _ay, 10.0F); |
matSetDiagonal(matR, _az, _az, 10.0F); |
matSetDiagonal(matR, _compass, _compass, 5.0F); |
|
#ifdef MELEXIS_GYRO |
/* set diagonal of system noise covariance Q, dim(X,X) */ |
matSetDiagonal(matQ, _Phi, _Phi, 0.000006F); |
matSetDiagonal(matQ, _Theta, _Theta, 0.000006F); |
matSetDiagonal(matQ, _Psi, _Psi, 0.000006F); |
#else |
/* set diagonal of system noise covariance Q */ |
matSetDiagonal(matQ, _Phi, _Phi, 0.00001F); |
matSetDiagonal(matQ, _Theta, _Theta, 0.00001F); |
matSetDiagonal(matQ, _Psi, _Psi, 0.00005F); |
#endif |
|
/* set diagonal init. estimation error (covariance) P0 */ |
matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
matSetDiagonal(matPs, _Psi, _Psi, 0.00001F); |
|
matSetFull(matXd, _Phi, 0, 0.0F); |
matSetFull(matXd, _Theta, 0, 0.0F); |
matSetFull(matXd, _Psi, 0, 0.0F); |
|
matSetFull(matXs, _Phi, 0, 0.0F); |
matSetFull(matXs, _Theta, 0, 0.0F); |
matSetFull(matXs, _Psi, 0, 0.0F); |
} |
|
/* **************************************************************************** |
Functionname: KAFIPrediction */ /*! |
Description: Predict new state Xs and Ps |
|
X_k = A * X_(k-1) + B * u_k |
P_k = A * P_(k-1) * A^T + Q |
|
@param[in] |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void KAFIPrediction() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
|
/*--- VARIABLES ---*/ |
|
/* Innovation: calculate Xd, and Pd */ |
|
/* X_k = A * X_(k-1) + B * u_k */ |
|
/* matX_tmp1 = matPhi * matXd */ |
/* |
matrix_mult(matPhi,matXd, matX_tmp1, |
KAFI_DIM_X, KAFI_DIM_X, 1); |
*/ |
/* matX_tmp2 = matB * matU */ |
matrix_mult( matB,matU, matX_tmp2, |
KAFI_DIM_X, KAFI_DIM_U, 1); |
|
/* matXs = matPhi * matXd + matB * matU */ |
/* |
matrix_add( matX_tmp1, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
matrix_add( matXd, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
*/ |
|
matXs[1][1] = matXd[1][1] + matX_tmp2[1][1]; |
matXs[2][1] = matXd[2][1] + matX_tmp2[2][1]; |
matXs[3][1] = matXd[3][1] + matX_tmp2[3][1]; |
|
/*P_k = A * P_(k-1)*A^T + Q */ |
/* |
matrix_mult_transpose( matP, matPhi, matP_tmp, // Can be skiped PHI is a ident matrix |
KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
matrix_mult( matPhi, matP_tmp, matP, |
KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
matrix_add( matP, matQ, matPs, KAFI_DIM_X, KAFI_DIM_X ); |
*/ |
|
matPs[1][1] = matP[1][1] + matQ[1][1]; |
matPs[2][2] = matP[2][2] + matQ[2][2]; |
matPs[3][3] = matP[3][3] + matQ[3][3]; |
} |
|
/* **************************************************************************** |
Functionname: trInnovate */ /*! |
Description: Innovation: calculate Xd, and Pd |
|
K = Ps * C^T * (C * Ps * C^T + R)^-1 |
X = Xs + K (Y - Ys) |
|
@param[in] |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void trInnovate() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
|
/*--- VARIABLES ---*/ |
#if 0 /* Runtime optimised. To be verified */ |
/* temp_meas_state = matC * matPs */ |
matrix_mult( matC, matPs, temp_meas_state, |
KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
|
/* temp_meas_meas = matC * matPs^T * matC^T */ |
matrix_mult_transpose( matC, temp_meas_state, temp_meas_meas, |
KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
|
/* temp_meas_meas = matC * matPs^T * matC^T + matR */ |
/* |
matrix_add( temp_meas_meas, matR, temp_meas_meas, |
KAFI_DIM_Y, KAFI_DIM_Y ); |
*/ |
temp_meas_meas[1][1] += matR[1][1]; |
temp_meas_meas[2][2] += matR[2][2]; |
temp_meas_meas[3][3] += matR[3][3]; |
temp_meas_meas[4][4] += matR[4][4]; |
|
/* temp_meas_2 = (matC * matPs^T * matC^T + matR)^-1 */ |
take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
|
/* matK = matPs^T * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
matrix_transpose_mult( temp_meas_state, temp_meas_2, matK, |
KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
|
/* temp_state_state = matK * matC * matPs */ |
matrix_mult( matK, temp_meas_state, temp_state_state, |
KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
|
/* matP = matPs - matK * matC * matPs */ |
matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
#else |
/* temp_state_meas = matPs * matC^T */ |
matrix_mult_transpose( matPs, matC, temp_state_meas, |
KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_Y ); |
|
/* temp_meas_meas = matC * matPs * matC^T */ |
matrix_mult( matC, temp_state_meas, temp_meas_meas, |
KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
|
/* temp_meas_meas = matC * matPs * matC^T + matR */ |
/* |
matrix_add( temp_meas_meas, matR, temp_meas_meas, |
KAFI_DIM_Y, KAFI_DIM_Y ); |
*/ |
temp_meas_meas[1][1] += matR[1][1]; |
temp_meas_meas[2][2] += matR[2][2]; |
temp_meas_meas[3][3] += matR[3][3]; |
temp_meas_meas[4][4] += matR[4][4]; |
|
/* temp_meas_2 = (matC * matPs * matC^T + matR)^-1 */ |
take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
|
/* matK = matPs * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
matrix_mult( temp_state_meas, temp_meas_2, matK, |
KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
|
/* temp_meas_state = matC * matPs*/ |
matrix_mult( matC, matPs, temp_meas_state, |
KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
|
/* temp_state_state = matK * matC * matPs */ |
matrix_mult( matK, temp_meas_state, temp_state_state, |
KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
|
/* matP = matPs - matK * matC * matPs */ |
matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
#endif |
|
/* matdY = matY - matYs */ |
/* |
matrix_sub( matY, matYs, matdY, |
KAFI_DIM_Y, 1 ); |
*/ |
matdY[1][1] = matY[1][1] - matYs[1][1]; |
matdY[2][1] = matY[2][1] - matYs[2][1]; |
matdY[3][1] = matY[3][1] - matYs[3][1]; |
|
/* We will only update the Heading if we got a valid compass signal */ |
if(fYValid[_compass] == 1) |
{ |
matdY[4][1] = matY[4][1] - matYs[4][1]; |
} |
else |
{ |
matdY[4][1] = 0.0F; |
} |
|
/* matX_tmp1 = matK * (Y -Ys) */ |
matrix_mult( matK, matdY, matX_tmp1, |
KAFI_DIM_X, KAFI_DIM_Y, 1 ); |
|
/* matXd = matXs + matK * (Y -Ys) */ |
/* |
matrix_add( matXs, matX_tmp1, matXd, |
KAFI_DIM_X, 1 ); |
*/ |
matXd[1][1] = matXs[1][1] + matX_tmp1[1][1]; |
matXd[2][1] = matXs[2][1] + matX_tmp1[2][1]; |
matXd[3][1] = matXs[3][1] + matX_tmp1[3][1]; |
matXd[4][1] = matXs[4][1] + matX_tmp1[4][1]; |
} |
|
|
/* **************************************************************************** |
Functionname: trUpdatePhi */ /*! |
Description: Setup matPhi, used to calculate the predicted state |
from the current state |
|
@param[in] |
@param[in] fCycleTime |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void trUpdatePhi() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
|
/*--- VARIABLES ---*/ |
matSetFull(matPhi, _Phi, _Phi, 1.0F); |
matSetFull(matPhi, _Theta, _Theta, 1.0F); |
matSetFull(matPhi, _Psi, _Psi, 1.0F); |
} |
|
/* **************************************************************************** |
Functionname: trUpdateJacobiMatrix */ /*! |
Description: |
|
@param[in] void |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
*****************************************************************************/ |
void trUpdateJacobiMatrix(void) |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/* |
Simplified Version d/dt |
Pre_ax = du - sinTheta * g; |
Pre_ay = dv + cosTheta_sinPhi * g; |
Pre_az = dw + cosTheta_cosPhi * g; |
*/ |
|
/*--- VARIABLES ---*/ |
f32_t Phi, Theta, Psi; |
f32_t g = 9.81F; |
|
Phi = status.Phi; |
Theta = status.Theta; |
Psi = status.Psi; |
|
sinPhi = sin(Phi); |
cosPhi = cos(Phi); |
sinTheta = sin(Theta); |
cosTheta = cos(Theta); |
|
//Pre_ax = du - sinTheta * g; |
//matSetFull(matC, _ax, _Phi , 0); |
matSetFull(matC, _ax, _Theta, -cosTheta * g); |
//matSetFull(matC, _ax, _Psi , 0); |
|
//Pre_ay = dv + cosTheta_sinPhi * g; |
matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g); |
matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g); |
//matSetFull(matC, _ay, _Psi , 0); |
|
//Pre_az = dw + cosTheta_cosPhi * g; |
matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g); |
matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g); |
//matSetFull(matC, _az, _Psi , 0); |
|
matSetFull(matC, _compass, _Psi , 1.0F); |
} |
|
/***************************************************************************** |
VARIABLES |
Functionname: trPredictMeasurement */ /*! |
Description:Predict Measurements: AX, AY, AZ, Compass |
|
|
@param[in] void |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
*****************************************************************************/ |
void trPredictMeasurement(void) |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
|
/* |
Simplified Version |
Pre_ax = du - sinTheta * g; |
Pre_ay = dv + cosTheta_sinPhi * g; |
Pre_az = dw + cosTheta_cosPhi * g; |
*/ |
|
/*--- VARIABLES ---*/ |
f32_t g = 9.81F; |
f32_t Pre_ax, Pre_ay, Pre_az; |
f32_t Phi, Theta, Psi; |
|
matGetFull(matXs, _Phi, 0, &Phi); |
matGetFull(matXs, _Theta, 0, &Theta); |
matGetFull(matXs, _Psi, 0, &Psi); |
|
sinPhi_P = sin(Phi); |
cosPhi_P = cos(Phi); |
sinTheta_P = sin(Theta); |
cosTheta_P = cos(Theta); |
|
/* Simplified Version */ |
Pre_ax = -sinTheta_P * g; |
Pre_ay = cosTheta_P * sinPhi_P * g; |
Pre_az = cosTheta_P * cosPhi_P * g; |
|
matSetFull(matYs, _ax, 0, Pre_ax); |
matSetFull(matYs, _ay, 0, Pre_ay); |
matSetFull(matYs, _az, 0, Pre_az); |
matSetFull(matYs, _compass, 0, Psi); |
} |
|
/* **************************************************************************** |
Functionname: trMeasure */ /*! |
Description: |
|
@param[in] |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void trMeasure() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
|
/*--- VARIABLES ---*/ |
#ifdef USE_COMPASS |
static int updCompass = 10; |
f32_t Psi, Compass; |
#endif |
|
f32_t ADC_ax = 0.0F; |
f32_t ADC_ay = 0.0F; |
f32_t ADC_az = 0.0F; |
|
fYValid[_ax] = 1; |
fYValid[_ay] = 1; |
fYValid[_az] = 1; |
|
#ifdef INTERNAL_REFERENCE |
ADC_ax = AdWertAccNick * 0.044F; |
ADC_ay = -AdWertAccRoll * 0.044F; |
ADC_az = AdWertAccHoch * 0.044F; |
#else |
ADC_ax = AdWertAccNick * 0.047F; |
ADC_ay = -AdWertAccRoll * 0.047F; |
ADC_az = AdWertAccHoch * 0.047F; |
#endif |
|
matSetFull(matY, _ax, 0, ADC_ax); |
matSetFull(matY, _ay, 0, ADC_ay); |
matSetFull(matY, _az, 0, ADC_az); |
|
#ifdef USE_COMPASS |
if (!updCompass--) |
{ |
updCompass = 10; // update only at 2ms*50 = 100ms (10Hz) |
KompassValue = MM3_Heading(); |
} |
Compass = KompassValue / 180.F * PI; |
|
matGetFull(matXs, _Psi, 0, &Psi); |
|
if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi)) |
{ |
Compass += 2 * PI; |
} |
else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi)) |
{ |
Compass -= 2 * PI; |
} |
|
matSetFull(matY, _compass, 0, Compass); |
|
/* Roll and Yaw angle are smaller 8 Deg*/ |
if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80)) |
{ |
fYValid[_compass] = 1; |
} |
else |
{ |
fYValid[_compass] = 0; |
} |
#else |
fYValid[_compass] = 0; |
#endif |
} |
|
/* **************************************************************************** |
Functionname: trUpdateBU */ /*! |
Description: Update control matrix B and vector U |
|
|
@param[in] |
|
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void trUpdateBU() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
|
/*--- VARIABLES ---*/ |
/* |
dPhi | 1 sinPhi_tanTheta cosPhi_tanTheta | p |
dTheta = | 0 cosPhi -sinPhi | * q |
dPsi | 0 sinPhi_secTheta cosPhi_secTheta | w |
*/ |
|
static f32_t fSumGier = 0.0F; |
static f32_t fSumNick = 0.0F; |
static f32_t fSumRoll = 0.0F; |
|
f32_t ADC_p = 0.0F; |
f32_t ADC_q = 0.0F; |
f32_t ADC_r = 0.0F; |
|
/* store predicted state vector in current state */ |
matGetFull(matXd, _Phi , 0, &Phi); |
matGetFull(matXd, _Theta , 0, &Theta); |
|
sinPhi = sin(Phi); |
cosPhi = cos(Phi); |
|
tanTheta = tan(Theta); |
secTheta = 1.0F / cos(Theta); |
|
matSetFull(matB, _Phi, _p, 1.0F); |
matSetFull(matB, _Phi, _q, sinPhi * tanTheta ); |
matSetFull(matB, _Phi, _r, cosPhi * tanTheta ); |
|
//matSetFull(matB, _Theta, _p, 0.0F); |
matSetFull(matB, _Theta, _q, cosPhi ); |
matSetFull(matB, _Theta, _r, -sinPhi ); |
|
//matSetFull(matB, _Psi, _p, 0.0F); |
matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
|
ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime; |
ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime; |
ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime; |
|
fSumGier += ADC_r * 180.F / 3.14F; |
fSumNick += ADC_q * 180.F / 3.14F; |
fSumRoll += ADC_p * 180.F / 3.14F; |
|
DebugOut.Analog[3] = fSumNick; |
DebugOut.Analog[4] = fSumRoll; |
DebugOut.Analog[5] = fSumGier; |
|
matSetFull(matU, _p, 0, ADC_p ); |
matSetFull(matU, _q, 0, ADC_q); |
matSetFull(matU, _r, 0, ADC_r); |
} |
|
|
|
/* **************************************************************************** |
Functionname: trLimitAngles */ /*! |
Description: |
|
@param[in] |
|
@return void |
@pre - |
@post - |
@author |
**************************************************************************** */ |
void trLimitAngles() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
|
/*--- VARIABLES ---*/ |
f32_t Psi; |
|
if (status.Phi > 2.0F * PI) |
{ |
status.Phi -= 2.0F * PI; |
matSetFull(matXs, _Phi, 0, status.Phi); |
} |
if (status.Phi < -2.0F * PI) |
{ |
status.Phi += 2.0F * PI; |
matSetFull(matXs, _Phi, 0, status.Phi); |
} |
if (status.Theta > 2.0F * PI) |
{ |
status.Theta -= 2.0F * PI; |
matSetFull(matXs, _Theta, 0, status.Theta); |
} |
if (status.Theta < -2.0F * PI) |
{ |
status.Theta += 2.0F * PI; |
matSetFull(matXs, _Theta, 0, status.Theta); |
} |
if (status.Psi > 2.0F * PI) |
{ |
status.Psi -= 2.0F * PI; |
sollGier -= 3600; |
|
matGetFull(matXs, _Psi, 0, &Psi); |
matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI); |
matGetFull(matXd, _Psi, 0, &Psi); |
matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI); |
} |
if (status.Psi < 0.0F * PI) |
{ |
status.Psi += 2.0F * PI; |
sollGier += 3600; |
|
matGetFull(matXs, _Psi, 0, &Psi); |
matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI); |
matGetFull(matXd, _Psi, 0, &Psi); |
matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI); |
} |
|
status.iPhi10 = (int) (status.Phi * 573.0F); |
status.iTheta10 = (int) (status.Theta * 573.0F); |
status.iPsi10 = (int) (status.Psi * 573.0F); |
|
if ((sollGier - status.iPsi10) > 3600) |
{ |
sollGier -= 3600; |
} |
|
if ((sollGier - status.iPsi10) < -3600) |
{ |
sollGier += 3600; |
} |
} |
|
|
/* **************************************************************************** |
Functionname: trEstimateVelocity */ /*! |
Description: |
|
@param[in] |
|
@return void |
@pre - |
@post - |
@author |
**************************************************************************** */ |
void trEstimateVelocity() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
|
/*--- VARIABLES ---*/ |
|
} |