//Значение сигнала
//t - с
//sin(fi0 - 2pi(fo*t + kt^2/2))
short signal(double t)
{
int rnd=0;
double pi=0;
short signal=0;
short signalInst=0;
double f0=DEV_WorkFreq;
double dfdt = 0;
//currentF = f0 + dfdt * t;
//rnd = rand() % 650;// * (((int)(t * DEV_SampleFreq) )% 2);
pi = MATH_Pi;
//short signal = 6550*sin( 2*pi*(f0 * t + dfdt * t*t / 2)) + 3000 * sin( 2*pi*(3*f0 * t + dfdt * t*t / 2)-0.6*pi) + 2000 * sin( 2*pi*(4*f0 * t + dfdt * t*t / 2)-0.2*pi) + 2000 * sin( 2*pi*(2*f0 * t + dfdt * t*t / 2)-0.2*pi)+ rnd;
//short signal = 6550*sin( 2*pi*(f0 * t + dfdt * t*t / 2)) + 500 * sin( 2*pi*(3*f0 * t + dfdt * t*t / 2)-0.6*pi) + 500 * sin( 2*pi*(4*f0 * t + dfdt * t*t / 2)-0.2*pi) + 500 * sin( 2*pi*(2*f0 * t + dfdt * t*t / 2)-0.2*pi);//+ rnd;
#ifdef MSVC
signal = 6550*sin(2*pi*(f0 * t + dfdt * t*t / 2))+3500*sin(2*pi*2*(f0 * t + dfdt * t*t / 2))+1000*sin(2*pi*3*(f0 * t + dfdt * t*t / 2));
//+4000*sin(2*5*pi*(f0 * t + dfdt * t*t / 2))+4500*sin(2*7*pi*(f0 * t + dfdt * t*t / 2))+5000*sin(2*9*pi*(f0 * t + dfdt * t*t / 2));//+rnd;
signalInst = 6550*sin(2*pi*(f0 * t + dfdt * t*t / 2));
#else
signal = 6550*arm_sin_f32( -2*pi*(f0 * t + dfdt * t*t / 2));//+rnd;
signalInst = 6550*arm_sin_f32( -2*pi*(f0 * t + dfdt * t*t / 2));
#endif
//fprintf(sinFile, "%i;", signalInst);
return signal;
}
void IMU::computeYaw(void) {
float32_t sqrt_result, arg1, arg2;
float32_t mag[3];
mag[0] = magnetometer.axis_f[0];
mag[1] = magnetometer.axis_f[1];
mag[2] = magnetometer.axis_f[2];
//Normalise measurement:
arg1 = (float32_t) (mag[0]) * (mag[0]) + (mag[1]) * (mag[1]) + (mag[2]) * (mag[2]);
arm_sqrt_f32(arg1, &sqrt_result);
mag[0] /= sqrt_result;
mag[1] /= sqrt_result;
mag[2] /= sqrt_result;
arg1 = arm_sin_f32(XRollAngleInRad) * mag[2] - arm_cos_f32(XRollAngleInRad) * mag[1];
arg2 = arm_cos_f32(YPitchAngleInRad) * mag[0]
+ arm_sin_f32(XRollAngleInRad) * arm_sin_f32(YPitchAngleInRad) * mag[1]
+ arm_cos_f32(XRollAngleInRad) * arm_sin_f32(YPitchAngleInRad) * mag[2];
ZYawAngleInRad = atan2f(arg1, arg2);
//Correct declination, and change to compass angles
// ZYawAngleInRad += (5.0 + (16.0 / 60.0))*PI/180.0; //Positive declination.
// if (ZYawAngleInRad < 0) {
// ZYawAngleInRad += 2 * PI;
// } else if (ZYawAngleInRad > 2 * PI) {
// ZYawAngleInRad -= 2 * PI;
// }
}
/**
* \brief ADC interrupt handler.
*/
void ADC_Handler(void)
{
uint32_t tmp;
uint32_t status ;
status = adc_get_status(ADC);
/* Checa se a interrupção é devido ao canal 5 */
static float rad_antes = 0;
if ((status & ADC_ISR_EOC5)) {
tmp = adc_get_channel_value(ADC, ADC_POT_CHANNEL);
ili93xx_set_foreground_color(COLOR_WHITE);
ili93xx_draw_filled_rectangle(9, 39, ILI93XX_LCD_WIDTH,55);
v=3.3*((float)tmp)/4095.0;
rad=2*pi*((float)tmp)/4095.0;
ili93xx_draw_line(120,160,120+54*arm_cos_f32(rad_antes),160+54*arm_sin_f32(rad_antes));
ili93xx_set_foreground_color(COLOR_BLACK);
sprintf(vet, "Tensao: %f", v);
ili93xx_draw_string(10, 40, vet);
ili93xx_draw_line(120,160,120+54*arm_cos_f32(rad),160+54*arm_sin_f32(rad));
rad_antes = rad;
}
}
void Sine_f32::generate(float32_t *sgn) {
for (uint32_t i=0; i<_block_size; i++) {
*sgn = (_amplitude * arm_sin_f32(_x));
sgn++;
_x += _dx;
}
}
void IMU::removeCentrifugalForceEffect(void) {
float32_t arg1 = .0, sumOfSquares = .0;
float32_t result = .0;
volatile float32_t centrifugalAcceleration[3] = { .0 };
float32_t rotationThresholdInRadPerSec = 5.0;
//Physical dimensions on board in mm
const float32_t dx = 0.015, //15 mm
dy = 0.01, //20 mm
dz = 0.003; //3 mm
float32_t *const pGyroAxis = gyro.axisInRadPerS, *const pAccAxis = accelerometer.axisInMPerSsquared;
if (pGyroAxis[0] > rotationThresholdInRadPerSec) {
centrifugalAcceleration[1] = pGyroAxis[0] * pGyroAxis[0] * dx;
}
if (pGyroAxis[1] > rotationThresholdInRadPerSec) {
centrifugalAcceleration[0] = pGyroAxis[1] * pGyroAxis[1] * dy;
}
if (pGyroAxis[2] > rotationThresholdInRadPerSec) {
arm_sqrt_f32(sumOfSquares, &result);
centrifugalAcceleration[0] = pGyroAxis[2] * pGyroAxis[2] * result;
centrifugalAcceleration[1] = pGyroAxis[2] * pGyroAxis[2] * result;
sumOfSquares = dx * dx + dy * dy;
arg1 = dy / sumOfSquares;
centrifugalAcceleration[0] *= arm_sin_f32(arg1);
arg1 = dx / sumOfSquares;
centrifugalAcceleration[1] *= arm_cos_f32(arg1);
}
pAccAxis[0] -= centrifugalAcceleration[0];
pAccAxis[1] -= centrifugalAcceleration[1];
pAccAxis[2] -= centrifugalAcceleration[2];
}
int32_t main(void)
{
float32_t diff;
uint32_t i;
for(i=0; i< blockSize; i++)
{
cosOutput = arm_cos_f32(testInput_f32[i]);
sinOutput = arm_sin_f32(testInput_f32[i]);
arm_mult_f32(&cosOutput, &cosOutput, &cosSquareOutput, 1);
arm_mult_f32(&sinOutput, &sinOutput, &sinSquareOutput, 1);
arm_add_f32(&cosSquareOutput, &sinSquareOutput, &testOutput, 1);
/* absolute value of difference between ref and test */
diff = fabsf(testRefOutput_f32 - testOutput);
/* Comparison of sin_cos value with reference */
if(diff > DELTA)
{
status = ARM_MATH_TEST_FAILURE;
}
if( status == ARM_MATH_TEST_FAILURE)
{
while(1);
}
}
while(1); /* main function does not return */
}
/**
* \brief Generate sinusoidale signals for voice changing.
*/
static void dsp_sin_init(void)
{
uint32_t i;
uint32_t j;
float32_t ratio;
float32_t freq;
freq = SAMPLING_FREQUENCY;
for (j = 0; j < SLIDER_SELECTOR_NB; ++j) {
/* Generate a sinus based on sampling freq with a delta. */
ratio = freq / SAMPLING_FREQUENCY;
for (i = 0 ; i < SAMPLE_BLOCK_SIZE; i++) {
/* Amplification is set to 1. */
if (ratio <= 1.0) {
/* It won't change anything. */
sin_buffer[j][i] = 1;
} else {
sin_buffer[j][i] = arm_sin_f32(2 * PI * i * ratio);
}
}
/* Increase delta. */
freq += VOICE_CHANGER_DELTA;
}
}
//
// Main Routine
//
int main(void) {
#ifdef DEBUG
CSerial ser; // declare a UART object
ser.enable();
CDebug dbg(ser); // Debug stream use the UART object
dbg.start();
#endif
//
// Your setup code here
//
swPWM pwm(TIMER_1);
pwm.period(0.01); // set the period time of PWM to 10ms.
pwm.enable(); // start the pwm
int pins[] = {LED_PIN_0, LED_PIN_1, LED_PIN_2, LED_PIN_3};
float y;
int x[4] = { 0, 10, 20, 30 }; // initialize all channels x degree
//
// Enter main loop.
//
while(1) {
//
// FireFly loop
//
for (int ch = 0; ch < 4; ch++) {
x[ch] = (x[ch] + 10) % 360; // degree 0~360, step by 2
y = arm_sin_f32((x[ch] * M_PI) / 180.0f); // y = sine @x
pwm.output(pins[ch], map(y, -1.0f, 1.0f, 0.0f, 1.0f)); // update the duty-cycle of channel
}
sleep(10); // speed
}
}
void Sine_f32::process(float32_t *sgn_in, float32_t *sgn_out) {
for (uint32_t i=0; i<_block_size; i++) {
*sgn_out = *sgn_in + (_amplitude * arm_sin_f32(_x));
sgn_in++; sgn_out++;
_x += _dx;
}
}
static mp_obj_t py_image_draw_keypoints(mp_obj_t image_obj, mp_obj_t kpts_obj)
{
image_t *image = NULL;
py_kp_obj_t *kpts=NULL;
/* get pointer */
image = py_image_cobj(image_obj);
kpts = (py_kp_obj_t*)kpts_obj;
/* sanity checks */
PY_ASSERT_TRUE_MSG(image->bpp == 1,
"This function is only supported on GRAYSCALE images");
PY_ASSERT_TYPE(kpts_obj, &py_kp_type);
color_t cl = {.r=0xFF, .g=0xFF, .b=0xFF};
for (int i=0; i<kpts->size; i++) {
kp_t *kp = &kpts->kpts[i];
float co = arm_cos_f32(kp->angle);
float si = arm_sin_f32(kp->angle);
imlib_draw_line(image, kp->x, kp->y, kp->x+(co*10), kp->y+(si*10));
imlib_draw_circle(image, kp->x, kp->y, 4, &cl);
}
return mp_const_none;
}
static mp_obj_t py_image_find_blobs(mp_obj_t image_obj)
{
/* C stuff */
array_t *blobs;
struct image *image;
mp_obj_t blob_obj[6];
/* MP List */
mp_obj_t objects_list = mp_const_none;
/* get image pointer */
image = py_image_cobj(image_obj);
/* run color dector */
blobs = imlib_count_blobs(image);
/* Create empty Python list */
objects_list = mp_obj_new_list(0, NULL);
if (array_length(blobs)) {
for (int j=0; j<array_length(blobs); j++) {
blob_t *r = array_at(blobs, j);
blob_obj[0] = mp_obj_new_int(r->x);
blob_obj[1] = mp_obj_new_int(r->y);
blob_obj[2] = mp_obj_new_int(r->w);
blob_obj[3] = mp_obj_new_int(r->h);
blob_obj[4] = mp_obj_new_int(r->c);
blob_obj[5] = mp_obj_new_int(r->id);
mp_obj_list_append(objects_list, mp_obj_new_tuple(6, blob_obj));
}
}
array_free(blobs);
return objects_list;
}
/* ----------------------------------------------------------------------------
* Calculation of Sine values from Cubic Interpolation and Linear interpolation
* ---------------------------------------------------------------------------- */
int32_t main(void)
{
uint32_t i;
arm_status status;
arm_linear_interp_instance_f32 S = {188495, -3.141592653589793238, XSPACING, &arm_linear_interep_table[0]};
/*------------------------------------------------------------------------------
* Method 1: Test out Calculated from Cubic Interpolation
*------------------------------------------------------------------------------*/
for(i=0; i< TEST_LENGTH_SAMPLES; i++)
{
testOutput[i] = arm_sin_f32(testInputSin_f32[i]);
}
/*------------------------------------------------------------------------------
* Method 2: Test out Calculated from Cubic Interpolation and Linear interpolation
*------------------------------------------------------------------------------*/
for(i=0; i< TEST_LENGTH_SAMPLES; i++)
{
testLinIntOutput[i] = arm_linear_interp_f32(&S, testInputSin_f32[i]);
}
/*------------------------------------------------------------------------------
* SNR calculation for method 1
*------------------------------------------------------------------------------*/
snr1 = arm_snr_f32(testRefSinOutput32_f32, testOutput, 2);
/*------------------------------------------------------------------------------
* SNR calculation for method 2
*------------------------------------------------------------------------------*/
snr2 = arm_snr_f32(testRefSinOutput32_f32, testLinIntOutput, 2);
/*------------------------------------------------------------------------------
* Initialise status depending on SNR calculations
*------------------------------------------------------------------------------*/
if( snr2 > snr1)
{
status = ARM_MATH_SUCCESS;
}
else
{
status = ARM_MATH_TEST_FAILURE;
}
/* ----------------------------------------------------------------------
** Loop here if the signals fail the PASS check.
** This denotes a test failure
** ------------------------------------------------------------------- */
if( status != ARM_MATH_SUCCESS)
{
while(1);
}
while(1); /* main function does not return */
}
/*=====================================================================================================*/
void Quaternion_ToNumQ( Quaternion *pNumQ, EulerAngle *pAngE )
{
float halfP = pAngE->Pitch/2.0f;
float halfR = pAngE->Roll/2.0f;
float halfY = pAngE->Yaw/2.0f;
float sinP = arm_sin_f32(halfP);
float cosP = arm_cos_f32(halfP);
float sinR = arm_sin_f32(halfR);
float cosR = arm_cos_f32(halfR);
float sinY = arm_sin_f32(halfY);
float cosY = arm_cos_f32(halfY);
pNumQ->q0 = cosY*cosR*cosP + sinY*sinR*sinP;
pNumQ->q1 = cosY*cosR*sinP - sinY*sinR*cosP;
pNumQ->q2 = cosY*sinR*cosP + sinY*cosR*sinP;
pNumQ->q3 = sinY*cosR*cosP - cosY*sinR*sinP;
}
// simply take average on a square patch, not even gaussian approx
static uint8_t mean_intensity(image_t *image, i_image_t *i_image, int kp_x, int kp_y, uint32_t rot, uint32_t point)
{
int pidx = (point/6)%8;
float alpha, beta, theta = 0;
if (rot) {
theta = rot*PI_2/(float)kNB_ORIENTATION; // orientation of the pattern
}
beta = PI/n[pidx]*(pidx%2); // orientation offset so that groups of points on each circles are staggered
alpha = (float)(point%n[pidx])*PI_2/(float)n[pidx]+beta+theta;
float px = radius[pidx] * PATTERN_SCALE * arm_cos_f32(alpha);
float py = radius[pidx] * PATTERN_SCALE * arm_sin_f32(alpha);
float psigma = sigma[pidx] * PATTERN_SCALE;
// get point position in image
float xf = px+kp_x;
float yf = py+kp_y;
int x = (int) xf;
int y = (int) yf;
int imagecols = image->w;
// calculate output:
int ret_val;
if (psigma < 0.5f) {
// interpolation multipliers:
int r_x = (xf-x)*1024;
int r_y = (yf-y)*1024;
int r_x_1 = (1024-r_x);
int r_y_1 = (1024-r_y);
uint8_t* ptr = image->data+(y*imagecols+x);
// linear interpolation:
ret_val = (r_x_1*r_y_1*(int)(*ptr++));
ret_val += (r_x*r_y_1*(int)(*ptr));
ptr += imagecols;
ret_val += (r_x*r_y*(int)(*ptr--));
ret_val += (r_x_1*r_y*(int)(*ptr));
return (ret_val+512)/1024;
} else {
int x_left = (int) (xf-psigma+0.5f);
int y_top = (int) (yf-psigma+0.5f);
int x_right = (int) (xf+psigma+1.5f);//integral image is 1px wider
int y_bottom = (int) (yf+psigma+1.5f);//integral image is 1px higher
ret_val = i_image->data[i_image->w*y_bottom+x_right];//bottom right corner
ret_val -= i_image->data[i_image->w*y_bottom+x_left];
ret_val += i_image->data[i_image->w*y_top+x_left];
ret_val -= i_image->data[i_image->w*y_top+x_right];
ret_val = ret_val/( (x_right-x_left)* (y_bottom-y_top) );
return ret_val;
}
}
void Rotate3dVector(float vector[3], float roll, float pitch, float yaw, float Retorno[3])
{
roll = roll/57.3;
pitch = pitch/57.3;
yaw = yaw/57.3;
float A = roll;
float B = pitch;
float C = yaw;
float cosA, sinA;
float cosB, sinB;
float cosC, sinC;
cosA = arm_cos_f32(A);
sinA = arm_sin_f32(A);
cosB = arm_cos_f32(B);
sinB = arm_sin_f32(B);
cosC = arm_cos_f32(C);
sinC = arm_sin_f32(C);
float RotationMatrix_f32[9] = {cosB*cosC, cosC*sinA*sinB-cosA*sinC, cosA*cosC*sinB+sinA*sinC,
cosB*sinC, cosA*cosC+sinA*sinB*sinC, -cosC*sinA+cosA*sinB*sinC,
-sinB, cosB*cosA, cosA*cosB};
arm_matrix_instance_f32 RotationMatrix;
arm_mat_init_f32(&RotationMatrix, 3, 3, RotationMatrix_f32);
arm_matrix_instance_f32 InVector;
arm_mat_init_f32(&InVector, 3, 1, vector);
arm_matrix_instance_f32 OutVector;
arm_mat_init_f32(&OutVector, 3, 1, Retorno);
arm_mat_mult_f32(&RotationMatrix, &InVector, &OutVector);
}
/**
* \brief Generate a sinus input signal.
*
* \param freq Sinus frequency.
*/
static void dsp_sin_input(float32_t freq)
{
uint32_t i;
uint32_t y;
float32_t ratio;
if (freq < 1) {
freq = 1;
}
ratio = freq / SAMPLING_FREQUENCY;
for (i = 0, y = 0; i < SAMPLE_BLOCK_SIZE; i++) {
/* Amplification is set to 1. */
wav_in_buffer[y++] = arm_sin_f32(2 * PI * i * ratio);
wav_in_buffer[y++] = 0;
}
}
float getValueForCoord(float x, float y, float time) {
// float value = arm_sin_f32(x * 10 + time);
float value =0.0;
value += arm_sin_f32(x * 10 + time);
value += arm_sin_f32(10*(x*arm_sin_f32(time/2) + y*arm_cos_f32(time/3))+time);
float cx = x+0.5*arm_sin_f32(time/5);
float cy = y+0.5*arm_sin_f32(time/3);
value += arm_sin_f32( sqrtf(100 * ((cx*cx)+(cy*cy))+1)+time);
return value;
}
void body_aimming_angle()
{
global_aimming_angle(&(global_yaw),&(global_pitch));
/*transform global vetor form global to body*/
vector_x_fc = vector_y;
vector_y_fc = -vector_x;
vector_z_fc = vector_z;
//printf("%f,%f,%f\r\n",vector_x_fc,vector_y_fc,vector_z_fc);
/*turn body by yaw,pitch,roll*/
vector_body_x = Mq11*vector_x_fc + Mq12*vector_y_fc + Mq13*vector_z_fc;
vector_body_y = Mq21*vector_x_fc + Mq22*vector_y_fc + Mq23*vector_z_fc;
vector_body_z = Mq31*vector_x_fc + Mq32*vector_y_fc + Mq33*vector_z_fc;
//printf("%f,%f,%f\r\n",vector_body_x,vector_body_y,vector_body_z);
//printf("global_yaw,%f,global_pitch,%f\r\n",global_yaw,global_pitch);
body_yaw = (atan2f(vector_body_y,vector_body_x));
body_pitch = toDeg(atan2f(-vector_body_z,(arm_cos_f32(body_yaw) * vector_body_x + arm_sin_f32(body_yaw) * vector_body_y)));
body_yaw = toDeg(body_yaw);
if(body_pitch > 90.0)
{
body_yaw = body_yaw + 180.0;
body_pitch= 180.0 - body_pitch;
}
/* else if(body_pitch < 0.0)
{
body_yaw = body_yaw + 180.0;
body_pitch= -body_pitch;
}
*/
//printf("body_yaw,%f,body_pitch,%f\r\n",body_yaw,body_pitch);
}
void DAC_Ch2_SineWaveConfig(void) {
float32_t xfactor = 2.0 * PI / SIN_TABLE_SIZE;
for (int i = 0; i < SIN_TABLE_SIZE; i++) {
uint16_t ival = arm_sin_f32(i * xfactor) * 2046.0 + 2046.0;
sin_table[i] = ival;
//display_bar(0, 4096, (int) sin_table[i]);
}
DMA_InitTypeDef DMA_InitStructure;
DAC_InitTypeDef DAC_InitStructure;
/* DAC channel1 Configuration */
DAC_InitStructure.DAC_Trigger = DAC_Trigger_T6_TRGO;
DAC_InitStructure.DAC_WaveGeneration = DAC_WaveGeneration_None;
DAC_InitStructure.DAC_OutputBuffer = DAC_OutputBuffer_Enable;
DAC_Init(DAC_Channel_1, &DAC_InitStructure);
/* DMA1_Stream5 channel1 configuration **************************************/
DMA_DeInit(DMA1_Stream5 );
DMA_InitStructure.DMA_Channel = DMA_Channel_7;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) DAC_DHR12R1_ADDRESS;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t) &sin_table;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DMA_InitStructure.DMA_BufferSize = SIN_TABLE_SIZE;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(DMA1_Stream5, &DMA_InitStructure);
DMA_Cmd(DMA1_Stream5, ENABLE);
DAC_Cmd(DAC_Channel_1, ENABLE);
DAC_DMACmd(DAC_Channel_1, ENABLE);
}
/* The main process of FFT.
* Using the butterfly algorithm.
*/
void butterfly()
{
int le, lei, ip, m, f = DATA_LENGTH;
int level, i, j;
struct cmplx u, w, t;
// Calculate the level of butterfly algorithm
for ( level = 1; (f>>=1) != 1; ++level )
;
for ( m = 1; m <= level; ++m )
{
le = 2 << ( m - 1 );
lei = le >> 1;
u.real = 1.0;
u.imag = 0.0;
w.real = arm_cos_f32( PI / lei );
w.imag = -arm_sin_f32( PI / lei );
for ( j = 0; j <= lei - 1; ++j )
{
for ( i = j; i < DATA_LENGTH; i = i + le )
{
ip = i + lei;
t = EE( fft_input[ip], u );
fft_input[ip].real = fft_input[i].real - t.real;
fft_input[ip].imag = fft_input[i].imag - t.imag;
fft_input[i].real = fft_input[i].real + t.real;
fft_input[i].imag = fft_input[i].imag + t.imag;
}
u = EE( u, w );
}
}
} // end of bufferfly()
int32_t main(void)
{
float32_t diff;
uint32_t i;
printf("Starting Test...\n");
for (i=0; i < blockSize; i++)
{
cosOutput = arm_cos_f32(testInput_f32[i]);
printf("Cos %f = %f\n", testInput_f32[i], cosOutput);
sinOutput = arm_sin_f32(testInput_f32[i]);
printf("Sin %f = %f\n", testInput_f32[i], sinOutput);
arm_mult_f32(&cosOutput, &cosOutput, &cosSquareOutput, 1);
printf("Cos squared %f = %f\n", cosOutput, cosSquareOutput);
arm_mult_f32(&sinOutput, &sinOutput, &sinSquareOutput, 1);
printf("Sin squared %f = %f\n", sinOutput, sinSquareOutput);
arm_add_f32(&cosSquareOutput, &sinSquareOutput, &testOutput, 1);
printf("Add %f and %f = %f\n", cosSquareOutput, sinSquareOutput, testOutput);
/* absolute value of difference between ref and test */
diff = fabsf(testRefOutput_f32 - testOutput);
/* Comparison of sin_cos value with reference */
if (diff > DELTA)
{
printf("Diff failure %f\n", diff);
exit(EXIT_FAILURE); /* just for QEMU testing */
while(1);
}
}
printf("Ending Test...\n");
exit(EXIT_SUCCESS); /* just for QEMU testing */
while(1); /* main function does not return */
}
void polarToRect(float m, float a, float32_t* x, float32_t* y)
{
*y = m * arm_sin_f32(a);
*x = m * arm_cos_f32(a);
}
void correct_sensor()
{
float Ellipse[5] = {0};
#define MovegAveFIFO_Size 250
switch (SensorMode) {
/************************** Mode_CorrectGyr **************************************/
case Mode_GyrCorrect:
/* Simple Moving Average */
Gyr.X = (s16)MoveAve_SMA(Gyr.X, GYR_FIFO[0], MovegAveFIFO_Size);
Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, GYR_FIFO[1], MovegAveFIFO_Size);
Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, GYR_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if (Correction_Time == SampleRateFreg) {
Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET); // 角速度為 0dps
Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET); // 角速度為 0dps
Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET); // 角速度為 0dps
Correction_Time = 0;
SensorMode = Mode_AccCorrect;
}
break;
/************************** Mode_CorrectAcc **************************************/
case Mode_AccCorrect:
/* Simple Moving Average */
Acc.X = (s16)MoveAve_SMA(Acc.X, ACC_FIFO[0], MovegAveFIFO_Size);
Acc.Y = (s16)MoveAve_SMA(Acc.Y, ACC_FIFO[1], MovegAveFIFO_Size);
Acc.Z = (s16)MoveAve_SMA(Acc.Z, ACC_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if (Correction_Time == SampleRateFreg) {
Acc.OffsetX += (Acc.X - ACC_X_OFFSET); // 重力加速度為 0g
Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET); // 重力加速度為 0g
Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET); // 重力加速度為 1g
Correction_Time = 0;
SensorMode = Mode_Quaternion; // Mode_MagCorrect;
}
break;
/************************** Mode_CorrectMag **************************************/
#define MagCorrect_Ave 100
#define MagCorrect_Delay 600 // DelayTime : SampleRate * 600
case Mode_MagCorrect:
Correction_Time++;
switch ((u16)(Correction_Time / MagCorrect_Delay)) {
case 0:
MagDataX[0] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[0] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 1:
MagDataX[1] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[1] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 2:
MagDataX[2] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[2] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 3:
MagDataX[3] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[3] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 4:
MagDataX[4] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[4] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 5:
MagDataX[5] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[5] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 6:
MagDataX[6] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[6] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 7:
MagDataX[7] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[7] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
default:
EllipseFitting(Ellipse, MagDataX, MagDataY, 8);
Mag.EllipseSita = Ellipse[0];
Mag.EllipseX0 = Ellipse[1];
Mag.EllipseY0 = Ellipse[2];
Mag.EllipseA = Ellipse[3];
Mag.EllipseB = Ellipse[4];
Correction_Time = 0;
SensorMode = Mode_Quaternion;
break;
}
break;
/************************** Algorithm Mode **************************************/
case Mode_Quaternion:
/* To Physical */
Acc.TrueX = Acc.X * MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y * MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z * MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X * MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y * MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z * MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X * MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y * MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z * MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T * MPU9150T_85degC; // degC/LSB
Ellipse[3] = (Mag.X * arm_cos_f32(Mag.EllipseSita) + Mag.Y * arm_sin_f32(Mag.EllipseSita)) / Mag.EllipseB;
Ellipse[4] = (-Mag.X * arm_sin_f32(Mag.EllipseSita) + Mag.Y * arm_cos_f32(Mag.EllipseSita)) / Mag.EllipseA;
AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
AngE.Roll = toDeg(-asinf(Acc.TrueX));
AngE.Yaw = toDeg(atan2f(Ellipse[3], Ellipse[4])) + 180.0f;
Quaternion_ToNumQ(&NumQ, &AngE);
SensorMode = Mode_Algorithm;
break;
}
}
void svpwm(void) {
uint8_t sector = Theta/_PIdiv3;
float32_t U_ref = hypotf(U_alpha,U_beta);
if (U_ref > U_max) {
U_ref = U_max;
}
float32_t angle = Theta - (sector*_PIdiv3);
float32_t U_ref_percent = (_SQRT3)*(U_ref/_U_DC); // previous: (2/_SQRT3)
float32_t t_1 = U_ref_percent*arm_sin_f32(_PIdiv3-angle)*T_halfsample;
float32_t t_2 = U_ref_percent*arm_sin_f32(angle)*T_halfsample;
float32_t t_0 = T_halfsample - t_1 - t_2;
float32_t t_0_half = t_0/2;
/* Switching counter values for Timer Interrupts */
/* Upper switches */
uint16_t ontime_t_0_half = (t_0_half) * counterfrequency;
uint16_t ontime_value_1 = (t_0_half + t_1) * counterfrequency;
uint16_t ontime_value_2 = (t_0_half + t_2) * counterfrequency;
uint16_t ontime_value_3 = (t_0_half + t_1 + t_2) * counterfrequency;
switch (sector) {
/* Upper switches */
/* Sector 1 */
case 0: switchtime[0] = &ontime_t_0_half;
switchtime[1] = &ontime_value_1;
switchtime[2] = &ontime_value_3;
break;
/* Sector 2 */
case 1: switchtime[0] = &ontime_value_2;
switchtime[1] = &ontime_t_0_half;
switchtime[2] = &ontime_value_3;
break;
/* Sector 3 */
case 2: switchtime[0] = &ontime_value_3;
switchtime[1] = &ontime_t_0_half;
switchtime[2] = &ontime_value_1;
break;
/* Sector 4 */
case 3: switchtime[0] = &ontime_value_3;
switchtime[1] = &ontime_value_2;
switchtime[2] = &ontime_t_0_half;
break;
/* Sector 5 */
case 4: switchtime[0] = &ontime_value_1;
switchtime[1] = &ontime_value_3;
switchtime[2] = &ontime_t_0_half;
break;
/* Sector 6 */
case 5: switchtime[0] = &ontime_t_0_half;
switchtime[1] = &ontime_value_3;
switchtime[2] = &ontime_value_2;
break;
}
}
/*=====================================================================================================*/
void AHRS_Update(void)
{
float tempX = 0, tempY = 0;
float Normalize;
float gx, gy, gz;
// float hx, hy, hz;
// float wx, wy, wz;
// float bx, bz;
float ErrX, ErrY, ErrZ;
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
// float MegX, MegY, MegZ;
float /*Mq11, Mq12, */Mq13,/* Mq21, Mq22, */Mq23,/* Mq31, Mq32, */Mq33;
static float AngZ_Temp = 0.0f;
static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
// Mq11 = NumQ.q0*NumQ.q0 + NumQ.q1*NumQ.q1 - NumQ.q2*NumQ.q2 - NumQ.q3*NumQ.q3;
// Mq12 = 2.0f*(NumQ.q1*NumQ.q2 + NumQ.q0*NumQ.q3);
Mq13 = 2.0f * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
// Mq21 = 2.0f*(NumQ.q1*NumQ.q2 - NumQ.q0*NumQ.q3);
// Mq22 = NumQ.q0*NumQ.q0 - NumQ.q1*NumQ.q1 + NumQ.q2*NumQ.q2 - NumQ.q3*NumQ.q3;
Mq23 = 2.0f * (NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
// Mq31 = 2.0f*(NumQ.q0*NumQ.q2 + NumQ.q1*NumQ.q3);
// Mq32 = 2.0f*(NumQ.q2*NumQ.q3 - NumQ.q0*NumQ.q1);
Mq33 = NumQ.q0 * NumQ.q0 - NumQ.q1 * NumQ.q1 - NumQ.q2 * NumQ.q2 + NumQ.q3 * NumQ.q3;
Normalize = invSqrtf(squa(Acc.TrueX) + squa(Acc.TrueY) + squa(Acc.TrueZ));
AccX = Acc.TrueX * Normalize;
AccY = Acc.TrueY * Normalize;
AccZ = Acc.TrueZ * Normalize;
// Normalize = invSqrtf(squa(Meg.TrueX) + squa(Meg.TrueY) + squa(Meg.TrueZ));
// MegX = Meg.TrueX*Normalize;
// MegY = Meg.TrueY*Normalize;
// MegZ = Meg.TrueZ*Normalize;
gx = Mq13;
gy = Mq23;
gz = Mq33;
// hx = MegX*Mq11 + MegY*Mq21 + MegZ*Mq31;
// hy = MegX*Mq12 + MegY*Mq22 + MegZ*Mq32;
// hz = MegX*Mq13 + MegY*Mq23 + MegZ*Mq33;
// bx = sqrtf(squa(hx) + squa(hy));
// bz = hz;
// wx = bx*Mq11 + bz*Mq13;
// wy = bx*Mq21 + bz*Mq23;
// wz = bx*Mq31 + bz*Mq33;
ErrX = (AccY * gz - AccZ * gy)/* + (MegY*wz - MegZ*wy)*/;
ErrY = (AccZ * gx - AccX * gz)/* + (MegZ*wx - MegX*wz)*/;
ErrZ = (AccX * gy - AccY * gx)/* + (MegX*wy - MegY*wx)*/;
exInt = exInt + ErrX * Ki;
eyInt = eyInt + ErrY * Ki;
ezInt = ezInt + ErrZ * Ki;
GyrX = toRad(Gyr.TrueX);
GyrY = toRad(Gyr.TrueY);
GyrZ = toRad(Gyr.TrueZ);
GyrX = GyrX + Kp * ErrX + exInt;
GyrY = GyrY + Kp * ErrY + eyInt;
GyrZ = GyrZ + Kp * ErrZ + ezInt;
Quaternion_RungeKutta(&NumQ, GyrX, GyrY, GyrZ, SampleRateHelf);
Quaternion_Normalize(&NumQ);
Quaternion_ToAngE(&NumQ, &AngE);
tempX = (Mag.X * arm_cos_f32(Mag.EllipseSita) + Mag.Y * arm_sin_f32(Mag.EllipseSita)) / Mag.EllipseB;
tempY = (-Mag.X * arm_sin_f32(Mag.EllipseSita) + Mag.Y * arm_cos_f32(Mag.EllipseSita)) / Mag.EllipseA;
AngE.Yaw = atan2f(tempX, tempY);
AngE.Pitch = toDeg(AngE.Pitch);
AngE.Roll = toDeg(AngE.Roll);
AngE.Yaw = toDeg(AngE.Yaw) + 180.0f;
/* 互補濾波 Complementary Filter */
#define CF_A 0.9f
#define CF_B 0.1f
AngZ_Temp = AngZ_Temp + GyrZ * SampleRate;
AngZ_Temp = CF_A * AngZ_Temp + CF_B * AngE.Yaw;
if (AngZ_Temp > 360.0f)
AngE.Yaw = AngZ_Temp - 360.0f;
else if (AngZ_Temp < 0.0f)
AngE.Yaw = AngZ_Temp + 360.0f;
else
AngE.Yaw = AngZ_Temp;
}
void vMovingControlTask(void *pvParameters)
{
Position TargetPos;
Position ExpectPos;
MoveCmd SendCmd, ReResult;
float DeltAngle, DeltDistance;
float XYi[3];
while (1)
{
if (pdTRUE == xQueueReceive(TargetPosQueue, &TargetPos, portMAX_DELAY))
{
// printf("get TargetPosQueue\r\n");
while ((fabs(TargetPos.Angle - CurPos.Angle) > 180 ? 360 - fabs(TargetPos.Angle - CurPos.Angle) : fabs(TargetPos.Angle - CurPos.Angle)) * STEP_ANGLE >= ADJUST_STEP_ANGLE\
|| sqrt(pow(TargetPos.y - CurPos.y, 2) + pow(TargetPos.x - CurPos.x, 2)) * STEP_METER >= ADJUST_STEP_DIRECT)
{
// Debug_printf("CurPos: %f %f %f\r\n", CurPos.Angle, CurPos.x, CurPos.y);
// Debug_printf("CurPos: %f %f %f\r\n", CurPos.Angle, CurPos.x, CurPos.y);
// Debug_printf("Compass: %f\r\n", CurAngle);
// Debug_printf("DAngle: %f\r\n", fabs(TargetPos.Angle - CurPos.Angle) * STEP_ANGLE);
// Debug_printf("Distance: %f\r\n", sqrt(pow(TargetPos.y - CurPos.y, 2) + pow(TargetPos.x - CurPos.x, 2)) * STEP_METER);
if (sqrt(pow(TargetPos.y - CurPos.y, 2) + pow(TargetPos.x - CurPos.x, 2)) * STEP_METER >= ADJUST_STEP_DIRECT)
{
ExpectPos.Angle = (atan2(TargetPos.y - CurPos.y, TargetPos.x - CurPos.x)/PI)*180;
// printf("ExpectPos.Angle1 : %f\r\n",ExpectPos.Angle);
ExpectPos.x = CurPos.x;
ExpectPos.y = CurPos.y;
DeltAngle = ExpectPos.Angle - CurPos.Angle;
// Debug_printf("DAngle: %f\r\n", DeltAngle);
if (DeltAngle > 180)
{
DeltAngle = DeltAngle - 360;
}
if(DeltAngle < -180)
{
DeltAngle = 360 + DeltAngle;
}
if (DeltAngle < 0)
{
SendCmd.direct = turnright;
}
else
{
SendCmd.direct = turnleft;
}
SendCmd.step = fabs(DeltAngle) * STEP_ANGLE;
// vTaskDelay(1000);
xQueueSend(MoveCmdQueue, &SendCmd, portMAX_DELAY);
if (pdTRUE == xQueueReceive(MoveRsultQueue, &ReResult, portMAX_DELAY))
{
// Debug_printf("get MoveRsultQueue\r\n");
//CurPos = ExpectPos;
if (SendCmd.direct == turnright)
{
CurPos.Angle = CurPos.Angle - ((float)ReResult.step) / STEP_ANGLE;
}
if (SendCmd.direct == turnleft)
{
CurPos.Angle = CurPos.Angle + ((float)ReResult.step) / STEP_ANGLE;
}
if (CurPos.Angle > 180)
{
CurPos.Angle = CurPos.Angle - 360;
}
if (CurPos.Angle < -180)
{
CurPos.Angle = CurPos.Angle + 360;
}
//??
//????????;
//printf("CurPos.Angle: %f\r\n", CurPos.Angle);
if (ANGLE_CORRECTION_THRESHOLD < fabs(CurPos.Angle - CurAngle))
{
CurPos.Angle = CurAngle;
}
//printf("FIXED CurPos.Angle: %f\r\n", CurPos.Angle);
if(pdTRUE == xQueueReceive(PositionUpdatekQueue, XYi, 2))
{
//????????;
switch (GlobleCmdDir)
{
case forward:
XYi[0] = XYi[0] + ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_cos_f32((CurPos.Angle / 180) * PI);
XYi[1] = XYi[1] + ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_sin_f32((CurPos.Angle / 180) * PI);
break;
case backward:
XYi[0] = XYi[0] - ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_cos_f32((CurPos.Angle / 180) * PI);
XYi[1] = XYi[1] - ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_sin_f32((CurPos.Angle / 180) * PI);
break;
case turnright:
break;
case turnleft:
break;
default:
break;
}
CurPos.x = XYi[0];
CurPos.y = XYi[1];
}
if(pdTRUE == xSemaphoreTake(FoundTargetSyn, 1))
{
// Debug_printf("I am IN 1\r\n");
TargetPos = CurPos;
}
}
}
else
{
ExpectPos.Angle = TargetPos.Angle;
// printf("ExpectPos.Angle2 : %f\r\n",ExpectPos.Angle);
ExpectPos.x = CurPos.x;
ExpectPos.y = CurPos.y;
DeltAngle = ExpectPos.Angle - CurPos.Angle;
if (DeltAngle > 180)
{
DeltAngle = DeltAngle - 360;
}
if(DeltAngle < -180)
{
DeltAngle = 360 + DeltAngle;
}
if (DeltAngle < 0)
{
SendCmd.direct = turnright;
}
else
{
SendCmd.direct = turnleft;
}
SendCmd.step = fabs(DeltAngle) * STEP_ANGLE;
// vTaskDelay(1000);
xQueueSend(MoveCmdQueue, &SendCmd, portMAX_DELAY);
if (pdTRUE == xQueueReceive(MoveRsultQueue, &ReResult, portMAX_DELAY))
{
//CurPos = ExpectPos;MoveRsultQueue
// Debug_printf("get MoveRsultQueue\r\n");
if (SendCmd.direct == turnright)
{
CurPos.Angle = CurPos.Angle - ((float)ReResult.step) / STEP_ANGLE;
}
if (SendCmd.direct == turnleft)
{
CurPos.Angle = CurPos.Angle + ((float)ReResult.step) / STEP_ANGLE;
}
if (CurPos.Angle > 180)
{
CurPos.Angle = CurPos.Angle - 360;
}
if (CurPos.Angle < -180)
{
CurPos.Angle = CurPos.Angle + 360;
}
//??
//????????;
//printf(" else CurPos.Angle: %f\r\n", CurPos.Angle);
if (ANGLE_CORRECTION_THRESHOLD < fabs(CurPos.Angle - CurAngle))
{
CurPos.Angle = CurAngle;
}
//printf("else FIXED CurPos.Angle: %f\r\n", CurPos.Angle);
if(pdTRUE == xQueueReceive(PositionUpdatekQueue, XYi, 2))
{
//????????;
switch (GlobleCmdDir)
{
case forward:
XYi[0] = XYi[0] + ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_cos_f32((CurPos.Angle / 180) * PI);
XYi[1] = XYi[1] + ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_sin_f32((CurPos.Angle / 180) * PI);
break;
case backward:
XYi[0] = XYi[0] - ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_cos_f32((CurPos.Angle / 180) * PI);
XYi[1] = XYi[1] - ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_sin_f32((CurPos.Angle / 180) * PI);
break;
case turnright:
break;
case turnleft:
break;
default:
break;
}
CurPos.x = XYi[0];
CurPos.y = XYi[1];
}
if(pdTRUE == xSemaphoreTake(FoundTargetSyn, 100))
{
// Debug_printf("I am IN 2\r\n");
TargetPos = CurPos;
}
}
}
if ((fabs(ExpectPos.Angle - CurPos.Angle) > 180 ? 360 - fabs(ExpectPos.Angle - CurPos.Angle) : fabs(ExpectPos.Angle - CurPos.Angle)) * STEP_ANGLE < ADJUST_STEP_ANGLE\
&& sqrt(pow(TargetPos.y - CurPos.y, 2) + pow(TargetPos.x - CurPos.x, 2)) * STEP_METER >= ADJUST_STEP_DIRECT)
{
ExpectPos.x = TargetPos.x;
ExpectPos.y = TargetPos.y;
DeltDistance = sqrt(pow(ExpectPos.y - CurPos.y, 2) + pow(ExpectPos.x - CurPos.x, 2));
SendCmd.direct = forward;
SendCmd.step = DeltDistance * STEP_METER;
// vTaskDelay(1000);
// Debug_printf("return1\r\n");
xQueueSend(MoveCmdQueue, &SendCmd, portMAX_DELAY);
if (pdTRUE == xQueueReceive(MoveRsultQueue, &ReResult, portMAX_DELAY))
{
// Debug_printf("return2\r\n");
//CurPos = ExpectPos;
// Debug_printf("get MoveRsultQueue\r\n");
CurPos.x = CurPos.x + (ReResult.step / STEP_METER) * arm_cos_f32((CurPos.Angle / 180) * PI);
CurPos.y = CurPos.y + (ReResult.step / STEP_METER) * arm_sin_f32((CurPos.Angle / 180) * PI);
//??
//????????;
if (ANGLE_CORRECTION_THRESHOLD < fabs(CurPos.Angle - CurAngle))
{
CurPos.Angle = CurAngle;
}
if(pdTRUE == xQueueReceive(PositionUpdatekQueue, XYi, 2))
{
//????????;
switch (GlobleCmdDir)
{
case forward:
XYi[0] = XYi[0] + ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_cos_f32((CurPos.Angle / 180) * PI);
XYi[1] = XYi[1] + ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_sin_f32((CurPos.Angle / 180) * PI);
break;
case backward:
XYi[0] = XYi[0] - ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_cos_f32((CurPos.Angle / 180) * PI);
XYi[1] = XYi[1] - ((float)(ReResult.step - XYi[2]) / STEP_METER) * arm_sin_f32((CurPos.Angle / 180) * PI);
break;
case turnright:
break;
case turnleft:
break;
default:
break;
}
CurPos.x = XYi[0];
CurPos.y = XYi[1];
}
if(pdTRUE == xSemaphoreTake(FoundTargetSyn, 1))
{
// Debug_printf("I am IN 3\r\n");
TargetPos = CurPos;
}
// Debug_printf("return3\r\n");
}
}
}
// Debug_printf("TargetPos: %f %f %f\r\n", CurPos.Angle, CurPos.x, CurPos.y);
// Debug_printf("CurPos: %f %f %f\r\n", CurPos.Angle, CurPos.x, CurPos.y);
// Debug_printf("Compass: %f\r\n", CurAngle);
xSemaphoreGive(MoveComplete);
// printf("Complete1\r\n");
}
}
}
/* Cálculos do seno utilizando as funções disponíveis nas funções de DSP do arm. */
float f_sin(float angle) {
return arm_sin_f32(angle);
}
/* Cálculos do tan utilizando as funções disponíveis nas funções de DSP do arm. */
float f_tan(float angle) {
return (arm_sin_f32(angle)/arm_cos_f32(angle));
}
static void GPS_ISR()
{
TIM_ClearITPendingBit(GPS_TIMER, TIM_IT_Update);
double l1 = (double)GetEncoder1Count() * Encoder1_Fact * Encoder1_DIR;
ClearEncoder1Count();
double l2 = (double)GetEncoder2Count() * Encoder2_Fact * Encoder2_DIR;
ClearEncoder2Count();
#ifndef TWO_ENCODERS
double l3 = (double)GetEncoder3Count() * Encoder3_Fact * Encoder3_DIR;
ClearEncoder3Count();
#endif
static double lastV = 0;
#ifdef TWO_ENCODERS
if ( ABS(l1) < ROTATE_MESURE && ABS(l2) < ROTATE_MESURE)
{
g_isRotate = FALSE;
}
else
{
g_isRotate = TRUE;
}
#else
if ( ABS(l1 - l3) < ROTATE_MESURE )
{
g_isRotate = FALSE;
}
else
{
g_isRotate = TRUE;
}
#endif
static double LastDeg = 0;
double currentDeg = CalculateGyroDeg();
double deltaDeg = currentDeg - LastDeg ;
double degData = (LastDeg + currentDeg);
if (deltaDeg > 180 )
{
deltaDeg -= 360;
degData += 360;
}
else if (deltaDeg < -180)
{
deltaDeg += 360;
degData += 360;
}
g_AnglurSpeed = deltaDeg * 200;
l1 -= deltaDeg * Encoder1_RotateFact;
l2 -= deltaDeg * Encoder2_RotateFact;
float _ts = l1 * l1 + l2 * l2;
arm_sqrt_f32(_ts,&_ts);
g_linerSpeed = _ts * 200;
lastV = g_linerSpeed;
#ifndef TWO_ENCODERS
l3 -= deltaDeg * Encoder3_RotateFact;
l1 = (l1 + l3) / 2.0;
#endif
//计算等效的平移方向
//
degData = degData *0.5f + ANGLE_C;
degData *= DEG2RAD;
LastDeg = currentDeg;
float degCosData, degSinData;
degCosData = arm_cos_f32(degData);
degSinData = arm_sin_f32(degData);
double deltaX = l2 * degCosData - l1 * degSinData ;
double deltaY = l1 * degCosData + l2 * degSinData ;
g_NowlinerSpeedAngle = atan2(deltaY , deltaX) * RAD2DEG;
g_X += deltaX;
g_Y += deltaY;
}
/*=====================================================================================================*/
void SysTick_Handler( void )
{
u8 IMU_Buf[20] = {0};
u16 Final_M1 = 0;
u16 Final_M2 = 0;
u16 Final_M3 = 0;
u16 Final_M4 = 0;
s16 Thr = 0, Pitch = 0, Roll = 0, Yaw = 0;
float Ellipse[5] = {0};
static u8 BaroCnt = 0;
static s16 ACC_FIFO[3][256] = {0};
static s16 GYR_FIFO[3][256] = {0};
static s16 MAG_FIFO[3][256] = {0};
static s16 MagDataX[8] = {0};
static s16 MagDataY[8] = {0};
static u16 Correction_Time = 0;
/* Time Count */
SysTick_Cnt++;
if(SysTick_Cnt == SampleRateFreg) {
SysTick_Cnt = 0;
Time_Sec++;
if(Time_Sec == 60) { // 0~59
Time_Sec = 0;
Time_Min++;
if(Time_Sec == 60)
Time_Min = 0;
}
}
/* 400Hz, Read Sensor ( Accelerometer, Gyroscope, Magnetometer ) */
MPU9150_Read(IMU_Buf);
/* 100Hz, Read Barometer */
BaroCnt++;
if(BaroCnt == 4) {
MS5611_Read(&Baro, MS5611_D1_OSR_4096);
BaroCnt = 0;
}
Acc.X = (s16)((IMU_Buf[0] << 8) | IMU_Buf[1]);
Acc.Y = (s16)((IMU_Buf[2] << 8) | IMU_Buf[3]);
Acc.Z = (s16)((IMU_Buf[4] << 8) | IMU_Buf[5]);
Temp.T = (s16)((IMU_Buf[6] << 8) | IMU_Buf[7]);
Gyr.X = (s16)((IMU_Buf[8] << 8) | IMU_Buf[9]);
Gyr.Y = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
Gyr.Z = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
Mag.X = (s16)((IMU_Buf[15] << 8) | IMU_Buf[14]);
Mag.Y = (s16)((IMU_Buf[17] << 8) | IMU_Buf[16]);
Mag.Z = (s16)((IMU_Buf[19] << 8) | IMU_Buf[18]);
/* Offset */
Acc.X -= Acc.OffsetX;
Acc.Y -= Acc.OffsetY;
Acc.Z -= Acc.OffsetZ;
Gyr.X -= Gyr.OffsetX;
Gyr.Y -= Gyr.OffsetY;
Gyr.Z -= Gyr.OffsetZ;
Mag.X *= Mag.AdjustX;
Mag.Y *= Mag.AdjustY;
Mag.Z *= Mag.AdjustZ;
#define MovegAveFIFO_Size 250
switch(SensorMode) {
/************************** Mode_CorrectGyr **************************************/
case Mode_GyrCorrect:
LED_R = !LED_R;
/* Simple Moving Average */
Gyr.X = (s16)MoveAve_SMA(Gyr.X, GYR_FIFO[0], MovegAveFIFO_Size);
Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, GYR_FIFO[1], MovegAveFIFO_Size);
Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, GYR_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if(Correction_Time == 400) {
Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET); // 角速度為 0dps
Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET); // 角速度為 0dps
Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET); // 角速度為 0dps
Correction_Time = 0;
SensorMode = Mode_MagCorrect; // Mode_AccCorrect;
}
break;
/************************** Mode_CorrectAcc **************************************/
case Mode_AccCorrect:
LED_R = ~LED_R;
/* Simple Moving Average */
Acc.X = (s16)MoveAve_SMA(Acc.X, ACC_FIFO[0], MovegAveFIFO_Size);
Acc.Y = (s16)MoveAve_SMA(Acc.Y, ACC_FIFO[1], MovegAveFIFO_Size);
Acc.Z = (s16)MoveAve_SMA(Acc.Z, ACC_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if(Correction_Time == 400) {
Acc.OffsetX += (Acc.X - ACC_X_OFFSET); // 重力加速度為 0g
Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET); // 重力加速度為 0g
Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET); // 重力加速度為 1g
Correction_Time = 0;
SensorMode = Mode_Quaternion; // Mode_MagCorrect;
}
break;
/************************** Mode_CorrectMag **************************************/
#define MagCorrect_Ave 100
#define MagCorrect_Delay 600 // 2.5ms * 600 = 1.5s
case Mode_MagCorrect:
LED_R = !LED_R;
Correction_Time++;
switch((u16)(Correction_Time/MagCorrect_Delay)) {
case 0:
LED_B = 0;
MagDataX[0] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[0] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 1:
LED_B = 1;
MagDataX[1] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[1] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 2:
LED_B = 0;
MagDataX[2] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[2] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 3:
LED_B = 1;
MagDataX[3] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[3] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 4:
LED_B = 0;
MagDataX[4] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[4] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 5:
LED_B = 1;
MagDataX[5] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[5] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 6:
LED_B = 0;
MagDataX[6] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[6] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 7:
LED_B = 1;
MagDataX[7] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[7] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
default:
LED_B = 1;
EllipseFitting(Ellipse, MagDataX, MagDataY, 8);
Mag.EllipseSita = Ellipse[0];
Mag.EllipseX0 = Ellipse[1];
Mag.EllipseY0 = Ellipse[2];
Mag.EllipseA = Ellipse[3];
Mag.EllipseB = Ellipse[4];
Correction_Time = 0;
SensorMode = Mode_Quaternion;
break;
}
break;
/************************** Algorithm Mode **************************************/
case Mode_Quaternion:
LED_R = !LED_R;
/* To Physical */
Acc.TrueX = Acc.X*MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X*MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y*MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z*MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T*MPU9150T_85degC; // degC/LSB
Ellipse[3] = ( Mag.X*arm_cos_f32(Mag.EllipseSita)+Mag.Y*arm_sin_f32(Mag.EllipseSita))/Mag.EllipseB;
Ellipse[4] = (-Mag.X*arm_sin_f32(Mag.EllipseSita)+Mag.Y*arm_cos_f32(Mag.EllipseSita))/Mag.EllipseA;
AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
AngE.Roll = toDeg(-asinf(Acc.TrueX));
AngE.Yaw = toDeg(atan2f(Ellipse[3], Ellipse[4]))+180.0f;
Quaternion_ToNumQ(&NumQ, &AngE);
SensorMode = Mode_Algorithm;
break;
/************************** Algorithm Mode ****************************************/
case Mode_Algorithm:
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, ACC_FIFO[0], 8);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, ACC_FIFO[1], 8);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, ACC_FIFO[2], 8);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, GYR_FIFO[0], 8);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, GYR_FIFO[1], 8);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, GYR_FIFO[2], 8);
Mag.X = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], 64);
Mag.Y = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], 64);
Mag.Z = (s16)MoveAve_WMA(Mag.Z, MAG_FIFO[2], 64);
/* To Physical */
Acc.TrueX = Acc.X*MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X*MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y*MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z*MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T*MPU9150T_85degC; // degC/LSB
/* Get Attitude Angle */
AHRS_Update();
// if(KEYL_U == 0) { PID_Roll.Kp += 0.001f; PID_Pitch.Kp += 0.001f; }
// if(KEYL_L == 0) { PID_Roll.Kp -= 0.001f; PID_Pitch.Kp -= 0.001f; }
// if(KEYL_R == 0) { PID_Roll.Ki += 0.0001f; PID_Pitch.Ki += 0.0001f; }
// if(KEYL_D == 0) { PID_Roll.Ki -= 0.0001f; PID_Pitch.Ki -= 0.0001f; }
// if(KEYR_R == 0) { PID_Roll.Kd += 0.0001f; PID_Pitch.Kd += 0.0001f; }
// if(KEYR_D == 0) { PID_Roll.Kd -= 0.0001f; PID_Pitch.Kd -= 0.0001f; }
// if(KEYR_L == 0) { PID_Roll.SumErr = 0.0f; PID_Pitch.SumErr = 0.0f; }
// if(KEYL_U == 0) { PID_Yaw.Kp += 0.001f; }
// if(KEYL_L == 0) { PID_Yaw.Kp -= 0.001f; }
// if(KEYL_R == 0) { PID_Yaw.Ki += 0.001f; }
// if(KEYL_D == 0) { PID_Yaw.Ki -= 0.001f; }
// if(KEYR_R == 0) { PID_Yaw.Kd += 0.0001f; }
// if(KEYR_D == 0) { PID_Yaw.Kd -= 0.0001f; }
// if(KEYR_L == 0) { PID_Roll.SumErr = 0.0f; }
/* Get ZeroErr */
PID_Pitch.ZeroErr = (float)((s16)Exp_Pitch/4.5f);
PID_Roll.ZeroErr = (float)((s16)Exp_Roll/4.5f);
PID_Yaw.ZeroErr = (float)((s16)Exp_Yaw)+180.0f;
/* PID */
Roll = (s16)PID_AHRS_Cal(&PID_Roll, AngE.Roll, Gyr.TrueX);
Pitch = (s16)PID_AHRS_Cal(&PID_Pitch, AngE.Pitch, Gyr.TrueY);
// Yaw = (s16)PID_AHRS_CalYaw(&PID_Yaw, AngE.Yaw, Gyr.TrueZ);
Yaw = (s16)(PID_Yaw.Kd*Gyr.TrueZ);
Thr = (s16)Exp_Thr;
/* Motor Ctrl */
Final_M1 = PWM_M1 + Thr + Pitch + Roll + Yaw;
Final_M2 = PWM_M2 + Thr - Pitch + Roll - Yaw;
Final_M3 = PWM_M3 + Thr - Pitch - Roll + Yaw;
Final_M4 = PWM_M4 + Thr + Pitch - Roll - Yaw;
/* Check Connection */
#define NoSignal 1 // 1 sec
if(KEYR_L == 0)
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
else if(((Time_Sec-RecvTime_Sec)>NoSignal) || ((Time_Sec-RecvTime_Sec)<-NoSignal))
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
else
Motor_Control(Final_M1, Final_M2, Final_M3, Final_M4);
/* DeBug */
Tmp_PID_KP = PID_Yaw.Kp*1000;
Tmp_PID_KI = PID_Yaw.Ki*1000;
Tmp_PID_KD = PID_Yaw.Kd*1000;
Tmp_PID_Pitch = Yaw;
break;
}
}
/*
*perform kalman filter at time K, estimate best navigation parameter error at time K
*estimate navigation parameters at current time when acc bias estimation is stable
*/
void vIEKFProcessTask(void* pvParameters)
{
u8 i,j;
u8 acc_bias_stable = 0; //indicate whether acc bias is stably estimated
char logData[100]={0};
ekf_filter filter;
float dt;
float measure[5]={0};
float insBufferK[INS_FRAME_LEN];
float insBufferCur[INS_FRAME_LEN];
float *p_insBuffer;
float direction;
u8 temp;
GPSDataType gdt={0.0,0.0,0.0,0.0,0.0};
portBASE_TYPE xstatus;
PosConDataType pcdt; //position message send to flight control task
/*initial filter*/
filter=ekf_filter_new(9,5,(float *)iQ,(float *)iR,INS_GetA,INS_GetH,INS_aFunc,INS_hFunc);
memcpy(filter->x,x,filter->state_dim*sizeof(float));
memcpy(filter->P,iP,filter->state_dim*filter->state_dim*sizeof(float));
/*capture an INS frame*/
xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY);
/*last number in buffer represent time interval, not time */
p_insBuffer[INDEX_DT]=0.0;
Blinks(LED1,4);
for(;;)
{
/*capture an INS frame*/
xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY);
PutToBuffer(p_insBuffer);
p_insBuffer[INDEX_DT]=0.0;
ReadBufferBack(insBufferK);
dt=insBufferK[INDEX_DT];
/*do INS integration at time K*/
INS_Update(navParamK, insBufferK);
/*do INS integration at current time*/
if(acc_bias_stable)
{
ReadBufferFront(insBufferCur);
INS_Update(navParamCur,insBufferCur);
}
/*predict navigation error at time K*/
EKF_predict(filter
, (void *)(insBufferK+3)
, NULL
, (void *)(&dt)
, (void *)(filter->A)
, NULL);
xstatus=xQueueReceive(xUartGPSQueue,&gdt,0); //get measurement data
if(xstatus == pdPASS)
{
float meas_Err[5]={0.0};
direction = gdt.COG*0.0174533;
temp=(u8)(gdt.Lati*0.01);
measure[0]=(0.01745329*(temp+(gdt.Lati-temp*100.0)*0.0166667)-initPos[0])*6371004;
temp=(u8)(gdt.Long*0.01);
measure[1]=(0.01745329*(temp+(gdt.Long-temp*100.0)*0.0166667)-initPos[1])*4887077;
measure[2]=gdt.Alti-initPos[2];
measure[3]=gdt.SPD*0.51444*arm_cos_f32(direction);
measure[4]=gdt.SPD*0.51444*arm_sin_f32(direction);
for(i=0;i<5;i++)
{
meas_Err[i] = navParamK[i] - measure[i];
}
/*data record*/
/*uncomment this to record data for matlab simulation*/
// sprintf(logData,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",
// insBufferK[0],insBufferK[1],insBufferK[2],
// insBufferK[3],insBufferK[4],insBufferK[5],
// insBufferK[6],insBufferK[7],
// measure[0],measure[1],measure[2],measure[3],measure[4],
// navParamK[3],navParamK[4]);
// xQueueSend(xDiskLogQueue,logData,0);
sprintf(logData, "%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",
measure[0],measure[1],measure[3],measure[4],
navParamK[0],navParamK[1],navParamK[3],navParamK[4],
navParamCur[0],navParamCur[1],navParamCur[3],navParamCur[4]);
xQueueSend(xDiskLogQueue,logData,0);
/*update*/
EKF_update(filter
, (void *)meas_Err
, NULL
, NULL
, (void *)(filter->x)
, NULL);
// printf("%.2f %.2f %.4f %.4f %.4f\r\n",meas_Err[0],meas_Err[1],filter->x[6],filter->x[7],filter->x[8]);
/*
*correct navParamCur
*/
if(acc_bias_stable)
{
for(i=0;i<6;i++)
navParamCur[i] -= filter->x[i];
navParamCur[6] = filter->x[6];
navParamCur[7] = filter->x[7];
navParamCur[8] = filter->x[8];
pcdt.posX = navParamCur[0];
pcdt.posY = navParamCur[1];
pcdt.posZ = navParamCur[2];
pcdt.veloX = navParamCur[3];
pcdt.veloY = navParamCur[4];
pcdt.veloZ = navParamCur[5];
/*put to queue*/
xQueueSend(INSToFlightConQueue,&pcdt,0);
}
/*correct navParameters at time K
*reset error state x*/
for(i=0;i<6;i++)
{
navParamK[i] -= filter->x[i];
filter->x[i] = 0.0;
}
navParamK[6] = filter->x[6];
navParamK[7] = filter->x[7];
navParamK[8] = filter->x[8];
/*when acc bias stable,
*calculate current navigation parameters*/
if(!acc_bias_stable && filter->P[60]<0.007)
{
acc_bias_stable = 1;
//set init value
for(i=0;i<filter->state_dim;i++)
{
navParamCur[i] = navParamK[i];
}
//extrapolate current navigation parameters from time K
for(i=0;i<GPS_DELAY_CNT;i++)
{
if(i+buffer_header >= GPS_DELAY_CNT)
{
for(j=0;j<INS_FRAME_LEN;j++)
insBufferCur[j] = IMU_delay_buffer[(i+buffer_header-GPS_DELAY_CNT)*INS_FRAME_LEN+j];
}
else
{
for(j=0;j<INS_FRAME_LEN;j++)
insBufferCur[j] = IMU_delay_buffer[(i+buffer_header)*INS_FRAME_LEN+j];
}
insBufferCur[0] -= navParamK[6];
insBufferCur[1] -= navParamK[7];
insBufferCur[2] -= navParamK[8];
INS_Update(navParamCur,insBufferCur);
}
Blinks(LED1,1);
}
// printf("%.1f %.1f %.1f %.1f %.1f\r\n",navParamK[0],navParamK[1],navParamK[2],navParamK[3],navParamK[4]);
}
else
{
/*data record*/
sprintf(logData,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",
insBufferK[0],insBufferK[1],insBufferK[2],
insBufferK[3],insBufferK[4],insBufferK[5],
insBufferK[6],insBufferK[7],
0.0,0.0,0.0,0.0,0.0,
navParamK[3],navParamK[4]);
xQueueSend(xDiskLogQueue,logData,0);
}
}
}
