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/formula.c | 166 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 166 insertions(+) create mode 100644 digital/beacon/src/formula.c (limited to 'digital/beacon/src/formula.c') diff --git a/digital/beacon/src/formula.c b/digital/beacon/src/formula.c new file mode 100644 index 00000000..8391b8f8 --- /dev/null +++ b/digital/beacon/src/formula.c @@ -0,0 +1,166 @@ +/* formula.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 +#include "debug.h" +#include "position.h" +#include "formula.h" + +/* This function computes coords using formula 3 mode ie for beacon 1 + 2*/ +TFormulaStatus formula3_compute_coord(coord_s * position, float angle_beacon1, float angle_beacon2) +{ + DEBUG_FORMULA_INFO("Formula 3 : angle_beacon1 = %f angle_beacon2 = %f\n",angle_beacon1,angle_beacon2); + if((angle_beacon1 < BLIND_ZONE_BEACON1_ANGLE12_MAX) && (angle_beacon2 < BLIND_ZONE_BEACON2_ANGLE12_MAX)) + { + DEBUG_FORMULA_ERROR("BLIND Zone detected\n"); + return FORMULA_BLIND_ZONE_DETECTED; + } + else + { + position->x = LARGEUR_TABLE * tan(angle_beacon2) * tan(angle_beacon1) / (tan(angle_beacon2)+tan(angle_beacon1)); + position->y = LARGEUR_TABLE * tan(angle_beacon1) / (tan(angle_beacon2)+tan(angle_beacon1)); + + if( (position->x < 0) || (position->x > LONGUEUR_TABLE) || (position->y < 0) || (position->y > LARGEUR_TABLE)) + { + DEBUG_FORMULA_ERROR("OUT of the Table\n"); + return FORMULA_OUT_OF_TABLE; + } + else + { + DEBUG_FORMULA_INFO("Formula 3 : X = %d Y = %d\n",position->x,position->y); + return FORMULA_VALID_POSITION; + } + } +} + +/* This function computes coords using formula 4 mode ie for beacon 1 + 3*/ +TFormulaStatus formula4_compute_coord(coord_s * position, float angle_beacon1, float angle_beacon3) +{ + DEBUG_FORMULA_INFO("Formula 4 : angle_beacon1 = %f angle_beacon3 = %f\n",angle_beacon1,angle_beacon3); + + if(angle_beacon3 > M_PI/2) + { + if((angle_beacon1 > BLIND_ZONE_BEACON1_ANGLE13_MIN) && (angle_beacon1 < BLIND_ZONE_BEACON1_ANGLE13_MAX) && ((M_PI - angle_beacon3) > BLIND_ZONE_BEACON3_ANGLE13_MIN) && ((M_PI - angle_beacon3) < BLIND_ZONE_BEACON3_ANGLE13_MAX)) + { + DEBUG_FORMULA_ERROR("BLIND Zone detected\n"); + return FORMULA_BLIND_ZONE_DETECTED; + } + position->y = (LARGEUR_DEMI_TABLE*tan(M_PI - angle_beacon3) - LARGEUR_TABLE*tan(angle_beacon1) + LONGUEUR_TABLE) / (tan(M_PI - angle_beacon3) - tan(angle_beacon1)); + } + else + { + position->y = (LARGEUR_DEMI_TABLE*tan(angle_beacon3) + LARGEUR_TABLE*tan(angle_beacon1)-LONGUEUR_TABLE) / (tan(angle_beacon1) + tan(angle_beacon3)); + } + position->x = (LARGEUR_TABLE - position->y)*tan(angle_beacon1); + + if( (position->x < 0) || (position->x > LONGUEUR_TABLE) || (position->y < 0) || (position->y > LARGEUR_TABLE)) + { + DEBUG_FORMULA_ERROR("OUT of the Table\n"); + return FORMULA_OUT_OF_TABLE; + } + else + { + DEBUG_FORMULA_INFO("Formula 4 : X = %d Y = %d\n",position->x,position->y); + return FORMULA_VALID_POSITION; + } +} + +/* This function computes coords using formula 5 mode ie for beacon 2 + 3*/ +TFormulaStatus formula5_compute_coord(coord_s * position, float angle_beacon2, float angle_beacon3) +{ + DEBUG_FORMULA_INFO("Formula 5 : angle_beacon2 = %f angle_beacon3 = %f\n",angle_beacon2,angle_beacon3); + + if(angle_beacon3 > M_PI/2) + { + position->y = (LONGUEUR_TABLE + LARGEUR_DEMI_TABLE * tan(M_PI - angle_beacon3)) / (tan(angle_beacon2) + tan(M_PI - angle_beacon3)); + } + else + { + if((angle_beacon2 > BLIND_ZONE_BEACON2_ANGLE23_MIN) && (angle_beacon2 < BLIND_ZONE_BEACON2_ANGLE23_MAX) && (angle_beacon3 > BLIND_ZONE_BEACON3_ANGLE23_MIN) && (angle_beacon3 < BLIND_ZONE_BEACON3_ANGLE23_MAX)) + { + DEBUG_FORMULA_ERROR("BLIND Zone detected\n"); + return FORMULA_BLIND_ZONE_DETECTED; + } + position->y = (LARGEUR_DEMI_TABLE*tan(angle_beacon3) - LONGUEUR_TABLE) / (tan(angle_beacon3) - tan(angle_beacon2)); + } + position->x = tan(angle_beacon2) * position->y; + + if( (position->x < 0) || (position->x > LONGUEUR_TABLE) || (position->y < 0) || (position->y > LARGEUR_TABLE)) + { + DEBUG_FORMULA_ERROR("OUT of the Table\n"); + return FORMULA_OUT_OF_TABLE; + } + else + { + DEBUG_FORMULA_INFO("Formula 5 : X = %d Y = %d\n",position->x,position->y); + return FORMULA_VALID_POSITION; + } +} + + +/* This function computes an absolute position using 2 angles from 2 beacons */ +TFormulaStatus formula_compute_position(int formula, int current_beacon_ID, float latest_angle, float current_angle, coord_s * result) +{ + int error = FORMULA_VALID_POSITION; + switch(formula) + { + case 3: + if(current_beacon_ID == 1) + { + error = formula3_compute_coord(result, current_angle, latest_angle); + } + else + { + error = formula3_compute_coord(result, latest_angle, current_angle); + } + break; + case 4: + if(current_beacon_ID == 1) + { + error = formula4_compute_coord(result, current_angle, latest_angle); + } + else + { + error = formula4_compute_coord(result, latest_angle, current_angle); + } + break; + case 5: + if(current_beacon_ID == 2) + { + error = formula5_compute_coord(result, current_angle,latest_angle); + } + else + { + error = formula5_compute_coord(result, latest_angle, current_angle); + } + break; + default: + DEBUG_FORMULA_ERROR("Unknown Formula = %d\r\n",formula); + error = FORMULA_UNKNOWN_FORMULA; + } + return error; +} + + -- cgit v1.2.3