Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: MS56XX: convert to threaded bus

Browse Source 
This converts MS56XX to use the thread started by SPI/I2C instead of
using the timer thread. This also fixes a possible starvation of the
main thread:

    1) INS driver registers itself to be sampled on timer thread
    2) MS56XX registers itself to be sampled on timer thread
    3) Main thread waits for a sample from INS with
       ins.wait_for_sample()
    4) timer thread is waiting on update from MS56XX and consequently
       the main thread is waiting on an I2C/SPI transfer

Besides this starvation there's another one due to reuse of the timer
lock in order to pump values from the timer thread to the main thread. A
call to the update() method when we have a sample available would need
to wait on any other driver holding the timer lock.

Now there's a lock just to pass the new values from the bus thread to
the main thread with a very tiny critical region, not waiting on any
bus transfers and/or syscalls.
This commit is contained in:
Lucas De Marchi 2016-07-25 10:33:28 -03:00
parent 052f30bc70
commit f7b453359d
2 changed files with 118 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -17,6 +17,8 @@
#include <utility>
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL &hal;
static const uint8_t CMD_MS56XX_RESET = 0x1E;
@ -39,6 +41,11 @@ static const uint8_t CMD_MS56XX_PROM = 0xA0;
#define ADDR_CMD_CONVERT_D2_OSR2048 0x56
#define ADDR_CMD_CONVERT_D2_OSR4096 0x58
#define CONVERSION_TIME_OSR_4096 9.04 * USEC_PER_MSEC
#define CONVERSION_TIME_OSR_2048 4.54 * USEC_PER_MSEC
#define CONVERSION_TIME_OSR_1024 2.28 * USEC_PER_MSEC
#define CONVERSION_TIME_OSR_0512 1.17 * USEC_PER_MSEC
#define CONVERSION_TIME_OSR_0256 0.60 * USEC_PER_MSEC
/*
use an OSR of 1024 to reduce the self-heating effect of the
sensor. Information from MS tells us that some individual sensors
@ -47,7 +54,7 @@ static const uint8_t CMD_MS56XX_PROM = 0xA0;
*/
static const uint8_t ADDR_CMD_CONVERT_PRESSURE = ADDR_CMD_CONVERT_D1_OSR1024;
static const uint8_t ADDR_CMD_CONVERT_TEMPERATURE = ADDR_CMD_CONVERT_D2_OSR1024;
static const uint32_t CONVERSION_TIME = CONVERSION_TIME_OSR_1024;
/*
constructor
*/
@ -63,11 +70,12 @@ void AP_Baro_MS56XX::_init()
AP_HAL::panic("AP_Baro_MS56XX: failed to use device");
}
_instance = _frontend.register_sensor();
_sem = hal.util->new_semaphore();
if (!_sem) {
AP_HAL::panic("AP_Baro_MS56XX: failed to create semaphore");
}
// we need to suspend timers to prevent other SPI drivers grabbing
// the bus while we do the long initialisation
hal.scheduler->suspend_timer_procs();
_instance = _frontend.register_sensor();
if (!_dev->get_semaphore()->take(10)) {
AP_HAL::panic("PANIC: AP_Baro_MS56XX: failed to take serial semaphore for init");
@ -91,20 +99,16 @@ void AP_Baro_MS56XX::_init()
// Send a command to read temperature first
_dev->transfer(&ADDR_CMD_CONVERT_TEMPERATURE, 1, nullptr, 0);
_last_timer = AP_HAL::micros();
_last_cmd_usec = AP_HAL::micros();
_state = 0;
_s_D1 = 0;
_s_D2 = 0;
_d1_count = 0;
_d2_count = 0;
memset(&_accum, 0, sizeof(_accum));
_dev->get_semaphore()->give();
hal.scheduler->resume_timer_procs();
/* timer needs to be called every 10ms so set the freq_div to 10 */
_timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Baro_MS56XX::_timer, void), 10);
/* Request 100Hz update */
_dev->register_periodic_callback(10 * USEC_PER_MSEC,
FUNCTOR_BIND_MEMBER(&AP_Baro_MS56XX::_timer, bool));
}
/**
@ -204,98 +208,95 @@ bool AP_Baro_MS5637::_read_prom(uint16_t prom[8])
}
/*
Read the sensor. This is a state machine
We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
temperature does not change so quickly...
* Read the sensor with a state machine
* We read one time temperature (state=0) and then 4 times pressure (states 1-4)
*
* Temperature is used to calculate the compensated pressure and doesn't vary
* as fast as pressure. Hence we reuse the same temperature for 4 samples of
* pressure.
*/
void AP_Baro_MS56XX::_timer(void)
bool AP_Baro_MS56XX::_timer(void)
{
// Throttle read rate to 100hz maximum.
if (!_timesliced &&
AP_HAL::micros() - _last_timer < 10000) {
return;
/*
* transfer is taking longer than it should or we got stuck by other
* sensors: skip one sample
*/
if (AP_HAL::micros() - _last_cmd_usec < CONVERSION_TIME) {
return true;
}
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
uint8_t next_cmd;
uint8_t next_state;
uint32_t adc_val = _read_adc();
if (_state == 0) {
// On state 0 we read temp
uint32_t d2 = _read_adc();
if (d2 != 0) {
_s_D2 += d2;
_d2_count++;
if (_d2_count == 32) {
// we have summed 32 values. This only happens
// when we stop reading the barometer for a long time
// (more than 1.2 seconds)
_s_D2 >>= 1;
_d2_count = 16;
}
if (_dev->transfer(&ADDR_CMD_CONVERT_PRESSURE, 1, nullptr, 0)) {
_state++;
}
} else {
/* if read fails, re-initiate a temperature read command or we are
* stuck */
_dev->transfer(&ADDR_CMD_CONVERT_TEMPERATURE, 1, nullptr, 0);
}
/*
* If read fails, re-initiate a read command for current state or we are
* stuck
*/
if (adc_val == 0) {
next_state = _state;
} else {
uint32_t d1 = _read_adc();
if (d1 != 0) {
// occasional zero values have been seen on the PXF
// board. These may be SPI errors, but safest to ignore
_s_D1 += d1;
_d1_count++;
if (_d1_count == 128) {
// we have summed 128 values. This only happens
// when we stop reading the barometer for a long time
// (more than 1.2 seconds)
_s_D1 >>= 1;
_d1_count = 64;
}
// Now a new reading exists
_updated = true;
if (_state == 4) {
if (_dev->transfer(&ADDR_CMD_CONVERT_TEMPERATURE, 1, nullptr, 0)) {
_state = 0;
}
} else {
if (_dev->transfer(&ADDR_CMD_CONVERT_PRESSURE, 1, nullptr, 0)) {
_state++;
}
}
} else {
/* if read fails, re-initiate a pressure read command or we are
* stuck */
_dev->transfer(&ADDR_CMD_CONVERT_PRESSURE, 1, nullptr, 0);
}
next_state = (_state + 1) % 5;
}
_last_timer = AP_HAL::micros();
_dev->get_semaphore()->give();
next_cmd = next_state == 0 ? ADDR_CMD_CONVERT_TEMPERATURE
: ADDR_CMD_CONVERT_PRESSURE;
_dev->transfer(&next_cmd, 1, nullptr, 0);
_last_cmd_usec = AP_HAL::micros();
/* if we had a failed read we are all done */
if (adc_val == 0) {
return true;
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (_state == 0) {
_update_and_wrap_accumulator(&_accum.s_D2, adc_val,
&_accum.d2_count, 32);
} else {
_update_and_wrap_accumulator(&_accum.s_D1, adc_val,
&_accum.d1_count, 128);
}
_sem->give();
_state = next_state;
}
return true;
}
void AP_Baro_MS56XX::_update_and_wrap_accumulator(uint32_t *accum, uint32_t val,
uint8_t *count, uint8_t max_count)
{
*accum += val;
*count += 1;
if (*count == max_count) {
*count = max_count / 2;
*accum = *accum / 2;
}
}
void AP_Baro_MS56XX::update()
{
if (!_updated) {
return;
}
uint32_t sD1, sD2;
uint8_t d1count, d2count;
// Suspend timer procs because these variables are written to
// in "_update".
hal.scheduler->suspend_timer_procs();
sD1 = _s_D1; _s_D1 = 0;
sD2 = _s_D2; _s_D2 = 0;
d1count = _d1_count; _d1_count = 0;
d2count = _d2_count; _d2_count = 0;
_updated = false;
hal.scheduler->resume_timer_procs();
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
if (_accum.d1_count == 0) {
_sem->give();
return;
}
sD1 = _accum.s_D1;
sD2 = _accum.s_D2;
d1count = _accum.d1_count;
d2count = _accum.d2_count;
memset(&_accum, 0, sizeof(_accum));
_sem->give();
if (d1count != 0) {
_D1 = ((float)sD1) / d1count;

35
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -4,6 +4,7 @@
#include "AP_Baro_Backend.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/Semaphores.h>
#include <AP_HAL/Device.h>
class AP_Baro_MS56XX : public AP_Baro_Backend
@ -12,6 +13,14 @@ public:
void update();
protected:
/*
* Update @accum and @count with the new sample in @val, taking into
* account a maximum number of samples given by @max_count; in case
* maximum number is reached, @accum and @count are updated appropriately
*/
static void _update_and_wrap_accumulator(uint32_t *accum, uint32_t val,
uint8_t *count, uint8_t max_count);
AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
void _init();
@ -21,18 +30,26 @@ protected:
uint16_t _read_prom_word(uint8_t word);
uint32_t _read_adc();
void _timer();
bool _timer();
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
/* Asynchronous state: */
volatile bool _updated;
volatile uint8_t _d1_count;
volatile uint8_t _d2_count;
volatile uint32_t _s_D1, _s_D2;
uint8_t _state;
uint32_t _last_timer;
bool _timesliced;
/*
* Synchronize access to _accum between thread sampling the HW and main
* thread using the values
*/
AP_HAL::Semaphore *_sem;
/* Shared values between thread sampling the HW and main thread */
struct {
uint32_t s_D1;
uint32_t s_D2;
uint8_t d1_count;
uint8_t d2_count;
} _accum;
uint8_t _state;
uint32_t _last_cmd_usec;
// Internal calibration registers
uint16_t _c1,_c2,_c3,_c4,_c5,_c6;