From da2ac877f829db1339948bcadd29bcbc6a9970eb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 29 Jun 2015 19:08:06 -0700 Subject: [PATCH 1/4] POSIX: Changed px4_poll to use hrt_work queue QuRT's pthread_cancel implementation is lacking, and causes px4_poll to always wait for the maximumn timeout. A cleaner implementation is provided that uses the HRT work queue for posix targets. In the future the posix code should be rtefactiored so that qurt (and other) implementations that are duplicated, use the posix implementation. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev_posix.cpp | 22 +++++-------------- .../posix/{px4_layer => include}/hrt_work.h | 5 +---- .../qurt/{px4_layer => include}/hrt_work.h | 2 +- 3 files changed, 8 insertions(+), 21 deletions(-) rename src/platforms/posix/{px4_layer => include}/hrt_work.h (95%) rename src/platforms/qurt/{px4_layer => include}/hrt_work.h (99%) 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/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/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); From 641fd26877ce50f52bc6d5f35809d325c92c89c5 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 30 Jun 2015 09:10:06 -0700 Subject: [PATCH 2/4] QuRT: Fixed PX4_ISFINITE QuRT needs to use the builtin version of isfinite so for the qurt build PX4_ISFINITE(x) is defined as __builtin_isfinite(x). Signed-off-by: Mark Charlebois --- src/platforms/px4_defines.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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) From 34d15fe63191873e3293bb7ba41cacb7172ba76b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 30 Jun 2015 09:23:37 -0700 Subject: [PATCH 3/4] Gyrosim cleanup Removed unused code. Reset reschedule interval for sampling when the sampling rate is changed. The rate is always 1000Hz as it is set to the default value. Signed-off-by: Mark Charlebois --- .../posix/drivers/gyrosim/gyrosim.cpp | 114 ++++++------------ 1 file changed, 37 insertions(+), 77 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index fa7f92abba..f2dd548679 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,18 @@ GYROSIM::measure_trampoline(void *arg) void GYROSIM::measure() { + static int x = 0; + +#if 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 +1031,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 +1328,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 +1348,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)); From 1b01c54dd1ab2681b8c571dfac08753919d8b66e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 30 Jun 2015 09:53:01 -0700 Subject: [PATCH 4/4] POSIX: fixed build error for unused variable Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index f2dd548679..0fde8b293a 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -999,9 +999,10 @@ GYROSIM::measure_trampoline(void *arg) void GYROSIM::measure() { - static int x = 0; #if 0 + static int x = 0; + // Verify the samples are being taken at the expected rate if (x == 99) { x = 0;