void AnalogDecoder::handleMagnitude(float val)
{
//
// Smooth
//
m_val = (FILTER_ALPHA)*m_val + (1.0 - FILTER_ALPHA)*val;
val = m_val;
//
// 1 of N
//
if(m_discardedSamples < (HW_RATIO-1))
{
m_discardedSamples++;
return;
}
m_discardedSamples = 0;
//
// Saturate
//
val = std::min(val, 1.0f);
//
// Threshold
//
m_ookMax -= OOK_DECAY_PER_SAMPLE;
m_ookMax = std::max(m_ookMax, val);
m_ookMax = std::max(m_ookMax, MIN_OOK_THRESHOLD/OOK_THRESHOLD_RATIO);
//
// Send to digital stage
//
int digital;
if(m_cb)
{
if(val > m_ookMax*OOK_THRESHOLD_RATIO)
{
digital = 1;
m_cb(1);
}
else
{
digital = 0;
m_cb(0);
}
}
}
point_count_t BpfReader::readPointMajor(PointViewPtr view, point_count_t count)
{
PointId nextId = view->size();
PointId idx = m_index;
point_count_t numRead = 0;
seekPointMajor(idx);
while (numRead < count && idx < numPoints())
{
for (size_t d = 0; d < m_dims.size(); ++d)
{
float f;
m_stream >> f;
view->setField(m_dims[d].m_id, nextId, f + m_dims[d].m_offset);
}
// Transformation only applies to X, Y and Z
double x = view->getFieldAs<double>(Dimension::Id::X, nextId);
double y = view->getFieldAs<double>(Dimension::Id::Y, nextId);
double z = view->getFieldAs<double>(Dimension::Id::Z, nextId);
m_header.m_xform.apply(x, y, z);
view->setField(Dimension::Id::X, nextId, x);
view->setField(Dimension::Id::Y, nextId, y);
view->setField(Dimension::Id::Z, nextId, z);
if (m_cb)
m_cb(*view, nextId);
idx++;
numRead++;
nextId++;
}
m_index = idx;
return numRead;
}
void invoke_cb(msg const& m)
{
if (!m_cb.empty())
{
m_cb(m);
m_cb.clear();
done();
}
}
void invoke_cb(msg const& m)
{
if (m_cb)
{
m_cb(m);
m_cb = nullptr;
done();
}
}
STDMETHODIMP CCameraDS::CSampleGrabberCB::BufferCB( double dblSampleTime, BYTE* pBuffer, long lBufferSize )
{
if (m_bCaptureVideo)
{
if (m_cb)
m_cb(m_nID, pBuffer, lBufferSize, m_pParam);
}
return S_OK;
}
bool PullerGetDeviceInfoTask::proceed(ModBusUART_Impl* modbus)
{
bool rc = true;
QString vendor,product,version;
if ( ! modbus->readDeviceInfo(getID(), getSpeed(), vendor, product, version))
{
if (++m_failCounter < Configurator::getRetryCount())
rc = false;
else
m_cb(std::make_shared<DeviceInfo>(m_uartName, getID()));
}
else
{
m_cb(std::make_shared<DeviceInfo>(m_uartName, getID(), getSpeed(), vendor, product, version));
}
return rc;
}
int ZKDataWatcher::onDataChange(const std::string& data, int version){
m_version = version;
if (m_data != data){
m_data = data;
if (m_cb){
return m_cb(m_ctx, version, data);
}
}
return 0;
}
point_count_t FauxReader::read(PointViewPtr view, point_count_t count)
{
for (PointId idx = 0; idx < count; ++idx)
{
PointRef point = view->point(idx);
if (!processOne(point))
break;
if (m_cb)
m_cb(*view, idx);
}
return count;
}
point_count_t SbetReader::read(PointViewPtr view, point_count_t count)
{
PointId nextId = view->size();
PointId idx = m_index;
point_count_t numRead = 0;
seek(idx);
while (numRead < count && idx < m_numPts)
{
PointRef point = view->point(nextId);
processOne(point);
if (m_cb)
m_cb(*view, nextId);
idx++;
nextId++;
numRead++;
}
m_index = idx;
return numRead;
}
void DisplayCallbackCintf::Display(IplImage* img, HandVu::HVAction action)
{
CV_FUNCNAME( "hvDisplayCallback" ); // declare cvFuncName
__BEGIN__;
hvAction act;
switch (action) {
case HandVu::HV_INVALID_ACTION:
act = HV_INVALID_ACTION; break;
case HandVu::HV_PROCESS_FRAME:
act = HV_PROCESS_FRAME; break;
case HandVu::HV_SKIP_FRAME:
act = HV_SKIP_FRAME; break;
case HandVu::HV_DROP_FRAME:
act = HV_DROP_FRAME; break;
default:
CV_ERROR(CV_StsError, "unknown HVAction code");
}
m_cb(img, act);
__END__;
}
point_count_t BpfReader::readDimMajor(PointViewPtr data, point_count_t count)
{
PointId idx(0);
PointId startId = data->size();
point_count_t numRead = 0;
for (size_t d = 0; d < m_dims.size(); ++d)
{
idx = m_index;
PointId nextId = startId;
numRead = 0;
seekDimMajor(d, idx);
for (; numRead < count && idx < numPoints(); idx++, numRead++, nextId++)
{
float f;
m_stream >> f;
data->setField(m_dims[d].m_id, nextId, f + m_dims[d].m_offset);
}
}
m_index = idx;
// Transformation only applies to X, Y and Z
for (PointId idx = startId; idx < data->size(); idx++)
{
double x = data->getFieldAs<double>(Dimension::Id::X, idx);
double y = data->getFieldAs<double>(Dimension::Id::Y, idx);
double z = data->getFieldAs<double>(Dimension::Id::Z, idx);
m_header.m_xform.apply(x, y, z);
data->setField(Dimension::Id::X, idx, x);
data->setField(Dimension::Id::Y, idx, y);
data->setField(Dimension::Id::Z, idx, z);
if (m_cb)
m_cb(*data, idx);
}
return numRead;
}
point_count_t OptechReader::read(PointViewPtr data,
point_count_t countRequested)
{
point_count_t numRead = 0;
point_count_t dataIndex = data->size();
while (numRead < countRequested)
{
if (m_returnIndex == 0)
{
if (!m_extractor.good())
{
if (m_recordIndex >= m_header.numRecords)
{
break;
}
m_recordIndex += fillBuffer();
}
m_extractor >> m_pulse.gpsTime >> m_pulse.returnCount >>
m_pulse.range[0] >> m_pulse.range[1] >> m_pulse.range[2] >>
m_pulse.range[3] >> m_pulse.intensity[0] >>
m_pulse.intensity[1] >> m_pulse.intensity[2] >>
m_pulse.intensity[3] >> m_pulse.scanAngle >> m_pulse.roll >>
m_pulse.pitch >> m_pulse.heading >> m_pulse.latitude >>
m_pulse.longitude >> m_pulse.elevation;
if (m_pulse.returnCount == 0)
{
m_returnIndex = 0;
continue;
}
// In all the csd files that we've tested, the longitude
// values have been less than -2pi.
if (m_pulse.longitude < -M_PI * 2)
{
m_pulse.longitude = m_pulse.longitude + M_PI * 2;
}
else if (m_pulse.longitude > M_PI * 2)
{
m_pulse.longitude = m_pulse.longitude - M_PI * 2;
}
}
georeference::Xyz gpsPoint = georeference::Xyz(
m_pulse.longitude, m_pulse.latitude, m_pulse.elevation);
georeference::RotationMatrix rotationMatrix =
createOptechRotationMatrix(m_pulse.roll, m_pulse.pitch,
m_pulse.heading);
georeference::Xyz point = pdal::georeference::georeferenceWgs84(
m_pulse.range[m_returnIndex], m_pulse.scanAngle,
m_boresightMatrix, rotationMatrix, gpsPoint);
data->setField(Dimension::Id::X, dataIndex, point.X * 180 / M_PI);
data->setField(Dimension::Id::Y, dataIndex, point.Y * 180 / M_PI);
data->setField(Dimension::Id::Z, dataIndex, point.Z);
data->setField(Dimension::Id::GpsTime, dataIndex, m_pulse.gpsTime);
if (m_returnIndex == MaximumNumberOfReturns - 1)
{
data->setField(Dimension::Id::ReturnNumber, dataIndex,
m_pulse.returnCount);
}
else
{
data->setField(Dimension::Id::ReturnNumber, dataIndex,
m_returnIndex + 1);
}
data->setField(Dimension::Id::NumberOfReturns, dataIndex,
m_pulse.returnCount);
data->setField(Dimension::Id::EchoRange, dataIndex,
m_pulse.range[m_returnIndex]);
data->setField(Dimension::Id::Intensity, dataIndex,
m_pulse.intensity[m_returnIndex]);
data->setField(Dimension::Id::ScanAngleRank, dataIndex,
m_pulse.scanAngle * 180 / M_PI);
if (m_cb)
m_cb(*data, dataIndex);
++dataIndex;
++numRead;
++m_returnIndex;
if (m_returnIndex >= m_pulse.returnCount ||
m_returnIndex >= MaximumNumberOfReturns)
{
m_returnIndex = 0;
}
}
return numRead;
}
bool utp_socket_manager::incoming_packet(error_code const& ec, udp::endpoint const& ep
, char const* p, int size)
{
// TODO: 2 we may want to take ec into account here. possibly close
// connections quicker
TORRENT_UNUSED(ec);
// UTP_LOGV("incoming packet size:%d\n", size);
if (size < int(sizeof(utp_header))) return false;
utp_header const* ph = reinterpret_cast<utp_header const*>(p);
// UTP_LOGV("incoming packet version:%d\n", int(ph->get_version()));
if (ph->get_version() != 1) return false;
const time_point receive_time = clock_type::now();
// parse out connection ID and look for existing
// connections. If found, forward to the utp_stream.
boost::uint16_t id = ph->connection_id;
// first test to see if it's the same socket as last time
// in most cases it is
if (m_last_socket
&& utp_match(m_last_socket, ep, id))
{
return utp_incoming_packet(m_last_socket, p, size, ep, receive_time);
}
std::pair<socket_map_t::iterator, socket_map_t::iterator> r =
m_utp_sockets.equal_range(id);
for (; r.first != r.second; ++r.first)
{
if (!utp_match(r.first->second, ep, id)) continue;
bool ret = utp_incoming_packet(r.first->second, p, size, ep, receive_time);
if (ret) m_last_socket = r.first->second;
return ret;
}
// UTP_LOGV("incoming packet id:%d source:%s\n", id, print_endpoint(ep).c_str());
if (!m_sett.get_bool(settings_pack::enable_incoming_utp))
return false;
// if not found, see if it's a SYN packet, if it is,
// create a new utp_stream
if (ph->get_type() == ST_SYN)
{
// possible SYN flood. Just ignore
if (int(m_utp_sockets.size()) > m_sett.get_int(settings_pack::connections_limit) * 2)
return false;
// UTP_LOGV("not found, new connection id:%d\n", m_new_connection);
boost::shared_ptr<socket_type> c(new (std::nothrow) socket_type(m_sock.get_io_service()));
if (!c) return false;
TORRENT_ASSERT(m_new_connection == -1);
// create the new socket with this ID
m_new_connection = id;
instantiate_connection(m_sock.get_io_service(), aux::proxy_settings(), *c
, m_ssl_context, this, true, false);
utp_stream* str = NULL;
#ifdef TORRENT_USE_OPENSSL
if (is_ssl(*c))
str = &c->get<ssl_stream<utp_stream> >()->next_layer();
else
#endif
str = c->get<utp_stream>();
TORRENT_ASSERT(str);
int link_mtu, utp_mtu;
mtu_for_dest(ep.address(), link_mtu, utp_mtu);
utp_init_mtu(str->get_impl(), link_mtu, utp_mtu);
bool ret = utp_incoming_packet(str->get_impl(), p, size, ep, receive_time);
if (!ret) return false;
m_cb(c);
// the connection most likely changed its connection ID here
// we need to move it to the correct ID
return true;
}
if (ph->get_type() == ST_RESET) return false;
// #error send reset
return false;
}
void callback(std::vector<uint8_t> data)
{
auto t = std::make_shared<T>();
Parse(*t, std::move(data));
m_cb(std::move(t));
}
void do_callback()
{
m_cb();
}
void GaitConfigItem::called(float value)
{
*m_value = (double)value;
if(m_cb)
m_cb();
}
point_count_t GreyhoundReader::read(PointViewPtr view, point_count_t count)
{
const std::string url(m_params.root() + "read" + m_params.qs());
log()->get(LogLevel::Debug) << "Reading: " << url << std::endl;
auto response(m_arbiter->getBinary(url));
const std::size_t pointSize(view->layout()->pointSize());
uint32_t numPoints(0);
std::copy(
response.data() + response.size() - sizeof(uint32_t),
response.data() + response.size(),
reinterpret_cast<char*>(&numPoints));
log()->get(LogLevel::Debug) <<
"Fetched " << numPoints << " points" << std::endl;
log()->get(LogLevel::Debug) <<
"Fetched " << response.size() << " bytes" << std::endl;
response.resize(response.size() - sizeof(uint32_t));
const auto dimTypes(m_readLayout.dimTypes());
#ifdef PDAL_HAVE_LAZPERF
auto cb = [this, &view, &dimTypes](char *buf, size_t bufsize)
{
view->setPackedPoint(dimTypes, view->size(), buf);
if (m_cb)
m_cb(*view, view->size() - 1);
};
LazPerfDecompressor(cb, dimTypes, numPoints).
decompress(response.data(), response.size());
#else
const char* end(response.data() + response.size());
for (const char* pos(response.data()); pos < end; pos += pointSize)
{
view->setPackedPoint(dimTypes, view->size(), pos);
if (m_cb)
m_cb(*view, view->size() - 1);
}
#endif
if (!m_params.obounds().isNull())
{
greyhound::Bounds obounds(m_params.obounds());
greyhound::Point p;
for (std::size_t i(0); i < view->size(); ++i)
{
p.x = view->getFieldAs<double>(Dimension::Id::X, i);
p.y = view->getFieldAs<double>(Dimension::Id::Y, i);
p.z = view->getFieldAs<double>(Dimension::Id::Z, i);
if (!obounds.contains(p))
view->setField(Dimension::Id::Omit, i, 1);
}
}
for (std::size_t i(0); i < view->size(); ++i)
{
view->setField(Dimension::Id::PointId, i, i);
}
return numPoints;
}
bool utp_socket_manager::incoming_packet(error_code const& ec, udp::endpoint const& ep
, char const* p, int size)
{
// UTP_LOGV("incoming packet size:%d\n", size);
if (size < int(sizeof(utp_header))) return false;
utp_header const* ph = (utp_header*)p;
// UTP_LOGV("incoming packet version:%d\n", int(ph->get_version()));
if (ph->get_version() != 1) return false;
const ptime receive_time = time_now_hires();
// parse out connection ID and look for existing
// connections. If found, forward to the utp_stream.
boost::uint16_t id = ph->connection_id;
// first test to see if it's the same socket as last time
// in most cases it is
if (m_last_socket
&& utp_match(m_last_socket, ep, id))
{
return utp_incoming_packet(m_last_socket, p, size, ep, receive_time);
}
std::pair<socket_map_t::iterator, socket_map_t::iterator> r =
m_utp_sockets.equal_range(id);
for (; r.first != r.second; ++r.first)
{
if (!utp_match(r.first->second, ep, id)) continue;
bool ret = utp_incoming_packet(r.first->second, p, size, ep, receive_time);
if (ret) m_last_socket = r.first->second;
return ret;
}
// UTP_LOGV("incoming packet id:%d source:%s\n", id, print_endpoint(ep).c_str());
if (!m_sett.enable_incoming_utp)
return false;
// if not found, see if it's a SYN packet, if it is,
// create a new utp_stream
if (ph->get_type() == ST_SYN)
{
// possible SYN flood. Just ignore
if (m_utp_sockets.size() > m_sett.connections_limit * 2)
return false;
// UTP_LOGV("not found, new connection id:%d\n", m_new_connection);
boost::shared_ptr<socket_type> c(new (std::nothrow) socket_type(m_sock.get_io_service()));
if (!c) return false;
TORRENT_ASSERT(m_new_connection == -1);
// create the new socket with this ID
m_new_connection = id;
instantiate_connection(m_sock.get_io_service(), proxy_settings(), *c, 0, this);
utp_stream* str = c->get<utp_stream>();
TORRENT_ASSERT(str);
int link_mtu, utp_mtu;
mtu_for_dest(ep.address(), link_mtu, utp_mtu);
utp_init_mtu(str->get_impl(), link_mtu, utp_mtu);
bool ret = utp_incoming_packet(str->get_impl(), p, size, ep, receive_time);
if (!ret) return false;
m_cb(c);
// the connection most likely changed its connection ID here
// we need to move it to the correct ID
return true;
}
// #error send reset
return false;
}
void timer::functor::call()
{
if (m_cb)
m_cb();
}
void EventListener::Notify()
{
m_cb( m_data );
}
point_count_t BpfReader::readByteMajor(PointViewPtr data, point_count_t count)
{
PointId idx(0);
PointId startId = data->size();
point_count_t numRead = 0;
// We need a temp buffer for the point data
union uu
{
float f;
uint32_t u32;
};
std::unique_ptr<union uu> uArr(
new uu[std::min(count, numPoints() - m_index)]);
for (size_t d = 0; d < m_dims.size(); ++d)
{
for (size_t b = 0; b < sizeof(float); ++b)
{
idx = m_index;
numRead = 0;
PointId nextId = startId;
seekByteMajor(d, b, idx);
for (;numRead < count && idx < numPoints();
idx++, numRead++, nextId++)
{
union uu& u = *(uArr.get() + numRead);
if (b == 0)
u.u32 = 0;
uint8_t u8;
m_stream >> u8;
u.u32 |= ((uint32_t)u8 << (b * CHAR_BIT));
if (b == 3)
{
u.f += m_dims[d].m_offset;
data->setField(m_dims[d].m_id, nextId, u.f);
}
}
}
}
m_index = idx;
// Transformation only applies to X, Y and Z
for (PointId idx = startId; idx < data->size(); idx++)
{
double x = data->getFieldAs<double>(Dimension::Id::X, idx);
double y = data->getFieldAs<double>(Dimension::Id::Y, idx);
double z = data->getFieldAs<double>(Dimension::Id::Z, idx);
m_header.m_xform.apply(x, y, z);
data->setField(Dimension::Id::X, idx, x);
data->setField(Dimension::Id::Y, idx, y);
data->setField(Dimension::Id::Z, idx, z);
if (m_cb)
m_cb(*data, idx);
}
return numRead;
}
void IMU_Gyro::read_report() {
gyro_report report;
IMU_Sensor::read_report(&report, sizeof (report));
m_cb(report);
}
