Return to ar.h CVS log

Jump to this file's LXR Page

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

Added a box version check (/etc/box_version) which should == 2 for new
sensor boards
Turns on and off the audio out amp correctly (in conjunction with
ensbox_sb)

A bit of a hack to clean up just now

/*
 *
 * 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 <libmisc/misc.h>
#include <math.h>
#include <libnl/nl.h>
#include <libmisc/misc.h>
#include <timesync/sync.h>
#include <devel/state/ssync.h>
#include <link/link.h>
#include <libdev/g_msg_queue.h>
#include <libdev/status_dev.h>
#include <pthread.h>
#include <sensors/vxp.h>
#include <emrun/fault_logger.h>

#ifndef __AR_H__
#define __AR_H__

/* change for 24 bit */
#define SAMPLE_TYPE  int16_t
#define CHANNEL_COUNT   4

#define AR_CHIRP_DEVICE         sim_path("/dev/loc/trigger_chirp")
#define AR_SEQNO_DEVICE         sim_path("/dev/loc/curr_seqno")
#define AR_INDEX_DEVICE         sim_path("/dev/loc/curr_index")
#define AR_LOCAL_RANGES_DEVICE  sim_path("/dev/loc/local_ranges")
#define AR_QUEUE_DEVICE         sim_path("/dev/loc/queue")

#define DEV_ENSBOX_POWER        sim_path("/dev/ensbox/power")
#define BOX_VERSION_FILE        sim_path("/etc/box_version")

#define CHIRP_CHIPS  1024
#define MOD_FACTOR      8
#define RANGE_FACTOR    4       /* this determines the max detect range.  2 = ~80m */
#define EXTRACT_LEN  (CHIRP_CHIPS*MOD_FACTOR*RANGE_FACTOR)
#define PRE_EXTRACT   100

/* noise estimation and distribution */
#define NORM_NOISEEST   500
#define NOISE_FLOOR     6.0

#define USE_NEW_ARRAY

/* some constants */
#define SPEED_OF_SOUND    (340.0) //m/s
#define SAMPLE_RATE       (48000) //samples/s
#define DYN_SAMPLE_RATE   (_ar.curr_status.sample_rate ? _ar.curr_status.sample_rate : SAMPLE_RATE)
#define AR_FORMAT         SND_PCM_FORMAT_S16_LE
#define HIGH_PASS_F       2000    //hz
#ifdef USE_NEW_ARRAY
# define SELF_RANGE_OFFSET    85   //mm
#else
# define SELF_RANGE_OFFSET    20   //mm
#endif
#define SELF_RANGE_MIN_CONF   40  //factor
#define SELF_RANGE_MIN_AVG  1000  //

#define SELF_FACTOR 4.0          // accelerate self ranging

#define MAX_CONVERSION_ERROR  15.0

/* exhaustive angle test parameters */
#define THETA_COUNT    360
#define THETA(x) (2.0*M_PI*((float)x)/((float)THETA_COUNT))

#define PHI_COUNT      180
#define PHI(x) (M_PI*((float)x)/((float)PHI_COUNT)-(M_PI/2.0))

/* options */
#define CORRECT_OFFSETS

/* peak detection adaptive filter parameters */
#define TRACE_MAX        10
#define ALPHA          0.01
#define STARTUP_WINDOW  PRE_EXTRACT
#define STD_DEV_FACTOR   12.0
#define SELFRANGE_STD_DEV_FACTOR   9.0
#define THRESH_FACTOR    0.50
#define SWING_FUZZ_FACTOR  0.9

/* DOA2 parameters */
#define CLIP_LENGTH   32
#define CLIP_RESAMPLE 8

/* chirp status values */
#define CHIRP_STATUS_WAIT        1
#define CHIRP_STATUS_PROCESSING  2
#define CHIRP_STATUS_DONE        3
#define CHIRP_STATUS_FAILED      4

/* options for LSFIT */
#define LSFIT_RESID_MAX  10.0
#define CORRECT_NEAR
#define DO_SPEED
#undef WEIGHT_AGAINST_DERIV

/* options for the excess correction algorithm
 *
 * CORRECT_EXCESS is a start at calibrating the array for more exact
 * positioning of the microphones.  a future system should apply per-array
 * calibration data in a similar fashion.  CORRECT_EXCESS provides a
 * correction to account for the obstruction of the signal by the self--ranging
 * speaker.  this excess is angle--dependent; mutliplied by the sin(elevation).
 * a future system would benefit from placing the self--range speaker to the
 * side where it does not obstruct the signal, but coupled to the microphone.
 * it would also be good to use smaller mounts for the microphones that don't
 * obstruct so much.
 *
 * when disabling CORRECT_EXCESS, enable DO_SPEED
 */

