rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

OP-117 AHRS: Binary streaming of the raw data within EKF. Doesn't keep up while running this but does give accurate data that went into algorithm. Once we can dynamically change sampling rate will be good.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1655 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-16 05:26:14 +00:00 committed by peabody124
parent 0b8756d55c
commit 832b61ab86
1 changed files with 38 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
flight/AHRS/ahrs.c
View File

@ -46,6 +46,19 @@ volatile enum algorithms ahrs_algorithm;
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
//#define DUMP_EKF
#ifdef DUMP_EKF
#define NUMX 13 // number of states, X is the state vector
#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 6 // number of deterministic inputs, U is the input vector
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
#endif
/**
* @addtogroup AHRS_Definitions
@ -416,6 +429,31 @@ int main()
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2]);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "q: %d %d %d %d\r\n", (int16_t)(Nav.q[0] * 1000), (int16_t)(Nav.q[1] * 1000), (int16_t)(Nav.q[2] * 1000), (int16_t)(Nav.q[3] * 1000));
#endif
#ifdef DUMP_EKF
uint8_t framing[16] = {15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0};
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
// Dump raw buffer
int8_t result;
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &mag_data, sizeof(mag_data));
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gps_data, sizeof(gps_data));
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &accel_data, sizeof(accel_data));
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gyro_data, sizeof(gyro_data));
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &Q, sizeof(float)*NUMX*NUMX);
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &K, sizeof(float)*NUMX*NUMV);
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &X, sizeof(float)*NUMX*NUMX);
if(result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
#endif
process_spi_request();
}