tracker_vio.C

Ir a la documentación de este archivo.

//===========================================================================
//= tracker_vio.cc                                        Noviembre de 1999 =
//=-------------------------------------------------------------------------=
//= Definiciones de la clase TRACKER_VIO.  Manejador del tracker del casco  =
//= de realidad virtual marca Virtual-IO.                                   =
//=-------------------------------------------------------------------------=
//= ADVERTENCIA: ESTE SOFTWARE NO ESTA CONCEBIDO NI DISENNADO PARA EL USO   =
//= EN EQUIPO DE CONTROL EN LINEA EN ENTORNOS PELIGROSOS QUE REQUIERAN UN   =
//= DESEMPENNO LIBRE DE FALLAS, COMO LA OPERACION DE PLANTAS NUCLEARES,     = 
//= SISTEMAS DE NAVEGACION O COMUNICACION EN AVIONES, TRAFICO AEREO,        =
//= EQUIPO MEDICO DEL CUAL DEPENDAN VIDAS HUMANAS O SISTEMAS DE ARMAMENTO,  =
//= EN LOS CUALES UNA FALLA EN EL SOFTWARE PUEDA IMPLICAR DIRECTAMENTE LA   =
//= MUERTE, DANNOS PERSONALES O DANNOS FISICOS Y/O AMBIENTALES GRAVES       =
//= ("ACTIVIDADES DE ALGO RIESGO").                                         =
//=-------------------------------------------------------------------------=
//= Jesus Blanco (de 1997 a Abril de 1998)         je-blanc@uniandes.edu.co =
//= Autor original: Oscar J. Chavarro G.  A.K.A. JEDILINK. Copyright (c),   =
//= 1997 - 2003, oscarchavarro@hotmail.com                                  =
//= AQUYNZA es software libre, y se rige bajo los terminos de la licencia   =
//= LGPL de GNU (http://www.gnu.org). Para mayor informacion respecto a la  =
//= licencia de uso, consulte el archivo ./doc/LICENCIA en la distribucion. =
//===========================================================================

#include "jed_defs.h"

#include <stdio.h>
#include <string.h>
#include "jed_defs.h"
#include "tracker_vio.h"

//===========================================================================
//= Constantes y macros                                                     =
//===========================================================================

#define TAM_BUFFER 32

#define VERIFICAR( p )                                                       \
    if ( !(p) ) {                                                            \
        fprintf(stderr, "<TRACKER_VIO> ERROR: No puedo crear un objeto!\n"); \
        fflush(stderr);                                                      \
        return FALSE;                                                        \
    }

//===========================================================================
//= Funciones utilitarias                                                   =
//===========================================================================

void
TRACKER_VIO_leer_com_callback(void *User_data, int /*accion*/)
{
    int bytes_leidos;
    char buffer_com[256];
    TRACKER_VIO *Tracker = (TRACKER_VIO*)User_data;
    
    //- Lea la entrada del tracker ------------------------------------------
    bytes_leidos = Tracker->Com->leer(buffer_com, 8);
    Tracker->actualizar_entrada(buffer_com, bytes_leidos);    
}

//===========================================================================
//= CLASE TRACKER_VIO                                                       =
//===========================================================================

TRACKER_VIO::TRACKER_VIO()
{
    primiparo = TRUE;
    _yaw = 0;
    _pitch = 0;
    _roll = 0;
    _yaw_ref = 0;
    _pitch_ref = 0;
    _roll_ref = 0;
    Com = NULL;
    memoria = new char[TAM_BUFFER];
    indice_memoria = 0;
}

TRACKER_VIO::~TRACKER_VIO()
{
    cerrar();
    delete memoria;
}

BOOLEAN
TRACKER_VIO::abrir(char *nombre_puerto_serial)
{
    //printf("<TRACKER_VIO> Me abro!\n");

    //- Abra el puerto en modo asincronico ----------------------------------
    Com = new PUERTO_SERIAL(nombre_puerto_serial);
    VERIFICAR(Com);
    Com->configurar(PSV19200, PSP_NONE, PSS_UNO, PSD8, PSF_NONE);
    if ( !Com->abrir() ) {
        fprintf(stderr, "<TRACKER_VIO> ERROR: No puedo abrir el puerto %s.\n",
            nombre_puerto_serial);
        fflush(stderr);
        return FALSE;
    }
    Com->registro_asincronico(TRACKER_VIO_leer_com_callback, this);

    //- Resetiee el tracker -------------------------------------------------
    Com->escribir("!R\n", 3);
    SLEEP_MICROSEC(100000); // De tiempo para que el tracker se resetee
    Com->escribir("!M2,P,B\n", 12);
    return TRUE;
}

