ADC: re-work for ADC code for full resolution and accurate timing
this changes the ADC code to return the full resolution of the sensors. It also adds a new Ch6() interface that returns 6 channels at once, so the IMU can read 3 accelerometers and 3 gyros at once, and get the exact time that the values were accumulated over
This commit is contained in:
Andrew Tridgell
parent
49d96726d5
commit
70bf945d40
@ -1,6 +1,8 @@
|
||||
#ifndef AP_ADC_H
|
||||
#define AP_ADC_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
|
||||
Code by James Goppert. DIYDrones.com
|
||||
@ -13,7 +15,7 @@
|
||||
Methods:
|
||||
Init() : Initialization of ADC. (interrupts etc)
|
||||
Ch(ch_num) : Return the ADC channel value
|
||||
|
||||
Ch6(channel_numbers, result) : Return 6 ADC channel values
|
||||
*/
|
||||
|
||||
class AP_ADC
|
||||
@ -21,11 +23,23 @@ class AP_ADC
|
||||
public:
|
||||
AP_ADC() {}; // Constructor
|
||||
virtual void Init() {};
|
||||
virtual int Ch(unsigned char ch_num) = 0;
|
||||
virtual int Ch_raw(unsigned char ch_num) = 0;
|
||||
private:
|
||||
};
|
||||
|
||||
|
||||
/* read one channel value */
|
||||
virtual uint16_t Ch(uint8_t ch_num) = 0;
|
||||
|
||||
/* read 6 channels values as a set, used by IMU for 3 gyros
|
||||
and 3 accelerometeres.
|
||||
|
||||
Pass in an array of 6 channel numbers and results are
|
||||
returned in result[]
|
||||
|
||||
The function returns the amount of time that the returned
|
||||
value has been averaged over, in 2.5 millisecond units
|
||||
*/
|
||||
virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0;
|
||||
private:
|
||||
};
|
||||
|
||||
#include "AP_ADC_ADS7844.h"
|
||||
#include "AP_ADC_HIL.h"
|
||||
|
||||
|
||||
@ -44,6 +44,7 @@
|
||||
extern "C" {
|
||||
// AVR LibC Includes
|
||||
#include <inttypes.h>
|
||||
#include <stdint.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include "WConstants.h"
|
||||
}
|
||||
@ -53,8 +54,12 @@ extern "C" {
|
||||
|
||||
// Commands for reading ADC channels on ADS7844
|
||||
static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 };
|
||||
static volatile uint16_t _filter[8][ADC_FILTER_SIZE];
|
||||
static volatile uint8_t _filter_index;
|
||||
|
||||
// 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];
|
||||
|
||||
static unsigned char ADC_SPI_transfer(unsigned char data)
|
||||
{
|
||||
@ -72,32 +77,33 @@ static unsigned char ADC_SPI_transfer(unsigned char data)
|
||||
ISR (TIMER2_OVF_vect)
|
||||
{
|
||||
uint8_t ch;
|
||||
uint16_t adc_tmp;
|
||||
|
||||
//bit_set(PORTL,6); // To test performance
|
||||
//bit_set(PORTL,6); // To test performance
|
||||
|
||||
bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
|
||||
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
|
||||
bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
|
||||
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
|
||||
|
||||
for (ch = 0; ch < 8; ch++){
|
||||
adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte
|
||||
uint16_t adc_tmp;
|
||||
adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte
|
||||
adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
|
||||
|
||||
// Fill our Moving average filter
|
||||
_filter[ch][_filter_index] = adc_tmp >> 3;
|
||||
_count[ch]++;
|
||||
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] += adc_tmp;
|
||||
}
|
||||
|
||||
// increment our filter
|
||||
_filter_index++;
|
||||
|
||||
// loop our filter
|
||||
if(_filter_index == ADC_FILTER_SIZE)
|
||||
_filter_index = 0;
|
||||
|
||||
|
||||
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
|
||||
//bit_clear(PORTL,6); // To test performance
|
||||
TCNT2 = 104; // 400 Hz
|
||||
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
|
||||
//bit_clear(PORTL,6); // To test performance
|
||||
TCNT2 = 104; // 400Hz interrupt
|
||||
}
|
||||
|
||||
|
||||
@ -123,6 +129,16 @@ void AP_ADC_ADS7844::Init(void)
|
||||
// Set Baud rate
|
||||
UBRR2 = 2; // SPI clock running at 2.6MHz
|
||||
|
||||
// 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;
|
||||
adc_tmp = ADC_SPI_transfer(0) << 8;
|
||||
adc_tmp |= ADC_SPI_transfer(adc_cmd[i + 1]);
|
||||
_count[i] = 1;
|
||||
_sum[i] = adc_tmp;
|
||||
}
|
||||
|
||||
|
||||
// Enable Timer2 Overflow interrupt to capture ADC data
|
||||
TIMSK2 = 0; // Disable interrupts
|
||||
@ -134,31 +150,60 @@ void AP_ADC_ADS7844::Init(void)
|
||||
}
|
||||
|
||||
// Read one channel value
|
||||
int AP_ADC_ADS7844::Ch(unsigned char ch_num)
|
||||
uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num)
|
||||
{
|
||||
uint16_t result = 0;
|
||||
uint16_t count;
|
||||
uint32_t sum;
|
||||
|
||||
//while(adc_counter[ch_num] < 2) { } // Wait for at least 2 samples in accumlator
|
||||
// ensure we have at least one value
|
||||
while (_count[ch_num] == 0) /* noop */ ;
|
||||
|
||||
// stop interrupts
|
||||
// grab the value with interrupts disabled, and clear the count
|
||||
cli();
|
||||
|
||||
// sum our filter
|
||||
for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){
|
||||
result += _filter[ch_num][i];
|
||||
count = _count[ch_num];
|
||||
sum = _sum[ch_num];
|
||||
_count[ch_num] = 0;
|
||||
_sum[ch_num] = 0;
|
||||
sei();
|
||||
|
||||
return sum/count;
|
||||
}
|
||||
|
||||
// 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
|
||||
uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
|
||||
{
|
||||
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 */;
|
||||
}
|
||||
|
||||
// resume interrupts
|
||||
// grab the values with interrupts disabled, and clear the counts
|
||||
cli();
|
||||
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;
|
||||
}
|
||||
sei();
|
||||
|
||||
// average our sampels
|
||||
result /= ADC_FILTER_SIZE;
|
||||
|
||||
return(result);
|
||||
}
|
||||
|
||||
// Read one channel value
|
||||
int AP_ADC_ADS7844::Ch_raw(unsigned char ch_num)
|
||||
{
|
||||
return _filter[ch_num][_filter_index]; // close enough
|
||||
// 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] / count[i];
|
||||
}
|
||||
|
||||
// this assumes the count in all channels is equal, which will
|
||||
// be true if the callers are using the interface consistently
|
||||
return count[0];
|
||||
}
|
||||
|
||||
@ -19,8 +19,12 @@ class AP_ADC_ADS7844 : public AP_ADC
|
||||
public:
|
||||
AP_ADC_ADS7844(); // Constructor
|
||||
void Init();
|
||||
int Ch(unsigned char ch_num);
|
||||
int Ch_raw(unsigned char ch_num);
|
||||
|
||||
// Read 1 sensor value
|
||||
uint16_t Ch(unsigned char ch_num);
|
||||
|
||||
// Read 6 sensors at once
|
||||
uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
@ -35,6 +35,8 @@ AP_ADC_HIL::AP_ADC_HIL()
|
||||
// set diff press and temp to zero
|
||||
setGyroTemp(0);
|
||||
setPressure(0);
|
||||
|
||||
last_hil_time = millis();
|
||||
}
|
||||
|
||||
void AP_ADC_HIL::Init(void)
|
||||
@ -42,14 +44,18 @@ void AP_ADC_HIL::Init(void)
|
||||
}
|
||||
|
||||
// Read one channel value
|
||||
int AP_ADC_HIL::Ch(unsigned char ch_num)
|
||||
uint16_t AP_ADC_HIL::Ch(unsigned char ch_num)
|
||||
{
|
||||
return adcValue[ch_num];
|
||||
return adcValue[ch_num];
|
||||
}
|
||||
// Read one channel value
|
||||
int AP_ADC_HIL::Ch_raw(unsigned char ch_num)
|
||||
|
||||
// Read 6 channel values
|
||||
uint16_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result)
|
||||
{
|
||||
return adcValue[ch_num];
|
||||
for (uint8_t i=0; i<6; i++) {
|
||||
result[i] = Ch(channel_numbers[i]);
|
||||
}
|
||||
return ((millis() - last_hil_time)*2)/5;
|
||||
}
|
||||
|
||||
// Set one channel value
|
||||
|
||||
@ -32,10 +32,11 @@ class AP_ADC_HIL : public AP_ADC
|
||||
|
||||
///
|
||||
// Read the sensor, part of public AP_ADC interface
|
||||
int Ch(unsigned char ch_num);
|
||||
uint16_t Ch(unsigned char ch_num);
|
||||
|
||||
///
|
||||
// Read the sensor, part of public AP_ADC interface
|
||||
int Ch_raw(unsigned char ch_num);
|
||||
// Read 6 sensors at once
|
||||
uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
||||
|
||||
///
|
||||
// Set the adc raw values given the current rotations rates,
|
||||
@ -49,16 +50,19 @@ class AP_ADC_HIL : public AP_ADC
|
||||
// The raw adc array
|
||||
uint16_t adcValue[8];
|
||||
|
||||
// the time in milliseconds when we last got a HIL update
|
||||
uint32_t last_hil_time;
|
||||
|
||||
///
|
||||
// sensor constants
|
||||
// constants declared in cpp file
|
||||
// @see AP_ADC_HIL.cpp
|
||||
static const uint8_t sensors[6];
|
||||
static const float gyroBias[3];
|
||||
static const float gyroScale[3];
|
||||
static const uint8_t sensors[6];
|
||||
static const float gyroBias[3];
|
||||
static const float gyroScale[3];
|
||||
static const float accelBias[3];
|
||||
static const float accelScale[3];
|
||||
static const int8_t sensorSign[6];
|
||||
static const float accelScale[3];
|
||||
static const int8_t sensorSign[6];
|
||||
|
||||
///
|
||||
// gyro set function
|
||||
|
||||
Loading…
Reference in New Issue
Block a user