Return to ar_offline.c CVS log

Jump to this file's LXR Page

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

added several features

* fixed estimation of noise floor, added normalization after clip extraction
* fixed confidence values to be against the better noise estimates
* recombination scaled by SNR.. (is this good?)
* added code to get noise stats

/*
 *
 * Copyright (c) 2005 The Regents of the University of California.  All 
 * rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * - Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * - Neither the name of the University nor the names of its
 *   contributors may be used to endorse or promote products derived
 *   from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <ar.h>
#include <emrun/emrun.h>

/*
 * ar_offline
 *
 * offline processing of ar data
 */


int we_are_sender = 0;
int recomp_sender = 0;
int writeback = 0;
int autodump = 0;
int use_map_locations = 0;
int do_sender_anyways = 0;

void usage(char *name)
{
  misc_print_usage
    (name, 
     "",
     "  --codec_offset <codec>: bump specified codec data forward one sample\n"
     "  --use_map: use the map locations not the locations in the sendlog\n"
     "  --do_expt <number>: only do this experiment number\n"
     "  --dump: dump plot files to analysis/  \n"
     "  --do_sender_anyways: fully compute sender as if receiver  \n"
     "  --first_swing: use first swing peak detection  \n"
     "  --recomp_sender: recompute the sender to correct the offset  \n"
     );
  exit(1);
}

QUEUE_FUNCTION_INSTANTIATIONS(ar_chirp_list,_,chirps,chirp_record_t,ar_state_t);

struct node_info {
  int id;
  float x,y,z,theta;
};

#define MAX_NODES 100
struct node_info nodes[MAX_NODES];
int node_count = 0;


struct expt_info {
  int sender;
  uint32_t id;
  float x,y,z,theta,temp,rh;
  char *note;
};
#define MAX_EXPTS 20000
struct expt_info expts[MAX_EXPTS];
int expt_count = 0;


struct node_info *node_lookup(int id)
{
  int i;
  for (i=0; i<node_count; i++) {
    if (id == nodes[i].id)
      return &(nodes[i]);
  }
  return NULL;
}


int ar_handle_result(msg_queue_opts_t *opts, buf_t *result_buf)
{
  return 0;
}

void ar_shutdown(void *data)
{
  elog(LOG_WARNING, "shutdown called..");
  exit(1);
}

ar_state_t _ar = {
  flood_interface: "flood",
  sensor_interface: "vxp",
  sound_dev: "hw:%d,%d",
  curr_temperature: 20,
  curr_RH: 50,
  gpio_channel: -1 ,
  chirp_len: CHIRP_CHIPS*MOD_FACTOR*RANGE_FACTOR
};


