void beacon_calc(void *dummy)
{
static uint8_t a=0;
static int32_t local_rising, local_falling;
static int32_t middle;
static float size = 0;
int32_t local_angle;
int32_t local_dist;
int32_t local_count_diff_rising ;
int32_t local_count_diff_falling ;
int32_t local_beacon_coeff;
int32_t result_x=0;
int32_t result_y=0;
int32_t temp=0;
int32_t edge=0;
//int32_t total_size=0;
uint8_t flags;
//uint8_t i;
int8_t local_valid;
if(a)
LED4_ON();
else
LED4_OFF();
a = !a;
if (falling == -1){
/* 0.5 second timeout */
if (invalid_count < 25)
invalid_count++;
else {
IRQ_LOCK(flags);
beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
IRQ_UNLOCK(flags);
}
return;
}
invalid_count = 0;
IRQ_LOCK(flags);
local_valid = valid_beacon;
local_count_diff_rising = count_diff_rising;
local_count_diff_falling = count_diff_falling ;
local_rising = rising;
local_falling = falling;
local_beacon_coeff = beacon_coeff;
IRQ_UNLOCK(flags);
if (local_valid){
invalid_count = 0;
//BEACON_DEBUG("rising= %ld\t",local_rising);
//BEACON_DEBUG("falling= %ld\r\n",local_falling);
/* recalculate number of pulse by adding the value of the counter, then put value back into motor's round range */
local_rising = ((local_rising + (local_count_diff_rising * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
local_falling = ((local_falling + (local_count_diff_falling * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
//BEACON_DEBUG("rising1= %ld\t",local_rising);
//BEACON_DEBUG("falling1= %ld\r\n",local_falling);
//BEACON_DEBUG("count diff rising= %ld\t",local_count_diff_rising);
//BEACON_DEBUG("count diff falling= %ld\r\n",local_count_diff_falling);
/* if around 360 deg, rising > falling, so invert both and recalculate size and middle */
if(local_falling < local_rising){
temp = local_rising;
local_rising = local_falling;
local_falling = temp;
size = BEACON_STEP_TOUR - local_falling + local_rising;
middle = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR);
edge = local_falling;
}
/* else rising > falling */
else{
size = local_falling - local_rising;
middle = local_rising + (size / 2);
edge = local_rising;
}
//for(i=BEACON_MAX_SAMPLE-1;i>0;i--){
// beacon_sample_size[i] = beacon_sample_size[i-1];
// total_size += beacon_sample_size[i];
//}
//beacon_sample_size[0] = size;
//total_size += size;
//total_size /= BEACON_MAX_SAMPLE;
//BEACON_DEBUG("rising2= %ld\t",local_rising);
//BEACON_DEBUG("falling2= %ld\r\n",local_falling);
/* BEACON_DEBUG("size= %ld %ld\t",size, total_size); */
BEACON_DEBUG("size= %f\r\n",size);
//BEACON_DEBUG("middle= %ld\r\n",middle);
local_angle = get_angle(middle,0);
BEACON_NOTICE("opponent angle= %ld\t",local_angle);
local_dist = get_dist(size);
BEACON_NOTICE("opponent dist= %ld\r\n",local_dist);
beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y);
IRQ_LOCK(flags);
beacon.opponent_x = result_x;
beacon.opponent_y = result_y;
beacon.opponent_angle = local_angle;
beacon.opponent_dist = local_dist;
/* for I2C test */
//beacon.opponent_x = OPPONENT_POS_X;
//beacon.opponent_y = OPPONENT_POS_Y;
IRQ_UNLOCK(flags);
BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x);
BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y);
}
else {
BEACON_NOTICE("non valid\r\n\n");
}
falling = -1;
}
void led4_on()
{
LED4_ENABLE(); LED4_ON();
}