mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Revert "Changed the MS5611 from the workq to hrt_call_every implementation, this seems to solve the SPI chip select overlaps"
This reverts commit c93e743374731ec06020ba6919c6d48594d4a58c.
This commit is contained in:
px4dev
parent
cfbdf7c903
commit
f6daafba03
@ -111,9 +111,8 @@ protected:
|
||||
|
||||
ms5611::prom_s _prom;
|
||||
|
||||
struct hrt_call _baro_call;
|
||||
|
||||
unsigned _call_baro_interval;
|
||||
struct work_s _work;
|
||||
unsigned _measure_ticks;
|
||||
|
||||
unsigned _num_reports;
|
||||
volatile unsigned _next_report;
|
||||
@ -144,12 +143,12 @@ protected:
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
void start_cycle();
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
void stop();
|
||||
void stop_cycle();
|
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
@ -167,11 +166,12 @@ protected:
|
||||
void cycle();
|
||||
|
||||
/**
|
||||
* Static trampoline; XXX is this needed?
|
||||
* Static trampoline from the workq context; because we don't have a
|
||||
* generic workq wrapper yet.
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
void cycle_trampoline(void *arg);
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
/**
|
||||
* Issue a measurement command for the current state.
|
||||
@ -184,7 +184,6 @@ protected:
|
||||
* Collect the result of the most recent measurement.
|
||||
*/
|
||||
virtual int collect();
|
||||
|
||||
};
|
||||
|
||||
/*
|
||||
@ -196,7 +195,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
|
||||
CDev("MS5611", BARO_DEVICE_PATH),
|
||||
_interface(interface),
|
||||
_prom(prom_buf.s),
|
||||
_call_baro_interval(0),
|
||||
_measure_ticks(0),
|
||||
_num_reports(0),
|
||||
_next_report(0),
|
||||
_oldest_report(0),
|
||||
@ -213,13 +212,14 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
|
||||
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
|
||||
{
|
||||
|
||||
// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
||||
MS5611::~MS5611()
|
||||
{
|
||||
/* make sure we are truly inactive */
|
||||
stop();
|
||||
stop_cycle();
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
||||
return -ENOSPC;
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_call_baro_interval > 0) {
|
||||
if (_measure_ticks > 0) {
|
||||
|
||||
/*
|
||||
* While there is space in the caller's buffer, and reports, copy them.
|
||||
@ -298,13 +298,41 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
||||
|
||||
/* manual measurement - run one conversion */
|
||||
/* XXX really it'd be nice to lock against other readers here */
|
||||
_measure_phase = 0;
|
||||
_oldest_report = _next_report = 0;
|
||||
measure();
|
||||
do {
|
||||
_measure_phase = 0;
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
/* do temperature first */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* now do a pressure measurement */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
|
||||
} while (0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
@ -319,8 +347,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
stop();
|
||||
_call_baro_interval = 0;
|
||||
stop_cycle();
|
||||
_measure_ticks = 0;
|
||||
return OK;
|
||||
|
||||
/* external signalling not supported */
|
||||
@ -334,14 +362,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_call_baro_interval == 0);
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* set interval for next measurement to minimum legal value */
|
||||
_baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL);
|
||||
_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start();
|
||||
start_cycle();
|
||||
|
||||
return OK;
|
||||
}
|
||||
@ -349,7 +377,7 @@ MS5611::ioctl(struct file *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_baro_interval == 0);
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
@ -359,11 +387,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
return -EINVAL;
|
||||
|
||||
/* update interval for next measurement */
|
||||
_baro_call.period = _call_baro_interval = ticks;
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start();
|
||||
start_cycle();
|
||||
|
||||
return OK;
|
||||
}
|
||||
@ -371,10 +399,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_call_baro_interval == 0)
|
||||
if (_measure_ticks == 0)
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
|
||||
return (1000 / _call_baro_interval);
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* add one to account for the sentinel in the ring */
|
||||
@ -391,11 +419,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
return -ENOMEM;
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop();
|
||||
stop_cycle();
|
||||
delete[] _reports;
|
||||
_num_reports = arg;
|
||||
_reports = buf;
|
||||
start();
|
||||
start_cycle();
|
||||
|
||||
return OK;
|
||||
}
|
||||
@ -429,32 +457,30 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
|
||||
void
|
||||
MS5611::start()
|
||||
MS5611::start_cycle()
|
||||
{
|
||||
/* make sure we are stopped first */
|
||||
stop();
|
||||
|
||||
/* reset the report ring */
|
||||
_oldest_report = _next_report = 0;
|
||||
/* reset to first measure phase */
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
_measure_phase = 0;
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this);
|
||||
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
|
||||
}
|
||||
|
||||
void
|
||||
MS5611::stop()
|
||||
MS5611::stop_cycle()
|
||||
{
|
||||
hrt_cancel(&_baro_call);
|
||||
work_cancel(HPWORK, &_work);
|
||||
}
|
||||
|
||||
void
|
||||
MS5611::cycle_trampoline(void *arg)
|
||||
{
|
||||
MS5611 *dev = (MS5611 *)arg;
|
||||
MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
|
||||
|
||||
cycle();
|
||||
dev->cycle();
|
||||
}
|
||||
|
||||
void
|
||||
@ -478,7 +504,7 @@ MS5611::cycle()
|
||||
//log("collection error %d", ret);
|
||||
}
|
||||
/* reset the collection state machine and try again */
|
||||
start();
|
||||
start_cycle();
|
||||
return;
|
||||
}
|
||||
|
||||
@ -491,16 +517,14 @@ MS5611::cycle()
|
||||
* doing pressure measurements at something close to the desired rate.
|
||||
*/
|
||||
if ((_measure_phase != 0) &&
|
||||
(_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
|
||||
(_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
|
||||
|
||||
// XXX maybe do something appropriate as well
|
||||
|
||||
// /* schedule a fresh cycle call when we are ready to measure again */
|
||||
// work_queue(HPWORK,
|
||||
// &_work,
|
||||
// (worker_t)&MS5611::cycle_trampoline,
|
||||
// this,
|
||||
// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&MS5611::cycle_trampoline,
|
||||
this,
|
||||
_measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
|
||||
|
||||
return;
|
||||
}
|
||||
@ -511,12 +535,19 @@ MS5611::cycle()
|
||||
if (ret != OK) {
|
||||
//log("measure error %d", ret);
|
||||
/* reset the collection state machine and try again */
|
||||
start();
|
||||
start_cycle();
|
||||
return;
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&MS5611::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(MS5611_CONVERSION_INTERVAL));
|
||||
}
|
||||
|
||||
int
|
||||
@ -665,14 +696,13 @@ MS5611::collect()
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
MS5611::print_info()
|
||||
{
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _call_baro_interval);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
printf("report queue: %u (%u/%u @ %p)\n",
|
||||
_num_reports, _oldest_report, _next_report, _reports);
|
||||
printf("TEMP: %d\n", _TEMP);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user