int process_file(char *input_file, chirp_result_t *result, int offset, int both_offset, 
		 int *orig_dist, int *new_dist, int we_are_sender, int *mod)
{
  int retval = 0;

  _ar.chirp_index = 0;

  memset(result, 0, sizeof(*result));
  elog(LOG_WARNING, "**** Processing file %s:\n", input_file);

  /* load file */
  chirp_record_t chirp = {};
  FILE *f = fopen(input_file, "r");
  if (f == NULL) {
    elog(LOG_CRIT, "**** unable to open file %s: %m", input_file);
    return 2;
  }
  if (fread(&chirp, sizeof(chirp), 1, f) != 1) {
    elog(LOG_CRIT, "**** cant read from file %s: %m", input_file);
    return 2;
  }
  int16_t *bpsk = g_new(int16_t,_ar.chirp_len);
  if (fread(bpsk, sizeof(int16_t) * _ar.chirp_len, 1, f) != 1) {
    elog(LOG_WARNING, "**** Failed to read bpsk data: %m");
    retval = 2; 
    goto fail;
  }  
  fclose(f);

  if (mod) *mod = chirp.notify.modulation;

  if (we_are_sender) {
    if (orig_dist) *orig_dist = chirp.start_sample;
    elog(LOG_WARNING, "**** Orig dist was %f ", chirp.start_sample/(float)CLIP_RESAMPLE);
  }
  elog(LOG_WARNING, "**** Timing error was %f ", chirp.timing_error);
    
  if (offset) {
    int i;
    int j=(offset-1)*2;
    for (i=EXTRACT_LEN-1; i>0; i--) {
      chirp.raw_data[i][j] = chirp.raw_data[i-1][j];
      chirp.raw_data[i][j+1] = chirp.raw_data[i-1][j+1];
    } 
  }

  if (both_offset) {
    if (both_offset > 0) {
      memmove((char*)chirp.raw_data, ((char*)chirp.raw_data)+both_offset*8, (EXTRACT_LEN-both_offset)*8);
    }
    if (both_offset < 0) {
      memmove(((char*)chirp.raw_data)-both_offset*8, (char*)chirp.raw_data, (EXTRACT_LEN+both_offset)*8);
      memset((char*)chirp.raw_data, 0, -both_offset*8);
    }
  }

  /* run */
  out_file = "";
  report_to_stdout = 1;
  chirp.notify.skip_computation = 0;
  printf("#Processing file %s:\n", input_file);
  fflush(stdout);

  if (we_are_sender)
    my_node_id = chirp.source;
  else
    my_node_id = 0;

  ar_process_chirp(&_ar, &chirp, _ar.chirp_len, result, bpsk, NULL);

  if (new_dist) *new_dist = result->entry.distance;
  if (we_are_sender && writeback) {
    chirp.start_sample = result->entry.distance;
    FILE *f = fopen(input_file, "w");
    if (f == NULL) {
      elog(LOG_CRIT, "**** unable to open file %s: %m", input_file);
      retval = 2; 
      goto fail;
    }
    if (fwrite(&chirp, sizeof(chirp), 1, f) != 1) {
      elog(LOG_CRIT, "**** cant write to file %s: %m", input_file);
      retval = 2;
      goto fail;
    }
    if (fwrite(bpsk, sizeof(int16_t) * _ar.chirp_len, 1, f) != 1) {
      elog(LOG_WARNING, "**** Failed to write bpsk data: %m");
      retval = 2;
      goto fail;
    }  
    fclose(f);
  }

  /* dump to file */
  if (_ar.dump_files_ctrl_string || 
      (chirp.notify.notation[0] && _ar.ar_log_raw)) {
    char buf[256];
    int i;
    
    sprintf(buf, _ar.dump_files_ctrl_string, "raw", _ar.chirp_index);
    
    FILE *f = fopen(buf, "w");
    if (f) {
      fprintf(f, "#index ref 0 1 2 3 \n");
      for (i=0; i<_ar.chirp_len; i++) {
	fprintf(f, "%d %d %d %d %d %d\n",
		i, bpsk[i],
		chirp.raw_data[i][0],
		chirp.raw_data[i][1],
		chirp.raw_data[i][2],
		chirp.raw_data[i][3]);
      }
      fclose(f);
    }
    else
      elog(LOG_WARNING, "Unable to open raw output file %s: %m", buf);
  }
  
  /* convert lag to mm */
  result->status = CHIRP_STATUS_DONE;
  int new_distance = 
    usec_to_mm(_ar.curr_temperature, _ar.curr_RH,
	       samples_to_usec
	       (result->entry.distance/(float)CLIP_RESAMPLE - (float)PRE_EXTRACT -
		result->sample_fraction));

  new_distance += SELF_RANGE_OFFSET;
  
  if (new_distance < 0) {
    elog(LOG_WARNING, "negative distance?? %d", new_distance);
    new_distance = 0;
    result->entry.conf = 0;
  }
  result->entry.distance = new_distance;
  
  /* convert uncombined range */
  result->uncombined_range = 
    usec_to_mm(_ar.curr_temperature, _ar.curr_RH, 
	       samples_to_usec(result->uncombined_range - PRE_EXTRACT));

  /* $$$ fixme.. should be actual node id.. do this for each node in turn, */
  elog(LOG_NOTICE, "node %d Recorded distance of %f [%f] from node %s",
       my_node_id,
       (float)result->entry.distance/1000.0, (float)result->entry.conf / 10.0, 
       print_if_id(result->entry.source));
  
  /* check input data for trailing zeroes */
  int i,j;
  float test=0;
  for (i=0; i<10; i++) {
    for (j=0; j<4; j++) {
      test += abs(chirp.raw_data[EXTRACT_LEN-1-i][j]);
    }
  }
  if (test == 0) {
    elog(LOG_WARNING, "**** trailing zeroes!");
    retval = 1;
  }

 fail:
  free(bpsk);
  return retval;
}

void test_temp();