void
TRACKER_VIO::cerrar(void)
{
    //printf("<TRACKER_VIO> Me cierro!\n");
    if ( Com ) {
        Com->cerrar();
        delete Com;
        Com = NULL;
    }
}

void
TRACKER_VIO::consultar(double *Yaw, double *Pitch, double *Roll)
{
    if ( !Com ) return;
    //Com->escribir("S", 1);
     
    (*Yaw) = _yaw;
    (*Pitch) = _pitch;
    (*Roll) = _roll;
}

static double TMP_double;

BOOLEAN
TRACKER_VIO::consultar_variable(const char *nombre_variable, int &tipo, void **ref)
{
    double y, p, r;

    consultar(&y, &p, &r);

    if ( strcmp(nombre_variable, "yaw") == 0 ) {
        TMP_double = DEG2RAD(y);
        (*ref) = &TMP_double;
        tipo = T_FLOAT;
      }
      else if ( strcmp(nombre_variable, "pitch") == 0 ) {
        TMP_double = DEG2RAD(p);
        (*ref) = &TMP_double;
        tipo = T_FLOAT;
      }
      else if ( strcmp(nombre_variable, "roll") == 0 ) {
        TMP_double = DEG2RAD(r);
        (*ref) = &TMP_double;
        tipo = T_FLOAT;
      }
      else {
        return FALSE;
      }
    ;
    return TRUE;
}

void
TRACKER_VIO::actualizar_entrada(char *buffer_com, int bytes_leidos)
{
    //-----------------------------------------------------------------------
    int i, j;
    BYTE l;
    BYTE sum;
    int b;
    
    #define F (360.0/16384.0)

    //-----------------------------------------------------------------------
    for ( i = 0; i < bytes_leidos; i++ ) {
        l = (BYTE)buffer_com[i];
        // Si es 0xff esto PUEDE ser un inicio de paquete...
        if ( l == 0xff ) {
            for ( sum = 0, j = indice_memoria - 8;
                  indice_memoria > 7 && j < indice_memoria-1; j++ ) {
                sum = (BYTE)((BYTE)sum + (BYTE)memoria[j]);
            }
            if ( sum == (BYTE)memoria[j] && indice_memoria > 7 ) {
                //- Agarro un paquete valido ----------------------------
                b = j - 6;
                if ( primiparo ) {
                   _yaw_ref = (double)(((memoria[b])<<8)+(memoria[b+1]))*F/2;
                   _pitch_ref = (double)(((memoria[b+2])<<8)+(memoria[b+3]))*F;
                   _roll_ref = -(double)(((memoria[b+4])<<8)+(memoria[b+5]))*F;
                   primiparo = FALSE;
                  }
                  else {
                   _yaw = (double)(((memoria[b])<<8) + (memoria[b+1]))*F/2;
                   _pitch = (double)(((memoria[b+2])<<8) + (memoria[b+3]))*F;
                   _roll = -(double)(((memoria[b+4])<<8) + (memoria[b+5]))*F;
                   _yaw -= _yaw_ref;
                   _pitch -= _pitch_ref;
                   _roll -= _roll_ref;
                }
                
                //- Dejo todo listo para el siguiente paquete -----------
                indice_memoria = 0;
            }
        }
        if ( indice_memoria >= TAM_BUFFER ) {
            fprintf(stderr, "<TRACKER_VIO> ERROR FATAL!\n"
                    "A ocurrido un desfase en la secuencia de datos del\n"
                    "tracker. Puede gritar, rezar o lo que sea, esto es\n"
                    "un error grave.");
            fflush(stderr);
            exit(1);
        }
        memoria[indice_memoria] = buffer_com[i];
        indice_memoria++;
    }

    //-----------------------------------------------------------------------
    Com->escribir("S", 1);
    fflush(stdout);
}

//===========================================================================
//= EOF                                                                     =
//===========================================================================

---

Este archivo HTML ha sido generado automáticamente a partir del código fuente AQUYNZA. NO LO EDITE. Para mayor información contacte al autor.