summaryrefslogtreecommitdiffhomepage
path: root/digital/beacon/src/position.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/beacon/src/position.c')
-rw-r--r--digital/beacon/src/position.c155
1 files changed, 155 insertions, 0 deletions
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