#include "os_sensor.h"
#include <windows.h> // CreateFile()
#include <shlwapi.h> // wnsprintf()
#include "main.h" // print()
Sensor::Sensor():
handle(0),
start_paddingbytes(0),
stop_paddingbytes(0) {}
Sensor::~Sensor() {if (handle) CloseHandle((HANDLE)handle);}
// Nur Baudrate der Schnittstelle setzen
bool Sensor::setBaud(unsigned rate) const{
DCB dcb;
dcb.DCBlength=sizeof dcb;
HANDLE h=(HANDLE)handle;
if (!GetCommState(h,&dcb)) return false;
dcb.BaudRate=rate;
((DWORD*)&dcb)[2]=1; // Clear all flags except fBinary which must be set
dcb.ByteSize=8;
dcb.StopBits=ONESTOPBIT;
dcb.Parity=NOPARITY;
if (!SetCommState(h,&dcb)) {
print("Kann Baudrate %u nicht setzen!",rate);
return false;
}
return true;
}
// Schnittstelle öffnen und Baudrate auf 19200 stellen
bool Sensor::connect(unsigned ComNr,unsigned flags) {
this->~Sensor();
TCHAR s[16];
wnsprintf(s,16,TEXT("\\\\.\\COM%u"),ComNr+1);
HANDLE h=CreateFile(s,GENERIC_READ|GENERIC_WRITE,0,0,OPEN_EXISTING,flags,0);
if (h==INVALID_HANDLE_VALUE) h=0;
handle=(size_t)h;
if (!h) {
print("COM%u lässt sich nicht öffnen!",ComNr+1);
return false;
}
if (!setBaud()) return false;
COMMTIMEOUTS to={0,0,100,0,100};
return !!SetCommTimeouts(h,&to);
}
int Sensor::read(void*v,unsigned l) const{
// TODO: Paddingbytes (gibt es sie überhaupt?) hier überlesen
DWORD br;
if (!ReadFile((HANDLE)handle,v,l,&br,0)) {
print("Failed to %s data.","read");
return -1;
}
if (br!=l) print("Partial %s, %u of %u bytes.","read",br,l);
return br;
}
int Sensor::write(const void*v,unsigned l) const{
DWORD bw;
if (!WriteFile((HANDLE)handle,v,l,&bw,0)) {
print("Failed to %s data.","write");
return -1;
}
if (bw!=l) print("Partial %s, %u of %u bytes.","write",bw,l);
return bw;
}
bool Sensor::getValue(index_t index, unsigned&result, action_t action) const{
char w[2]={char(action),char(index)};
char r[5];
if (!query(w,2,r,5)) return false;
result=r[1]<<24|r[2]<<16|r[3]<<8|r[4]; // handmade byteswap avoids unaligned access
return true;
}
// Optional third parameter allows suppressing answer checking,
// which is needed for baudrate changing (maybe badly implemented on device side)
bool Sensor::setValue(index_t index, unsigned value, bool expect_answer) const{
char w[6]={
char(WRITE_VAL),
char(index),
char(value>>24), // handmade byteswap avoids unaligned access
char(value>>16),
char(value>>8),
char(value)};
char r;
return query(w,6,&r,expect_answer?1:0);
}
// Some high-level queries
unsigned Sensor::get_sensor_type() const{
unsigned ret;
if (!getValue(SENSOR_TYPE,ret)) {
print("Get sensor type failed.");
return 0;
}
return ret;
}
unsigned Sensor::get_result_data_selector() const{
unsigned ret;
if (!getValue(RESULT_DATA,ret)) {
print("Get result data selector failed.");
return 0;
}
return ret;
}
unsigned Sensor::get_baudrate() const{
unsigned ret;
if (!getValue(BAUDRATE,ret)) {
print("Get baudrate failed.");
return 0;
}
return ret;
}
unsigned Sensor::get_number_samples(unsigned*pro) const{
unsigned r;
if (!getValue(RADAR_PROFILE,r)) return 0;
unsigned h = r/100, l = r%100; // what an old-style f**k!!
if (pro) *pro=l; // return (and don't check) modulo-result
else if (l!=BIGBW_MEDIUM) return 0; // must be 16 (possibly other values are possible but undocumented)
if (h>5) return 0; // 0 upto 5 seem to be valid
return 1U<<(10-h); // 0:1024, 1:512, 2:256, 3:128, 4:64, 5:32 samples
}
bool Sensor::set_number_samples(unsigned n,unsigned pro) const{
switch (n) {
case 1024: break;
case 512: pro+=100; break;
case 256: pro+=200; break;
case 128: pro+=300; break;
case 64: pro+=400; break;
case 32: pro+=500; break;
default: return false;
}
return setValue(RADAR_PROFILE,pro);
}
// some functions returning strings
const char* Sensor::get_sensor_type_string() const{
return get_sensor_type()==SENSOR_TYPE_REACH_RS485 ? "Reach RS485" : "unknown";
}
const char* Sensor::get_exact_sensor_type_string() const{
unsigned r;
if (getValue(EXACT_SENSOR_TYPE,r)) switch (r) {
case EXACT_TYPE_REACH_GOD_MODE: return "EXACT_TYPE_REACH_GOD_MODE";
case EXACT_TYPE_REACH_SR: return "EXACT_TYPE_REACH_SR";
case EXACT_TYPE_REACH_BE: return "EXACT_TYPE_REACH_BE";
case EXACT_TYPE_REACH_HS: return "EXACT_TYPE_REACH_HS";
case EXACT_TYPE_REACH_HP: return "EXACT_TYPE_REACH_HP";
}
return "unknown";
}
char* Sensor::version_number_to_string(unsigned version_number) {
static char buf[32];
wnsprintfA(buf,sizeof buf,"%u.%u.%u%s",
(version_number >> 24) & 0xff, // major
(version_number >> 16) & 0xff, // minor
(version_number >> 8) & 0xff, // patch
version_number & 0xff ? "_WIP" : "");
return buf;
}
const char* Sensor::status_to_string(char error_code) {
switch (error_code) {
case API_SUCCESS: return "API_SUCCESS";
case API_SUCCESS_WEAK_SIGNAL: return "API_SUCCESS_WEAK_SIGNAL";
case API_ERROR: return "API_ERROR";
case API_ERROR_COMMAND: return "API_ERROR_COMMAND";
case API_ERROR_PARAMETER: return "API_ERROR_PARAMETER";
case API_ERROR_RANGE: return "API_ERROR_RANGE";
case API_ERROR_FORBIDDEN: return "API_ERROR_FORBIDDEN";
case API_ERROR_NO_TARGET: return "API_ERROR_NO_TARGET";
}
return "UNKNOWN";
}
const char** Sensor::get_result_data_selector_string() const{
const char**argv=new const char*[2]; // maximal 1 Argument und 1 Terminator
int argc=0;
unsigned result_data_selector = get_result_data_selector();
if (result_data_selector & SELECT_IQDATA) argv[argc++]="SELECT_IQDATA";
argv[argc]=0; // Liste terminieren
return argv;
}
// Some setters
bool Sensor::set_result_data_selector(unsigned measurement_type) const{
return setValue(RESULT_DATA,measurement_type);
}
bool Sensor::set_baudrate(unsigned baudrate) const{
if (!setValue(BAUDRATE,baudrate,false)) return false;
Sleep(1);
if (!setBaud(baudrate)) return false;
Sleep(1);
return true;
}
bool Sensor::save_parameters() const{
char cmd=SAVE_ALL, r;
return query(&cmd,1,&r,1);
}
bool Sensor::deserialize_iq_data(IqDataF&ret) const{
char cmd=MEASURE;
BYTE info[3];
if (!query(&cmd,1,info,sizeof info)) return false;
int nsamp = info[1]<<8 | info[2];
// Here, variable-length sample data follows the MEASURE query
int l=nsamp<<1; // Länge in Bytes
BYTE*p=(BYTE*)_alloca(l);
// Die A/D-Daten kommen 80h-zentriert an, wie bei einer Steinzeit-Soundkarte.
if (read(p,l)!=l) return false;
if (!ret.commit_undefined(nsamp)) return false;
float*q=ret.lineardata();
if (l) do *q++=float(*p++-0x80); while(--l); // zero-center and convert to float
return true;
}
bool Sensor::get_measurement(IqDataF&ret,unsigned data_selector) const{
if (!data_selector) data_selector = get_result_data_selector();
if (data_selector & SELECT_IQDATA) return deserialize_iq_data(ret);
return false;
}
void Sensor::disconnect() {
this->~Sensor();
handle=0;
}
bool Sensor::query(const void*w, int wlen, void*r, int rlen, bool check_result) const{
// Basic query method. Can be used not-writing and not-reading.
// Sends and returns fixed-size byte streams as-is.
// Can check first byte of answer whether it's API_SUCCESS (=1) value
if (wlen && write(w,wlen)!=wlen) return false;
if (!rlen) return true;
if (read(r,rlen)!=rlen) return false; // incomplete reads are errors here
if (check_result) {
char ans=*((char*)r);
if (ans!=API_SUCCESS) {
print("Wrong answer byte %d = %s",ans,status_to_string(ans));
PurgeComm((HANDLE)handle,15); // As PurgeComm() may be slow, it's only used in case of error
return false;
}
}
return true;
}
| Detected encoding: ANSI (CP1252) | 4
|
|
|