int main(int argc, char **argv)
{
  int offset=0;
  int do_expt = 0;
  int usetemp = 0;
  float scaletemp = 0;
  float offsettemp = 0;
  
  extern int offline_mode;
  offline_mode = 1;

  /* generic init */
  misc_init(&argc, argv, CVSTAG);

  /* parse input filename */
  char *input_file = misc_parse_out_option(&argc, argv, "input", 0);
  _ar.dump_files_ctrl_string = misc_parse_out_option(&argc, argv, "dump-files", 0);
  autodump = misc_parse_out_switch(&argc, argv, "dump", 0);
  char *list_file = misc_parse_out_option(&argc, argv, "list", 0);
  char *angle_file = misc_parse_out_option(&argc, argv, "angle", 0);
  char *map_file = misc_parse_out_option(&argc, argv, "map", 0);
  misc_parse_option_as_float(&argc, argv, "temp", 0, &(_ar.curr_temperature));
  usetemp = misc_parse_out_switch(&argc, argv, "usetemp", 0);
  misc_parse_option_as_float(&argc, argv, "scaletemp", 0, &(scaletemp));
  misc_parse_option_as_float(&argc, argv, "offsettemp", 0, &(offsettemp));
  misc_parse_option_as_float(&argc, argv, "rh", 0, &(_ar.curr_RH));
  if (misc_parse_option_as_int(&argc, argv, "codec_offset", 0, &offset) >= 0) {
    offset++;
  }
  _ar.no_delta_corr = misc_parse_out_switch(&argc, argv, "no_delta_corr", 0);
  use_map_locations = misc_parse_out_switch(&argc, argv, "use_map", 0);
  _ar.use_first_arrival = misc_parse_out_switch(&argc, argv, "first_arrival", 0);
  _ar.use_first_swing = misc_parse_out_switch(&argc, argv, "first_swing", 0);
  misc_parse_option_as_int(&argc, argv, "do_expt", 0, &do_expt);
  we_are_sender = misc_parse_out_switch(&argc, argv, "sender", 0);
  _ar.simple_selfrange = misc_parse_out_switch(&argc, argv, "simple_selfrange", 0);
  recomp_sender = misc_parse_out_switch(&argc, argv, "recomp_sender", 0);
  do_sender_anyways = misc_parse_out_switch(&argc, argv, "do_sender_anyways", 0);

  if (autodump && input_file) {
    static char ctrl[128];
    char *ptr = strstr(input_file, "ar-raw-");
    if (ptr) {
      int sender, rcv, expt;
      sscanf(ptr+7, "%d-%d-%d", &expt, &sender, &rcv);
      sprintf(ctrl, "analysis/%d-%d-%%s-%%d", expt, rcv);
      _ar.dump_files_ctrl_string = ctrl;
    }
  }

  /* writeback is for fixing old data.. adding sender info */
  //writeback = misc_parse_out_switch(&argc, argv, "writeback", 0);

  if (misc_args_remain(&argc, argv))
    usage(argv[0]);

  if (input_file) {
    chirp_result_t result;
    process_file(input_file, &result, offset, 0, NULL, NULL, we_are_sender, NULL);
    return 0;
  }

  if (map_file && (angle_file || list_file)) {
    int status;

    elog(LOG_WARNING, "opening map file %s", map_file);
    FILE *map = fopen(map_file, "r");
    if (map == NULL) {
      elog(LOG_CRIT, "unable to open map file %s: %m", map_file);
      exit(1);
    }

    while (1) {
      status = fscanf(map, "%d %f %f %f %f", 
		      &(nodes[node_count].id),
		      &(nodes[node_count].x),
		      &(nodes[node_count].y),
		      &(nodes[node_count].z),
		      &(nodes[node_count].theta));
      if (status != 5)
	break;
      fprintf(stderr, "index %d: node %d: %f,%f,%f,%f\n",
	      node_count, 
	      nodes[node_count].id,
	      nodes[node_count].x,
	      nodes[node_count].y,
	      nodes[node_count].z,
	      nodes[node_count].theta);
      node_count++;
      if (node_count >= MAX_NODES) {
	elog(LOG_WARNING, "too many nodes!");
	exit(1);
      }
    }
    fclose(map);

    elog(LOG_WARNING, "opening %s file %s", list_file ? "list" : "angle",
	 list_file ? list_file : angle_file);
    FILE *list = fopen(list_file ? list_file : angle_file, "r");
    if (list == NULL) {
      elog(LOG_CRIT, "unable to open list file %s: %m", list_file);
      exit(1);
    }

    while (1) {
      char note[20];
      note[0] = 0;

      status = fscanf(list, "%d %u %f %f %f %f %f %s", 
		      &(expts[expt_count].sender),
		      &(expts[expt_count].id),
		      &(expts[expt_count].x),
		      &(expts[expt_count].y),
		      &(expts[expt_count].z),
		      &(expts[expt_count].temp),
		      &(expts[expt_count].rh),
		      note);
      if (usetemp) {
	expts[expt_count].temp = _ar.curr_temperature;
	expts[expt_count].rh = _ar.curr_RH;
      }
      if (offsettemp != 0) expts[expt_count].temp += offsettemp;
      if (scaletemp != 0) expts[expt_count].temp *= scaletemp;
      if (strcasecmp(note, "OK"))
	expts[expt_count].note = strdup(note);
      else
	expts[expt_count].note = NULL;

      struct node_info *n;
      if (use_map_locations && (n = node_lookup(expts[expt_count].sender))) {
	expts[expt_count].x = n->x;
	expts[expt_count].y = n->y;
	expts[expt_count].z = n->z;
	expts[expt_count].theta = n->theta;
      }

      expts[expt_count].id = expts[expt_count].id % 10000;
      
      if (status != 8) {
	elog(LOG_WARNING, "stopping.. status=%d", status);
	break;
      }

      fprintf(stderr, "read expt record: %d %f %f %f %f %f %s\n", 
	      (expts[expt_count].id),
	      (expts[expt_count].x),
	      (expts[expt_count].y),
	      (expts[expt_count].z),
	      (expts[expt_count].temp),
	      (expts[expt_count].rh),
	      note);

      expt_count++;
      if (expt_count >= MAX_EXPTS) {
	elog(LOG_WARNING, "too many experiments!");
	exit(1);
      }
    }
    fclose(list);

    /* OK, now we compute */
    int i,j;
    for (i=0; i<expt_count; i++) {
      int both_offset = 0;

      if (do_expt && do_expt != expts[i].id)
	continue;

      /* recompute sender ? */
      if (recomp_sender) {
	char name[128];
	int orig_dist;
	int new_dist;
	sprintf(name, "ar-raw-%04d-%d-%d",
		expts[i].id, expts[i].sender, expts[i].sender);

	if (autodump) {
	  static char ctrl[128];
	  sprintf(ctrl, "analysis/%04d-%d-%d-%%s-%%d", expts[i].id, expts[i].sender, expts[i].sender);
	  _ar.dump_files_ctrl_string = ctrl;
	}

	chirp_result_t result;
	status = process_file(name, &result, 0, 0, &orig_dist, &new_dist, 1, NULL);
	if (status == 0) {
	  both_offset = (new_dist - orig_dist) / CLIP_RESAMPLE;
	  if (both_offset != 0) {
	    elog(LOG_WARNING, "**** OFFSET=%d!!!", both_offset);
	    if (abs(both_offset) > EXTRACT_LEN) {
	      elog(LOG_CRIT, "*&*&*&* offset too big!!!!");
	      both_offset = 0;
	    }
	  }
	}
      }

      for (j=0; j<node_count; j++) {
	int status;
	
	/* skip selfrange */
	if (!do_sender_anyways && expts[i].sender == nodes[j].id) continue;

	if (autodump) {
	  static char ctrl[128];
	  sprintf(ctrl, "analysis/%04d-%d-%d-%%s-%%d", expts[i].id, expts[i].sender, nodes[j].id);
	  _ar.dump_files_ctrl_string = ctrl;
	}

	char name[128];
	sprintf(name, "ar-raw-%04d-%d-%d",
		expts[i].id, expts[i].sender, nodes[j].id);

	_ar.curr_temperature = expts[i].temp;
	_ar.curr_RH = expts[i].rh;

	chirp_result_t result;
	int mod;
	status = process_file(name, &result, offset, both_offset, NULL, NULL, 0, &mod);
	if (status == 2) continue;

	/* compute ground truth */
	float theta, terr, range;

	if (angle_file) {
	  /* special case for angle tests... */
	  theta = expts[i].x;
	  range = 803;
	}

	else {
	  theta = r2d(atan2f((expts[i].y - nodes[j].y), (expts[i].x - nodes[j].x))) - 90.0;
	  if (theta < 0) theta += 360.0;
	  if (theta > 360.0) theta -= 360.0;
	  range = sqrtf(sqrf(expts[i].x - nodes[j].x) + sqrf(expts[i].y - nodes[j].y) +
			sqrf(expts[i].z - nodes[j].z)) * 100.0;
	}

	terr = fabsf(theta-result.entry.theta/10.0);
	if (terr > 180) terr = 360.0 - terr;

	/* node expt Tg Tm Te Tc Rg Rm Re Rc Pg Pm max note note */
	printf("%d %d %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %4.f "
	       "%.1f %.1f mod=%d %s %s,%s\n",
	       nodes[j].id, expts[i].id, 
	       theta, result.entry.theta/10.0, terr, result.entry.a_conf / 10.0,
	       range, result.entry.distance/10.0,
	       fabsf(range - result.entry.distance/10.0), result.entry.conf / 10.0, 
	       result.max_value,
	       0.0, result.entry.phi/10.0,
	       mod, 
	       (status == 1) ? "**zeroes**" : ((result.entry.conf == 0) ? "**noconf**" : "OK"),
	       expts[i].note ? expts[i].note : "ok",
	       name);
	fflush(stdout);
      }
    }

    void test_offsets();
    test_offsets();

    return 0;
  }

  elog(LOG_CRIT, "Please specify input file OR map and list files!");
  usage(argv[0]);

  return 0;
}