#define CH0_SPK_EXCESS 0.015
#undef CORRECT_EXCESS
#undef WEIGHT_CH0
#undef DIRECT_EXCESS_CORR


typedef struct chirp_record chirp_record_t;
typedef struct chirp_result chirp_result_t;
typedef struct range_neighbor range_neighbor_t;

typedef struct {
  char *flood_interface;
  char *sensor_interface;
  char *sound_dev;
  char *platform;
  int gpio_channel;
  uint32_t card_index;

  /* current temp */
  float curr_temperature;   /* 0-30 */
  float curr_RH;            /* 0-100 */

  /* debug mode */
  char *dump_files_ctrl_string;
  uint32_t use_old_doa:1;
  uint32_t no_delta_corr:1;
  uint32_t use_first_arrival:1;
  uint32_t use_first_swing:1;
  uint32_t simple_selfrange:1;

  /* network events */
  lu_context_t *flood_event;
  lu_context_t *flood_trigger_event;

  /* thread state */
  pthread_t thread;
  msg_queue_t *result_event;
  active_fault_t *selfrange_fault;

  /* chirp list */
  pthread_mutex_t chirp_list_mutex;
  pthread_cond_t chirp_list_cond;
  QUEUE_DECL(chirps,chirp_record_t);
  status_context_t *local_queue;

  /* chirp state */
  uint32_t chirp_seqno;
  struct timeval last_chirp_time;
  status_context_t *chirp_cmd;
  status_context_t *seqno_status;
  status_context_t *index_status;
  active_fault_t *selfrange_late;

  /* neighbor list */
  QUEUE_DECL(neighbors, range_neighbor_t);
  status_context_t *local_ranges;
  int published_count;

  /* audio server status */
  status_client_context_t *audio_status;
  audiod_status_t curr_status;

  int chirp_len;

  /* which array to use? */
  int array_type;
  char *array_spec;

  /* log file */
  char *ar_log_file;
  char *ar_log_raw;
  int chirp_index;
  int send_index;
  int raw_as_text;
  int log_timestamps;

  /* next chirp to be sent */
  g_event_t *amp_at_time;
  g_event_t *chirp_refractory;
  g_event_t *chirp_delay;
  g_event_t *computation_holdoff;
  int next_seed;
  int next_mod;
  char *next_note;
  int16_t *next_chirp;
  int next_nocomp;

  /* stats */
  int fail_local_delay;
  int fail_self_range_noconf;
  int fail_no_snr;
  int fail_future;
  int fail_no_convert;
  int fail_convert_error;
  int warn_uncorrectable_offset;
  int fail_other;
  int trigger_fail_no_convert;
  int succeeded;
} ar_state_t;


struct chirp_record {
  node_id_t source;
  float timing_error;
  int64_t start_sample;
  range_notify_pkt_t notify;
  float sample_fraction;
  float observed_len;
  float temperature;
  int priority;
  int status;
  buf_t *stamplog;
  SAMPLE_TYPE raw_data[EXTRACT_LEN][4];
  QUEUE_ELEMENT_DECL(_,chirp_record_t);
};


struct range_neighbor {
  node_id_t node;
  range_entry_t published;
  chirp_result_t *best;

  /* result list */
  QUEUE_DECL(results,chirp_result_t);

  QUEUE_ELEMENT_DECL(_,range_neighbor_t);
};

struct chirp_result {
  range_notify_pkt_t notify;
  range_entry_t entry;
  int32_t uncombined_range;
  float max_value;
  int64_t start_sample;
  float sample_fraction;
  float temperature;

  uint16_t status;
  uint16_t curr_angle:1;
  uint16_t used:1;
  uint16_t best:1;
  uint16_t pad:5;
  uint16_t angle_group:8;

  float mode_value;
  float angle_value;
  float mode;
  QUEUE_ELEMENT_DECL(_,chirp_result_t);
};


QUEUE_INLINE_INSTANTIATIONS(ar_chirp_list,_,chirps,chirp_record_t,ar_state_t);
QUEUE_INLINE_INSTANTIATIONS(ar_neighbor_list,_,neighbors,range_neighbor_t,ar_state_t);
QUEUE_INLINE_INSTANTIATIONS(ar_result_list,_,results,chirp_result_t,range_neighbor_t);


#define ARRAY_TYPE_ORIG     0
#define ARRAY_TYPE_8CM      1
#define ARRAY_TYPE_12CM     2

