hello everybody, I’m a novice. this is my first time to post a question. forgiving my poor English, I will do my best to clearly describe my question.
today i had read the firmware code of IMU of CHR-6dm
In this project, the ADC on ARM chip was set to readed/writed mode through DMA which has a ping-pong buffer for receiving data from gyroscope and accelerometer. the buffer size for every channel is 512*8 bytes. There are two DMA triggered interrupts, one for a complete buffer transfer, and one for a half buffer transfer. The DMA buffer in memory
stores ADC data from the sensors, and is divided into two halves saved in ping-pong buffer. so there is some codes about getting gyroscope and accelerometer data.
// ---------------------------------------------------------------------------------------
// Decimation step. Take ADC data in buffer, convert to signed data, sum, and decimate.
// ---------------------------------------------------------------------------------------
// Pre-initialize sum data
for( i = 0; i < CHANNEL_COUNT; i++ )
{
sum_data[i] = 0;
}
// Convert to signed numbers and sum data
for( i = 0; i < SAMPLES_PER_BUFFER; i++ )
{
gyro_ref_voltage = input_buffer[CHANNEL_COUNT*i + 6];
sum_data[0] += (int32_t)(input_buffer[CHANNEL_COUNT*i] - 2048);
sum_data[1] += (int32_t)(input_buffer[CHANNEL_COUNT*i + 1] - 2048);
sum_data[2] += (int32_t)(input_buffer[CHANNEL_COUNT*i + 2] - 2048);
sum_data[3] += (int32_t)(input_buffer[CHANNEL_COUNT*i + 3] - gyro_ref_voltage);
sum_data[4] += (int32_t)(input_buffer[CHANNEL_COUNT*i + 4] - gyro_ref_voltage);
sum_data[5] += (int32_t)(input_buffer[CHANNEL_COUNT*i + 5] - gyro_ref_voltage);
}
// Decimate
signed_decimated[0] = (int16_t)((sum_data[0] >> 5) & 0x0FFFF); // Accel X
signed_decimated[1] = (int16_t)((sum_data[1] >> 5) & 0x0FFFF); // Accel Y
signed_decimated[2] = (int16_t)((sum_data[2] >> 5) & 0x0FFFF); // Accel Z
signed_decimated[3] = (int16_t)((sum_data[3] >> 5) & 0x0FFFF); // Gyro Y
signed_decimated[4] = (int16_t)((sum_data[4] >> 5) & 0x0FFFF); // Gyro X
signed_decimated[5] = (int16_t)((sum_data[5] >> 5) & 0x0FFFF); // Gyro Z
signed_decimated[6] = (int16_t)((sum_data[6] >> 5) & 0x0FFFF); // Temperature
now that’s my first question based upon.
1: as the red line shown, why to operate right shift five times but not other number to sum_data[i]? I guess that is intended to calculate the average angle velocity in a short time. if that’s true, why not make division from the number of all samples in half buffer?
then, I have another quesion. let me check it. when get new data from ADC finished, The next action is kalman filter. but the solution about angle velocity integration seems working inexactly due to use interval deduced from elapse of TIM3. I think that should be instead by n*T, n is the number of samples and T is the internal sample cycle of gyroscope, even though elapse of TIM3 is close to time interval in theoretic. the code is below.
// Retrieve angular rates from vector
p = pqr.data[0];
q = pqr.data[1];
r = pqr.data[2];
// Get current state estimates
phi = estimated_states->phi;
theta = estimated_states->theta;
psi = estimated_states->psi;
TIM_Cmd(TIM3, DISABLE);
timer_value = TIM_GetCounter(TIM3);
TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM3, ENABLE);
T = (float)(0.000001)*(float)timer_value;
//something else//
//............
////////////////
// Compute expected angle rates based on gyro outputs. Note that to do this, the measured rotations
// must be transformed into the inertial frame (we can't just integrate the gyro outputs).
phi_dot = p + sin_phi*tan_theta*q + cos_phi*tan_theta*r;
theta_dot = cos_phi*q - sin_phi*r;
psi_dot = (sin_phi/cos_theta)*q + (cos_phi/cos_theta)*r;
////something else
//....................
so, my second question:
2: why not prefer cycle accmulation of n samples of gyroscope rather than elapse of TIM3 in the processing of calculating angle?
It’s a little long to read, I am not sure you have get what i said, but i will be very appreciate about your answer. thanks!