Return to ar_fixangles.c CVS log

Jump to this file's LXR Page

Up to [CENS] / emstar / devel / loc / ar

fixed bug in AOA estimation:
simple_mnax() was finding max abs() of xcorrelation, not max.

#include <math.h>
#include <stdio.h>	
#include <stdlib.h>	

float anglewrap(float d)
{
  if (d < 0) d += 360.0;
  if (d >= 360.0) d -= 360.0;
  return d;
}

float r2d(float r)
{
  float d = r*180.0/M_PI;
  return anglewrap(d);
}

float iterate(float laser_off, float mX, float mY, float max)
{
  float theta = 0;			   
  float last_theta = 0;
  
  do {
    float hx = laser_off * cos(theta + M_PI_2);
    float hy = laser_off * sin(theta + M_PI_2);
    last_theta = theta;
    theta = atan2((max/2.0)-mY-hy, (max/2.0)-mX-hx);
    //printf("#theta = %f, x,hx=%f,%f, y,hy=%f,%f\n", r2d(theta), (max/2.0)-mX, hx, (max/2.0)-mY, hy);
  } while (fabs(theta - last_theta) > 0.00001);
  
  return r2d(theta);
}


float f2m(float f) 
{
  return 0.3048*f;
}

void convert(float feet, float *mx, float *my)
{
  if (feet<=24) {
    *mx = f2m(24);
    *my = f2m(feet);
  }
  else if (feet<=49) {
    *mx = f2m(24.0-(feet-25.0));
    *my = f2m(24);    
  }
  else if (feet<=74) {
    *my = f2m(24.0-(feet-50.0));    
    *mx = 0;
  }
  else {
    *mx = f2m(feet-75.0);    
    *my = 0;
  }
}


int main(int argc, char **argv)
{
  while (1) {
    int sender;
    int id;
    float x,y,z,temp,rh;
    char note[20];
    note[0] = 0;
    int status = scanf("%d %u %f %f %f %f %f %s", 
		       &(sender),
		       &(id),
		       &(x),
		       &(y),
		       &(z),
		       &(temp),
		       &(rh),
		       note);

    if (status != 8) return 0;

    /*
     *  assumes this setup:
     *

Emitter is E
Receiver is R
measurements are in feet from one contiguous tape measure
at each corner 1 foot is used to loop over to continue on the
next foot.

   99    (mx)     75  (0,0)
 0  ---------------E 74
   |               |
   |               |
   |       R       | (my)
   |               |
   |               |
24 |_______________| 50
    25           49

        ^
        |
    ____|
   |    L|
  0|  * L|180  (laser side)
   |____L|
   |XXXXX|
     90
   (board 
    side)

     */

#define LASER_OFFSETX 0.095
#define LASER_OFFSETZ 0.03

    float mX,mY;

    float angle = 0;
    if (x) {
      convert(x, &mX, &mY);
      angle = anglewrap(270.0 + (45.0 - iterate(LASER_OFFSETX, mX, mY, f2m(24))));
    }
    else if (y) {
      convert(y, &mX, &mY);
      angle = anglewrap(270.0 + (45.0 - iterate(LASER_OFFSETX, mX, mY, f2m(24))));
    }
    else if (z) {
      convert(z, &mX, &mY);
      angle = anglewrap((iterate(LASER_OFFSETZ, mX, mY, f2m(24)) - 45.0));
      if (angle > 90.0 && angle < 270.0) angle = anglewrap(180.0 - angle);
    }

    printf("%d %u %f %f %f %f %f %s\n", 
	   sender, id, angle, 0.0, 0.0, temp, rh, note);
  }
  return 0;
}