2011-12-16 04:30:46 -04:00
|
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
2013-08-26 06:36:56 -03:00
|
|
|
|
/*
|
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
|
*/
|
|
|
|
|
|
2011-12-16 04:30:46 -04:00
|
|
|
|
/*
|
2012-08-17 02:39:21 -03:00
|
|
|
|
* AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega
|
|
|
|
|
* Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
|
|
|
|
|
*
|
|
|
|
|
* Modified by John Ihlein 6 / 19 / 2010 to:
|
|
|
|
|
* 1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably
|
|
|
|
|
* only an issue on initial read of ADC at program start.
|
|
|
|
|
* 2)Reorder analog read order as follows:
|
|
|
|
|
* p, q, r, ax, ay, az
|
|
|
|
|
* External ADC ADS7844 is connected via Serial port 2 (in SPI mode)
|
|
|
|
|
* TXD2 = MOSI = pin PH1
|
|
|
|
|
* RXD2 = MISO = pin PH0
|
|
|
|
|
* XCK2 = SCK = pin PH2
|
|
|
|
|
* Chip Select pin is PC4 (33) [PH6 (9)]
|
|
|
|
|
* We are using the 16 clocks per conversion timming to increase efficiency (fast)
|
|
|
|
|
*
|
|
|
|
|
* The sampling frequency is 1kHz (Timer2 overflow interrupt)
|
|
|
|
|
*
|
|
|
|
|
* So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
|
|
|
|
|
* we have an 10x oversampling and averaging.
|
|
|
|
|
*
|
|
|
|
|
* Methods:
|
|
|
|
|
* Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
|
|
|
|
|
* Ch(ch_num) : Return the ADC channel value
|
|
|
|
|
*
|
|
|
|
|
* // HJI - Input definitions. USB connector assumed to be on the left, Rx and servo
|
|
|
|
|
* // connector pins to the rear. IMU shield components facing up. These are board
|
|
|
|
|
* // referenced sensor inputs, not device referenced.
|
|
|
|
|
* On Ardupilot Mega Hardware, oriented as described above:
|
|
|
|
|
* Chennel 0 : yaw rate, r
|
|
|
|
|
* Channel 1 : roll rate, p
|
|
|
|
|
* Channel 2 : pitch rate, q
|
|
|
|
|
* Channel 3 : x / y gyro temperature
|
|
|
|
|
* Channel 4 : x acceleration, aX
|
|
|
|
|
* Channel 5 : y acceleration, aY
|
|
|
|
|
* Channel 6 : z acceleration, aZ
|
|
|
|
|
* Channel 7 : Differential pressure sensor port
|
|
|
|
|
*
|
|
|
|
|
*/
|
2012-10-09 19:39:43 -03:00
|
|
|
|
|
|
|
|
|
#include <AP_Progmem.h>
|
|
|
|
|
#include <AP_Common.h>
|
|
|
|
|
#include <AP_HAL.h>
|
|
|
|
|
|
2012-01-27 23:25:47 -04:00
|
|
|
|
#include "AP_ADC_ADS7844.h"
|
|
|
|
|
|
2012-10-09 19:39:43 -03:00
|
|
|
|
extern const AP_HAL::HAL& hal;
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
2012-10-09 19:39:43 -03:00
|
|
|
|
// DO NOT CHANGE FROM 8!!
|
|
|
|
|
#define ADC_ACCEL_FILTER_SIZE 8
|
2011-12-16 04:30:46 -04:00
|
|
|
|
// Commands for reading ADC channels on ADS7844
|
2012-12-17 21:25:42 -04:00
|
|
|
|
static const unsigned char adc_cmd[17] =
|
|
|
|
|
{ 0x87, 0, 0xC7, 0, 0x97, 0, 0xD7, 0, 0xA7, 0, 0xE7, 0, 0xB7, 0, 0xF7};
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
|
|
|
|
// the sum of the values since last read
|
|
|
|
|
static volatile uint32_t _sum[8];
|
|
|
|
|
|
|
|
|
|
// how many values we've accumulated since last read
|
|
|
|
|
static volatile uint16_t _count[8];
|
|
|
|
|
|
2012-10-09 19:39:43 -03:00
|
|
|
|
// variables to calculate time period over which a group of samples were
|
|
|
|
|
// collected
|
|
|
|
|
// time we start collecting sample (reset on update)
|
|
|
|
|
static volatile uint32_t _ch6_delta_time_start_micros = 0;
|
|
|
|
|
// time latest sample was collected
|
|
|
|
|
static volatile uint32_t _ch6_last_sample_time_micros = 0;
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
2013-09-28 03:30:16 -03:00
|
|
|
|
void AP_ADC_ADS7844::read(void)
|
2011-12-16 04:30:46 -04:00
|
|
|
|
{
|
2012-12-05 21:18:47 -04:00
|
|
|
|
static int semfail_ctr = 0;
|
2012-08-17 02:39:21 -03:00
|
|
|
|
uint8_t ch;
|
|
|
|
|
|
2013-01-03 14:49:28 -04:00
|
|
|
|
/** Take nonblocking: ::read happens from the TimerProcess context! */
|
|
|
|
|
bool got = _spi_sem->take_nonblocking();
|
|
|
|
|
if (!got) {
|
|
|
|
|
semfail_ctr++;
|
|
|
|
|
if (semfail_ctr > 100) {
|
|
|
|
|
hal.scheduler->panic(PSTR("PANIC: failed to take _spi_sem "
|
|
|
|
|
"100 times in AP_ADC_ADS7844::read"));
|
|
|
|
|
}
|
|
|
|
|
return;
|
|
|
|
|
} else {
|
|
|
|
|
semfail_ctr = 0;
|
|
|
|
|
}
|
2012-10-09 19:39:43 -03:00
|
|
|
|
|
2012-12-17 21:25:42 -04:00
|
|
|
|
uint8_t rx[17];
|
|
|
|
|
_spi->transaction(adc_cmd, rx, 17);
|
2012-08-17 02:39:21 -03:00
|
|
|
|
|
|
|
|
|
for (ch = 0; ch < 8; ch++) {
|
2012-12-17 21:25:42 -04:00
|
|
|
|
uint16_t v = (rx[2*ch+1] << 8) | rx[2*ch+2];
|
2012-08-17 02:39:21 -03:00
|
|
|
|
if (v & 0x8007) {
|
|
|
|
|
// this is a 12-bit ADC, shifted by 3 bits.
|
|
|
|
|
// if we get other bits set then the value is
|
|
|
|
|
// bogus and should be ignored
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
if (++_count[ch] == 0) {
|
|
|
|
|
// overflow ... shouldn't happen too often
|
|
|
|
|
// unless we're just not using the
|
|
|
|
|
// channel. Notice that we overflow the count
|
|
|
|
|
// to 1 here, not zero, as otherwise the
|
|
|
|
|
// reader below could get a division by zero
|
|
|
|
|
_sum[ch] = 0;
|
|
|
|
|
_count[ch] = 1;
|
|
|
|
|
}
|
|
|
|
|
_sum[ch] += (v >> 3);
|
|
|
|
|
}
|
|
|
|
|
|
2013-01-03 14:49:28 -04:00
|
|
|
|
_spi_sem->give();
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
2012-08-30 04:48:36 -03:00
|
|
|
|
// record time of this sample
|
2012-10-09 19:39:43 -03:00
|
|
|
|
_ch6_last_sample_time_micros = hal.scheduler->micros();
|
2011-12-16 04:30:46 -04:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Constructors ////////////////////////////////////////////////////////////////
|
2012-10-09 19:39:43 -03:00
|
|
|
|
AP_ADC_ADS7844::AP_ADC_ADS7844() { }
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
2011-11-12 23:12:53 -04:00
|
|
|
|
// Public Methods //////////////////////////////////////////////////////////////
|
2012-10-09 19:39:43 -03:00
|
|
|
|
void AP_ADC_ADS7844::Init()
|
2011-11-12 23:12:53 -04:00
|
|
|
|
{
|
2012-10-09 19:39:43 -03:00
|
|
|
|
hal.scheduler->suspend_timer_procs();
|
|
|
|
|
_spi = hal.spi->device(AP_HAL::SPIDevice_ADS7844);
|
|
|
|
|
if (_spi == NULL) {
|
2013-01-03 14:49:28 -04:00
|
|
|
|
hal.scheduler->panic(PSTR("PANIC: AP_ADC_ADS7844 missing SPI device "
|
|
|
|
|
"driver\n"));
|
2012-10-09 19:39:43 -03:00
|
|
|
|
}
|
2013-01-03 14:49:28 -04:00
|
|
|
|
|
2012-10-09 19:39:43 -03:00
|
|
|
|
_spi_sem = _spi->get_semaphore();
|
2013-01-03 14:49:28 -04:00
|
|
|
|
|
|
|
|
|
if (_spi_sem == NULL) {
|
|
|
|
|
hal.scheduler->panic(PSTR("PANIC: AP_ADC_ADS7844 missing SPI device "
|
|
|
|
|
"semaphore"));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (!_spi_sem->take(0)) {
|
|
|
|
|
hal.scheduler->panic(PSTR("PANIC: failed to take _spi_sem in"
|
|
|
|
|
"AP_ADC_ADS7844::Init"));
|
2012-10-09 19:39:43 -03:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_spi->cs_assert();
|
2012-08-17 02:39:21 -03:00
|
|
|
|
// get an initial value for each channel. This ensures
|
|
|
|
|
// _count[] is never zero
|
|
|
|
|
for (uint8_t i=0; i<8; i++) {
|
|
|
|
|
uint16_t adc_tmp;
|
2012-10-09 19:39:43 -03:00
|
|
|
|
adc_tmp = _spi->transfer(0) << 8;
|
|
|
|
|
adc_tmp |= _spi->transfer(adc_cmd[i + 1]);
|
2012-08-17 02:39:21 -03:00
|
|
|
|
_count[i] = 1;
|
|
|
|
|
_sum[i] = adc_tmp;
|
|
|
|
|
}
|
2012-10-09 19:39:43 -03:00
|
|
|
|
|
|
|
|
|
_spi->cs_release();
|
2012-08-17 02:39:21 -03:00
|
|
|
|
|
2013-01-03 14:49:28 -04:00
|
|
|
|
_spi_sem->give();
|
2011-11-12 23:12:53 -04:00
|
|
|
|
|
2012-10-09 19:39:43 -03:00
|
|
|
|
_ch6_last_sample_time_micros = hal.scheduler->micros();
|
|
|
|
|
|
2013-09-28 03:30:16 -03:00
|
|
|
|
hal.scheduler->register_timer_process( reinterpret_cast<AP_HAL::TimedProc>(&AP_ADC_ADS7844::read), this );
|
2012-12-05 21:18:47 -04:00
|
|
|
|
hal.scheduler->resume_timer_procs();
|
2011-11-12 23:12:53 -04:00
|
|
|
|
|
|
|
|
|
}
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
|
|
|
|
// Read one channel value
|
|
|
|
|
float AP_ADC_ADS7844::Ch(uint8_t ch_num)
|
|
|
|
|
{
|
2012-08-17 02:39:21 -03:00
|
|
|
|
uint16_t count;
|
|
|
|
|
uint32_t sum;
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
2012-08-17 02:39:21 -03:00
|
|
|
|
// ensure we have at least one value
|
|
|
|
|
while (_count[ch_num] == 0) /* noop */;
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
2013-01-03 19:42:22 -04:00
|
|
|
|
// grab the value with timer procs disabled, and clear the count
|
|
|
|
|
hal.scheduler->suspend_timer_procs();
|
2012-08-17 02:39:21 -03:00
|
|
|
|
count = _count[ch_num];
|
|
|
|
|
sum = _sum[ch_num];
|
|
|
|
|
_count[ch_num] = 0;
|
|
|
|
|
_sum[ch_num] = 0;
|
2013-01-03 19:42:22 -04:00
|
|
|
|
hal.scheduler->resume_timer_procs();
|
2011-12-16 04:30:46 -04:00
|
|
|
|
|
2012-08-17 02:39:21 -03:00
|
|
|
|
return ((float)sum)/count;
|
2011-12-16 04:30:46 -04:00
|
|
|
|
}
|
|
|
|
|
|
2012-03-03 03:31:09 -04:00
|
|
|
|
// see if Ch6() can return new data
|
|
|
|
|
bool AP_ADC_ADS7844::new_data_available(const uint8_t *channel_numbers)
|
|
|
|
|
{
|
2012-08-17 02:39:21 -03:00
|
|
|
|
uint8_t i;
|
2012-03-03 03:31:09 -04:00
|
|
|
|
|
2012-08-17 02:39:21 -03:00
|
|
|
|
for (i=0; i<6; i++) {
|
|
|
|
|
if (_count[channel_numbers[i]] == 0) {
|
2012-03-03 03:31:09 -04:00
|
|
|
|
return false;
|
|
|
|
|
}
|
2012-08-17 02:39:21 -03:00
|
|
|
|
}
|
2012-03-03 03:31:09 -04:00
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
2011-12-16 04:30:46 -04:00
|
|
|
|
// Read 6 channel values
|
|
|
|
|
// this assumes that the counts for all of the 6 channels are
|
|
|
|
|
// equal. This will only be true if we always consistently access a
|
|
|
|
|
// sensor by either Ch6() or Ch() and never mix them. If you mix them
|
|
|
|
|
// then you will get very strange results
|
2012-03-07 00:14:29 -04:00
|
|
|
|
uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, float *result)
|
2011-12-16 04:30:46 -04:00
|
|
|
|
{
|
2012-08-17 02:39:21 -03:00
|
|
|
|
uint16_t count[6];
|
|
|
|
|
uint32_t sum[6];
|
|
|
|
|
uint8_t i;
|
|
|
|
|
|
|
|
|
|
// ensure we have at least one value
|
|
|
|
|
for (i=0; i<6; i++) {
|
|
|
|
|
while (_count[channel_numbers[i]] == 0) /* noop */;
|
|
|
|
|
}
|
|
|
|
|
|
2013-01-03 19:42:22 -04:00
|
|
|
|
// grab the values with timer procs disabled, and clear the counts
|
|
|
|
|
hal.scheduler->suspend_timer_procs();
|
2012-08-17 02:39:21 -03:00
|
|
|
|
for (i=0; i<6; i++) {
|
|
|
|
|
count[i] = _count[channel_numbers[i]];
|
|
|
|
|
sum[i] = _sum[channel_numbers[i]];
|
|
|
|
|
_count[channel_numbers[i]] = 0;
|
|
|
|
|
_sum[channel_numbers[i]] = 0;
|
|
|
|
|
}
|
2012-08-30 04:48:36 -03:00
|
|
|
|
|
|
|
|
|
// calculate the delta time.
|
|
|
|
|
// we do this before re-enabling interrupts because another sensor read could fire immediately and change the _last_sensor_time value
|
|
|
|
|
uint32_t ret = _ch6_last_sample_time_micros - _ch6_delta_time_start_micros;
|
|
|
|
|
_ch6_delta_time_start_micros = _ch6_last_sample_time_micros;
|
|
|
|
|
|
2013-01-03 19:42:22 -04:00
|
|
|
|
hal.scheduler->resume_timer_procs();
|
2012-08-17 02:39:21 -03:00
|
|
|
|
|
|
|
|
|
// calculate averages. We keep this out of the cli region
|
|
|
|
|
// to prevent us stalling the ISR while doing the
|
|
|
|
|
// division. That costs us 36 bytes of stack, but I think its
|
|
|
|
|
// worth it.
|
|
|
|
|
for (i = 0; i < 6; i++) {
|
|
|
|
|
result[i] = sum[i] / (float)count[i];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// return number of microseconds since last call
|
|
|
|
|
return ret;
|
2011-12-16 04:30:46 -04:00
|
|
|
|
}
|
2012-08-30 04:50:03 -03:00
|
|
|
|
|
|
|
|
|
/// Get minimum number of samples read from the sensors
|
|
|
|
|
uint16_t AP_ADC_ADS7844::num_samples_available(const uint8_t *channel_numbers)
|
|
|
|
|
{
|
|
|
|
|
// get count of first channel as a base
|
|
|
|
|
uint16_t min_count = _count[channel_numbers[0]];
|
|
|
|
|
|
|
|
|
|
// reduce to minimum count of all other channels
|
|
|
|
|
for (uint8_t i=1; i<6; i++) {
|
|
|
|
|
if (_count[channel_numbers[i]] < min_count) {
|
|
|
|
|
min_count = _count[channel_numbers[i]];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return min_count;
|
|
|
|
|
}
|