fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2497 from mcharleb/bringup-m5

Browse Source 
Bringup m5
This commit is contained in:
Lorenz Meier 2015-06-30 20:02:57 +02:00
commit 428bd0a9ec
5 changed files with 47 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/drivers/device/vdev_posix.cpp
View File

@ -43,6 +43,7 @@
#include "device.h"
#include "vfile.h"
#include <hrt_work.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
@ -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;

115
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
View File

@ -41,6 +41,9 @@
* @author Mark Charlebois
*/
#define __STDC_FORMAT_MACROS
#include <inttypes.h>
#include <px4_config.h>
#include <px4_getopt.h>
@ -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));

5
src/platforms/posix/px4_layer/hrt_work.h → src/platforms/posix/include/hrt_work.h
View File

@ -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");

2
src/platforms/px4_defines.h
View File

@ -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)

2
src/platforms/qurt/px4_layer/hrt_work.h → src/platforms/qurt/include/hrt_work.h
View File

@ -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);