From 8ac7f52728a8ec2a09a6eeebc3cf5d4afff9b485 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Tue, 13 Mar 2012 22:50:23 +0100 Subject: digital/beacon, digital/lol: beacon folders reorganisation --- digital/beacon/src/position.c | 155 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 155 insertions(+) create mode 100644 digital/beacon/src/position.c (limited to 'digital/beacon/src/position.c') diff --git a/digital/beacon/src/position.c b/digital/beacon/src/position.c new file mode 100644 index 00000000..ca98b7b5 --- /dev/null +++ b/digital/beacon/src/position.c @@ -0,0 +1,155 @@ +/* position.c */ +/* Beacon triangulation algorithms. {{{ + * + * Copyright (C) 2011 Florent Duchon + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#include "position.h" +#include "debug.h" +#include "recovery.h" +#include "update.h" +#include "formula.h" +#include "trust.h" + +beacon_s beacon[MAX_BEACON+1]; +opponent_s opponent[MAX_OBSTACLE+1]; + +/* This function is used to initialize all needed structures */ +void init_struct(void) +{ + int i = 0; + int j = 0; + for(i = 1; i <= MAX_BEACON; i++) + { + beacon[i].angleNumber = 0; + for(j = 1; j <= MAX_OBSTACLE ; j++) + { + beacon[i].angle[j] = 1; + } + } + + for(i = 1; i <= MAX_OBSTACLE; i++) + { + opponent[i].x = 0; + opponent[i].y = 0; + opponent[i].trust = TRUST_MIN; + } +} + +int update_position(int beaconID, int angleID, double angle) +{ + static int last_ID[2] = {0}; + int last_valid_id = 0; + int which_formula = 0; + coord_s temp_position[MAX_TEMP_POSITION]; + int i = 0; + + int formula_status = 0; + int global_status = 0; + int update_status = UPDATE_OBSTACLE_NOT_FOUND; + int recovery_status = 0; + + DEBUG_POSITION("Update_position with beaconID = %d and angleID = %d and angle = %f\n",(int)beaconID,(int) angleID, (double)angle); + DEBUG_POSITION("last_ID[0] = %d last_ID[1] = %d\n",(int)last_ID[0],(int)last_ID[1]); + + /* Calculate which formula need to be used to compute position */ + for(i = 0 ; i < 2; i++) + { + if(beaconID != last_ID[i]) + { + last_valid_id = last_ID[i]; + } + } + which_formula = beaconID + last_valid_id; + + + if(last_valid_id != 0) + { + if(trust_check_level() == TRUST_LEVEL_OK) + { + /* Compute all hypotheticals positions and save them into temporary_position tab*/ + for(i = 1 ; i <= beacon[last_valid_id].angleNumber ; i++) + { + formula_status = formula_compute_position(which_formula,beaconID,beacon[last_valid_id].angle[i],angle,&temp_position[i]); + if(formula_status == FORMULA_VALID_POSITION) + { + update_status += update(&temp_position[i]); + if(update_status == UPDATE_OBSTACLE_FOUND) + { + break; + } + } + } + if(update_status == UPDATE_OBSTACLE_NOT_FOUND) + { + /* Obstacle not found */ + trust_decrease(); + } + global_status = POSITION_NO_ERROR; + } + else /* Need Recovery */ + { + /* Compute all hypotheticals positions and save them into temporary_position tab*/ + for(i = 1 ; i <= beacon[last_valid_id].angleNumber ; i++) + { + if(beacon[last_valid_id].angle[i] != IGNORE_ANGLE) + { + formula_status = formula_compute_position(which_formula,beaconID,beacon[last_valid_id].angle[i],angle,&temp_position[i]); + if(formula_status == FORMULA_VALID_POSITION) + { + /* If the angle is not ignored and the computed position is valid, feed the recovery system */ + recovery_status = recovery(&temp_position[i],&opponent[0]); + if((recovery_status == RECOVERY_IN_PROGRESS)||(recovery_status == RECOVERY_FINISHED)) + { + global_status = POSITION_NO_ERROR; + break; + } + else if(recovery_status == RECOVERY_IGNORE_ANGLE_NEXT_TIME) + { + global_status = POSITION_IGNORE_ANGLE; + break; + } + } + } + } + } + } + + /* Save angle context */ + beacon[beaconID].angleNumber = angleID; + if(global_status == POSITION_NO_ERROR) + { + beacon[beaconID].angle[angleID] = angle; + } + else /* Angle must be ignored next time */ + { + beacon[beaconID].angle[angleID] = IGNORE_ANGLE; + } + + /* Save ID context */ + if(beaconID != last_valid_id) + { + last_ID[1] = last_ID[0]; + last_ID[0] = beaconID; + } + return 0; +} \ No newline at end of file -- cgit v1.2.3