/* global state var */
extern ar_state_t _ar;

/* ar_doa.c */
int ar_find_doa3 (float range, float lags[4][4], float lags_conf[4][4], float array[4][3], float *theta, float *phi,
		  float *speed, float *fit);

/* ar_pn.c */
/* rate_adjust is peer_rate / our_rate
 * this generates a waveform that matches their rate */
void ar_pn_generate_chirp(int16_t *wav, size_t count, int seed, int modulation, float rate_adjust);

/* ar_send.c */
int ar_send_init();
int ar_send_trigger(ar_state_t *ar, int seed, int modulation, char *notation, int nocomp);
int ar_send_schedule_chirp(ar_state_t *ar, struct timeval *tv, int seed, int delay, int count);
void ar_send_invalidate(ar_state_t *ar);

/* ar_emit.c */
void ar_amplifier_enable(ar_state_t *ar, int enable);
void ar_emit_current_chirp(ar_state_t *ar, int loop);

/* ar_net.c */
void ar_net_init(ar_state_t *ar);
int ar_handle_result(msg_queue_opts_t *opts, buf_t *result);
void ar_record_chirp(ar_state_t *ar, node_id_t source, range_notify_pkt_t *notify, int data_len);

/* ar_table.c */
void ar_table_init(ar_state_t *ar);
void ar_update_published(ar_state_t *ar, chirp_result_t *result);

/* ar_dsp.c */
void ar_thread_init(ar_state_t *ar);
float samples_to_usec(float samples);
float usec_to_samples(float usec);
int usec_to_mm(float temp, float rh, int usec);
int mm_to_usec(float temp, float rh, int mm);
void ar_compute_offsets(float theta, float phi, float array[4][3],
			float offsets[4]);
void ar_find_max(int *max_index, float *max,
		 float alpha, int thresh, float std_dev_factor,
		 float *signal, size_t points, float *observed_std_dev);
void ar_complex_vector_free(complex *vectors[]);
void ar_float_vector_free(float *vectors[]);
void ar_process_chirp(ar_state_t *ar, chirp_record_t *chirp, int chirp_len,
		      chirp_result_t *ret_result, int16_t *pn_override, int16_t **ret_bpsk);
int ar_update_array_spec(ar_state_t *ar, char *spec);

/* ar_main.c */
void ar_shutdown(void *data);


/*
 *  logging support
 */

extern int report_to_stdout;
extern char *out_file;
extern buf_t *report_line;

#define REPORT(args...) \
do { if (out_file) { \
  if (report_line == NULL) report_line = buf_new(); \
  bufprintf(report_line, args); \
} } while (0)

static inline
void REPORT_DONE()
{
  if (out_file && report_line) {
    REPORT("\n");
    int fd;
    if (report_to_stdout) fd = STDOUT_FILENO;
    else fd = open(out_file, O_WRONLY|O_APPEND|O_CREAT, 0666);
    if (fd < 0)
      elog(LOG_WARNING, "can't write to log file %s: %m", out_file);
    else {
      if (write(fd, report_line->buf, report_line->len) != report_line->len) {
	elog(LOG_WARNING, "write to log file %s failed: %m", out_file);
      }
      if (!report_to_stdout) close(fd);
    }
    buf_free(report_line);
    report_line=NULL;
  }
}

static inline
void REPORT_ERROR(node_id_t source, range_notify_pkt_t *notify, char *msg)
{
  if (out_file) {
    buf_t *buf = buf_new();
    bufprintf(buf, "ERR %d %u %d %d %s\n", source, notify->seqno,
	      notify->seed, notify->modulation, msg);
    int fd;
    if (report_to_stdout) fd = STDOUT_FILENO;
    else fd = open(out_file, O_WRONLY|O_APPEND|O_CREAT, 0666);
    if (fd < 0) elog(LOG_WARNING, "can't write to log file %s: %m", out_file);
    else {
      if (write(fd,  buf->buf, buf->len) != buf->len) {
	elog(LOG_WARNING, "write to log file %s failed: %m", out_file);
      }
      if (!report_to_stdout) close(fd);
    }
    buf_free(buf);
  }

  if (report_line) buf_free(report_line);
  report_line=NULL;
}


/* original doa algorithm.. */
int ar_find_doa(ar_state_t *ar, float array[4][3], float *correls[], complex *fft_input[],
		complex *fft_ref, float *correl_filtered, size_t chirp_len,
		float *theta, float *phi, float *SNR,
		int32_t *max_ind, float *max, float *conf,
		int32_t *uncombined_range, char **reason);

#endif