/**
* \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 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;
// }
}
void process_tilt_correction(APP_Attitude_I& attitude){
float pitch_rad = attitude.pitch*(2.0*M_PI/360.0);
float roll_rad = attitude.roll*(2.0*M_PI/360.0);
float tilt_correction = throttle / (arm_cos_f32(pitch_rad*0.8) * arm_cos_f32(roll_rad*0.8));
tilt_correction = contrain((tilt_correction - throttle), 0, 50);
throttle += tilt_correction;
}
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;
}
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 */
}
/*=====================================================================================================*/
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;
}
float32_t benchAngle(drv_accelData_t *data) {
int forCounter;
float32_t temp;
int angle;
for (forCounter = 0; forCounter < 91; forCounter++) {
temp = arm_cos_f32(forCounter);
if (temp >= (data.x - 0.2) && temp <= (data.x + 0.2)) {
angle = forCounter;
}
}
}
// 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);
}
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;
}
int main()
{
int n;
gpio_set_mode(P2_10, Output);
for(n=0 ; n<N ; n++)
{
samples[2*n] = arm_cos_f32(2*PI*TESTFREQ*n/SAMPLING_FREQ);
samples[2*n+1] = 0.0;
}
gpio_set(P2_10, HIGH);
arm_cfft_f32(&arm_cfft_sR_f32_len128, samples, 0, 1);
gpio_set(P2_10, LOW);
while(1){}
}
void FiltIntInit(FiltInt * flt )
{
int index =0;
double cosres;
//Расчет коэффициентов косинусной части алгортма Фурье для первой гармоники
for (index = 0; index < DEV_PeriodSamples; index++)
{
#ifndef MSVC
cosres = arm_cos_f32(2.0f * MATH_Pi / (float)DEV_PeriodSamples*(float)index)*((int64_t)1<<32);
#else
cosres = cos(2.0f * MATH_Pi / (float)DEV_PeriodSamples*(float)index)*((int64_t)1<<32);
#endif
flt->filtCoef[index] = (int64_t)cosres;
}
}
void IMU::computePitchRollTilt(void) {
float32_t arg1;
float32_t sqrt_result;
float32_t acc[3];
acc[0] = accelerometer.axisInMPerSsquared[0];
acc[1] = accelerometer.axisInMPerSsquared[1];
acc[2] = accelerometer.axisInMPerSsquared[2];
//Normalise measurement:
arg1 = (float32_t) (acc[0]) * (acc[0]) + (acc[1]) * (acc[1]) + (acc[2]) * (acc[2]);
arm_sqrt_f32(arg1, &sqrt_result);
acc[0] /= sqrt_result;
acc[1] /= sqrt_result;
acc[2] /= sqrt_result;
int8_t signZ;
const float32_t mi = 0.01;
//Compute angles:
//wzor jest nieprawdziwy, gdy z=0 i y=0. nie ma jednego dobreg rozwiazania. Mozna np. aproksymowac w ten sposob:
if (acc[2] > 0) {
signZ = 1;
} else {
signZ = -1;
}
arg1 = (acc[2]) * (acc[2]) + mi * (acc[0]) * (acc[0]);
arm_sqrt_f32(arg1, &sqrt_result);
XRollAngleInRad = atan2(acc[1], signZ * sqrt_result);
arg1 = 0;
arg1 += (float32_t) (acc[2]) * (float32_t) (acc[2]);
arg1 += (float32_t) (acc[1]) * (float32_t) (acc[1]);
arm_sqrt_f32(arg1, &sqrt_result);
YPitchAngleInRad = atan2(-(acc[0]), sqrt_result);
//YPitchAngleInRad_2 = arm_sin_f32((float32_t)(-acc[0])/(float32_t)(g));
arg1 = 0;
arg1 += (float32_t) (acc[0]) * (float32_t) (acc[0]);
arg1 += (float32_t) (acc[1]) * (float32_t) (acc[1]);
arg1 += (float32_t) (acc[2]) * (float32_t) (acc[2]);
arm_sqrt_f32(arg1, &sqrt_result);
TiltAngleInRad = arm_cos_f32((acc[2]) / sqrt_result);
}
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);
}
/* 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 */
}
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;
}
CCM_FUNC static THD_FUNCTION(ThreadKnock, arg)
{
(void)arg;
chRegSetThreadName("Knock");
q15_t* knockDataPtr;
size_t knockDataSize;
float32_t maxValue = 0;
uint32_t maxIndex = 0;
uint16_t i;
/* ADC 3 Ch1 Offset. -2048 */
ADC3->OFR1 = ADC_OFR1_OFFSET1_EN | ((1 << 26) & ADC_OFR1_OFFSET1_CH) | (2048 & 0xFFF);
dacPutChannelX(&DACD1, 0, 2048); // This sets the offset for the knock ADC opamp.
chThdSleepMilliseconds(200);
adcStartConversion(&ADCD3, &adcgrpcfg_knock, samples_knock, ADC_GRP2_BUF_DEPTH);
/* Initialize the CFFT/CIFFT module */
arm_rfft_fast_instance_f32 S1;
arm_rfft_fast_init_f32(&S1, FFT_SIZE);
while (TRUE)
{
while (!recvFreeSamples(&knockMb, (void*)&knockDataPtr, &knockDataSize))
chThdSleepMilliseconds(2);
/* Copy and convert ADC samples */
for (i=0; i<FFT_SIZE*2; i+=4)
{
/* Hann Window */
float32_t multiplier = (1.0 - arm_cos_f32((2.0*PI*(float32_t)i)/(((float32_t)FFT_SIZE*2.0)-1.0)));
input[i] = multiplier*(float32_t)knockDataPtr[i];
input[i+1] = multiplier*(float32_t)knockDataPtr[i+1];
input[i+2] = multiplier*(float32_t)knockDataPtr[i+2];
input[i+3] = multiplier*(float32_t)knockDataPtr[i+3];
}
/* Process the data through the RFFT module */
arm_rfft_fast_f32(&S1, input, output, 0);
/* Process the data through the Complex Magnitude Module for
calculating the magnitude at each bin */
arm_cmplx_mag_f32(output, mag_knock, FFT_SIZE/2); // Calculate magnitude, outputs q2.14
arm_max_f32(mag_knock, FFT_SIZE/2, &maxValue, &maxIndex); // Find max magnitude
// Convert 2.14 to 8 Bits unsigned
for (i=0; i < sizeof(output_knock); i++)
{
uint16_t tmp = (mag_knock[i]/16384);
if (tmp > 0xFF)
tmp = 0xFF;
output_knock[i] = tmp; // 8 bits minus the 2 fractional bits
}
sensors_data.knock_freq = settings.knockFreq;
if (settings.sensorsInput == SENSORS_INPUT_TEST) {
sensors_data.knock_value = rand16(0, 255);
continue;
}
sensors_data.knock_value = calculateKnockIntensity(
settings.knockFreq,
settings.knockRatio,
FFT_FREQ,
output_knock,
sizeof(output_knock));
}
return;
}
inline void hamming_init()
{
uint16_t i;
for(i = 0; i < FRAME_SIZE; ++i)
Hamming[i] = 0.54-0.46*arm_cos_f32((2*PI*i)/(FRAME_SIZE-1));
}
void populateCoeficients(int bandwidth, int sideband, int offset)
{
//Chapter 17 of DSP Guide* //TODO: Make a bibliography!
//1. Take as input, desired filter response in array, both magnitude and phase (it's okay for phase to be zero)
// Looks like magnitude can be any non-negative value. First and last values of Phase must be zero.
//2. Convert to rectangular form. ***I really wish there was a built in function for this :<
//3. Run through an inverse FFT.
//4. Shift
//5. Truncate
//6. Window
//7. Reverse the FFT in preparation for FFT Convolution?
uint16_t filterKernelLength = 100; //what's a good value? How does it relate to the FFT size?
//1:
//sideband: 0 = LSB, 1 = USB, 2 = Both (AM)
//I think the code below is all wrong. At least for LSB, if the magnitude is zero, then phase doesn't matter, yeah?
if(sideband > 2) return; //Error
int i;
for(i = 0; i < FFT_BUFFER_SIZE; i++)
{
switch(sideband)
{
case 0:
if((i > FFT_BUFFER_SIZE - (offset + bandwidth)) && (i < FFT_BUFFER_SIZE - offset))
fftFilterCoeficient[i] = 1;
else
fftFilterCoeficient[i] = 0;
break;
case 1:
if((i > offset) && (i < offset + bandwidth))
fftFilterCoeficient[i] = 1;
else
fftFilterCoeficient[i] = 0;
break;
case 2:
if(((i > FFT_BUFFER_SIZE - (offset + bandwidth)) && (i < FFT_BUFFER_SIZE - offset))
|| ((i > offset) && (i < offset + bandwidth)))
fftFilterCoeficient[i] = 1;
else
fftFilterCoeficient[i] = 0;
break;
}
}
fftFilterCoeficient[FFT_BUFFER_SIZE / 2] = 0;
fftFilterCoeficient[FFT_BUFFER_SIZE - 1] = 0;
//return; //Skipping all the later stuff doesn't seem to make a huge difference yet...
//2:
// float x, y;
// for(i = 0; i < FFT_SIZE; i++)
// {
// polarToRect(fftFilterCoeficient[i], fftFilterCoeficient[FFT_BUFFER_SIZE - 1 - i], &x, &y);
// fftFilterCoeficient[i] = x;
// fftFilterCoeficient[FFT_BUFFER_SIZE - 1 - i] = y;
// }
//3:
arm_cfft_radix4_instance_f32 fft_co;
arm_cfft_radix4_init_f32(&fft_co, FFT_SIZE, 1, 1);
arm_cfft_radix4_f32(&fft_co, fftFilterCoeficient);
//4:
int index;
for (i = 0; i < FFT_BUFFER_SIZE; i++)
{
index = i + filterKernelLength/2;
if(index > FFT_BUFFER_SIZE - 1) index = index - FFT_BUFFER_SIZE;
filterTemp[index] = fftFilterCoeficient[i];
}
for(i = 0; i < FFT_BUFFER_SIZE; i++)
{
fftFilterCoeficient[i] = filterTemp[i];
}
//5 & 6:
for(i = 0; i < FFT_BUFFER_SIZE; i++)
{
if(i <= filterKernelLength) fftFilterCoeficient[i] =
fftFilterCoeficient[i] * (0.54 - 0.46 * arm_cos_f32(2.0 * 3.14159265 * i / filterKernelLength));
if(i > filterKernelLength) fftFilterCoeficient[i] = 0;
}
// arm_cfft_radix4_instance_f32 fft_co;
arm_cfft_radix4_init_f32(&fft_co, FFT_SIZE, 0, 1);
arm_cfft_radix4_f32(&fft_co, fftFilterCoeficient);
// for(i = 0; i < FFT_SIZE; i++)
// {
// filterTemp[i] = fftFilterCoeficient[i * 2];
// filterTemp[FFT_BUFFER_SIZE - 1 - i] = fftFilterCoeficient[i * 2 + 1];
// }
//
// for(i = 0; i < FFT_BUFFER_SIZE; i++)
// {
// fftFilterCoeficient[i] = filterTemp[i];
// }
}
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 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 EllipseFitting(float *Ans, volatile s16 *MagDataX, volatile s16 *MagDataY, u8 Num)
{
s8 i, j, k;
float temp, temp1, temp2;
float tempArrX[8] = {0};
float tempArrY[8] = {0};
float MAG_X1Y0 = 0.0f;
float MAG_X2Y0 = 0.0f;
float MAG_X3Y0 = 0.0f;
float MAG_X0Y1 = 0.0f;
float MAG_X0Y2 = 0.0f;
float MAG_X0Y3 = 0.0f;
float MAG_X0Y4 = 0.0f;
float MAG_X1Y1 = 0.0f;
float MAG_X2Y1 = 0.0f;
float MAG_X1Y2 = 0.0f;
float MAG_X1Y3 = 0.0f;
float MAG_X2Y2 = 0.0f;
float MAG_X3Y1 = 0.0f;
float MagArr[5][6] = {{0}};
for (i = 0; i < Num; i++) {
tempArrX[i] = (float)MagDataX[i] / 1000.0f;
tempArrY[i] = (float)MagDataY[i] / 1000.0f;
}
for (i = 0; i < Num; i++) {
MAG_X1Y0 += tempArrX[i];
MAG_X2Y0 += tempArrX[i] * tempArrX[i];
MAG_X3Y0 += tempArrX[i] * tempArrX[i] * tempArrX[i];
MAG_X0Y1 += tempArrY[i];
MAG_X0Y2 += tempArrY[i] * tempArrY[i];
MAG_X0Y3 += tempArrY[i] * tempArrY[i] * tempArrY[i];
MAG_X0Y4 += tempArrY[i] * tempArrY[i] * tempArrY[i] * tempArrY[i];
MAG_X1Y1 += tempArrX[i] * tempArrY[i];
MAG_X2Y1 += tempArrX[i] * tempArrX[i] * tempArrY[i];
MAG_X1Y2 += tempArrX[i] * tempArrY[i] * tempArrY[i];
MAG_X1Y3 += tempArrX[i] * tempArrY[i] * tempArrY[i] * tempArrY[i];
MAG_X2Y2 += tempArrX[i] * tempArrX[i] * tempArrY[i] * tempArrY[i];
MAG_X3Y1 += tempArrX[i] * tempArrX[i] * tempArrX[i] * tempArrY[i];
}
MagArr[0][0] = MAG_X2Y2;
MagArr[0][1] = MAG_X1Y3;
MagArr[0][2] = MAG_X2Y1;
MagArr[0][3] = MAG_X1Y2;
MagArr[0][4] = MAG_X1Y1;
MagArr[0][5] = -MAG_X3Y1;
MagArr[1][0] = MAG_X1Y3;
MagArr[1][1] = MAG_X0Y4;
MagArr[1][2] = MAG_X1Y2;
MagArr[1][3] = MAG_X0Y3;
MagArr[1][4] = MAG_X0Y2;
MagArr[1][5] = -MAG_X2Y2;
MagArr[2][0] = MAG_X2Y1;
MagArr[2][1] = MAG_X1Y2;
MagArr[2][2] = MAG_X2Y0;
MagArr[2][3] = MAG_X1Y1;
MagArr[2][4] = MAG_X1Y0;
MagArr[2][5] = -MAG_X3Y0;
MagArr[3][0] = MAG_X1Y2;
MagArr[3][1] = MAG_X0Y3;
MagArr[3][2] = MAG_X1Y1;
MagArr[3][3] = MAG_X0Y2;
MagArr[3][4] = MAG_X0Y1;
MagArr[3][5] = -MAG_X2Y1;
MagArr[4][0] = MAG_X1Y1;
MagArr[4][1] = MAG_X0Y2;
MagArr[4][2] = MAG_X1Y0;
MagArr[4][3] = MAG_X0Y1;
MagArr[4][4] = Num;
MagArr[4][5] = -MAG_X2Y0;
for (i = 0; i < 5; i++)
for (j = i + 1; j < 6; j++)
for (k = 5; k > i - 1; k--)
MagArr[j][k] = MagArr[j][k] - MagArr[j][i] / MagArr[i][i] * MagArr[i][k];
for (i = 0; i < 5; i++) {
temp = MagArr[i][i];
for (j = i; j < 6; j++)
MagArr[i][j] = MagArr[i][j] / temp;
}
for (j = 4; j > 0; j--)
for (i = 0; i < j; i++)
MagArr[i][5] = MagArr[i][5] - MagArr[i][j] * MagArr[j][5];
temp = (1.0f - MagArr[1][5]) / MagArr[0][5];
temp1 = temp + sqrtf(temp * temp + 1.0f);
Ans[0] = atanf(temp1); // Theta
temp = MagArr[0][5] * MagArr[0][5] - 4 * MagArr[1][5];
Ans[1] = (2.0f * MagArr[1][5] * MagArr[2][5] - MagArr[0][5] * MagArr[3][5]) / temp; // X0
Ans[2] = (2.0f * MagArr[3][5] - MagArr[0][5] * MagArr[2][5]) / temp; // Y0
temp = arm_cos_f32(Ans[0]);
temp2 = (Ans[1] * Ans[1] + MagArr[0][5] * Ans[1] * Ans[2] + MagArr[1][5] * Ans[2] * Ans[2] - MagArr[4][5]) * (temp1 * temp1 * temp1 * temp1 - 1.0f);
Ans[3] = temp / sqrtf((MagArr[1][5] * temp1 * temp1 - 1) / temp2); // a
Ans[4] = temp / sqrtf((temp1 * temp1 - MagArr[1][5]) / temp2); // b
Ans[1] = Ans[1] * 1000.0f;
Ans[2] = Ans[2] * 1000.0f;
Ans[3] = Ans[3] * 1000.0f;
Ans[4] = Ans[4] * 1000.0f;
}
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 cos utilizando as funções disponíveis nas funções de DSP do arm. */
float f_cos(float angle) {
return arm_cos_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));
}
/* Cálculos do sec utilizando as funções disponíveis nas funções de DSP do arm. */
float f_sec(float angle) {
return (1/arm_cos_f32(angle));
}
/*=====================================================================================================*/
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;
}
}
