rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Added PIP_DUMP_RAW option in ahrs.c and brought back public acces to function PIOS_ADC_GetOverSampling(void).

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2623 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
pip 2011-01-29 15:59:35 +00:00 committed by pip
parent 000a45f1bf
commit 5daf6fd1c0
2 changed files with 75 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
flight/AHRS/ahrs.c
View File

@ -60,6 +60,7 @@
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_EKF
//#define PIP_DUMP_RAW
volatile int8_t ahrs_algorithm;
@ -419,6 +420,7 @@ void print_ekf_binary() {}
/**
* @brief Debugging function to output all the ADC samples
*/
#if defined(DUMP_RAW)
void print_ahrs_raw()
{
int result;
@ -455,6 +457,77 @@ void print_ahrs_raw()
PIOS_LED_On(LED1);
}
}
#endif
#if defined(PIP_DUMP_RAW)
#define MAX_OVERSAMPLING PIOS_ADC_MAX_OVERSAMPLING
void print_ahrs_raw()
{
int16_t accel_x[MAX_OVERSAMPLING], accel_y[MAX_OVERSAMPLING], accel_z[MAX_OVERSAMPLING];
int16_t gyro_x[MAX_OVERSAMPLING], gyro_y[MAX_OVERSAMPLING], gyro_z[MAX_OVERSAMPLING];
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
int16_t mag[3];
int16_t mag_x, mag_y, mag_z;
#endif
static int previous_conversion = 0;
uint8_t framing[3] = {0xD2, 0x73, 0x00};
// wait for new raw samples
while (previous_conversion == total_conversion_blocks);
if ((previous_conversion + 1) != total_conversion_blocks)
PIOS_LED_On(LED1); // we are not keeping up
previous_conversion = total_conversion_blocks;
// fetch the buffer address for the new samples
int16_t *valid_data_buffer = PIOS_ADC_GetRawBuffer();
// fetch number of raw samples in the buffer (per channel)
int over_sampling = PIOS_ADC_GetOverSampling();
framing[2] = over_sampling;
// copy the raw samples into their own buffers
for (uint16_t i = 0, j = 0; i < over_sampling; i++, j += PIOS_ADC_NUM_CHANNELS)
{
accel_x[i] = valid_data_buffer[j + 0];
accel_y[i] = valid_data_buffer[j + 2];
accel_z[i] = valid_data_buffer[j + 4];
gyro_x[i] = valid_data_buffer[j + 3];
gyro_y[i] = valid_data_buffer[j + 1];
gyro_z[i] = valid_data_buffer[j + 5];
}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if (PIOS_HMC5843_NewDataAvailable())
{
PIOS_HMC5843_ReadMag(mag);
mag_x = mag[MAG_RAW_X_IDX];
mag_y = mag[MAG_RAW_Y_IDX];
mag_z = mag[MAG_RAW_Z_IDX];
}
#endif
// send the raw samples
int result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, framing, sizeof(framing));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_x, over_sampling * sizeof(accel_x[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_y, over_sampling * sizeof(accel_y[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_z, over_sampling * sizeof(accel_z[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_x, over_sampling * sizeof(gyro_x[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_y, over_sampling * sizeof(gyro_y[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_z, over_sampling * sizeof(gyro_z[0]));
if (result != 0)
PIOS_LED_On(LED1); // all data not sent
else
PIOS_LED_Off(LED1);
}
#endif
/**
* @brief AHRS Main function
@ -525,7 +598,7 @@ for all data to be up to date before doing anything*/
calibration_callback(AHRSCalibrationHandle()); //force an update
#ifdef DUMP_RAW
#if defined(DUMP_RAW) || defined(PIP_DUMP_RAW)
while (1) {
AhrsPoll();
print_ahrs_raw();

1
flight/PiOS/inc/pios_adc.h
View File

@ -42,6 +42,7 @@ void PIOS_ADC_Init();
void PIOS_ADC_Config(uint32_t oversampling);
int32_t PIOS_ADC_PinGet(uint32_t pin);
int16_t * PIOS_ADC_GetRawBuffer(void);
uint8_t PIOS_ADC_GetOverSampling(void);
void PIOS_ADC_SetCallback(ADCCallback new_function);
extern void PIOS_ADC_DMA_Handler(void);