zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Baro: MS56XX: convert to threaded bus 

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.
mission-4.1.18
Lucas De Marchi 9 years ago
parent
052f30bc70
commit
f7b453359d
2 changed files with 114 additions and 96 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 175
      libraries/AP_Baro/AP_Baro_MS5611.cpp
  2. 35
      libraries/AP_Baro/AP_Baro_MS5611.h

175
libraries/AP_Baro/AP_Baro_MS5611.cpp
Unescape View File

@ -17,6 +17,8 @@ @@ -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; @@ -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; @@ -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() @@ -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() @@ -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]) @@ -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 read fails, re-initiate a read command for current state or we are
* stuck
*/
if (adc_val == 0) {
next_state = _state;
} else {
next_state = (_state + 1) % 5;
}
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;
}
next_cmd = next_state == 0 ? ADDR_CMD_CONVERT_TEMPERATURE
: ADDR_CMD_CONVERT_PRESSURE;
_dev->transfer(&next_cmd, 1, nullptr, 0);
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);
}
} 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;
_last_cmd_usec = AP_HAL::micros();
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++;
}
}
/* 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 {
/* if read fails, re-initiate a pressure read command or we are
* stuck */
_dev->transfer(&ADDR_CMD_CONVERT_PRESSURE, 1, nullptr, 0);
_update_and_wrap_accumulator(&_accum.s_D1, adc_val,
&_accum.d1_count, 128);
}
_sem->give();
_state = next_state;
}
_last_timer = AP_HAL::micros();
_dev->get_semaphore()->give();
return true;
}
void AP_Baro_MS56XX::update()
void AP_Baro_MS56XX::_update_and_wrap_accumulator(uint32_t *accum, uint32_t val,
uint8_t *count, uint8_t max_count)
{
if (!_updated) {
return;
*accum += val;
*count += 1;
if (*count == max_count) {
*count = max_count / 2;
*accum = *accum / 2;
}
}
void AP_Baro_MS56XX::update()
{
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
Unescape View File

@ -4,6 +4,7 @@ @@ -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: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save