mirror of https://github.com/ArduPilot/ardupilot
change ADC sample rate to 1kHz
this costs us about 9% of our CPU, but should make aliasing much less of a problem.
This commit is contained in:
parent
55bcb8a1c0
commit
1c5e8f0381
|
@ -33,10 +33,11 @@ class AP_ADC
|
||||||
Pass in an array of 6 channel numbers and results are
|
Pass in an array of 6 channel numbers and results are
|
||||||
returned in result[]
|
returned in result[]
|
||||||
|
|
||||||
The function returns the amount of time that the returned
|
The function returns the amount of time (in microseconds)
|
||||||
value has been averaged over, in 2.5 millisecond units
|
since the last call to Ch6().
|
||||||
*/
|
*/
|
||||||
virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0;
|
virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -19,9 +19,11 @@
|
||||||
XCK2 = SCK = pin PH2
|
XCK2 = SCK = pin PH2
|
||||||
Chip Select pin is PC4 (33) [PH6 (9)]
|
Chip Select pin is PC4 (33) [PH6 (9)]
|
||||||
We are using the 16 clocks per conversion timming to increase efficiency (fast)
|
We are using the 16 clocks per conversion timming to increase efficiency (fast)
|
||||||
The sampling frequency is 400Hz (Timer2 overflow interrupt)
|
|
||||||
|
The sampling frequency is 1kHz (Timer2 overflow interrupt)
|
||||||
|
|
||||||
So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
|
So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
|
||||||
we have an 4x oversampling and averaging.
|
we have an 10x oversampling and averaging.
|
||||||
|
|
||||||
Methods:
|
Methods:
|
||||||
Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
|
Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
|
||||||
|
@ -61,10 +63,10 @@ static volatile uint32_t _sum[8];
|
||||||
// how many values we've accumulated since last read
|
// how many values we've accumulated since last read
|
||||||
static volatile uint16_t _count[8];
|
static volatile uint16_t _count[8];
|
||||||
|
|
||||||
static unsigned char ADC_SPI_transfer(unsigned char data)
|
static uint32_t last_ch6_micros;
|
||||||
|
|
||||||
|
static inline unsigned char ADC_SPI_transfer(unsigned char data)
|
||||||
{
|
{
|
||||||
/* Wait for empty transmit buffer */
|
|
||||||
while ( !( UCSR2A & (1 << UDRE2)) );
|
|
||||||
/* Put data into buffer, sends the data */
|
/* Put data into buffer, sends the data */
|
||||||
UDR2 = data;
|
UDR2 = data;
|
||||||
/* Wait for data to be received */
|
/* Wait for data to be received */
|
||||||
|
@ -78,32 +80,33 @@ ISR (TIMER2_OVF_vect)
|
||||||
{
|
{
|
||||||
uint8_t ch;
|
uint8_t ch;
|
||||||
|
|
||||||
//bit_set(PORTL,6); // To test performance
|
|
||||||
|
|
||||||
bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
|
bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
|
||||||
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
|
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
|
||||||
|
|
||||||
for (ch = 0; ch < 8; ch++){
|
for (ch = 0; ch < 8; ch++) {
|
||||||
uint16_t adc_tmp;
|
uint16_t count = _count[ch];
|
||||||
adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte
|
uint32_t sum = _sum[ch];
|
||||||
adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
|
|
||||||
|
|
||||||
_count[ch]++;
|
if (++count == 0) {
|
||||||
if (_count[ch] == 0) {
|
|
||||||
// overflow ... shouldn't happen too often
|
// overflow ... shouldn't happen too often
|
||||||
// unless we're just not using the
|
// unless we're just not using the
|
||||||
// channel. Notice that we overflow the count
|
// channel. Notice that we overflow the count
|
||||||
// to 1 here, not zero, as otherwise the
|
// to 1 here, not zero, as otherwise the
|
||||||
// reader below could get a division by zero
|
// reader below could get a division by zero
|
||||||
_sum[ch] = 0;
|
sum = 0;
|
||||||
_count[ch] = 1;
|
count = 1;
|
||||||
|
last_ch6_micros = micros();
|
||||||
}
|
}
|
||||||
_sum[ch] += adc_tmp;
|
_count[ch] = count;
|
||||||
|
|
||||||
|
sum += ADC_SPI_transfer(0) << 8; // Read first byte
|
||||||
|
sum += ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
|
||||||
|
|
||||||
|
_sum[ch] = sum;
|
||||||
}
|
}
|
||||||
|
|
||||||
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
|
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
|
||||||
//bit_clear(PORTL,6); // To test performance
|
TCNT2 = 200;
|
||||||
TCNT2 = 104; // 400Hz interrupt
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -139,6 +142,7 @@ void AP_ADC_ADS7844::Init(void)
|
||||||
_sum[i] = adc_tmp;
|
_sum[i] = adc_tmp;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
last_ch6_micros = micros();
|
||||||
|
|
||||||
// Enable Timer2 Overflow interrupt to capture ADC data
|
// Enable Timer2 Overflow interrupt to capture ADC data
|
||||||
TIMSK2 = 0; // Disable interrupts
|
TIMSK2 = 0; // Disable interrupts
|
||||||
|
@ -174,7 +178,7 @@ uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num)
|
||||||
// equal. This will only be true if we always consistently access a
|
// 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
|
// sensor by either Ch6() or Ch() and never mix them. If you mix them
|
||||||
// then you will get very strange results
|
// then you will get very strange results
|
||||||
uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
|
uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
|
||||||
{
|
{
|
||||||
uint16_t count[6];
|
uint16_t count[6];
|
||||||
uint32_t sum[6];
|
uint32_t sum[6];
|
||||||
|
@ -203,7 +207,9 @@ uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
|
||||||
result[i] = sum[i] / count[i];
|
result[i] = sum[i] / count[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
// this assumes the count in all channels is equal, which will
|
// return number of microseconds since last call
|
||||||
// be true if the callers are using the interface consistently
|
uint32_t us = micros();
|
||||||
return count[0];
|
uint32_t ret = us - last_ch6_micros;
|
||||||
|
last_ch6_micros = us;
|
||||||
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
|
@ -24,7 +24,7 @@ class AP_ADC_ADS7844 : public AP_ADC
|
||||||
uint16_t Ch(unsigned char ch_num);
|
uint16_t Ch(unsigned char ch_num);
|
||||||
|
|
||||||
// Read 6 sensors at once
|
// Read 6 sensors at once
|
||||||
uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
};
|
};
|
||||||
|
|
|
@ -50,7 +50,7 @@ uint16_t AP_ADC_HIL::Ch(unsigned char ch_num)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read 6 channel values
|
// Read 6 channel values
|
||||||
uint16_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result)
|
uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result)
|
||||||
{
|
{
|
||||||
for (uint8_t i=0; i<6; i++) {
|
for (uint8_t i=0; i<6; i++) {
|
||||||
result[i] = Ch(channel_numbers[i]);
|
result[i] = Ch(channel_numbers[i]);
|
||||||
|
|
|
@ -36,7 +36,7 @@ class AP_ADC_HIL : public AP_ADC
|
||||||
|
|
||||||
///
|
///
|
||||||
// Read 6 sensors at once
|
// Read 6 sensors at once
|
||||||
uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
||||||
|
|
||||||
///
|
///
|
||||||
// Set the adc raw values given the current rotations rates,
|
// Set the adc raw values given the current rotations rates,
|
||||||
|
|
|
@ -279,7 +279,7 @@ AP_IMU_Oilpan::update(void)
|
||||||
int tc_temp = _adc->Ch(_gyro_temp_ch);
|
int tc_temp = _adc->Ch(_gyro_temp_ch);
|
||||||
uint16_t adc_values[6];
|
uint16_t adc_values[6];
|
||||||
|
|
||||||
_ticks = _adc->Ch6(_sensors, adc_values);
|
_sample_time = _adc->Ch6(_sensors, adc_values);
|
||||||
|
|
||||||
// convert corrected gyro readings to delta acceleration
|
// convert corrected gyro readings to delta acceleration
|
||||||
//
|
//
|
||||||
|
|
|
@ -81,7 +81,7 @@ public:
|
||||||
///
|
///
|
||||||
/// @returns number of seconds
|
/// @returns number of seconds
|
||||||
///
|
///
|
||||||
float get_delta_time(void) { return _ticks * (2.5/1000.0); }
|
float get_delta_time(void) { return _sample_time * 1.0e-6; }
|
||||||
|
|
||||||
/// A count of bad sensor readings
|
/// A count of bad sensor readings
|
||||||
///
|
///
|
||||||
|
@ -98,9 +98,9 @@ protected:
|
||||||
/// Most recent gyro reading obtained by ::update
|
/// Most recent gyro reading obtained by ::update
|
||||||
Vector3f _gyro;
|
Vector3f _gyro;
|
||||||
|
|
||||||
/// number of 2.5ms ticks that the accel and gyro values
|
/// number of microseconds that the accel and gyro values
|
||||||
/// were calculated from
|
/// were sampled over
|
||||||
uint16_t _ticks;
|
uint32_t _sample_time;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue