diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 33aaa1647f..f9a6ebc559 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -43,6 +43,7 @@ #include "device.h" #include "vfile.h" +#include #include #include #include @@ -61,7 +62,7 @@ struct timerData { ~timerData() {} }; -static void *timer_handler(void *data) +static void timer_cb(void *data) { struct timerData *td = (struct timerData *)data; @@ -72,7 +73,6 @@ static void *timer_handler(void *data) sem_post(&(td->sem)); PX4_DEBUG("timer_handler: Timer expired"); - return 0; } #define PX4_MAX_FD 200 @@ -239,25 +239,16 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { if (timeout >= 0) { - pthread_t pt; - void *res; + // Use a work queue task + work_s _hpwork; - ts.tv_sec = timeout/1000; - ts.tv_nsec = (timeout % 1000)*1000000; - - // Create a timer to unblock struct timerData td(sem, ts); - int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td); - if (rv != 0) { - count = -1; - goto cleanup; - } + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&td, 1000*timeout); sem_wait(&sem); // Make sure timer thread is killed before sem goes // out of scope - (void)pthread_cancel(pt); - (void)pthread_join(pt, &res); + hrt_work_cancel(&_hpwork); } else { @@ -283,7 +274,6 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) } } -cleanup: sem_destroy(&sem); return count; diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index fa7f92abba..0fde8b293a 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -41,6 +41,9 @@ * @author Mark Charlebois */ +#define __STDC_FORMAT_MACROS +#include + #include #include @@ -85,37 +88,19 @@ #define MPUREG_CONFIG 0x1A #define MPUREG_GYRO_CONFIG 0x1B #define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_INT_PIN_CFG 0x37 -#define MPUREG_INT_ENABLE 0x38 #define MPUREG_INT_STATUS 0x3A -#define MPUREG_USER_CTRL 0x6A -#define MPUREG_PWR_MGMT_1 0x6B -#define MPUREG_PWR_MGMT_2 0x6C #define MPUREG_PRODUCT_ID 0x0C // Product ID Description for GYROSIM // high 4 bits low 4 bits // Product Name Product Revision #define GYROSIMES_REV_C4 0x14 -#define GYROSIMES_REV_C5 0x15 -#define GYROSIMES_REV_D6 0x16 -#define GYROSIMES_REV_D7 0x17 -#define GYROSIMES_REV_D8 0x18 -#define GYROSIM_REV_C4 0x54 -#define GYROSIM_REV_C5 0x55 -#define GYROSIM_REV_D6 0x56 -#define GYROSIM_REV_D7 0x57 -#define GYROSIM_REV_D8 0x58 -#define GYROSIM_REV_D9 0x59 -#define GYROSIM_REV_D10 0x5A -#define GYROSIM_ACCEL_DEFAULT_RATE 1000 -#define GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define GYROSIM_ACCEL_DEFAULT_RATE 1000 -#define GYROSIM_GYRO_DEFAULT_RATE 1000 -#define GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 +#define GYROSIM_GYRO_DEFAULT_RATE 1000 -#define GYROSIM_ONE_G 9.80665f +#define GYROSIM_ONE_G 9.80665f #ifdef PX4_SPI_BUS_EXT #define EXTERNAL_BUS PX4_SPI_BUS_EXT @@ -186,16 +171,6 @@ private: perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; - uint8_t _register_wait; - uint64_t _reset_wait; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; - enum Rotation _rotation; // last temperature reading for print_info() @@ -372,14 +347,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")), _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), - _register_wait(0), - _reset_wait(0), - _accel_filter_x(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_y(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_x(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_y(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _last_temperature(0) { @@ -571,6 +538,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { + PX4_INFO("GYROSIM::_set_sample_rate %uHz", desired_sample_rate_hz); if (desired_sample_rate_hz == 0 || desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { @@ -580,8 +548,16 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) uint8_t div = 1000 / desired_sample_rate_hz; if(div>200) div=200; if(div<1) div=1; + + // This does nothing in the simulator but writes the value in the "register" so + // register dumps look correct write_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; + PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); + _call_interval = 1000000/_sample_rate; + hrt_cancel(&_call); + hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); } ssize_t @@ -775,9 +751,6 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* 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; @@ -785,22 +758,11 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - - - float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _call_interval = ticks; + + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); /* if we need to start the poll state machine, do it */ if (want_start) @@ -839,14 +801,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) _set_sample_rate(arg); return OK; - case ACCELIOCGLOWPASS: - return _accel_filter_x.get_cutoff_freq(); - case ACCELIOCSLOWPASS: - // set software filtering - _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case ACCELIOCSSCALE: @@ -915,13 +870,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) _set_sample_rate(arg); return OK; - case GYROIOCGLOWPASS: - return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: - // set hardware filtering - _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case GYROIOCSSCALE: @@ -981,9 +930,6 @@ GYROSIM::set_accel_range(unsigned max_g_in) // workaround for bugged versions of MPU6k (rev C) switch (_product) { case GYROSIMES_REV_C4: - case GYROSIMES_REV_C5: - case GYROSIM_REV_C4: - case GYROSIM_REV_C5: write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); _accel_range_scale = (GYROSIM_ONE_G / 4096.0f); _accel_range_m_s2 = 8.0f * GYROSIM_ONE_G; @@ -1030,7 +976,9 @@ GYROSIM::start() _gyro_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + if (_call_interval > 0) { + hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + } } void @@ -1051,6 +999,19 @@ GYROSIM::measure_trampoline(void *arg) void GYROSIM::measure() { + +#if 0 + static int x = 0; + + // Verify the samples are being taken at the expected rate + if (x == 99) { + x = 0; + PX4_INFO("GYROSIM::measure %" PRIu64, hrt_absolute_time()); + } + else { + x++; + } +#endif struct MPUReport mpu_report; /* start measuring */ @@ -1071,7 +1032,7 @@ GYROSIM::measure() * Report buffers. */ accel_report arb; - gyro_report grb; + gyro_report grb; // for now use local time but this should be the timestamp of the simulator grb.timestamp = hrt_absolute_time(); @@ -1368,7 +1329,7 @@ test() } /* do a simple demand read */ - sz = read(fd, &a_report, sizeof(a_report)); + sz = px4_read(fd, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); @@ -1388,7 +1349,7 @@ test() (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ - sz = read(fd_gyro, &g_report, sizeof(g_report)); + sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); diff --git a/src/platforms/posix/px4_layer/hrt_work.h b/src/platforms/posix/include/hrt_work.h similarity index 95% rename from src/platforms/posix/px4_layer/hrt_work.h rename to src/platforms/posix/include/hrt_work.h index d926a6d250..39e53f95d2 100644 --- a/src/platforms/posix/px4_layer/hrt_work.h +++ b/src/platforms/posix/include/hrt_work.h @@ -43,12 +43,9 @@ extern sem_t _hrt_work_lock; extern struct wqueue_s g_hrt_work; void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); -//inline void hrt_work_lock(void); -//inline void hrt_work_unlock(void); - static inline void hrt_work_lock() { //PX4_INFO("hrt_work_lock"); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index d2cc7c2439..f85baa3b75 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -225,7 +225,7 @@ __END_DECLS #define SIOCDEVPRIVATE 999999 // Missing math.h defines -#define PX4_ISFINITE(x) isfinite(x) +#define PX4_ISFINITE(x) __builtin_isfinite(x) // FIXME - these are missing for clang++ but not for clang #if defined(__cplusplus) diff --git a/src/platforms/qurt/px4_layer/hrt_work.h b/src/platforms/qurt/include/hrt_work.h similarity index 99% rename from src/platforms/qurt/px4_layer/hrt_work.h rename to src/platforms/qurt/include/hrt_work.h index 566684eb86..92b079ac6b 100644 --- a/src/platforms/qurt/px4_layer/hrt_work.h +++ b/src/platforms/qurt/include/hrt_work.h @@ -43,7 +43,7 @@ extern sem_t _hrt_work_lock; extern struct wqueue_s g_hrt_work; void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); inline void hrt_work_lock(void);