fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Gyro sim IOCTL fix

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-14 00:08:01 +01:00
parent 6c43b6c964
commit fd0584e95d
1 changed files with 20 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+20 -23
View File
@@ -128,7 +128,7 @@ public:
virtual int start(); virtual int start();
virtual ssize_t devRead(void *buffer, size_t buflen); virtual ssize_t devRead(void *buffer, size_t buflen);
virtual int devIOCTL(unsigned long cmd, void *arg); virtual int devIOCTL(unsigned long cmd, unsigned long arg);
int transfer(uint8_t *send, uint8_t *recv, unsigned len); int transfer(uint8_t *send, uint8_t *recv, unsigned len);
/** /**
@@ -142,7 +142,7 @@ protected:
friend class GYROSIM_gyro; friend class GYROSIM_gyro;
virtual ssize_t gyro_read(void *buffer, size_t buflen); virtual ssize_t gyro_read(void *buffer, size_t buflen);
virtual int gyro_ioctl(unsigned long cmd, void *arg); virtual int gyro_ioctl(unsigned long cmd, unsigned long arg);
private: private:
GYROSIM_gyro *_gyro; GYROSIM_gyro *_gyro;
@@ -284,7 +284,7 @@ public:
virtual ~GYROSIM_gyro() {} virtual ~GYROSIM_gyro() {}
virtual ssize_t devRead(void *buffer, size_t buflen); virtual ssize_t devRead(void *buffer, size_t buflen);
virtual int devIOCTL(unsigned long cmd, void *arg); virtual int devIOCTL(unsigned long cmd, unsigned long arg);
virtual int init(); virtual int init();
@@ -726,9 +726,8 @@ GYROSIM::gyro_read(void *buffer, size_t buflen)
} }
int int
GYROSIM::devIOCTL(unsigned long cmd, void *arg) GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
{ {
unsigned long ul_arg = (unsigned long)arg;
switch (cmd) { switch (cmd) {
@@ -736,7 +735,7 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg)
return reset(); return reset();
case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
switch (ul_arg) { switch (arg) {
/* switching to manual polling */ /* switching to manual polling */
case SENSOR_POLLRATE_MANUAL: case SENSOR_POLLRATE_MANUAL:
@@ -753,15 +752,15 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg)
/* set default/max polling rate */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
return devIOCTL(SENSORIOCSPOLLRATE, (void *)1000); return devIOCTL(SENSORIOCSPOLLRATE, 1000);
case SENSOR_POLLRATE_DEFAULT: case SENSOR_POLLRATE_DEFAULT:
return devIOCTL(SENSORIOCSPOLLRATE, (void *)GYROSIM_ACCEL_DEFAULT_RATE); return devIOCTL(SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */ /* adjust to a legal polling interval in Hz */
default: { default: {
/* convert hz to hrt interval via microseconds */ /* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / ul_arg; unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */ /* check against maximum sane rate */
if (ticks < 1000) { if (ticks < 1000) {
@@ -793,11 +792,11 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg)
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((ul_arg < 1) || (ul_arg > 100)) { if ((arg < 1) || (arg > 100)) {
return -EINVAL; return -EINVAL;
} }
if (!_accel_reports->resize(ul_arg)) { if (!_accel_reports->resize(arg)) {
return -ENOMEM; return -ENOMEM;
} }
@@ -811,7 +810,7 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg)
return _sample_rate; return _sample_rate;
case ACCELIOCSSAMPLERATE: case ACCELIOCSSAMPLERATE:
_set_sample_rate(ul_arg); _set_sample_rate(arg);
return OK; return OK;
case ACCELIOCSLOWPASS: case ACCELIOCSLOWPASS:
@@ -819,7 +818,7 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg)
case ACCELIOCSSCALE: { case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */ /* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) ul_arg; struct accel_scale *s = (struct accel_scale *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale; float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) { if (sum > 2.0f && sum < 4.0f) {
@@ -833,11 +832,11 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg)
case ACCELIOCGSCALE: case ACCELIOCGSCALE:
/* copy scale out */ /* copy scale out */
memcpy((struct accel_scale *) ul_arg, &_accel_scale, sizeof(_accel_scale)); memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
return OK; return OK;
case ACCELIOCSRANGE: case ACCELIOCSRANGE:
return set_accel_range(ul_arg); return set_accel_range(arg);
case ACCELIOCGRANGE: case ACCELIOCGRANGE:
return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f);
@@ -852,10 +851,8 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg)
} }
int int
GYROSIM::gyro_ioctl(unsigned long cmd, void *arg) GYROSIM::gyro_ioctl(unsigned long cmd, unsigned long arg)
{ {
unsigned long ul_arg = (unsigned long)arg;
switch (cmd) { switch (cmd) {
/* these are shared with the accel side */ /* these are shared with the accel side */
@@ -866,11 +863,11 @@ GYROSIM::gyro_ioctl(unsigned long cmd, void *arg)
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((ul_arg < 1) || (ul_arg > 100)) { if ((arg < 1) || (arg > 100)) {
return -EINVAL; return -EINVAL;
} }
if (!_gyro_reports->resize(ul_arg)) { if (!_gyro_reports->resize(arg)) {
return -ENOMEM; return -ENOMEM;
} }
@@ -884,7 +881,7 @@ GYROSIM::gyro_ioctl(unsigned long cmd, void *arg)
return _sample_rate; return _sample_rate;
case GYROIOCSSAMPLERATE: case GYROIOCSSAMPLERATE:
_set_sample_rate(ul_arg); _set_sample_rate(arg);
return OK; return OK;
case GYROIOCSLOWPASS: case GYROIOCSLOWPASS:
@@ -1161,7 +1158,7 @@ GYROSIM::print_registers()
GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) :
// Set sample interval to 0 since device is read by parent // Set sample interval to 0 since device is read by parent
VirtDevObj("GYROSIM_gyro", path, nullptr, 0), VirtDevObj("GYROSIM_gyro", path, "", 0),
_parent(parent), _parent(parent),
_gyro_topic(nullptr), _gyro_topic(nullptr),
_gyro_orb_class_instance(-1) _gyro_orb_class_instance(-1)
@@ -1188,7 +1185,7 @@ GYROSIM_gyro::devRead(void *buffer, size_t buflen)
} }
int int
GYROSIM_gyro::devIOCTL(unsigned long cmd, void *arg) GYROSIM_gyro::devIOCTL(unsigned long cmd, unsigned long arg)
{ {
switch (cmd) { switch (cmd) {