/**
* \brief Returns a PIX_FMT_RGBA32 buffer containg a frame from the video.
*
* \param pginfo Recording to grab from.
* \param filename File containing recording.
* \param seektime Seconds or frames into the video to seek before
* capturing a frame.
* \param time_in_secs if true time is in seconds, otherwise it is in frames.
* \param bufferlen Returns size of buffer returned (in bytes).
* \param video_width Returns width of frame grabbed.
* \param video_height Returns height of frame grabbed.
* \param video_aspect Returns aspect ratio of frame grabbed.
* \return Buffer allocated with new containing frame in RGBA32 format if
* successful, NULL otherwise.
*/
char *PreviewGenerator::GetScreenGrab(
const ProgramInfo &pginfo, const QString &filename,
long long seektime, bool time_in_secs,
int &bufferlen,
int &video_width, int &video_height, float &video_aspect)
{
(void) pginfo;
(void) filename;
(void) seektime;
(void) time_in_secs;
(void) bufferlen;
(void) video_width;
(void) video_height;
char *retbuf = NULL;
bufferlen = 0;
if (!MSqlQuery::testDBConnection())
{
LOG(VB_GENERAL, LOG_ERR, LOC + "Previewer could not connect to DB.");
return NULL;
}
// pre-test local files for existence and size. 500 ms speed-up...
if (filename.left(1)=="/")
{
QFileInfo info(filename);
bool invalid = (!info.exists() || !info.isReadable() ||
(info.isFile() && (info.size() < 8*1024)));
if (invalid)
{
LOG(VB_GENERAL, LOG_ERR, LOC + "Previewer file " +
QString("'%1'").arg(filename) + " is not valid.");
return NULL;
}
}
RingBuffer *rbuf = RingBuffer::Create(filename, false, false, 0);
if (!rbuf->IsOpen())
{
LOG(VB_GENERAL, LOG_ERR, LOC + "Previewer could not open file: " +
QString("'%1'").arg(filename));
delete rbuf;
return NULL;
}
PlayerContext *ctx = new PlayerContext(kPreviewGeneratorInUseID);
ctx->SetRingBuffer(rbuf);
ctx->SetPlayingInfo(&pginfo);
ctx->SetPlayer(new MythPlayer());
ctx->player->SetPlayerInfo(NULL, NULL, true, ctx);
if (time_in_secs)
retbuf = ctx->player->GetScreenGrab(seektime, bufferlen,
video_width, video_height, video_aspect);
else
retbuf = ctx->player->GetScreenGrabAtFrame(
seektime, true, bufferlen,
video_width, video_height, video_aspect);
delete ctx;
if (retbuf)
{
LOG(VB_GENERAL, LOG_INFO, LOC +
QString("Grabbed preview '%0' %1x%2@%3%4")
.arg(filename).arg(video_width).arg(video_height)
.arg(seektime).arg((time_in_secs) ? "s" : "f"));
}
return retbuf;
}
int
LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_accel_interval = 0;
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_accel_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 500)
return -EINVAL;
/* adjust filters */
accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_accel_call.period = _call_accel_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_accel_interval == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_accel_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_accel_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _accel_reports->size();
case SENSORIOCRESET:
reset();
return OK;
case ACCELIOCSSAMPLERATE:
return accel_set_samplerate(arg);
case ACCELIOCGSAMPLERATE:
return _accel_samplerate;
case ACCELIOCSLOWPASS: {
return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
}
case ACCELIOCGLOWPASS:
return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
memcpy(&_accel_scale, s, sizeof(_accel_scale));
return OK;
} else {
return -EINVAL;
}
}
case ACCELIOCSRANGE:
/* arg needs to be in G */
return accel_set_range(arg);
case ACCELIOCGRANGE:
/* convert to m/s^2 and return rounded in G */
return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSELFTEST:
return accel_self_test();
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
int mythfile_open(const char *pathname, int flags)
{
LOG(VB_FILE, LOG_DEBUG, QString("mythfile_open('%1', %2)")
.arg(pathname).arg(flags));
struct stat fileinfo;
if (mythfile_stat(pathname, &fileinfo))
return -1;
if (S_ISDIR( fileinfo.st_mode )) // libmythdvdnav tries to open() a dir
return errno = EISDIR, -1;
int fileID = -1;
if (strncmp(pathname, "myth://", 7))
{
int lfd = open(pathname, flags);
if (lfd < 0)
return -1;
m_fileWrapperLock.lockForWrite();
fileID = getNextFileID();
m_localfiles[fileID] = lfd;
m_filenames[fileID] = pathname;
m_fileWrapperLock.unlock();
}
else
{
RingBuffer *rb = NULL;
RemoteFile *rf = NULL;
if ((fileinfo.st_size < 51200) &&
(fileinfo.st_mtime < (time(NULL) - 300)))
{
if (flags & O_WRONLY)
rf = new RemoteFile(pathname, true, false); // Writeable
else
rf = new RemoteFile(pathname, false, true); // Read-Only
if (!rf)
return -1;
}
else
{
if (flags & O_WRONLY)
rb = RingBuffer::Create(
pathname, true, false,
RingBuffer::kDefaultOpenTimeout, true); // Writeable
else
rb = RingBuffer::Create(
pathname, false, true,
RingBuffer::kDefaultOpenTimeout, true); // Read-Only
if (!rb)
return -1;
rb->Start();
}
m_fileWrapperLock.lockForWrite();
fileID = getNextFileID();
if (rf)
m_remotefiles[fileID] = rf;
else if (rb)
m_ringbuffers[fileID] = rb;
m_filenames[fileID] = pathname;
m_fileWrapperLock.unlock();
}
m_callbackLock.lock();
if (!m_fileOpenCallbacks.isEmpty())
{
QString path(pathname);
QHashIterator<QString,Callback> it(m_fileOpenCallbacks);
while (it.hasNext())
{
it.next();
if (path.startsWith(it.key()))
it.value().m_callback(it.value().m_object);
}
}
m_callbackLock.unlock();
return fileID;
}
int
LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_mag_interval = 0;
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* 100 Hz is max for mag */
return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_mag_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 1000)
return -EINVAL;
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_mag_call.period = _call_mag_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_mag_interval == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_mag_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_mag_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _mag_reports->size();
case SENSORIOCRESET:
reset();
return OK;
case MAGIOCSSAMPLERATE:
return mag_set_samplerate(arg);
case MAGIOCGSAMPLERATE:
return _mag_samplerate;
case MAGIOCSLOWPASS:
case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
case MAGIOCSSCALE:
/* copy scale in */
memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
return OK;
case MAGIOCGSCALE:
/* copy scale out */
memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
return OK;
case MAGIOCSRANGE:
return mag_set_range(arg);
case MAGIOCGRANGE:
return _mag_range_ga;
case MAGIOCSELFTEST:
return mag_self_test();
case MAGIOCGEXTERNAL:
/* no external mag board yet */
return 0;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
void
LSM303D::mag_measure()
{
if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
perf_count(_reg7_resets);
reset();
return;
}
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_mag_report;
#pragma pack(pop)
mag_report mag_report;
/* start the performance counter */
perf_begin(_mag_sample_perf);
/* fetch data from the sensor */
memset(&raw_mag_report, 0, sizeof(raw_mag_report));
raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
mag_report.timestamp = hrt_absolute_time();
mag_report.x_raw = raw_mag_report.x;
mag_report.y_raw = raw_mag_report.y;
mag_report.z_raw = raw_mag_report.z;
mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report.scaling = _mag_range_scale;
mag_report.range_ga = (float)_mag_range_ga;
_mag_reports->force(&mag_report);
/* XXX please check this poll_notify, is it the right one? */
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
/* stop the perf counter */
perf_end(_mag_sample_perf);
}
int VSICurlStreamingHandle::ReceivedBytes(GByte *buffer, size_t count, size_t nmemb)
{
size_t nSize = count * nmemb;
nBodySize += nSize;
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Receiving %d bytes...", (int)nSize);
if( bHasCandidateFileSize && bCanTrustCandidateFileSize && !bHastComputedFileSize )
{
poFS->AcquireMutex();
CachedFileProp* cachedFileProp = poFS->GetCachedFileProp(pszURL);
cachedFileProp->fileSize = fileSize = nCandidateFileSize;
cachedFileProp->bHastComputedFileSize = bHastComputedFileSize = TRUE;
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "File size = " CPL_FRMT_GUIB, fileSize);
poFS->ReleaseMutex();
}
AcquireMutex();
if (eExists == EXIST_UNKNOWN)
{
poFS->AcquireMutex();
CachedFileProp* cachedFileProp = poFS->GetCachedFileProp(pszURL);
cachedFileProp->eExists = eExists = EXIST_YES;
poFS->ReleaseMutex();
}
else if (eExists == EXIST_NO)
{
ReleaseMutex();
return 0;
}
while(TRUE)
{
size_t nFree = oRingBuffer.GetCapacity() - oRingBuffer.GetSize();
if (nSize <= nFree)
{
oRingBuffer.Write(buffer, nSize);
/* Signal to the consumer that we have added bytes to the buffer */
CPLCondSignal(hCondProducer);
if (bAskDownloadEnd)
{
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Download interruption asked");
ReleaseMutex();
return 0;
}
break;
}
else
{
oRingBuffer.Write(buffer, nFree);
buffer += nFree;
nSize -= nFree;
/* Signal to the consumer that we have added bytes to the buffer */
CPLCondSignal(hCondProducer);
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Waiting for reader to consume some bytes...");
while(oRingBuffer.GetSize() == oRingBuffer.GetCapacity() && !bAskDownloadEnd)
{
CPLCondWait(hCondConsumer, hRingBufferMutex);
}
if (bAskDownloadEnd)
{
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Download interruption asked");
ReleaseMutex();
return 0;
}
}
}
ReleaseMutex();
return nmemb;
}
int
LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return 0;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return 0;
}
break;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
static int RunCCExtract(const ProgramInfo &program_info)
{
QString filename = program_info.GetPlaybackURL();
if (filename.startsWith("myth://"))
{
QString msg =
QString("Only locally accessible files are supported (%1).")
.arg(program_info.GetPathname());
cerr << qPrintable(msg) << endl;
return GENERIC_EXIT_INVALID_CMDLINE;
}
if (!QFile::exists(filename))
{
cerr << qPrintable(
QString("Could not open input file (%1).").arg(filename)) << endl;
return GENERIC_EXIT_INVALID_CMDLINE;
}
RingBuffer *tmprbuf = RingBuffer::Create(filename, false);
if (!tmprbuf)
{
cerr << qPrintable(QString("Unable to create RingBuffer for %1")
.arg(filename)) << endl;
return GENERIC_EXIT_PERMISSIONS_ERROR;
}
if (program_info.GetRecordingEndTime() > MythDate::current())
{
cout << "Program will end @ "
<< qPrintable(program_info.GetRecordingEndTime(MythDate::ISODate))
<< endl;
tmprbuf->SetWaitForWrite();
}
PlayerFlags flags = (PlayerFlags)(kVideoIsNull | kAudioMuted |
kDecodeNoLoopFilter | kDecodeFewBlocks |
kDecodeLowRes | kDecodeSingleThreaded |
kDecodeNoDecode);
MythCCExtractorPlayer *ccp = new MythCCExtractorPlayer(flags, true, filename);
PlayerContext *ctx = new PlayerContext(kCCExtractorInUseID);
ctx->SetPlayingInfo(&program_info);
ctx->SetRingBuffer(tmprbuf);
ctx->SetPlayer(ccp);
ccp->SetPlayerInfo(NULL, NULL, ctx);
if (ccp->OpenFile() < 0)
{
cerr << "Failed to open " << qPrintable(filename) << endl;
return GENERIC_EXIT_NOT_OK;
}
if (!ccp->run())
{
cerr << "Failed to decode " << qPrintable(filename) << endl;
return GENERIC_EXIT_NOT_OK;
}
delete ctx;
return GENERIC_EXIT_OK;
}
int
AK8975::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} ak8975_report;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret = -EIO;
uint8_t cmd;
perf_begin(_sample_perf);
struct mag_report new_report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* check data ready bit */
if (read_reg(ADDR_DRDY, cmd)) {
perf_count(_comms_errors);
debug("data ready read error");
goto out;
}
if (!(cmd & STATUS_REG_DATA_READY)) {
/* data not ready */
perf_count(_comms_errors);
debug("data not ready [%02X]", cmd);
goto out;
}
/* get measurements from the device */
cmd = ADDR_DATA_OUT_X_LSB;
ret = transfer(&cmd, 1, (uint8_t *)&ak8975_report, sizeof(ak8975_report));
if (ret != OK) {
perf_count(_comms_errors);
debug("data/status read error");
goto out;
}
/* check for overflow and not ready error */
if (read_reg(ADDR_STATUS, cmd)) {
perf_count(_comms_errors);
debug("data status read error");
goto out;
}
if (cmd & (STATUS_REG_DATA_ERROR | STATUS_REG_HOFL)) {
/* data value error */
perf_count(_comms_errors);
debug("data value error");
goto out;
}
/* swap the data we just received */
report.x = (((int16_t)ak8975_report.x[1]) << 8) + ak8975_report.x[0];
report.y = (((int16_t)ak8975_report.y[1]) << 8) + ak8975_report.y[0];
report.z = (((int16_t)ak8975_report.z[1]) << 8) + ak8975_report.z[0];
/*
* If any of the values are -4096, there was an internal math error in the sensor.
* Generalise this to a simple range check that will also catch some bit errors.
*/
if ((abs(report.x) > 4096) ||
(abs(report.y) > 4096) ||
(abs(report.z) > 4096)) {
perf_count(_comms_errors);
goto out;
}
/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
new_report.x_raw = report.x;
new_report.y_raw = report.y;
/* z remains z */
new_report.z_raw = report.z;
/* scale values for output */
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
* therefore switch x and y and invert y */
//new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_report.x = ((report.x * _scale.x_scale) - _scale.x_offset) * _range_scale;
/* flip axes and negate value for y */
//new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_report.y = ((report.y * _scale.y_scale) - _scale.y_offset) * _range_scale;
/* z remains z */
//new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
new_report.z = ((report.z * _scale.z_scale) - _scale.z_offset) * _range_scale;
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
/* post a report to the ring */
if (_reports->force(&new_report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
void
BMA180::measure()
{
/* BMA180 measurement registers */
// #pragma pack(push, 1)
// struct {
// uint8_t cmd;
// int16_t x;
// int16_t y;
// int16_t z;
// } raw_report;
// #pragma pack(pop)
struct accel_report report;
/* start the performance counter */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMA180 in one pass;
* starting from the X LSB.
*/
//raw_report.cmd = ADDR_ACC_X_LSB;
// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
/*
* Adjust and scale results to SI units.
*
* Note that we ignore the "new data" bits. At any time we read, each
* of the axis measurements are the "most recent", even if we've seen
* them before. There is no good way to synchronise with the internal
* measurement flow without using the external interrupt.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0;
/*
* y of board is x of sensor and x of board is -y of sensor
* perform only the axis assignment here.
* Two non-value bits are discarded directly
*/
report.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
report.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
report.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
/* discard two non-value bits in the 16 bit measurement */
report.x_raw = (report.x_raw / 4);
report.y_raw = (report.y_raw / 4);
report.z_raw = (report.z_raw / 4);
/* invert y axis, due to 14 bit data no overflow can occur in the negation */
report.y_raw = -report.y_raw;
report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
report.scaling = _accel_range_scale;
report.range_m_s2 = _accel_range_m_s2;
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
/* stop the perf counter */
perf_end(_sample_perf);
}
void
BMA180::print_info()
{
perf_print_counter(_sample_perf);
_reports->print_info("report queue");
}
int
BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* With internal low pass filters enabled, 250 Hz is sufficient */
return ioctl(filp, SENSORIOCSPOLLRATE, 250);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 1000)
return -EINVAL;
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_interval == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement */
return -EINVAL;
case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */
return -EINVAL;
case ACCELIOCGSAMPLERATE:
return 1200; /* always operating in low-noise mode */
case ACCELIOCSLOWPASS:
return set_lowpass(arg);
case ACCELIOCGLOWPASS:
return _current_lowpass;
case ACCELIOCSSCALE:
/* copy scale in */
memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
return OK;
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
return set_range(arg);
case ACCELIOCGRANGE:
return _current_range;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
int main(void) {
RingBuffer<int, 3> rb;
rb.print();
std::cout << std::boolalpha;
std::cout << "Empty: " << rb.empty() << " Full: " << rb.full() << std::endl;
rb.push_back(2);
std::cout << "Empty: " << rb.empty() << " Full: " << rb.full() << std::endl;
rb.push_front(1);
rb.push_back(3);
std::cout << "Empty: " << rb.empty() << " Full: " << rb.full() << std::endl;
rb.push_back(4);
std::cout << "Empty: " << rb.empty() << " Full: " << rb.full() << std::endl;
rb.print();
std::cout << "pop_front: " << rb.pop_front() << std::endl;
std::cout << "Empty: " << rb.empty() << " Full: " << rb.full() << std::endl;
std::cout << "pop_front: " << rb.pop_front() << std::endl;
rb.push_back(5);
rb.print();
std::cout << "pop_back: " << rb.pop_back() << std::endl;
std::cout << "pop_back: " << rb.pop_back() << std::endl;
rb.print();
try {
rb.pop_back();
std::cout << "This should not be printed!" << std::endl;
} catch (std::exception& rangeError) {
std::cout << "Error: " << rangeError.what() << std::endl;
}
}
int getAvailableFrameCount() const {
if (!m_buffer) return 0;
return m_buffer->getReadSpace() / m_rs->getChannelCount();
}
static int internal_play_media(const QString &mrl, const QString &plot,
const QString &title, const QString &subtitle,
const QString &director, int season, int episode,
int lenMins, const QString &year)
{
int res = -1;
QFile checkFile(mrl);
if ((!checkFile.exists() && !mrl.startsWith("dvd:")
&& !mrl.startsWith("bd:")
&& !mrl.startsWith("myth:")))
{
QString errorText = QObject::tr("Failed to open \n '%1' in %2 \n"
"Check if the video exists")
.arg(mrl.section('/', -1))
.arg(mrl.section('/', 0, -2));
ShowOkPopup(errorText);
return res;
}
ProgramInfo *pginfo = new ProgramInfo(
mrl, plot, title, subtitle, director, season, episode,
lenMins, (year.toUInt()) ? year.toUInt() : 1900);
int64_t pos = 0;
if (pginfo->IsVideoDVD())
{
RingBuffer *tmprbuf = RingBuffer::Create(pginfo->GetPathname(), false);
if (!tmprbuf)
{
delete pginfo;
return res;
}
QString name;
QString serialid;
if (tmprbuf->IsDVD() &&
tmprbuf->DVD()->GetNameAndSerialNum(name, serialid))
{
QStringList fields = pginfo->QueryDVDBookmark(serialid);
if (!fields.empty())
{
QStringList::Iterator it = fields.begin();
pos = (int64_t)((*++it).toLongLong() & 0xffffffffLL);
}
}
delete tmprbuf;
}
else if (pginfo->IsVideo())
pos = pginfo->QueryBookmark();
if (pos > 0)
{
MythScreenStack *mainStack = GetMythMainWindow()->GetMainStack();
BookmarkDialog *bookmarkdialog = new BookmarkDialog(pginfo, mainStack);
if (!bookmarkdialog->Create())
{
delete bookmarkdialog;
delete pginfo;
return res;
}
}
else
{
TV::StartTV(pginfo, kStartTVNoFlags);
res = 0;
delete pginfo;
}
return res;
}
int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
return reset();
case MAGIOCSSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return ioctl(filp, SENSORIOCSPOLLRATE, arg);
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000/TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
case MAGIOCGRANGE:
return _range_ga;
case MAGIOCSLOWPASS:
case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
/* check calibration, but not actually return an error */
(void)check_calibration();
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
return calibrate(filp, arg);
case MAGIOCEXSTRAP:
return set_excitement(arg);
case MAGIOCSELFTEST:
return check_calibration();
case MAGIOCGEXTERNAL:
if (_bus == PX4_I2C_BUS_EXPANSION)
return 1;
else
return 0;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
size_t VSICurlStreamingHandle::Read( void *pBuffer, size_t nSize, size_t nMemb )
{
GByte* pabyBuffer = (GByte*)pBuffer;
size_t nBufferRequestSize = nSize * nMemb;
if (nBufferRequestSize == 0)
return 0;
size_t nRemaining = nBufferRequestSize;
AcquireMutex();
int bHastComputedFileSizeLocal = bHastComputedFileSize;
vsi_l_offset fileSizeLocal = fileSize;
ReleaseMutex();
if (bHastComputedFileSizeLocal && curOffset >= fileSizeLocal)
{
CPLDebug("VSICURL", "Read attempt beyond end of file");
bEOF = TRUE;
}
if (bEOF)
return 0;
if (curOffset < nRingBufferFileOffset)
PutRingBufferInCache();
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Read [" CPL_FRMT_GUIB ", " CPL_FRMT_GUIB "[ in %s",
curOffset, curOffset + nBufferRequestSize, pszURL);
#ifdef notdef
if( pCachedData != NULL && nCachedSize >= 1024 &&
nRecomputedChecksumOfFirst1024Bytes == 0 )
{
for(size_t i = 0; i < 1024 / sizeof(int); i ++)
{
int nVal;
memcpy(&nVal, pCachedData + i * sizeof(int), sizeof(int));
nRecomputedChecksumOfFirst1024Bytes += nVal;
}
if( bHastComputedFileSizeLocal )
{
poFS->AcquireMutex();
CachedFileProp* cachedFileProp = poFS->GetCachedFileProp(pszURL);
if( cachedFileProp->nChecksumOfFirst1024Bytes == 0 )
{
cachedFileProp->nChecksumOfFirst1024Bytes = nRecomputedChecksumOfFirst1024Bytes;
}
else if( nRecomputedChecksumOfFirst1024Bytes != cachedFileProp->nChecksumOfFirst1024Bytes )
{
CPLDebug("VSICURL", "Invalidating previously cached file size. First bytes of file have changed!");
AcquireMutex();
bHastComputedFileSize = FALSE;
cachedFileProp->bHastComputedFileSize = FALSE;
cachedFileProp->nChecksumOfFirst1024Bytes = 0;
ReleaseMutex();
}
poFS->ReleaseMutex();
}
}
#endif
/* Can we use the cache ? */
if( pCachedData != NULL && curOffset < nCachedSize )
{
size_t nSz = MIN(nRemaining, (size_t)(nCachedSize - curOffset));
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Using cache for [%d, %d[ in %s",
(int)curOffset, (int)(curOffset + nSz), pszURL);
memcpy(pabyBuffer, pCachedData + curOffset, nSz);
pabyBuffer += nSz;
curOffset += nSz;
nRemaining -= nSz;
}
/* Is the request partially covered by the cache and going beyond file size ? */
if ( pCachedData != NULL && bHastComputedFileSizeLocal &&
curOffset <= nCachedSize &&
curOffset + nRemaining > fileSizeLocal &&
fileSize == nCachedSize )
{
size_t nSz = (size_t) (nCachedSize - curOffset);
if (ENABLE_DEBUG && nSz != 0)
CPLDebug("VSICURL", "Using cache for [%d, %d[ in %s",
(int)curOffset, (int)(curOffset + nSz), pszURL);
memcpy(pabyBuffer, pCachedData + curOffset, nSz);
pabyBuffer += nSz;
curOffset += nSz;
nRemaining -= nSz;
bEOF = TRUE;
}
/* Has a Seek() being done since the last Read() ? */
if (!bEOF && nRemaining > 0 && curOffset != nRingBufferFileOffset)
{
/* Backward seek : we need to restart the download from the start */
if (curOffset < nRingBufferFileOffset)
StopDownload();
StartDownload();
#define SKIP_BUFFER_SIZE 32768
GByte* pabyTmp = (GByte*)CPLMalloc(SKIP_BUFFER_SIZE);
CPLAssert(curOffset >= nRingBufferFileOffset);
vsi_l_offset nBytesToSkip = curOffset - nRingBufferFileOffset;
while(nBytesToSkip > 0)
{
vsi_l_offset nBytesToRead = nBytesToSkip;
AcquireMutex();
if (nBytesToRead > oRingBuffer.GetSize())
nBytesToRead = oRingBuffer.GetSize();
if (nBytesToRead > SKIP_BUFFER_SIZE)
nBytesToRead = SKIP_BUFFER_SIZE;
oRingBuffer.Read(pabyTmp, (size_t)nBytesToRead);
/* Signal to the producer that we have ingested some bytes */
CPLCondSignal(hCondConsumer);
ReleaseMutex();
if (nBytesToRead)
AddRegion(nRingBufferFileOffset, (size_t)nBytesToRead, pabyTmp);
nBytesToSkip -= nBytesToRead;
nRingBufferFileOffset += nBytesToRead;
if (nBytesToRead == 0 && nBytesToSkip != 0)
{
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Waiting for writer to produce some bytes...");
AcquireMutex();
while(oRingBuffer.GetSize() == 0 && bDownloadInProgress)
CPLCondWait(hCondProducer, hRingBufferMutex);
int bBufferEmpty = (oRingBuffer.GetSize() == 0);
ReleaseMutex();
if (bBufferEmpty && !bDownloadInProgress)
break;
}
}
CPLFree(pabyTmp);
if (nBytesToSkip != 0)
{
bEOF = TRUE;
return 0;
}
}
if (!bEOF && nRemaining > 0)
{
StartDownload();
CPLAssert(curOffset == nRingBufferFileOffset);
}
/* Fill the destination buffer from the ring buffer */
while(!bEOF && nRemaining > 0)
{
AcquireMutex();
size_t nToRead = oRingBuffer.GetSize();
if (nToRead > nRemaining)
nToRead = nRemaining;
oRingBuffer.Read(pabyBuffer, nToRead);
/* Signal to the producer that we have ingested some bytes */
CPLCondSignal(hCondConsumer);
ReleaseMutex();
if (nToRead)
AddRegion(curOffset, nToRead, pabyBuffer);
nRemaining -= nToRead;
pabyBuffer += nToRead;
curOffset += nToRead;
nRingBufferFileOffset += nToRead;
if (nToRead == 0 && nRemaining != 0)
{
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Waiting for writer to produce some bytes...");
AcquireMutex();
while(oRingBuffer.GetSize() == 0 && bDownloadInProgress)
CPLCondWait(hCondProducer, hRingBufferMutex);
int bBufferEmpty = (oRingBuffer.GetSize() == 0);
ReleaseMutex();
if (bBufferEmpty && !bDownloadInProgress)
break;
}
}
if (ENABLE_DEBUG)
CPLDebug("VSICURL", "Read(%d) = %d",
(int)nBufferRequestSize, (int)(nBufferRequestSize - nRemaining));
size_t nRet = (nBufferRequestSize - nRemaining) / nSize;
if (nRet < nMemb)
bEOF = TRUE;
return nRet;
}
int
HMC5883::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t z[2];
uint8_t y[2];
} hmc_report;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret;
uint8_t cmd;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
cmd = ADDR_DATA_OUT_X_MSB;
ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
if (ret != OK) {
perf_count(_comms_errors);
debug("data/status read error");
goto out;
}
/* swap the data we just received */
report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1];
report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1];
report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1];
/*
* If any of the values are -4096, there was an internal math error in the sensor.
* Generalise this to a simple range check that will also catch some bit errors.
*/
if ((abs(report.x) > 2048) ||
(abs(report.y) > 2048) ||
(abs(report.z) > 2048)) {
perf_count(_comms_errors);
goto out;
}
/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
new_report.x_raw = report.y;
new_report.y_raw = -report.x;
/* z remains z */
new_report.z_raw = report.z;
/* scale values for output */
#ifdef PX4_I2C_BUS_ONBOARD
if (_bus == PX4_I2C_BUS_ONBOARD) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
report.x = -report.x;
}
#endif
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
// apply user specified rotation
rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
if (!(_pub_blocked)) {
if (_mag_topic != -1) {
/* publish it */
orb_publish(_mag_orb_id, _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise(_mag_orb_id, &new_report);
if (_mag_topic < 0)
debug("ADVERT FAIL");
}
}
_last_report = new_report;
/* post a report to the ring */
if (_reports->force(&new_report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the range register and configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 0) {
check_range();
}
if (check_counter == 128) {
check_conf();
}
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
int
PX4FLOW::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
perf_begin(_sample_perf);
if (PX4FLOW_REG == 0x00) {
ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
}
if (ret < 0) {
debug("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
if (PX4FLOW_REG == 0) {
memcpy(&f, val, I2C_FRAME_SIZE);
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
}
struct optical_flow_s report;
report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = f_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
/* rotate measurements according to parameter */
float zeroval = 0.0f;
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
if (_px4flow_topic < 0) {
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
} else {
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
}
/* post a report to the ring */
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
L3G4200D::measure()
{
#if L3G4200D_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
return;
}
#endif
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_report;
#pragma pack(pop)
gyro_report report;
/* start the performance counter */
perf_begin(_sample_perf);
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
#if L3G4200D_USE_DRDY
if ((raw_report.status & 0xF) != 0xF) {
/*
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return;
}
#endif
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0; // not recorded
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
/* keep axes in place */
report.x_raw = raw_report.x;
report.y_raw = raw_report.y;
break;
case SENSOR_BOARD_ROTATION_090_DEG:
/* swap x and y */
report.x_raw = raw_report.y;
report.y_raw = raw_report.x;
break;
case SENSOR_BOARD_ROTATION_180_DEG:
/* swap x and y and negate both */
/* since sensor is mounted on top, need to rotate and then negate y, z */
report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
// report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); /* leave as is, 180 rotate + negate */
report.y_raw = raw_report.y;
report.z_raw = ((raw_report.z == -32768) ? 32767 : -raw_report.z);
break;
case SENSOR_BOARD_ROTATION_270_DEG:
/* swap x and y and negate y */
report.x_raw = raw_report.y;
report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
break;
}
// report.z_raw = raw_report.z;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report.x = _gyro_filter_x.apply(report.x);
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0)
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
_read++;
/* stop the perf counter */
perf_end(_sample_perf);
}
Usart2560::Usart2560()
{
g_tx.reset();
g_rx.reset();
}
int utTypes(){
// Array
{ // test the basic C functionality
const int comps = 1; //
const int dims = 2; // no. dimensions
const int size0 = 2; // no. elements in dimension 1
const int size1 = 64; // no. elements in dimension 2
const int stride0 = comps * sizeof(data_t); // byte offset at dim 0
const int stride1 = stride0*size0;
data_t data[comps*size0*size1];
AlloArrayHeader hdr;
hdr.type = AlloFloat64Ty;
hdr.components = comps;
hdr.dimcount = dims;
hdr.dim[0] = size0;
hdr.dim[1] = size1;
hdr.stride[0] = stride0;
hdr.stride[1] = stride1;
AlloArray lat;
lat.data.ptr = (char *)&data;
allo_array_setheader(&lat, &hdr);
assert(allo_array_elements(&lat) == size0*size1);
assert(allo_array_size(&lat) == sizeof(data));
}
{
AlloArrayHeader hdr;
hdr.type = AlloSInt8Ty;
hdr.components = 1;
hdr.dim[0] = 7;
hdr.dim[1] = 7;
// 2D test
hdr.dimcount = 2;
allo_array_setstride(&hdr, 1);
assert(hdr.stride[0] == 1);
assert(hdr.stride[1] == 7);
allo_array_setstride(&hdr, 4);
assert(hdr.stride[1] == 8);
allo_array_setstride(&hdr, 8);
assert(hdr.stride[1] == 8);
// 3D test
hdr.dimcount = 3;
allo_array_setstride(&hdr, 4);
assert(hdr.stride[2] == 8*7);
}
{ // Basic Array usage
// TODO: move this to an example
// // a 1D array of float-pairs (e.g. interleaved audio buffer)
// Array buf(2, AlloFloat32Ty, 64);
//
// // a 2D array of char[4] data (e.g. ARGB image matrix with color values as 0-255)
// Array img(4, AlloUInt8Ty, 720, 480);
//
// // a 3D array of float triplets (e.g. vector field)
// Array field(3, Array::type<float>(), 16, 16, 16);
{ Array a;
assert(a.size() == 0);
assert(!a.hasData());
assert(a.type() == AlloVoidTy);
assert(a.isType(AlloVoidTy));
assert(a.isFormat(a));
}
// Run tests on Arrays with various sizes and dimensions
const int Nc= 3;
const int Ns[] = {1, 4, 5, 6, 7, 64, 128, 129};
for(unsigned iN=0; iN<sizeof(Ns)/sizeof(*Ns); ++iN){
int N = Ns[iN]; // number of cells along each dimension
{ // Constructors
{ Array a(Nc, AlloFloat32Ty, N);
assert(a.hasData());
assert(a.type() == AlloFloat32Ty);
assert(a.components() == Nc);
assert(a.dimcount() == 1);
}
{ Array a(Nc, AlloFloat32Ty, N,N);
assert(a.hasData());
assert(a.type() == AlloFloat32Ty);
assert(a.components() == Nc);
assert(a.dimcount() == 2);
}
{ Array a(Nc, AlloFloat32Ty, N,N,N);
assert(a.hasData());
assert(a.type() == AlloFloat32Ty);
assert(a.components() == Nc);
assert(a.dimcount() == 3);
}
}
{ // Memory allocation
Array a;
a.formatAligned(Nc, AlloFloat32Ty, N, 1);
assert((int)a.size() == Nc*N*4);
assert(a.hasData());
assert(a.isType(AlloFloat32Ty));
assert(a.isType<float>());
a.formatAligned(Nc, AlloSInt8Ty, N, N, 1);
assert((int)a.size() == Nc*N*N*1);
assert(a.hasData());
assert(a.isType(AlloSInt8Ty));
assert(a.isType<int8_t>());
a.formatAligned(Nc, AlloSInt16Ty, N, N, N, 1);
assert((int)a.size() == Nc*N*N*N*2);
assert(a.hasData());
assert(a.isType(AlloSInt16Ty));
assert(a.isType<int16_t>());
}
{ // 1-D element access
Array a(Nc, AlloSInt8Ty, N);
for(int i=0,t=0; i<N; ++i){
int8_t x[Nc] = {int8_t(i), int8_t(i+1), int8_t(i+2)};
int8_t y[Nc] = {-1,-1,-1};
a.write(x, i);
a.read(y, i);
for(int c=0; c<Nc; ++c) assert(y[c] == x[c]);
for(int c=0; c<Nc; ++c){
a.elem<int8_t>(c,i) = t+1;
assert(a.elem<int8_t>(c,i) == int8_t(t+1));
++t;
}
}
}
{ // 2-D element access
Array a(Nc, AlloSInt8Ty, N,N);
for(int j=0,t=0; j<N; ++j){
for(int i=0; i<N; ++i){
int8_t x[Nc] = {int8_t(j), int8_t(j+1), int8_t(j+2)};
int8_t y[Nc] = {-1,-1,-1};
a.write(x, i,j);
a.read(y, i,j);
for(int c=0; c<Nc; ++c) assert(y[c] == x[c]);
for(int c=0; c<Nc; ++c){
a.elem<int8_t>(c,i,j) = t+1;
assert(a.elem<int8_t>(c,i,j) == int8_t(t+1));
++t;
}
}}
}
{ // 3-D element access
Array a(Nc, AlloSInt8Ty, N,N,N);
for(int k=0,t=0; k<N; ++k){
for(int j=0; j<N; ++j){
for(int i=0; i<N; ++i){
int8_t x[Nc] = {int8_t(k), int8_t(k+1), int8_t(k+2)};
int8_t y[Nc] = {-1,-1,-1};
a.write(x, i,j,k);
a.read(y, i,j,k);
for(int c=0; c<Nc; ++c) assert(y[c] == x[c]);
for(int c=0; c<Nc; ++c){
a.elem<int8_t>(c,i,j,k) = t+1;
assert(a.elem<int8_t>(c,i,j,k) == int8_t(t+1));
++t;
}
}}}
}
} // end size loop
}
{
Buffer<int> a(0,2);
assert(a.size() == 0);
assert(a.capacity() == 2);
//assert(a.fill() == 0);
a.append(1);
assert(a[0] == 1);
assert(a.size() == 1);
assert(a.last() == 1);
a.append(2);
a.append(3);
assert(a.size() == 3);
assert(a.capacity() == 4);
assert(a.last() == 3);
a.reset();
assert(a.size() == 0);
assert(a.capacity() == 4);
a.append(7);
a.repeatLast();
assert(a[0] == 7);
assert(a[1] == 7);
// Appending another Buffer
{
Buffer<int> b(4);
for(int i=0; i<b.size(); ++i) b[i] = i+4;
a.size(4);
for(int i=0; i<a.size(); ++i) a[i] = i;
// Append non-zero sized to non-zero sized
{
int N = a.size() + b.size();
a.append(b);
assert(a.size() == N);
for(int i=0; i<N; ++i) assert(a[i] == i);
}
// Append non-zero sized to zero sized
a.size(0);
a.append(b);
assert(a.size() == b.size());
for(int i=0; i<a.size(); ++i) assert(a[i] == b[i]);
// Append zero sized to non-zero sized
{
int N = a.size();
b.size(0);
a.append(b);
assert(a.size() == N);
}
}
}
{
RingBuffer<int> a;
// Test ring buffering
a.resize(4);
assert(a.size() == 4);
a.write(1);
a.write(2);
//assert(a.fill() == 2);
a.write(3);
a.write(4);
assert(a.pos() == 3);
assert(a[0] == 1);
assert(a[1] == 2);
assert(a[2] == 3);
assert(a[3] == 4);
assert(a.read(0) == 4);
assert(a.read(1) == 3);
assert(a.read(2) == 2);
assert(a.read(3) == 1);
//assert(a.fill() == 4);
a.write(5);
//assert(a.fill() == 4);
assert(a[0] == 5);
assert(a.read(0) == 5);
assert(a.read(1) == 4);
assert(a.read(2) == 3);
assert(a.read(3) == 2);
}
return 0;
}
void
LSM303D::measure()
{
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
return;
}
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
return;
}
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_accel_report;
#pragma pack(pop)
accel_report accel_report;
/* start the performance counter */
perf_begin(_accel_sample_perf);
/* fetch data from the sensor */
memset(&raw_accel_report, 0, sizeof(raw_accel_report));
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
accel_report.timestamp = hrt_absolute_time();
accel_report.error_count = 0; // not reported
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;
accel_report.z_raw = raw_accel_report.z;
float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
_accel_reports->force(&accel_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_accel_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_accel_read++;
/* stop the perf counter */
perf_end(_accel_sample_perf);
}
void
OREOLED::cycle()
{
/* check time since startup */
uint64_t now = hrt_absolute_time();
bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS);
/* if not leds found during start-up period, exit without rescheduling */
if (startup_timeout && _num_healthy == 0) {
warnx("did not find oreoled");
return;
}
/* during startup period keep searching for unhealthy LEDs */
if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) {
/* prepare command to turn off LED*/
uint8_t msg[] = {OREOLED_PATTERN_OFF};
/* attempt to contact each unhealthy LED */
for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
if (!_healthy[i]) {
/* set I2C address */
set_address(OREOLED_BASE_I2C_ADDR + i);
/* send I2C command and record health*/
if (transfer(msg, sizeof(msg), nullptr, 0) == OK) {
_healthy[i] = true;
_num_healthy++;
warnx("oreoled %d ok", (unsigned)i);
}
}
}
/* schedule another attempt in 0.1 sec */
work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
USEC2TICK(OREOLED_STARTUP_INTERVAL_US));
return;
}
/* get next command from queue */
oreoled_cmd_t next_cmd;
while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) {
/* send valid messages to healthy LEDs */
if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num]
&& (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) {
/* set I2C address */
set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num);
/* send I2C command */
transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0);
}
}
/* send general call every 4 seconds*/
if ((now - _last_gencall) > OREOLED_GENERALCALL_US) {
send_general_call();
}
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
USEC2TICK(OREOLED_UPDATE_INTERVAL_US));
}
int
LSM303D::init()
{
int ret = ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
warnx("SPI init failed");
goto out;
}
/* allocate basic report buffers */
_accel_reports = new RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
goto out;
/* advertise accel topic */
_mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
goto out;
reset();
/* do CDev init for the mag device node */
ret = _mag->init();
if (ret != OK) {
warnx("MAG init failed");
goto out;
}
/* fill report structures */
measure();
if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
if (_mag->_mag_topic < 0)
debug("failed to create sensor_mag publication");
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
}
out:
return ret;
}
int
OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = -ENODEV;
oreoled_cmd_t new_cmd;
switch (cmd) {
case OREOLED_SET_RGB:
/* set the specified color */
new_cmd.led_num = ((oreoled_rgbset_t *) arg)->instance;
new_cmd.buff[0] = ((oreoled_rgbset_t *) arg)->pattern;
new_cmd.buff[1] = OREOLED_PARAM_BIAS_RED;
new_cmd.buff[2] = ((oreoled_rgbset_t *) arg)->red;
new_cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN;
new_cmd.buff[4] = ((oreoled_rgbset_t *) arg)->green;
new_cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE;
new_cmd.buff[6] = ((oreoled_rgbset_t *) arg)->blue;
new_cmd.num_bytes = 7;
/* special handling for request to set all instances rgb values */
if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
/* add command to queue for all healthy leds */
if (_healthy[i]) {
new_cmd.led_num = i;
_cmd_queue->force(&new_cmd);
ret = OK;
}
}
} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
/* request to set individual instance's rgb value */
if (_healthy[new_cmd.led_num]) {
_cmd_queue->force(&new_cmd);
ret = OK;
}
}
return ret;
case OREOLED_RUN_MACRO:
/* run a macro */
new_cmd.led_num = ((oreoled_macrorun_t *) arg)->instance;
new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE;
new_cmd.buff[1] = OREOLED_PARAM_MACRO;
new_cmd.buff[2] = ((oreoled_macrorun_t *) arg)->macro;
new_cmd.num_bytes = 3;
/* special handling for request to set all instances */
if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
/* add command to queue for all healthy leds */
if (_healthy[i]) {
new_cmd.led_num = i;
_cmd_queue->force(&new_cmd);
ret = OK;
}
}
} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
/* request to set individual instance's rgb value */
if (_healthy[new_cmd.led_num]) {
_cmd_queue->force(&new_cmd);
ret = OK;
}
}
return ret;
case OREOLED_SEND_BYTES:
/* send bytes */
new_cmd = *((oreoled_cmd_t *) arg);
/* special handling for request to set all instances */
if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
/* add command to queue for all healthy leds */
if (_healthy[i]) {
new_cmd.led_num = i;
_cmd_queue->force(&new_cmd);
ret = OK;
}
}
} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
/* request to set individual instance's rgb value */
if (_healthy[new_cmd.led_num]) {
_cmd_queue->force(&new_cmd);
ret = OK;
}
}
return ret;
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filp, cmd, arg);
break;
}
return ret;
}
int main()
{
vagg_start(vagg_display_success);
RingBuffer<AudioBuffer, 2> buffer;
AudioBuffer a = {0.1, 0.2, 0.3, 0.4};
AudioBuffer b = {0.2, 1.2, 1.3, 2.4};
AudioBuffer c = {3.1, 8.2, 3.3, 3.4};
AudioBuffer d;
for(int i = 0; i < 10; i++) {
vagg_ok(! buffer.full(), "Not full.");
vagg_ok(buffer.empty(), "Empty.");
float raw[4] = {0.1, 0.3, 1.8, 4.2};
vagg_ok(buffer.push_raw(raw, 4), "Add a buffer on a not full RingBuffer.");
vagg_ok(! buffer.empty(), "Not full.");
vagg_ok(! buffer.full(), "Not empty.");
vagg_ok(buffer.push(b), "Add a buffer on a not full RingBuffer.");
vagg_ok(buffer.full(), "Should be full");
vagg_ok(! buffer.empty(), "Should not be empty");
vagg_ok(! buffer.push(c), "Add a buffer on a full RingBuffer.");
vagg_ok(buffer.full(), "Should be full");
vagg_ok(! buffer.empty(), "Should not be empty");
vagg_ok(buffer.pop(d), "Get a buffer back.");
vagg_bufeq((void*)&a.front(), a.size(),(void*)&d.front(), d.size(), "First buffer out should be equal to first buffer in.");
vagg_ok(! buffer.empty(), "Should not be empty");
vagg_ok(! buffer.full(), "Should not be full");
vagg_ok(buffer.pop(d), "Get another buffer back.");
vagg_ok(buffer.empty(), "Should be empty.");
vagg_ok(! buffer.full(), "Should not be full.");
vagg_bufeq((void*)&b.front(), b.size(),(void*)&d.front(), d.size(), "Second buffer out should be equal to second buffer in.");
vagg_ok(! buffer.pop(d), "Get a buffer back on an empty RingBuffer should not be possible.");
vagg_ok(buffer.empty(), "Should be empty.");
vagg_ok(! buffer.full(), "Should not be full.");
}
vagg_end();
return 0;
}
int get_pending() const {
return rb.data_left();
};
int
L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
if (_is_l3g4200d) {
return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
}
return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 1000)
return -EINVAL;
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
/* adjust filters */
float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f/ticks;
set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_interval == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
reset();
return OK;
case GYROIOCSSAMPLERATE:
return set_samplerate(arg, _current_bandwidth);
case GYROIOCGSAMPLERATE:
return _current_rate;
case GYROIOCSLOWPASS: {
// set the software lowpass cut-off in Hz
float cutoff_freq_hz = arg;
float sample_rate = 1.0e6f / _call_interval;
set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
return OK;
}
case GYROIOCGLOWPASS:
return _gyro_filter_x.get_cutoff_freq();
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* copy scale out */
memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:
/* arg should be in dps */
return set_range(arg);
case GYROIOCGRANGE:
/* convert to dps and round */
return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
case GYROIOCSELFTEST:
return self_test();
case GYROIOCSHWLOWPASS:
return set_samplerate(_current_rate, arg);
case GYROIOCGHWLOWPASS:
return _current_bandwidth;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
void UpdateGesture()
{
const sTouchPoint& Pt1 = FInitialPoint;
const sTouchPoint& Pt2 = FCurrentPoint;
sMotionData* Prev0 = FPrevMotionData.prev( 0 );
sMotionData* Prev1 = FPrevMotionData.prev( 1 );
sMotionData* Prev2 = FPrevMotionData.prev( 2 );
g_Responder->Event_UpdateGesture( FMotionData );
// check double tap
if ( Prev0 && Prev1 && Prev2 )
{
size_t NumPts0 = Prev0->GetNumTouchPoints();
size_t NumPts1 = Prev1->GetNumTouchPoints();
size_t NumPts2 = Prev2->GetNumTouchPoints();
if ( NumPts2 == 0 && NumPts1 == 1 && NumPts0 == 0 && FMotionData.GetNumTouchPoints() == 1 )
{
float TimeSpan = float( FMotionData.GetTouchPoint( 0 ).FTimeStamp - Prev1->GetTouchPoint( 0 ).FTimeStamp );
float Offset = ( FMotionData.GetTouchPointPos( 0 ) - Prev1->GetTouchPointPos( 0 ) ).Length();
// it looks like a double tap
if ( Offset <= DefaultDoubleTapOffset )
{
FPrevMotionData.clear();
if ( g_Responder->GetDoubleTapTimeout() > TimeSpan )
{
g_Responder->Event_DoubleTap( FMotionData.GetTouchPoint( 0 ).FPoint );
}
}
}
}
if ( IsDraggingValid() )
{
// react on single point dragging
if ( GetPositionDelta().Length() > FlingThresholdSensitivity )
{
g_Responder->Event_Drag( Pt1, Pt2 );
FFlingWasValid = true;
}
}
else
{
// finish gesture
if ( FFlingWasValid )
{
if ( GetPositionDelta().Length() > FlingStartSensitivity )
{
// will be only 1 event
g_Responder->Event_Fling( Pt1, Pt2 );
}
else
{
g_Responder->Event_Drag( Pt1, Pt2 );
}
FFlingWasValid = false;
}
}
if ( IsPinchZoomValid() )
{
if ( FPinchZoomWasValid )
{
g_Responder->Event_Pinch( FInitialPoint1, FInitialPoint2, FCurrentPoint1, FCurrentPoint2 );
}
else
{
g_Responder->Event_PinchStart( FInitialPoint1, FInitialPoint2 );
}
FPinchZoomWasValid = true;
}
else if ( FPinchZoomWasValid )
{
FPinchZoomWasValid = false;
g_Responder->Event_PinchStop( FInitialPoint1, FInitialPoint2, FCurrentPoint1, FCurrentPoint2 );
}
}