mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
Antoine Drouin
110 lines
1.9 KiB
C
110 lines
1.9 KiB
C
#include <math.h>
|
|
|
|
#include "ahrs_utils.h"
|
|
|
|
float C[4];
|
|
|
|
float dcm00;
|
|
float dcm01;
|
|
float dcm02;
|
|
float dcm12;
|
|
float dcm22;
|
|
|
|
float phi;
|
|
float theta;
|
|
float psi;
|
|
|
|
float q0;
|
|
float q1;
|
|
float q2;
|
|
float q3;
|
|
|
|
|
|
/* d_euler / dq */
|
|
/*
|
|
phi = atan ( 2(q2q3 + q0q1) / (q0^2 - q1^2 - q2^2 + q3^2))
|
|
|
|
*/
|
|
void test_dphi_dq ( void ) {
|
|
float my_dcm22 = q0*q0 - q1*q1 - q2*q2 + q3*q3;
|
|
float dcm22_sq = my_dcm22 * my_dcm22;
|
|
float my_dcm12 = 2*(q2*q3 + q0*q1);
|
|
float dcm12_sq = my_dcm12 * my_dcm12;
|
|
C[0] = 2 * q1 * my_dcm22 / (dcm22_sq + dcm12_sq);
|
|
C[1] = 2 * q0 * my_dcm22 / (dcm22_sq + dcm12_sq);
|
|
C[2] = 2 * q3 * my_dcm22 / (dcm22_sq + dcm12_sq);
|
|
C[3] = 2 * q2 * my_dcm22 / (dcm22_sq + dcm12_sq);
|
|
}
|
|
/*
|
|
theta = asin(-2(q1q3 - q0q2))
|
|
|
|
dasin = 1/sqrt(1-x^2)
|
|
*/
|
|
void test_dtheta_dq ( void ) {
|
|
float my_dcm02 = 2 * (q1*q3 - q0*q2);
|
|
float dcm02_sq = my_dcm02 * my_dcm02;
|
|
C[0] = 2 * q2 / (sqrt(1 - dcm02_sq));
|
|
C[1] = -2 * q3 / (sqrt(1 - dcm02_sq));
|
|
C[2] = 2 * q0 / (sqrt(1 - dcm02_sq));
|
|
C[3] = -2 * q1 / (sqrt(1 - dcm02_sq));
|
|
}
|
|
|
|
/*
|
|
psi = atan ( 2(q1q2 + q0q3) / (q0^2 + q1^2 - q2^2 - q3^2))
|
|
|
|
|
|
datan = 1/(1+x^2)
|
|
*/
|
|
void test_dpsi_dq ( void ) {
|
|
float my_dcm00 = 1 - 2. * q2 * q2 - 2. * q3 * q3;
|
|
float my_dcm01 = 2*(q1*q2 +q0*q3);
|
|
float dcm00_sq = my_dcm00 * my_dcm00;
|
|
float dcm01_sq = my_dcm01 * my_dcm01;
|
|
C[0] = 2. * q3 * my_dcm00 / (dcm00_sq + dcm01_sq);
|
|
C[1] = 2. * q2 * my_dcm00 / (dcm00_sq + dcm01_sq);
|
|
C[2] = 2. * q1 * my_dcm00 / (dcm00_sq + dcm01_sq);
|
|
C[3] = 2. * q0 * my_dcm00 / (dcm00_sq + dcm01_sq);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int main (int argc, char** argv) {
|
|
|
|
phi = 0.3;
|
|
theta = 0.1;
|
|
psi = 0.2;
|
|
PrintEuler()
|
|
|
|
quat_of_eulers();
|
|
PrintQuat();
|
|
DCM_of_quat();
|
|
PrintDCM();
|
|
#if 1
|
|
printf("phi\n");
|
|
compute_dphi_dq();
|
|
PrintC();
|
|
test_dphi_dq();
|
|
PrintC();
|
|
#endif
|
|
|
|
#if 1
|
|
printf("theta\n");
|
|
compute_dtheta_dq();
|
|
PrintC();
|
|
test_dtheta_dq();
|
|
PrintC();
|
|
#endif
|
|
|
|
#if 1
|
|
printf("psi\n");
|
|
compute_dpsi_dq();
|
|
PrintC();
|
|
test_dpsi_dq();
|
|
PrintC();
|
|
#endif
|
|
|
|
return 0;
|
|
}
|