#include "main.h"
#include "os_sensor.h"
#include "mathe/window.h"
#include "mathe/generator.h"
#include "plotxy/plotxy.h" // XY_Float, Float (via gdix.h)
#include <cmath>
#include "util/compat.h" // lrint()
/* Worker-Thread */
HANDLE ghThread;
DWORD gThreadId;
volatile bool gEndThread;
static Sensor sensor;
//typedef float Float;
//static IqDataF plot1data;
static struct G{
Float A,offset,f1,f2;
}g;
static IqDataF noreflectsignal;
static char signal_sum_count;
bool worker::SaveSettings() {
HKEY k1,k2;
if (RegCreateKey(HKEY_CURRENT_USER,TEXT("Software\\h#s"),&k1)) return false;
TCHAR exepath[MAX_PATH];
if (!RegCreateKey(k1,getExeName(exepath),&k2)) {
DWORD l=DWORD(noreflectsignal.size())<<1;
if (l) { // don't save empty data
char*p=(char*)_alloca(l);
const float*q=noreflectsignal.lineardata();
for (DWORD i=0; i<l; i++) p[i]=char(lrint(limit(q[i],-128.f,127.f)));
RegSetValueEx(k2,TEXT("subsignal"),0,REG_BINARY,(BYTE*)p,l);
// TODO: Save other values
}
RegCloseKey(k2);
}
RegCloseKey(k1);
return true;
}
bool worker::LoadSettings() {
HKEY k1,k2;
bool okay=false;
if (RegOpenKey(HKEY_CURRENT_USER,TEXT("Software\\h#s"),&k1)) return false;
TCHAR exepath[MAX_PATH];
if (!RegOpenKey(k1,getExeName(exepath),&k2)) {
DWORD l;
if (!RegQueryValueEx(k2,TEXT("subsignal"),0,0,0,&l)
&& (l==64 || l==128 || l==256 || l==512 || l==1024 || l==2048)
&& noreflectsignal.commit_undefined(l>>1)) {
char*p=(char*)_alloca(l);
if (!RegQueryValueEx(k2,TEXT("subsignal"),0,0,(BYTE*)p,&l)) {
float*q=noreflectsignal.lineardata();
do *q++=*p++; while(--l); // char to float
okay=true;
}
}
// TODO: Load other values
RegCloseKey(k2);
}
RegCloseKey(k1);
return okay;
}
static class Fps{
DWORD tic,frames;
public:
void onFrame() {
frames++;
DWORD dt=GetTickCount()-tic;
if (dt>=200) { // mit max. 5 Hz aktualisieren
if (dt<=1000) { // sinnvolle Zeitspanne (<= 1 s)
SetDlgItemInt(ghMainWnd,107,MulDiv(frames,1000,dt),FALSE);
}
frames=0;
tic+=dt;
}
}
}fps;
static void doPlot(const IqDataF&data, bool polar, char itrace) {
if (!IsDlgButtonChecked(ghMainWnd,106)) return;
int sz = (int)data.size();
XY_Float*t0=new XY_Float[sz],*t1=new XY_Float[sz];
for (int i=0; i<sz; i++) {
const complex<Float>&m = data[i];
t0[i] = polar ? m.abs() : m.real();
t1[i] = polar ? m.arg(turns) : m.imag();
}
HWND hPlot=GetDlgItem(ghMainWnd,103);
PostMessage(hPlot,XY_SET_Y,MAKELONG(sz,itrace+0),(LPARAM)t0);
PostMessage(hPlot,XY_SET_Y,MAKELONG(sz,itrace+1),(LPARAM)t1);
PostMessage(hPlot,XY_UPDATE,0,3<<itrace);
}
static void doPlot(const vector<Float>&data, char itrace) {
if (!IsDlgButtonChecked(ghMainWnd,106)) return;
int sz = (int)data.size();
XY_Float*t=new XY_Float[sz]; // wird vom PlotXY freigegeben (Murks!)
memcpy(t,data.data(),sz*sizeof(Float));
HWND hPlot=GetDlgItem(ghMainWnd,103);
PostMessage(hPlot,XY_SET_Y,MAKELONG(sz,itrace),(LPARAM)t);
PostMessage(hPlot,XY_UPDATE,0,1<<itrace);
}
/* Python-Gedöhns */
static bool GleicherKram(IqDataF&result) {
static DWORD tic;
IqDataF mess;
if (!sensor.get_measurement(mess,sensor.SELECT_IQDATA)) {
print("get_measurement: Fehler!");
return false;
}
int sz = (int)mess.size();
if (!sz) {
print("get_measurement: Keine Daten!");
return false;
}
result=mess; // convert char to float
if (IsDlgButtonChecked(ghMainWnd,32)) {
if (signal_sum_count) noreflectsignal+=result; // add; .size() should match, otherwise, partial or repeated addition
else noreflectsignal=result; // assign and possibly change noreflectsignal.size()
if (++signal_sum_count==32) CheckDlgButton(ghMainWnd,32,BST_UNCHECKED);
// The user may abort acquiring the nonreflectsignal earlier,
// but in general case, the acquisition ends after 32 cycles, so it will take 1..2 seconds.
}else if (signal_sum_count) { // freshly unchecked?
noreflectsignal/=signal_sum_count;
signal_sum_count=0; // no more fresh
}
if (IsDlgButtonChecked(ghMainWnd,33)) {
result-=noreflectsignal; // does nothing when noreflectsignal.size() == 0
}
if (IsDlgButtonChecked(ghMainWnd,34)) {
static BlackmanWindow<Float> window;
window.resize(sz); // Normally, it does nothing, but sometimes, size changes
result*=window;
}
DWORD toc=GetTickCount();
if (DWORD(toc-tic)>=50) {
doPlot(result,false,0); // Ausgabe auf 20 Hz begrenzen: Auch das hilft nicht!
tic=toc; // Ohne Subtraktion kommt es zu keiner Plotausgabe!! Überlaufeffekt??
}
fps.onFrame();
return true;
}
Float plotWindowedFundamentalFreq(const numvec<Float>&sig,const SoftWindow<Float>&wnd,int tracebase) {
//FFT
ASSERT(sig.size()==wnd.size());
IqDataF X(FFT(sig*wnd));
// doPlot(X.arg(degrees),tracebase+10); // zu viel Gezappel bei uninteressanten Freqenzen!
vector<Float> ampspektrum(X.abs());
if (!(tracebase&1)) doPlot(ampspektrum,8+(tracebase>>1));
// Maximum im Amplitudenspektrum suchen.
// Da die Eingangswerte reell sind ist das Amplitudenspektrum mittensymmetrisch
int maxidx=-1, idx, size=int(ampspektrum.size());
Float maxval=0,maxphase;
for (idx=0; idx<size>>1; idx++) {
if (maxval<ampspektrum[idx]) {maxval=ampspektrum[idx]; maxidx=idx;}
}
maxval/=wnd.damp(); // Fensterdämpfung ausgleichen
maxval*=2; // Dämpfung durch Weglassen der Spiegelamplitude ausgleichen
maxval/=sig.size(); // FFT-Additionen zum Mittelwert umformen
// Da kommt, für 256 Samples, maxidx==5 heraus
// 1 cm Abstand, kurzes Rohr. Die Frequenz wird mit zunehmendem Abstand höher. Etwa maxidx==7.
// Da könnte man noch eine Kurveninterpolation machen, aber viel wird nicht passieren
maxphase=X[maxidx].arg(radians); // Zugehörige Phase im Bogenmaß entnehmen
// Mit dieser Information einen Ersatzsinus in den Graf einzeichnen.
// Dazu ist Amplitude und Phase bekannt.
// maxidx=0 wäre Gleichspannung (f=0)
// maxidx=size/2 wäre maximale Frequenz (jedes Sample wechselt die Seite)
// FFT arbeitet kosinusbasiert.
doPlot(generateCosinus(size,Float(maxidx)/size,maxphase,maxval)*wnd,tracebase+4);
return maxphase;
}
void update() {
IqDataF signal;
GleicherKram(signal);
static SoftWindow<float> windowMiddle,windowEnd;
int sz=int(signal.size());
if (windowMiddle.empty()) {
windowMiddle.rebuild(sz,MulDiv(sz,33,100),MulDiv(sz,66,100),sz>>4);
windowEnd.rebuild(sz,MulDiv(sz,66,100),sz,sz>>4);
doPlot(windowMiddle,2);
doPlot(windowEnd,3);
}
Float phase[4]={
plotWindowedFundamentalFreq(signal.real(),windowMiddle,0),
plotWindowedFundamentalFreq(signal.imag(),windowMiddle,1),
plotWindowedFundamentalFreq(signal.real(),windowEnd,2),
plotWindowedFundamentalFreq(signal.imag(),windowEnd,3)};
print("Mitte: Phase=%.1f°, Differenz=%.1f°; Ende: Phase=%.1f°, Differenz=%.1f°",
radians2degrees(phase[0]),
radians2degrees(radians_norm(phase[1]-phase[0])),
radians2degrees(phase[2]),
radians2degrees(radians_norm(phase[3]-phase[2])));
}
static DWORD innerThread(void*) {
// Tell the sensor to communicate IQ data
if (!sensor.set_result_data_selector(sensor.SELECT_IQDATA)) return 12;
ASSERT(sensor.set_number_samples(256));
int dps=216;
//print(sensor.safe_write(ip.RADAR_PROFILE_SELECTOR, dps)) // Das hier war's !!!
//print(sensor.get_number_samples())
// Request and retrieve IQ data and store it in a measurement object.
IqDataF signal_cal;
if (!GleicherKram(signal_cal)) return 13;
//Ohne Rohr 0.006, 0,016
//100 mm Rohr 0.016, 0.022
//195 mm Rohr 0.023, 0.032
//350 mm Rohr 0.035, 0.045
//200 mm Druckluft 0.023, 0.032
//350 mm Druckluft 0.035, 0.042
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float f1 = 0.008f, f2 = 0.03f; // ZoomFFT range of interest
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// initialize arrays
//IqDataF X_cal(ZoomFFT(signal_cal,f1,f2));
//IqDataF X_cal_sum = X_cal,
// X_start_sum = X_cal;
// unsigned i,k=10; // 10 is a good start value. To reduce noise, increase k
/*
//Calibarition against air at and of waveguide
if (MBox(16,MB_OKCANCEL)!=IDOK) return 16;
for (i=0; i<k; i++) {
IqDataF signal_cal;
GleicherKram(signal_cal);
X_cal = ZoomFFT(signal_cal,f1,f2);
X_cal_sum+=X_cal;
}
X_cal = X_cal_sum / (float)k;
//X_cal = 0 // switch off calibration
// measure spectrum at start position
if (MBox(17,MB_OKCANCEL)!=IDOK) return 17;
*/
// everage start value
// X_start_sum.clear();
// for (i=0; i<k; i++) {
// IqDataF signal_start;
// GleicherKram(signal_start);
// X_start_sum += ZoomFFT(signal_start,f1,f2);
// }
// IqDataF X_start = X_start_sum / (float)k;
// X_start -= X_cal;
// IqDataF X_start_phase = np.angle(X_start);
// start_range = np.argmax(abs(X_start));
// start_phase = X_start_phase[start_range];
// if (MBox(18,MB_OKCANCEL)!=IDOK) return 18;
while (!gEndThread) update();
return 0;
}
DWORD CALLBACK worker::ThreadProc(void*p) {
if (!sensor.connect(comsel.port)) return 10;
// Set the baud rate to a higher value for optimum speed:
if (!sensor.set_baudrate(decodeBaud(comsel.baudcode))) return 11;
DWORD ret=innerThread(p);
sensor.disconnect();
SetTimer(ghMainWnd,21,0,0); // Laufenden Timer auf 0 ms verkürzen
return ret;
}
#if 0
static struct X1DATA:public DATA{ // Abszisse
int size() const {return static_cast<int>(plot1data.size());}
Float get(int i) const {return (float)i;}
const TCHAR*name() const {return TEXT("Samples");}
const TCHAR*unit() const {return TEXT("Sa");}
void getRange(Range<Float>&r) const {r.init(0,float(plot1data.size()-1));}
}x1data;
static struct Y1DATA:public DATA{ // Werte
int size() const {return static_cast<int>(plot1data.size());}
Float get(int i) const {return plot1data[i].x;}
const TCHAR*name() const {return TEXT("Werte");}
}y1data;
static struct Y2DATA:public Y1DATA{ // Imaginärteile
Float get(int i) const {return plot1data[i].y;}
}y2data;
#endif
| Detected encoding: ANSI (CP1252) | 4
|
|
|