/* Get x, y, theta, filterupdate control */
void update_position(void)
{
// Create Buffer
unsigned int blobs[12];
float* robotCenter;
// Read in Locations
m_wii_read(blobs);
unsigned int x1 = blobs[0];
unsigned int y1 = blobs[1];
unsigned int x2 = blobs[3];
unsigned int y2 = blobs[4];
unsigned int x3 = blobs[6];
unsigned int y3 = blobs[7];
unsigned int x4 = blobs[9];
unsigned int y4 = blobs[10];
int rawStarData[8] = {x1, x2, x3, x4, y1, y2, y3, y4};
robotCenter = localization_calc(rawStarData, robotCenterPrev);
robotCenterPrev[2] = robotCenter[2];
// Correct for offset discontinuity
robotCenter[2] = theta_error_correction(robotCenter[2]);
x = lowpass(ALPHA_LOW, x, robotCenter[0]);
y = lowpass(ALPHA_LOW, y, robotCenter[1]);
theta = lowpass(ALPHA_LOW, theta, robotCenter[2]);
robotCenterPrev[0] = x;
robotCenterPrev[1] = y;
}
static void update_filters(float dt)
{
float voltage = 0, current = 0;
motor_rtctl_get_input_voltage_current(&voltage, ¤t);
if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
// Current sensor offset calibration, corner frequency is much lower.
const float offset_tau = _params.voltage_current_lowpass_tau * 100;
_state.input_curent_offset = lowpass(_state.input_curent_offset, current, offset_tau, dt);
}
current -= _state.input_curent_offset;
_state.input_voltage = lowpass(_state.input_voltage, voltage, _params.voltage_current_lowpass_tau, dt);
_state.input_current = lowpass(_state.input_current, current, _params.voltage_current_lowpass_tau, dt);
}
/* apply highpass filter to greymap. Return 0 on success, 1 on error
with errno set. */
static int highpass(greymap_t *gm, double lambda) {
greymap_t *gm1;
double f;
int x, y;
/* create a copy */
gm1 = gm_dup(gm);
if (!gm1) {
return 1;
}
/* apply lowpass filter to the copy */
lowpass(gm1, lambda);
/* subtract copy from original */
for (y=0; y<gm->h; y++) {
for (x=0; x<gm->w; x++) {
f = GM_UGET(gm, x, y);
f -= GM_UGET(gm1, x, y);
f += 128; /* normalize! */
GM_UPUT(gm, x, y, f);
}
}
gm_free(gm1);
return 0;
}
sample bassfilt(const Bass* instru, BassState* state, sample s) {
const int lowfreq = 100; // TODO: this also to the instrument? lp low freq?
int lfo = lfo_iexp(state->lfotime, instru->lfofreq);
state->lfotime++; // TODO: decide units
state->lp.coef = lowpass_coef(lowfreq + lfo * (instru->lp.freq - lowfreq));
return lowpass(&state->lp, s);
}
int main(void) {
int data[13] = { 0 };
int lowPass[33] = { 0 };
int highPass[5] = { 0 };
int deriv[31] = { 0 };
int squar[31] = { 0 };
int mwi[3] = { 0 };
int time[8] = { 0 };
int rPeaks[8] = { 0 };
int counter = 0;
for (int i = 0; i < 10000; i++) {
data[i % 13] = getNextData();
lowpass(data, lowPass, 13, 33, i);
highpass(lowPass, highPass, 33, 5, i);
derivative(highPass, deriv, 5, 31, i);
squaring(deriv, squar, 31, 31, i);
movingWindow(squar, mwi, 31, 3, i);
counter = findPeaks(mwi, i, time, rPeaks);
if (counter != -1) {
int intensity = rPeaks[counter % 8];
int RRinterval = time[counter % 8] - time[(counter - 1 + 8) % 8];
int pulse = 60000 / (RRinterval * 1000 / 250);
printf("R-Peak intensity: %d\nTime-value: %d s (%d samples)\nRR-interval: %d\nPulse: %d beats/minute\n\n",
intensity, i / 250, i, RRinterval, pulse);
if (rPeaks[counter % 8] < 2000) {
printf("WARNING! low R-peak value\n");
}
}
}
return 0;
}
/*The filters function uses filter.h's lowpass(), highpass(), derivative(), squarring() and mwi() functions on the
*data loaded from the ecgScanner() function.
**/
int filters() {
// LOW-PASS FILTER
mem.lpmem[getIndex(33, mem.index[1]-first, 1)] = lowpass(mem.input[getIndex(13, mem.index[0], 0)],
mem.input[getIndex(13, mem.index[0], -6)],
mem.input[getIndex(13, mem.index[0], -12)],
mem.lpmem[getIndex(33, mem.index[1], 0)],
mem.lpmem[getIndex(33, mem.index[1], -1)]);
mem.index[1] += 1-first;
// HIGH-PASS FILTER
mem.hpmem[getIndex(5, mem.index[2]-first, 1)] = highpass(mem.lpmem[getIndex(33, mem.index[1], 0)],
mem.lpmem[getIndex(33, mem.index[1], -16)],
mem.lpmem[getIndex(33, mem.index[1], -17)],
mem.lpmem[getIndex(33, mem.index[1], -32)],
mem.hpmem[getIndex(5, mem.index[2], 0)]);
mem.index[2] += 1;
// DERIVATIVE FILTER
mem.derivativemem = derivative(mem.hpmem[getIndex(5, mem.index[2], 0)],
mem.hpmem[getIndex(5, mem.index[2], -1)],
mem.hpmem[getIndex(5, mem.index[2], -3)],
mem.hpmem[getIndex(5, mem.index[2], -4)]);
// SQUARING FILTER
mem.squarredmem[getIndex(30, mem.index[3]-first, 1)] = squarring(mem.derivativemem);
mem.index[3] += 1-first;
// MOVING WINDOW INTEGRATION FILTER
mem.mwimem[getIndex(3, mem.index[4]-first, 1)] = mwi(mem.squarredmem, 30);
mem.index[4] += 1-first;
return 0;
}
void process_op(void)
{
switch(op)
{
case 1:
y = 0.5 + 0.5 * sin(2.0 * PI * 1.0 * t) + 0.2 * sin(2.0 * PI * 25.0 * t);
lowpass(y, &yfil);
break;
case 2:
y = 0.50 + frand() * 0.09;
kalman(y, &yfil);
break;
default:
y = 0.5 + 0.5 * sin(2.0 * PI * 1.0 * t) + 0.2 * sin(2.0 * PI * 25.0 * t);
lowpass(y, &yfil);
break;
};
}
void ghetto_lp(void) {
static float theta = 0;
static rd_t accelrd = -1;
if(accelrd == -1) {
accelrd = open_discovery_accel();
}
read(accelrd, (char *)data, 6);
theta = lowpass(theta, atan2(G(data->z), G(data->y)), ACC_LP_GAIN);
printf("Filtered roll: %f\r\n", theta);
}
int main(int argc, char *argv[])
{
FILE *fp;
int n;
int16_t buf[BB], out[BB];
int error;
static const pa_sample_spec ss = {
.format = PA_SAMPLE_S16LE, .rate = 44100, .channels = 1};
pa_simple *s = NULL;
if (!(s = pa_simple_new(NULL, argv[0], PA_STREAM_PLAYBACK, NULL, "playback",
&ss, NULL, NULL, &error)))
{
fprintf(stderr, __FILE__ ": pa_simple_new() failed: %s\n",
pa_strerror(error));
goto finish;
}
fp = fopen("out.wav", "rb");
if (!fp)
exit(-1);
fread(buf, 1, 44, fp);
n = fread(buf, 2, BB, fp);
while (!feof(fp))
{
highpass(buf, out, BB, 1.0f, 10);
memcpy(buf, out, BB * 2);
lowpass(buf, out, BB, 1.0f / 44100.0f, 0.00002);
if (pa_simple_write(s, out, (size_t)n * 2, &error) < 0)
{
fprintf(stderr, __FILE__ ": pa_simple_write() failed: %s\n",
pa_strerror(error));
goto finish;
}
n = fread(buf, 2, BB, fp);
}
if (pa_simple_drain(s, &error) < 0)
{
fprintf(stderr, __FILE__ ": pa_simple_drain() failed: %s\n",
pa_strerror(error));
goto finish;
}
finish:
if (s)
pa_simple_free(s);
return 0;
}
void signal::downconvert(double frequency, double bandwidth, uint16_t points) {
// Choose nearest frequency bin
uint16_t size = samples();
uint16_t n = points/2;
uint16_t nyquist = rate/2;
double width = (double)nyquist/n;
frequency = width * ceil(frequency/width);
// Perform downwards frequency shift and apply low pass filter
double period = (nyquist/frequency)*2.0;
for(int i = 0; i < channels; i++)
for(int j = 0; j < size; j++)
data[i][j] *= exp(std::complex<double>(0, -2*PI*(j/period)));
lowpass(bandwidth/2);
}
static double *
make_low_pass_filter(double omega_pass, double omega_stop, double error, int *length_p)
{
struct filter_param p = filter(omega_pass, omega_stop, error);
int length;
int n;
double *lpf;
length = p.M + 1;
lpf = calloc (length, sizeof(double));
for (n = 0; n < length; n++)
lpf[n] = lowpass(n, p.M, omega_pass) * kaiser(n, p.M, p.beta);
*length_p = length;
return lpf;
}
void CIIRfilter::iir_design( int iord, char *type, float fl, float fh, float ts )
{
double flw;
double fhw;
complex p[10];
int nps;
char ptype[10];
butter_poles(p, ptype, &nps, iord);
if (strncmp(type, "BP", 2) == 0)
{
fl = fl*ts/2.f;
fh = fh*ts/2.f;
flw = tangent_warp( fl, 2.f );
fhw = tangent_warp( fh, 2.f );
lp_to_bp(p, ptype, nps, flw, fhw, sn, sd, &nsects);
}
else if (strncmp(type, "BR", 2) == 0 )
{
fl = fl*ts/2.f;
fh = fh*ts/2.f;
flw = tangent_warp( fl, 2.f );
fhw = tangent_warp( fh, 2.f );
lp_to_br(p, ptype, nps, flw, fhw, sn, sd, &nsects);
}
else if (strncmp(type, "LP", 2) == 0 )
{
fh = fh*ts/2.f;
fhw = tangent_warp( fh, 2.f );
lowpass( p, ptype, nps, sn, sd, &nsects );
cutoff_alter( sn, sd, nsects, fhw );
}
else if (strncmp(type, "HP", 2) == 0 )
{
fl = fl*ts/2.f;
flw = tangent_warp( fl, 2.f );
lp_to_hp( p, ptype, nps, sn, sd, &nsects );
cutoff_alter( sn, sd, nsects, flw );
}
bilinear( sn, sd, nsects );
for (int i = 0; i < nsects; i++)
{
x1[i] = x2[i] = y1[i] = y2[i] = 0.f;
}
}
av_always_inline
static void denoise_temporal(uint8_t *src, uint8_t *dst,
uint16_t *frame_ant,
int w, int h, int sstride, int dstride,
int16_t *temporal, int depth)
{
long x, y;
uint32_t tmp;
temporal += 0x1000;
for (y = 0; y < h; y++) {
for (x = 0; x < w; x++) {
frame_ant[x] = tmp = lowpass(frame_ant[x], LOAD(x), temporal);
STORE(x, tmp);
}
src += sstride;
dst += dstride;
frame_ant += w;
}
}
av_always_inline
static void denoise_spatial(HQDN3DContext *hqdn3d,
uint8_t *src, uint8_t *dst,
uint16_t *line_ant, uint16_t *frame_ant,
int w, int h, int sstride, int dstride,
int16_t *spatial, int16_t *temporal, int depth)
{
long x, y;
uint32_t pixel_ant;
uint32_t tmp;
spatial += 256 << LUT_BITS;
temporal += 256 << LUT_BITS;
/* First line has no top neighbor. Only left one for each tmp and
* last frame */
pixel_ant = LOAD(0);
for (x = 0; x < w; x++) {
line_ant[x] = tmp = pixel_ant = lowpass(pixel_ant, LOAD(x), spatial, depth);
frame_ant[x] = tmp = lowpass(frame_ant[x], tmp, temporal, depth);
STORE(x, tmp);
}
for (y = 1; y < h; y++) {
src += sstride;
dst += dstride;
frame_ant += w;
if (hqdn3d->denoise_row[depth]) {
hqdn3d->denoise_row[depth](src, dst, line_ant, frame_ant, w, spatial, temporal);
continue;
}
pixel_ant = LOAD(0);
for (x = 0; x < w-1; x++) {
line_ant[x] = tmp = lowpass(line_ant[x], pixel_ant, spatial, depth);
pixel_ant = lowpass(pixel_ant, LOAD(x+1), spatial, depth);
frame_ant[x] = tmp = lowpass(frame_ant[x], tmp, temporal, depth);
STORE(x, tmp);
}
line_ant[x] = tmp = lowpass(line_ant[x], pixel_ant, spatial, depth);
frame_ant[x] = tmp = lowpass(frame_ant[x], tmp, temporal, depth);
STORE(x, tmp);
}
}
av_always_inline
static void denoise_spatial(uint8_t *src, uint8_t *dst,
uint16_t *line_ant, uint16_t *frame_ant,
int w, int h, int sstride, int dstride,
int16_t *spatial, int16_t *temporal, int depth)
{
long x, y;
uint32_t pixel_ant;
uint32_t tmp;
spatial += 0x1000;
temporal += 0x1000;
/* First line has no top neighbor. Only left one for each tmp and
* last frame */
pixel_ant = LOAD(0);
for (x = 0; x < w; x++) {
line_ant[x] = tmp = pixel_ant = lowpass(pixel_ant, LOAD(x), spatial);
frame_ant[x] = tmp = lowpass(frame_ant[x], tmp, temporal);
STORE(x, tmp);
}
for (y = 1; y < h; y++) {
src += sstride;
dst += dstride;
frame_ant += w;
pixel_ant = LOAD(0);
for (x = 0; x < w-1; x++) {
line_ant[x] = tmp = lowpass(line_ant[x], pixel_ant, spatial);
pixel_ant = lowpass(pixel_ant, LOAD(x+1), spatial);
frame_ant[x] = tmp = lowpass(frame_ant[x], tmp, temporal);
STORE(x, tmp);
}
line_ant[x] = tmp = lowpass(line_ant[x], pixel_ant, spatial);
frame_ant[x] = tmp = lowpass(frame_ant[x], tmp, temporal);
STORE(x, tmp);
}
}
static vec_t lowpass_limited(vec_t value, vec_t frac, vec_t limit, vec_t *store)
{
lowpass(value, frac, store);
return (*store = bound(value - limit, *store, value + limit));
}
bool OutLame::apply_profile() {
if(enc_flags) lame_close(enc_flags);
enc_flags = lame_init();
lame_set_errorf(enc_flags,(void (*)(const char*, va_list))error);
lame_set_debugf(enc_flags,(void (*)(const char*, va_list))func);
lame_set_msgf(enc_flags,(void (*)(const char*, va_list))act);
lame_set_num_samples(enc_flags,OUT_CHUNK);
lame_set_num_channels(enc_flags,2); // the mixed input stream is stereo
lame_set_in_samplerate(enc_flags,SAMPLE_RATE); // the mixed input stream is 44khz
lame_set_error_protection(enc_flags,1); // 2 bytes per frame for a CRC checksum
lame_set_compression_ratio(enc_flags,0);
lame_set_quality(enc_flags,2); // 1..9 1=best 9=worst (suggested: 2, 5, 7)
// lame_set_VBR(enc_flags,vbr_abr);
/* BITRATE */
lame_set_brate(enc_flags,bps());
Shouter *ice = (Shouter*)icelist.begin();
while(ice) {
char tmp[256];
snprintf(tmp,256,"%u",bps()); ice->bps( tmp );
snprintf(tmp,256,"%u",freq()); ice->freq( tmp );
snprintf(tmp,256,"%u",channels()); ice->channels( tmp );
ice = (Shouter*)ice->next;
}
lame_set_out_samplerate(enc_flags,freq());
/* CHANNELS
mode 2 (double channel) is unsupported */
int mode;
switch( channels() ) {
/* 0,1,2,3 stereo,jstereo,dual channel,mono */
case 1: mode = 3; break;
case 2: mode = 1; break;
default: mode = 3; break;
}
lame_set_mode(enc_flags,(MPEG_mode_e)mode);
/* in case of VBR
func("reversed quality is %i, guessed bps %i",(int)fabs( 10 - quality()),bps());
lame_set_VBR_q(enc_flags,(int)fabs( 10 - quality() ));
lame_set_VBR_mean_bitrate_kbps(enc_flags,bps());
*/
/* lame chooses for us frequency filtering when values are 0 */
lame_set_lowpassfreq(enc_flags,lowpass());
lame_set_highpassfreq(enc_flags,highpass());
int res = lame_init_params(enc_flags);
if(res<0) {
error("lame_init_params failed");
lame_close(enc_flags);
enc_flags = NULL;
}
return (res<0)?false:true;
}
int BestPt(int n, short *data, float cut)
{
/*
* Local variables
*/
float dmax;
int j, ipmid, ret_val;
float d1, d2, dx, *signal;
int ipleft, ip1, ip2, ip3, ipright;
/*
* First lowpass the signal (returns a floating point array of length N
* in signal).
*/
signal = lowpass(n, data, cut);
/*
* Now find most likely run. We do so by tracking the magnitude of the
* difference between successive maxima and minima in the filtered input.
* This magnitude is weighted by the run slope to favor runs of steep
* slope, and we add the difference between an adjacent pair of runs to the
* second (later) run if the later run is larger. This favors large runs
* that follow small runs.
*
*/
ip1 = 0;
ret_val = 0; /* Default return value is left edge of the window */
if (!irun(n, signal, &ip1, &ip2)) {
d1 = signal[ip2] - signal[ip1];
if (ip2 > ip1)
d1 = (d1*d1)/(float)(ip2 - ip1);
else
d1 = 0.0;
ipleft = -1;
dmax = (float)0.0;
while(!irun(n, signal, &ip2, &ip3)) {
d2 = signal[ip3] - signal[ip2];
if (ip3 > ip2)
d2 = (d2*d2)/(float)(ip3 - ip2);
else
d2 = 0.0;
if (d2 > d1)
dx = (float) (d2 * 2 - d1);
else
dx = d2;
if (dx > dmax) {
ipleft = ip1;
ipmid = ip2;
ipright = ip3;
dmax = dx;
}
ip1 = ip2;
ip2 = ip3;
d1 = d2;
}
/*
* If a valid point was located, find a zero crossing in the chosen run
* and return the first sample location after the zero crossing.
*/
if (ipleft >= 0) {
if (data[ipleft] < data[ipmid]) {
for (j=ipleft; j<ipmid; ++j) {
if (data[j] >= 0) {
ret_val = j;
goto done;
}
}
ret_val = ipmid;
} else {
for (j=ipmid; j<ipright; ++j) {
if (data[j] >= 0) {
ret_val = j;
goto done;
}
}
ret_val = ipright;
}
}
}
done:
free(signal);
return ret_val;
}
int main(int argc, char **argv) {
int commandArgs = 1;
int i;
char x;
char performContrast = 0;
char fft_filename[FILENAME_LENGTH];
char cdf_filename[FILENAME_LENGTH];
char histo_filename[FILENAME_LENGTH];
float contrastLow = 0.0;
float contrastHigh = 0.0;
float highpasslevel;
float lowpasslevel;
float percentCorrupt;
float sigma;
float brightness;
float sat;
int m;
int replaceWithM;
int performHistogram = 0;
int performCDF = 0;
int performFFT = 0;
int performVectorMedianFilter = 0;
int performMedianFilter = 0;
int performMeanFilter = 0;
int performSpacialFilter = 0;
int performLevelSlicing = 0;
int performEqualize = 0;
int performColorScale = 0;
int performSpatialReduceWidth = 0;
int performSpatialReduceHeigth = 0;
int performHighpass = 0;
int performLowpass = 0;
int performComponentMedianFilter = 0;
int performVectorOrderStatistic = 0;
int performVectorSpacialOrderStatistic = 0;
int performVectorMedianOrderStatistic = 0;
int performMinkowskiAddition = 0;
int performMinkowskiSubtraction = 0;
int performMinkowskiOpening = 0;
int performMinkowskiClosing = 0;
int performEdgeDetectOne = 0;
int performEdgeDetectTwo = 0;
int performEdgeDetectThree = 0;
int performEdgeDetectFour = 0;
int performAddGaussianNoise = 0;
int performAddSaltPepperNoise = 0;
int performSetBrightness = 0;
int performSetSaturation = 0;
int performBrightFilter = 0;
int imageWriteNecessary = 0;
int maskwidth = 0;
int maskheight = 0;
int maskrepeat = 0;
FILE *in = stdin;
FILE *out = stdout;
struct portImage *pi;
if (argc < 3) {
printf("\n");
printf(" Usage: %s [inputFile] ([outputFile]) [option] ([option] ...)\n", argv[0]);
printf("\n");
printf(" InputFile: Either a filename or '-' for stdin.\n");
printf(" OutputFile: Either a filename or '-' for stdout. (Not needed if no output necessary.)\n");
printf("\n");
printf(" Options:\n");
printf(" -ghisto FILENAME Graph Histogram\n");
printf(" -gcdf FILENAME Graph Cumulative Distribution\n");
printf(" -gfft FILENAME Graph FFT plot\n");
printf(" -color n Reduce color scale to n\n");
printf(" -spatial WIDTH-HEIGHT Perform spacial reduction to Width and Height\n");
printf(" -level n Perform level slicing from graylevel n to graylevel n+10\n");
printf(" -con LOW-HIGH Scale image contrast from LOW graylevel percentage to HIGH graylevel percentage\n");
printf(" -equ Histogram Eqaulization\n");
printf(" -medianf n Simple Median Filter of window size n*n\n");
printf(" -meanf n Simple Mean Filter of window size n*n\n");
printf(" -cmf n Component Median Filter of window size n*n\n");
printf(" -vmf n Vector Median Filter of window n*n\n");
printf(" -sf Spacial Filter\n");
printf(" -vos n v Vector Order Stat of window size n*n and value v\n");
printf(" -vmos n m [01] Vector Median Order Stat of window size n*n, m threshold, and 0 or 1(True) replace with m\n");
printf(" -vsos n m [01] Vector Spacial Order Stat of window size n*n, m threshold, and 0 or 1(True) replace with m\n");
printf(" -brightf n Perform an Brightness filter using HSV colorspace on size n*n window.\n");
printf(" -bright %% Set brightness to %% percent\n");
printf(" -sat %% Set saturation to %% percent\n");
printf(" -hp %% Highpass filter of %% percent\n");
printf(" -lp %% Lowpass filter of %% percent\n");
printf(" -ma NxM R Perform Minkowski Addition using NxM mask, repeated R times.\n");
printf(" -ms NxM R Perform Minkowski Subtraction using NxM mask, repeated R times.\n");
printf(" -mo NxM R Perform Minkowski Opening using NxM mask, repeated R times.\n");
printf(" -mc NxM R Perform Minkowski Closing using NxM mask, repeated R times.\n");
printf(" -e1 Perform Edge Detection using X/(X – B)\n");
printf(" -e2 Perform Edge Detection using (X + B)/X\n");
printf(" -e3 Perform Edge Detection using [(X+B)/(X-B)]-B\n");
printf(" -e4 n Experimental Edge Detection on Color Images using n*n window.\n");
printf(" -noiseG p s Add Gaussian noise to p (0 to 1 floating) percent of image with s sigma noise.\n");
printf(" -noiseSP p Add Salt and Pepper noise to p (0 to 1 floating) percent of image.\n");
printf("\n");
return(1);
}
if (strcmp(argv[commandArgs], "-") != 0) {
in = fopen(argv[1],"r");
if (in == NULL) {
fprintf(stderr, "File '%s' failed to open for reading.\n", argv[1]);
exit(1);
}
}
commandArgs++;
if (strcmp(argv[commandArgs], "-") != 0 && argv[commandArgs][0] != '-') {
commandArgs++;
out = fopen(argv[2],"w");
if (out == NULL) {
fprintf(stderr, "File '%s' failed to open for writing.\n", argv[2]);
exit(1);
}
}
for (; commandArgs < argc; commandArgs++) {
if (strcmp(argv[commandArgs], "-color") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performColorScale);
}
if (strcmp(argv[commandArgs], "-spatial") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d%c%d", &performSpatialReduceWidth, &x, &performSpatialReduceHeigth);
}
if (strcmp(argv[commandArgs], "-level") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performLevelSlicing);
}
if (strcmp(argv[commandArgs], "-con") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%f%c%f", &contrastLow, &performContrast, &contrastHigh);
}
if (strcmp(argv[commandArgs], "-vos") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performVectorOrderStatistic);
commandArgs++;
sscanf(argv[commandArgs], "%d", &m);
}
if (strcmp(argv[commandArgs], "-vmf") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performVectorMedianFilter);
}
if (strcmp(argv[commandArgs], "-sf") == 0) {
imageWriteNecessary = 1;
performSpacialFilter = 1;
}
if (strcmp(argv[commandArgs], "-vmos") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performVectorMedianOrderStatistic);
commandArgs++;
sscanf(argv[commandArgs], "%d", &m);
commandArgs++;
sscanf(argv[commandArgs], "%d", &replaceWithM);
}
if (strcmp(argv[commandArgs], "-vsos") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performVectorSpacialOrderStatistic);
commandArgs++;
sscanf(argv[commandArgs], "%d", &m);
commandArgs++;
sscanf(argv[commandArgs], "%d", &replaceWithM);
}
if (strcmp(argv[commandArgs], "-cmf") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performComponentMedianFilter);
}
if (strcmp(argv[commandArgs], "-medianf") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performMedianFilter);
}
if (strcmp(argv[commandArgs], "-meanf") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performMeanFilter);
}
if (strcmp(argv[commandArgs], "-equ") == 0) {
imageWriteNecessary = 1;
performEqualize = 1;
}
if (strcmp(argv[commandArgs], "-hp") == 0) {
imageWriteNecessary = 1;
commandArgs++;
performHighpass = 1;
sscanf(argv[commandArgs], "%f", &highpasslevel);
}
if (strcmp(argv[commandArgs], "-lp") == 0) {
imageWriteNecessary = 1;
commandArgs++;
performLowpass = 1;
sscanf(argv[commandArgs], "%f", &lowpasslevel);
}
if (strcmp(argv[commandArgs], "-brightf") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performBrightFilter);
}
if (strcmp(argv[commandArgs], "-bright") == 0) {
imageWriteNecessary = 1;
commandArgs++;
performSetBrightness = 1;
sscanf(argv[commandArgs], "%f", &brightness);
}
if (strcmp(argv[commandArgs], "-sat") == 0) {
imageWriteNecessary = 1;
commandArgs++;
performSetSaturation = 1;
sscanf(argv[commandArgs], "%f", &sat);
}
if (strcmp(argv[commandArgs], "-ghisto") == 0) {
commandArgs++;
performHistogram = 1;
strncpy(histo_filename, argv[commandArgs], FILENAME_LENGTH);
}
if (strcmp(argv[commandArgs], "-gcdf") == 0) {
commandArgs++;
performCDF = 1;
strncpy(cdf_filename, argv[commandArgs], FILENAME_LENGTH);
}
if (strcmp(argv[commandArgs], "-gfft") == 0) {
commandArgs++;
performFFT = 1;
strncpy(fft_filename, argv[commandArgs], FILENAME_LENGTH);
}
if (strcmp(argv[commandArgs], "-ma") == 0) {
imageWriteNecessary = 1;
performMinkowskiAddition = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d%c%d", &maskwidth, &x, &maskheight);
commandArgs++;
sscanf(argv[commandArgs], "%d", &maskrepeat);
}
if (strcmp(argv[commandArgs], "-ms") == 0) {
imageWriteNecessary = 1;
performMinkowskiSubtraction = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d%c%d", &maskwidth, &x, &maskheight);
commandArgs++;
sscanf(argv[commandArgs], "%d", &maskrepeat);
}
if (strcmp(argv[commandArgs], "-mo") == 0) {
imageWriteNecessary = 1;
performMinkowskiOpening = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d%c%d", &maskwidth, &x, &maskheight);
commandArgs++;
sscanf(argv[commandArgs], "%d", &maskrepeat);
}
if (strcmp(argv[commandArgs], "-mc") == 0) {
imageWriteNecessary = 1;
performMinkowskiClosing = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d%c%d", &maskwidth, &x, &maskheight);
commandArgs++;
sscanf(argv[commandArgs], "%d", &maskrepeat);
}
if (strcmp(argv[commandArgs], "-e1") == 0) {
imageWriteNecessary = 1;
performEdgeDetectOne = 1;
}
if (strcmp(argv[commandArgs], "-e2") == 0) {
imageWriteNecessary = 1;
performEdgeDetectTwo = 1;
}
if (strcmp(argv[commandArgs], "-e3") == 0) {
imageWriteNecessary = 1;
performEdgeDetectThree = 1;
}
if (strcmp(argv[commandArgs], "-e4") == 0) {
imageWriteNecessary = 1;
commandArgs++;
sscanf(argv[commandArgs], "%d", &performEdgeDetectFour);
}
if (strcmp(argv[commandArgs], "-noiseG") == 0) {
imageWriteNecessary = 1;
performAddGaussianNoise = 1;
commandArgs++;
sscanf(argv[commandArgs], "%f", &percentCorrupt);
commandArgs++;
sscanf(argv[commandArgs], "%f", &sigma);
}
if (strcmp(argv[commandArgs], "-noiseSP") == 0) {
imageWriteNecessary = 1;
performAddSaltPepperNoise = 1;
commandArgs++;
sscanf(argv[commandArgs], "%f", &percentCorrupt);
}
}
pi = readImage(in);
if (performHighpass || performLowpass || performFFT) {
FFT2D(pi);
if (performHighpass) highpass(pi, highpasslevel);
if (performLowpass) lowpass(pi, lowpasslevel);
if (performFFT) graph_fftlogplot(pi, fft_filename);
IFFT2D(pi);
}
if (performEdgeDetectOne || performEdgeDetectTwo || performEdgeDetectThree ||
performMinkowskiAddition || performMinkowskiSubtraction || performMinkowskiOpening || performMinkowskiClosing)
thresholdImage(pi);
if (performAddGaussianNoise) addGaussianNoise(pi, percentCorrupt, sigma);
if (performAddSaltPepperNoise) addSaltPepperNoise(pi, percentCorrupt);
if (performMedianFilter) simpleMedianFilter(pi, performMedianFilter);
if (performMeanFilter) simpleMeanFilter(pi, performMeanFilter);
if (performComponentMedianFilter) componentMedianFilter(pi, performComponentMedianFilter);
if (performVectorOrderStatistic) vectorOrderStatistic(pi, performVectorOrderStatistic, m);
if (performVectorSpacialOrderStatistic) vectorSpacialOrderStatistic(pi, performVectorSpacialOrderStatistic, m, replaceWithM);
if (performVectorMedianOrderStatistic) vectorMedianOrderStatistic(pi, performVectorMedianOrderStatistic, m, replaceWithM);
if (performVectorMedianFilter) vectorMedianFilter(pi, performVectorMedianFilter);
if (performSpacialFilter) spacialFilter(pi);
if (performBrightFilter) HSV_ValueFilter(pi, performBrightFilter);
if (performColorScale) scale_reduce(pi,performColorScale);
if (performSetBrightness) setBrightness(pi,brightness);
if (performSetSaturation) setSaturation(pi,sat);
if (performSpatialReduceWidth) spacial_reduce(pi,performSpatialReduceWidth, performSpatialReduceHeigth);
if (performContrast) contrast_stretching(pi, contrastLow, contrastHigh);
if (performLevelSlicing) level_slice(pi, performLevelSlicing);
if (performEqualize) equalize(pi);
if (performHistogram) graph_histogram(pi, histo_filename);
if (performCDF) graph_cdf(pi, cdf_filename);
if (performMinkowskiAddition)
for (i = 0; i < maskrepeat; i++)
minkowskiAddition(pi, maskwidth, maskheight);
if (performMinkowskiSubtraction)
for (i = 0; i < maskrepeat; i++)
minkowskiSubtraction(pi, maskwidth, maskheight);
if (performMinkowskiOpening)
for (i = 0; i < maskrepeat; i++)
minkowskiOpening(pi, maskwidth, maskheight);
if (performMinkowskiClosing)
for (i = 0; i < maskrepeat; i++)
minkowskiClosing(pi, maskwidth, maskheight);
if (performEdgeDetectOne) {
struct portImage *pc = copyImage(pi);
imageWriteNecessary = 1;
minkowskiSubtraction(pc, 3, 3);
minkowskiDivision(pi, pc);
freeImage(pc);
}
if (performEdgeDetectTwo) {
struct portImage *pc = copyImage(pi);
imageWriteNecessary = 1;
minkowskiAddition(pi, 3, 3);
minkowskiDivision(pi, pc);
freeImage(pc);
}
if (performEdgeDetectThree) {
struct portImage *pd = copyImage(pi);
maskrepeat = 3;
imageWriteNecessary = 1;
for (i = 0; i < maskrepeat; i++) {
minkowskiAddition(pi, 3, 3);
minkowskiSubtraction(pd, 3, 3);
}
minkowskiDivision(pi, pd);
minkowskiSubtraction(pi, 3, 3);
freeImage(pd);
}
if (imageWriteNecessary)
writeImage(pi, out);
freeImage(pi);
if (in != stdin) fclose(in);
if (out != stdout) fclose(out);
return 0;
} /* End Main */
Real_Vector smooth(Real_Vector data, int ndata, int factor)
{
/*
Routine smooths array data[] of length ndata with a Kaiser-Bessel FIR
lowpass filter. The resulting output (smoothed) array y[] is also of
length ndata.
factor controls the amount of smoothing & must be between 2 and 10.
The most smoothing occurs for factor = 10; the least smoothing
occurs for factor = 2. The smoothing results from reducing the
bandwidth of the input data by an amount = factor. Thus; some
care must be taken, for smoothing with large factors, to assure
that the input data is appropriately oversampled or the input
data will be filtered out with the noise.
nfilt is the length of the FIR filter and is computed by this routine
empiricallly according to :
(1) nfilt = [1.08*|log10(error_R)|+1.174]*factor + 0.5; where:
error_R is the relative error introduced by the smoothing process.
(error_R is set to 1.e-3 in this program)
An equivalent expression for nfilt is given in the program from a
digital signal processing point of view:
(2) nfilt=[0.027*sl+0.587]/fc + 0.5; where:
sl = |20*log10(error_R)| ; sl is the resampling ambiguity level in dB
fc = 1./(2*factor); fc is the normalized cutoff freq of the filter
The routine smooths only the central part of the array data[], the
upper and lower edges (i.e. (i < nfilt/2) or (i > ndata-nfilt/2) ) of the
array are filtered with the fixed 3-pt. FIR filter: 1/4, 1/2, 1/4.
There must be at least nfilt data points, or nothing is done.
*/
int ndec,itype,isym,i,i1,ny,ndm1,nl,nr,nn,half;
double sl,fc;
static Real h[23]; /* dimensioned to length MAX(nfilt)/2 if half!=0 */
static int nd2,nfilt,factor1;
Real_Vector y;
if( data == NULL) {
matherr_("smooth", E_NULLPTR);
return NULL;
}
if(factor < 2 && factor > 10) {
matherr_("smooth", E_FACTOR);
return NULL;
}
if(factor1 != factor) {
factor1=factor;/* design Kaiser-Bessel lowpass FIR filter for smoothing */
sl = 60.; /* set low pass filter sidelobe level to about -60 dB */
fc = 1./(factor+factor); /* define smoothing filter cutoff frequency */
nfilt= (int) ((0.027*sl+0.587)/fc + 0.5); /* for these values; nfilt < 45 */
nd2=nfilt/2;
nn=nfilt-(nd2+nd2);
if(nn == 0) nfilt=nfilt+1; /* make sure nfilt is odd */
half = 1;
lowpass(h, nfilt, fc, sl, half);
}
if(ndata < nfilt) {
matherr_("smooth", E_NOTENOUGH);
return NULL;
}
/* convolve data with smoothing filter */
ndec=1;
itype=1;
isym=1;
y = convolve(data,ndata,h,nfilt,ndec,itype,isym,&ny);
if (y == NULL) return NULL;
/* ny=(int)(ndata-nfilt)/ndec +1 */
/* fill edges of smoothed array with a 3pt. weighted average of original data */
ndm1=ndata-1;
y[ndm1] = data[ndm1];
for (i=1; i<nd2; i++) { /* filter right edge of data */
i1=ndm1-i;
y[i1]= data[i1-1]+2*data[i1]+data[i1+1];
y[i1] /= 4.;
}
nl = ndm1-nd2;
nr = ny-1;
for (i=0; i<ny; i++) y[nl-i]=y[nr-i]; /*shift data right nfilt/2 places*/
y[0] = data[0]; /* filter left edge of data */
for (i=1; i<nd2; i++) {
y[i]=data[i-1]+2*data[i]+data[i+1];
y[i] /= 4.;
}
return y;
}
int main(int argc, char *argv[]) {
// mallocated stuff
float * wave_buffer;
float * aa_buffer;
float * work_buffer;
float * frame_buffer;
float * dft_bins;
unsigned int * frequencies;
unsigned int * volumes;
unsigned char * out_buffer;
unsigned char consolidate;
unsigned long wave_size, work_size;
unsigned int frame_size, frame_inc;
unsigned int i;
int ofs;
unsigned long idx;
unsigned long c, m, k, t, f;
unsigned int kmax;
float max_power;
float in_r, sum_r, sum_i;
float ratio;
unsigned long frame_idx = 0, frames;
char outfilepath[256];
unsigned int ext;
puts("PSGTalk 0.3 - furrtek 2017\n");
// Overlap = 0
// Data: ######################## (24)
// Frames: AAAAAAAA
// BBBBBBBB
// CCCCCCCC
// Count: 3
// Length: 8 = 24 / 3 * (0 + 1)
// Offset: 8 = Length * (1 - 0)
// Overlap = 0.5
// Data: ######################## (24)
// Frames: AAAAAAAAAAAA
// BBBBBBBBBBBB
// CCCCCCCCCCCC
// Count: 3
// Length: 12 = 24 / 3 * (0.5 + 1)
// Offset: Length * (1 - 0.5)
// Defaults
overlap = 0.25;
updates_per_frame = 2;
psg_channels = 3;
sim = 0;
mode = MODE_VGM;
fps = 60;
freq_res = 64;
if (parse_args(argc, argv))
return 1;
wave_size = load_wav(argv[argc - 1], &wave_buffer);
if (!wave_size) {
puts("Can't load wave file.\n");
return 1;
}
// Anti-alias filtering
aa_buffer = calloc(wave_size, sizeof(float));
if (aa_buffer == NULL) {
puts("Memory allocation failed\n");
free(wave_buffer);
return 1;
}
lowpass(wave_buffer, aa_buffer, 8192.0, samplerate_in, wave_size);
// Decimate
ratio = ((float)samplerate_in / 8192.0);
work_size = (int)(wave_size / ratio);
work_buffer = calloc(work_size, sizeof(float));
if (work_buffer == NULL) {
puts("Memory allocation failed\n");
free(wave_buffer);
free(aa_buffer);
return 1;
}
for (i = 0; i < work_size; i++)
work_buffer[i] = aa_buffer[(int)(i * ratio)];
update_rate = fps * updates_per_frame;
frame_size = (8192.0 * (overlap + 1.0)) / update_rate;
frame_inc = (float)frame_size * (1.0 - overlap);
m = 1; // Discrimination between power peaks
frames = work_size / frame_inc; // Total number of frames
freq_step = (8192 / freq_res) / 2;
frame_buffer = calloc(frame_size, sizeof(float));
dft_bins = calloc(freq_res, sizeof(float));
frequencies = calloc(frames * psg_channels, sizeof(unsigned int));
volumes = calloc(frames * psg_channels, sizeof(unsigned int));
if ((dft_bins == NULL) || (frame_buffer == NULL) || (frequencies == NULL) || (volumes == NULL)) {
puts("Memory allocation failed\n");
return 1;
}
if (!make_LUTs(frame_size)) {
puts("Table generation failed\n");
return 1;
}
// Show recap
printf("Frames: %lu\n", frames);
printf("Overlap: %u%%\n", (unsigned int)(overlap * 100.0));
printf("Samplerate: %luHz -> %uHz\n", samplerate_in, 8192);
printf("Resolution: %uHz\n", freq_step);
printf("Update rate: %u/frame (%uHz @ %ufps)\n", updates_per_frame, update_rate, fps);
printf("PSG channels: %u\n", psg_channels);
printf("Mode: %s\n", modestr[mode]);
f = 0;
do {
// Copy frame
memcpy(frame_buffer, &work_buffer[f], frame_size * sizeof(float));
// Apply window
for (t = 0; t < frame_size; t++)
frame_buffer[t] *= window_lut[t];
// Do DFT
for (k = 0; k < freq_res; k++) { // For each output element
sum_r = 0;
sum_i = 0;
for (t = 0; t < frame_size; t++) { // For each input element
c = ((32768 * t * k / 2) / frame_size) & 0x7FFF;
in_r = frame_buffer[t];
sum_r += (in_r * cos_lut[c]);
sum_i += (in_r * sin_lut[c]);
}
sum_r /= frame_size;
sum_i /= frame_size;
dft_bins[k] = sqrt((sum_i * sum_i) + (sum_r * sum_r));
//if (f == 0) printf("SAMP=%f POW=%f (%uHz)\n", frame_buffer[k], dft_bins[k], k * freq_step);
}
// Find highest peaks
consolidate = 0;
for (c = 0; c < psg_channels; c++) {
idx = (frame_idx * psg_channels) + c;
if (consolidate) {
max_power -= 0.4;
frequencies[idx] = frequencies[idx - 1];
volumes[idx] = lintolog(max_power);
} else {
// Find highest power and its associated frequency (skip DC bin)
max_power = 0;
for (k = 1; k < (frame_size - 1); k++) {
if (dft_bins[k] > max_power) {
max_power = dft_bins[k];
kmax = k;
}
}
// Clear surrounding bins if needed
for (t = 0; t < m; t++) {
ofs = kmax - t - 1;
if (ofs >= 0) dft_bins[ofs] = 0;
ofs = kmax + t + 1;
if (ofs < freq_res) dft_bins[ofs] = 0;
}
frequencies[idx] = kmax;
volumes[idx] = lintolog(max_power);
}
consolidate = (max_power >= 0.4) ? 1 : 0;
if (frame_idx == 0) printf("C%lu=%uHz\n", c, kmax * freq_step); // DEBUG
}
frame_idx++;
f += frame_inc;
printf("\rComputing frame %lu/%lu.", frame_idx, frames);
} while (f < (work_size - frame_size));
puts(" Done.\n");
// Generate output file
strcpy(outfilepath, argv[argc - 1]);
ext = strlen(outfilepath) - 3;
if (mode == MODE_VGM) {
outfilepath[ext++] = 'v';
outfilepath[ext++] = 'g';
outfilepath[ext] = 'm';
} else {
outfilepath[ext++] = 'b';
outfilepath[ext++] = 'i';
outfilepath[ext] = 'n';
}
FILE * fo = fopen(outfilepath, "wb");
if (mode == MODE_RAW) {
out_raw(&out_buffer, frequencies, volumes, frame_idx, freq_res);
} else if (mode == MODE_VGM) {
out_vgm(&out_buffer, frequencies, volumes, frame_idx);
} else if (mode == MODE_NGP) {
out_ngp(&out_buffer, frequencies, volumes, frame_idx);
}
fwrite(out_buffer, file_length, 1, fo);
fclose(fo);
free(out_buffer);
puts("Output file written.");
printf("Size: %lukB\n", file_length / 1024);
// Generate simulation file if needed
if (sim) {
if (gen_sim(frame_size, frame_idx, psg_channels, frequencies, volumes))
puts("\nSimulation file written.\n");
}
free(frequencies);
free(volumes);
free(frame_buffer);
free(wave_buffer);
free(work_buffer);
free(dft_bins);
system("pause");
//return 0;
return 0;
}
uavcan::MonotonicTime getMonotonic()
{
uavcan::uint64_t usec = 0;
// Scope Critical section
{
CriticalSectionLocker locker;
volatile uavcan::uint64_t time = time_mono;
# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
volatile uavcan::uint32_t cnt = TIMX->CNT;
if (TIMX->SR & TIM_SR_UIF)
{
cnt = TIMX->CNT;
# endif
# if UAVCAN_STM32_NUTTX
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
{
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
# endif
time += USecPerOverflow;
}
usec = time + cnt;
# ifndef NDEBUG
static uavcan::uint64_t prev_usec = 0; // Self-test
UAVCAN_ASSERT(prev_usec <= usec);
(void)prev_usec;
prev_usec = usec;
# endif
} // End Scope Critical section
return uavcan::MonotonicTime::fromUSec(usec);
}
uavcan::UtcTime getUtc()
{
if (utc_set)
{
uavcan::uint64_t usec = 0;
{
CriticalSectionLocker locker;
usec = sampleUtcFromCriticalSection();
}
return uavcan::UtcTime::fromUSec(usec);
}
return uavcan::UtcTime();
}
static float lowpass(float xold, float xnew, float corner, float dt)
{
const float tau = 1.F / corner;
return (dt * xnew + tau * xold) / (dt + tau);
}
static void updateRatePID(uavcan::UtcDuration adjustment)
{
const uavcan::MonotonicTime ts = getMonotonic();
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
prev_utc_adj_at = ts;
const float adj_usec = float(adjustment.toUSec());
/*
* Target relative rate in PPM
* Positive to go faster
*/
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
/*
* Current relative rate in PPM
* Positive if the local clock is faster
*/
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
utc_prev_adj = adj_usec;
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
if (dt > 10)
{
utc_rel_rate_error_integral = 0;
}
else
{
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
utc_rel_rate_error_integral =
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
utc_rel_rate_error_integral =
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
}
/*
* Rate controller
*/
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
// lowsyslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
// total_rate_correction_ppm);
}
void adjustUtc(uavcan::UtcDuration adjustment)
{
MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized);
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set)
{
const uavcan::int64_t adj_usec = adjustment.toUSec();
{
CriticalSectionLocker locker;
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
{
time_utc = 1;
}
else
{
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
}
}
utc_set = true;
utc_locked = false;
utc_jump_cnt++;
utc_prev_adj = 0;
utc_rel_rate_ppm = 0;
}
else
{
updateRatePID(adjustment);
if (!utc_locked)
{
utc_locked =
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
(std::abs(utc_prev_adj) < utc_sync_params.lock_thres_offset.toUSec());
}
}
}
//-------------------------------------------------------------------
int fdct_wrapping(int N1, int N2, int nbscales, int nbangles_coarse, int allcurvelets, CpxNumMat& x, vector< vector<CpxNumMat> >& c)
{
//---------------------------------------------
assert(N1==x.m() && N2==x.n());
int F1 = N1/2; int F2 = N2/2;
// ifft original data
CpxNumMat T(x);
fftwnd_plan p = fftw2d_create_plan(N2, N1, FFTW_FORWARD, FFTW_ESTIMATE | FFTW_IN_PLACE);
fftwnd_one(p, (fftw_complex*)T.data(), NULL);
fftwnd_destroy_plan(p);
double sqrtprod = sqrt(double(N1*N2));
for(int j=0; j<N2; j++) for(int i=0; i<N1; i++) T(i,j) /= sqrtprod;
CpxOffMat O(N1, N2);
fdct_wrapping_fftshift(T, O);
//-----------------------------------------------------------------------------
vector<CpxOffMat> Xhghs;
Xhghs.resize(nbscales);
CpxOffMat X;
//unfold or not
if(allcurvelets==1) {
//--------------------------
double XL1 = 4.0*N1/3.0; double XL2 = 4.0*N2/3.0; //range
int XS1, XS2; int XF1, XF2; double XR1, XR2; fdct_wrapping_rangecompute(XL1, XL2, XS1, XS2, XF1, XF2, XR1, XR2);
IntOffVec t1(XS1);
for(int i=-XF1; i<-XF1+XS1; i++) if( i<-N1/2) t1(i) = i+int(N1); else if(i>(N1-1)/2) t1(i) = i-int(N1); else t1(i) = i;
IntOffVec t2(XS2);
for(int i=-XF2; i<-XF2+XS2; i++) if( i<-N2/2) t2(i) = i+int(N2); else if(i>(N2-1)/2) t2(i) = i-int(N2); else t2(i) = i;
X.resize(XS1, XS2);
for(int j=-XF2; j<-XF2+XS2; j++)
for(int i=-XF1; i<-XF1+XS1; i++)
X(i,j) = O(t1(i), t2(j));
DblOffMat lowpass(XS1,XS2);
fdct_wrapping_lowpasscompute(XL1, XL2, lowpass); //compute the low pass filter
for(int j=-XF2; j<-XF2+XS2; j++)
for(int i=-XF1; i<-XF1+XS1; i++)
X(i,j) *= lowpass(i,j);
} else {
//--------------------------
X = O;
}
//separate
double XL1 = 4.0*N1/3.0; double XL2 = 4.0*N2/3.0; //range
for(int sc=nbscales-1; sc>0; sc--) {
double XL1n = XL1/2; double XL2n = XL2/2;
int XS1n, XS2n; int XF1n, XF2n; double XR1n, XR2n;
fdct_wrapping_rangecompute(XL1n, XL2n, XS1n, XS2n, XF1n, XF2n, XR1n, XR2n);
//computer filter
DblOffMat lowpass(XS1n, XS2n);
fdct_wrapping_lowpasscompute(XL1n, XL2n, lowpass);
DblOffMat hghpass(XS1n, XS2n);
for(int j=-XF2n; j<-XF2n+XS2n; j++)
for(int i=-XF1n; i<-XF1n+XS1n; i++)
hghpass(i,j) = sqrt(1-lowpass(i,j)*lowpass(i,j));
//separate
CpxOffMat Xhgh(X);
for(int j=-XF2n; j<-XF2n+XS2n; j++)
for(int i=-XF1n; i<-XF1n+XS1n; i++)
Xhgh(i,j) *= hghpass(i,j);
CpxOffMat Xlow(XS1n, XS2n);
for(int j=-XF2n; j<-XF2n+XS2n; j++)
for(int i=-XF1n; i<-XF1n+XS1n; i++)
Xlow(i,j) = X(i,j) * lowpass(i,j);
//store and prepare for next level
Xhghs[sc] = Xhgh;
X = Xlow;
XL1 = XL1/2; XL2 = XL2/2;
}
Xhghs[0] = X;
//-----------------------------------------------------------------------------
vector<int> nbangles(nbscales);
if(allcurvelets==1) {
//nbangles
nbangles[0] = 1;
for(int sc=1; sc<nbscales; sc++) nbangles[sc] = nbangles_coarse * pow2( int(ceil(double(sc-1)/2)) );
//c
c.resize(nbscales);
for(int sc=0; sc<nbscales; sc++) c[sc].resize( nbangles[sc] );
double XL1 = 4.0*N1/3.0; double XL2 = 4.0*N2/3.0; //range
for(int sc=nbscales-1; sc>0; sc--) {
fdct_wrapping_sepangle(XL1, XL2, nbangles[sc], Xhghs[sc], c[sc]);
XL1 /= 2; XL2 /= 2;
}
fdct_wrapping_wavelet(Xhghs[0], c[0]);
} else {
//nbangles
nbangles[0] = 1;
for(int sc=1; sc<nbscales-1; sc++) nbangles[sc] = nbangles_coarse * pow2( int(ceil(double(sc-1)/2)) );
nbangles[nbscales-1] = 1;
//c
c.resize(nbscales);
for(int sc=0; sc<nbscales; sc++) c[sc].resize( nbangles[sc] );
fdct_wrapping_wavelet(Xhghs[nbscales-1], c[nbscales-1]);
double XL1 = 2.0*N1/3.0; double XL2 = 2.0*N2/3.0; //range
for(int sc=nbscales-2; sc>0; sc--) {
fdct_wrapping_sepangle(XL1, XL2, nbangles[sc], Xhghs[sc], c[sc]);
XL1 /= 2; XL2 /= 2;
}
fdct_wrapping_wavelet(Xhghs[0], c[0]);
}
return 0;
}
static void process_file(FILE *fin, FILE *fout, char *infile, char *outfile) {
int r;
greymap_t *gm;
potrace_bitmap_t *bm;
void *sm;
int x, y;
int count;
for (count=0; ; count++) {
r = gm_read(fin, &gm);
switch (r) {
case -1: /* system error */
fprintf(stderr, "mkbitmap: %s: %s\n", infile, strerror(errno));
exit(2);
case -2: /* corrupt file format */
fprintf(stderr, "mkbitmap: %s: file format error: %s\n", infile, gm_read_error);
exit(2);
case -3: /* empty file */
if (count>0) { /* end of file */
return;
}
fprintf(stderr, "mkbitmap: %s: empty file\n", infile);
exit(2);
case -4: /* wrong magic */
if (count>0) {
fprintf(stderr, "mkbitmap: %s: warning: junk at end of file\n", infile);
return;
}
fprintf(stderr, "mkbitmap: %s: file format not recognized\n", infile);
fprintf(stderr, "Possible input file formats are: pnm (pbm, pgm, ppm), bmp.\n");
exit(2);
case 1: /* unexpected end of file */
fprintf(stderr, "mkbitmap: %s: warning: premature end of file\n", infile);
break;
}
if (info.invert) {
for (y=0; y<gm->h; y++) {
for (x=0; x<gm->w; x++) {
GM_UPUT(gm, x, y, 255-GM_UGET(gm, x, y));
}
}
}
if (info.highpass) {
r = highpass(gm, info.lambda);
if (r) {
fprintf(stderr, "mkbitmap: %s: %s\n", infile, strerror(errno));
exit(2);
}
}
if (info.lowpass) {
lowpass(gm, info.lambda1);
}
if (info.scale == 1 && info.bilevel) { /* no interpolation necessary */
sm = threshold(gm, info.level);
gm_free(gm);
} else if (info.scale == 1) {
sm = gm;
} else if (info.linear) { /* linear interpolation */
sm = interpolate_linear(gm, info.scale, info.bilevel, info.level);
gm_free(gm);
} else { /* cubic interpolation */
sm = interpolate_cubic(gm, info.scale, info.bilevel, info.level);
gm_free(gm);
}
if (!sm) {
fprintf(stderr, "mkbitmap: %s: %s\n", infile, strerror(errno));
exit(2);
}
if (info.bilevel) {
bm = (potrace_bitmap_t *)sm;
bm_writepbm(fout, bm);
bm_free(bm);
} else {
gm = (greymap_t *)sm;
gm_writepgm(fout, gm, NULL, 1, GM_MODE_POSITIVE, 1.0);
gm_free(gm);
}
}
}
//-------------------------------------------------------------------------------
int ifdct_wrapping(int N1, int N2, int nbscales, int nbangles_coarse, int allcurvelets, vector< vector<CpxNumMat> >& c, CpxNumMat& x)
{
assert(nbscales==c.size() && nbangles_coarse==c[1].size());
//int F1 = N1/2; int F2 = N2/2;
//-------------------------------------------angles to Xhgh
vector<int> nbangles(nbscales);
vector<CpxOffMat> Xhghs; Xhghs.resize(nbscales);
if(allcurvelets==1) {
//----
nbangles[0] = 1;
for(int sc=1; sc<nbscales; sc++) nbangles[sc] = nbangles_coarse * pow2( int(ceil(double(sc-1)/2)) );
double XL1 = 4.0*N1/3.0; double XL2 = 4.0*N2/3.0;
for(int sc=nbscales-1; sc>0; sc--) {
fdct_wrapping_invsepangle(XL1, XL2, nbangles[sc], c[sc], Xhghs[sc]);
XL1 /= 2; XL2 /= 2;
}
fdct_wrapping_invwavelet(c[0], Xhghs[0]);
} else {
//----
nbangles[0] = 1;
for(int sc=1; sc<nbscales-1; sc++) nbangles[sc] = nbangles_coarse * pow2( int(ceil(double(sc-1)/2)) );
nbangles[nbscales-1] = 1;
fdct_wrapping_invwavelet(c[nbscales-1], Xhghs[nbscales-1]);
double XL1 = 2.0*N1/3.0; double XL2 = 2.0*N2/3.0;
for(int sc=nbscales-2; sc>0; sc--) {
fdct_wrapping_invsepangle(XL1, XL2, nbangles[sc], c[sc], Xhghs[sc]);
XL1 /= 2; XL2 /= 2;
}
fdct_wrapping_invwavelet(c[0], Xhghs[0]);
}
//-------------------------------------------xhghs to O
//combine
CpxOffMat X;
if(allcurvelets==1) {
double XL1 = 4.0*N1/3.0; double XL2 = 4.0*N2/3.0; //range
int XS1, XS2; int XF1, XF2; double XR1, XR2; fdct_wrapping_rangecompute(XL1, XL2, XS1, XS2, XF1, XF2, XR1, XR2);
X.resize(XS1, XS2);
} else {
X.resize(N1, N2);
}
double XL1 = 4.0*N1/3.0; double XL2 = 4.0*N2/3.0;
int XS1, XS2; int XF1, XF2; double XR1, XR2; fdct_wrapping_rangecompute(XL1, XL2, XS1, XS2, XF1, XF2, XR1, XR2);
for(int sc=nbscales-1; sc>0; sc--) {
double XL1n = XL1/2; double XL2n = XL2/2;
int XS1n, XS2n; int XF1n, XF2n; double XR1n, XR2n;
fdct_wrapping_rangecompute(XL1n, XL2n, XS1n, XS2n, XF1n, XF2n, XR1n, XR2n);
DblOffMat lowpass(XS1n, XS2n);
fdct_wrapping_lowpasscompute(XL1n, XL2n, lowpass);
DblOffMat hghpass(XS1n, XS2n);
for(int j=-XF2n; j<-XF2n+XS2n; j++)
for(int i=-XF1n; i<-XF1n+XS1n; i++)
hghpass(i,j) = sqrt(1-lowpass(i,j)*lowpass(i,j));
for(int j=-XF2n; j<-XF2n+XS2n; j++)
for(int i=-XF1n; i<-XF1n+XS1n; i++)
Xhghs[sc](i,j) *= hghpass(i,j);
for(int j=-XF2n; j<-XF2n+XS2n; j++)
for(int i=-XF1n; i<-XF1n+XS1n; i++)
Xhghs[sc-1](i,j) *= lowpass(i,j);
CpxOffMat& G = Xhghs[sc];
for(int j=G.t(); j<G.t()+G.n(); j++)
for(int i=G.s(); i<G.s()+G.m(); i++)
X(i,j) += G(i,j);
XL1 = XL1/2; XL2 = XL2/2;
fdct_wrapping_rangecompute(XL1, XL2, XS1, XS2, XF1, XF2, XR1, XR2);
}
for(int j=-XF2; j<-XF2+XS2; j++)
for(int i=-XF1; i<-XF1+XS1; i++)
X(i,j) += Xhghs[0](i,j);
// fold
CpxOffMat O(N1, N2);
if(allcurvelets==1) {
double XL1 = 4.0*N1/3.0; double XL2 = 4.0*N2/3.0;
int XS1, XS2; int XF1, XF2; double XR1, XR2; fdct_wrapping_rangecompute(XL1, XL2, XS1, XS2, XF1, XF2, XR1, XR2);
//times pou;
DblOffMat lowpass(XS1,XS2);
fdct_wrapping_lowpasscompute(XL1, XL2, lowpass);
for(int j=-XF2; j<-XF2+XS2; j++)
for(int i=-XF1; i<-XF1+XS1; i++)
X(i,j) *= lowpass(i,j);
IntOffVec t1(XS1);
for(int i=-XF1; i<-XF1+XS1; i++) if( i<-N1/2) t1(i) = i+int(N1); else if(i>(N1-1)/2) t1(i) = i-int(N1); else t1(i) = i;
IntOffVec t2(XS2);
for(int i=-XF2; i<-XF2+XS2; i++) if( i<-N2/2) t2(i) = i+int(N2); else if(i>(N2-1)/2) t2(i) = i-int(N2); else t2(i) = i;
for(int j=-XF2; j<-XF2+XS2; j++)
for(int i=-XF1; i<-XF1+XS1; i++)
O(t1(i), t2(j)) += X(i,j);
} else {
O = X;
}
//------------------------------------------------------------
CpxNumMat T(N1,N2);
fdct_wrapping_ifftshift(O, T);
fftwnd_plan p = fftw2d_create_plan(N2, N1, FFTW_BACKWARD, FFTW_ESTIMATE | FFTW_IN_PLACE);
fftwnd_one(p, (fftw_complex*)T.data(), NULL);
fftwnd_destroy_plan(p);
double sqrtprod = sqrt(double(N1*N2)); //scale
for(int i=0; i<N1; i++) for(int j=0; j<N2; j++) T(i,j) /= sqrtprod;
x = T;
//x.resize(N1, N2);
//fdct_wrapping_fftshift(T, x);
return 0;
}
FramePtr Demodulator::decode(const FrameAudio& frame_audio)
{
QVector<double> buffer;
buffer.reserve(frame_audio.count());
FIRFilter bandpass(FIRFilter::FIR_BANDPASS, settings_.window, SAMPLING_RATE, 220, 900, 2500);
FIRFilter lowpass(FIRFilter::FIR_LOWPASS, settings_.window, 528000, 12000, SAMPLING_RATE / 2);
int index = 0;
for (double value : frame_audio) {
double low = lowpass.process(value);
if (index % 40 == 0) {
buffer.push_back(bandpass.process(low));
}
index++;
for (int i = 0; i < 10; ++i) {
low = lowpass.process(0);
if (index % 40 == 0) {
buffer.push_back(bandpass.process(low));
}
index++;
}
}
RingBuffer<double> bufs1200(BITS_PER_BYTE);
RingBuffer<double> bufc1200(BITS_PER_BYTE);
RingBuffer<double> bufs2200(BITS_PER_BYTE);
RingBuffer<double> bufc2200(BITS_PER_BYTE);
QVector<double> diff_buff;
for (int i = 0; i < buffer.size(); ++i) {
bufs1200.push_back(sin(M_PI * 2 / SAMPLING_RATE * 1200.0 * i) * buffer[i]);
bufc1200.push_back(cos(M_PI * 2 / SAMPLING_RATE * 1200.0 * i) * buffer[i]);
bufs2200.push_back(sin(M_PI * 2 / SAMPLING_RATE * 2200.0 * i) * buffer[i]);
bufc2200.push_back(cos(M_PI * 2 / SAMPLING_RATE * 2200.0 * i) * buffer[i]);
double low = std::hypotf(bufs1200.sum(), bufc1200.sum());
double high = std::hypotf(bufs2200.sum() , bufc2200.sum());
diff_buff.push_back(low - high);
}
QVector<double> diff_buff_center = diff_buff;
qSort(diff_buff_center);
double min = diff_buff_center[diff_buff_center.size() / 4];
double max = diff_buff_center[diff_buff_center.size() / 4 + diff_buff_center.size() / 2];
double center = diff_buff_center[diff_buff_center.size() / 2];
for (int i = 0; i <= THRESHOLD_RESOLUTION; ++i) {
FramePtr frame;
process(settings_.verfy_fcs, center - std::fabs(center - min / THRESHOLD_WIDTH_RATIO) *
(1.0 * i / THRESHOLD_RESOLUTION), diff_buff, &frame);
if (frame) {
return frame;
}
process(settings_.verfy_fcs, center + std::fabs(center - max / THRESHOLD_WIDTH_RATIO) *
(1.0 * i / THRESHOLD_RESOLUTION), diff_buff, &frame);
if (frame) {
return frame;
}
}
return FramePtr();
}
static vec_t highpass(vec_t value, vec_t frac, vec_t *store)
{
return value - lowpass(value, frac, store);
}
static void lowpass3(vec3_t value, vec_t fracx, vec_t fracy, vec_t fracz, vec3_t store, vec3_t out)
{
out[0] = lowpass(value[0], fracx, &store[0]);
out[1] = lowpass(value[1], fracy, &store[1]);
out[2] = lowpass(value[2], fracz, &store[2]);
}
/************************************************************
Main Loop
************************************************************/
int main(void)
{
/* Confirm Power */
m_red(ON);
/* Initializations */
init();
usb_enable();
timer3_init();
int gy_previous_reading = 0;
/* Confirm successful initialization(s) */
m_green(ON);
/* Run */
while (1){
if (m_imu_raw(data))
{
m_green(ON);
m_red(OFF);
ax = lowpass(0.85,ax,data[0])+AX_OFFSET;
az = lowpass(0.85,az,data[2])+AZ_OFFSET;
gy = lowpass(ALPHA_LOW,gy,data[4])+GY_OFFSET;
gy = highpass(ALPHA_HIGH,gy,gy_previous_reading,data[4]);
gy_previous_reading = data[4];
/*
m_usb_tx_string("ax= ");
m_usb_tx_int(ax);
m_usb_tx_string(" az=");
m_usb_tx_int(az);
m_usb_tx_string(" gy=");
m_usb_tx_long(gy);
m_usb_tx_string("\n");
*/
int angle = ((float)ax*RAD2DEG)/sqrt(((float)ax*ax+(float)az*az));
if (check(TIFR3,OCF3A)){ //check if timestep has completed
angle += gy*TIMESTEP; //add thetadot*timestep to angle
set(TIFR3,OCF3A); //reset flag
}
m_usb_tx_int(angle);
m_usb_tx_string("\n");
/*
m_usb_tx_string("ax= ");
m_usb_tx_int(data[0]);
m_usb_tx_string(" ay= ");
m_usb_tx_int(data[1]);
m_usb_tx_string(" az= ");
m_usb_tx_int(data[2]);
m_usb_tx_string(" gx= ");
m_usb_tx_int(data[3]);
m_usb_tx_string(" gy= ");
m_usb_tx_int(data[4]);
m_usb_tx_string(" gz= ");
m_usb_tx_int(data[5]);
m_usb_tx_string("\n");
*/
}
else
{
m_green(OFF);
m_red(ON);
}
}
}
