mirror of https://github.com/ArduPilot/ardupilot
fixed formatting of ADC driver
it was a nasty mix of DOS and UNIX
This commit is contained in:
Andrew Tridgell
parent
5b4717ea55
commit
9e9b229a1d
|
|
@ -1,133 +1,134 @@
|
||||||
/*
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega
|
/*
|
||||||
Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
|
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
|
Modified by John Ihlein 6 / 19 / 2010 to:
|
||||||
only an issue on initial read of ADC at program start.
|
1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably
|
||||||
2)Reorder analog read order as follows:
|
only an issue on initial read of ADC at program start.
|
||||||
p, q, r, ax, ay, az
|
2)Reorder analog read order as follows:
|
||||||
|
p, q, r, ax, ay, az
|
||||||
This library is free software; you can redistribute it and / or
|
|
||||||
modify it under the terms of the GNU Lesser General Public
|
This library is free software; you can redistribute it and / or
|
||||||
License as published by the Free Software Foundation; either
|
modify it under the terms of the GNU Lesser General Public
|
||||||
version 2.1 of the License, or (at your option) any later version.
|
License as published by the Free Software Foundation; either
|
||||||
|
version 2.1 of the License, or (at your option) any later version.
|
||||||
External ADC ADS7844 is connected via Serial port 2 (in SPI mode)
|
|
||||||
TXD2 = MOSI = pin PH1
|
External ADC ADS7844 is connected via Serial port 2 (in SPI mode)
|
||||||
RXD2 = MISO = pin PH0
|
TXD2 = MOSI = pin PH1
|
||||||
XCK2 = SCK = pin PH2
|
RXD2 = MISO = pin PH0
|
||||||
Chip Select pin is PC4 (33) [PH6 (9)]
|
XCK2 = SCK = pin PH2
|
||||||
We are using the 16 clocks per conversion timming to increase efficiency (fast)
|
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)
|
|
||||||
|
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.
|
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)
|
Methods:
|
||||||
Ch(ch_num) : Return the ADC channel value
|
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
|
// HJI - Input definitions. USB connector assumed to be on the left, Rx and servo
|
||||||
// referenced sensor inputs, not device referenced.
|
// connector pins to the rear. IMU shield components facing up. These are board
|
||||||
On Ardupilot Mega Hardware, oriented as described above:
|
// referenced sensor inputs, not device referenced.
|
||||||
Chennel 0 : yaw rate, r
|
On Ardupilot Mega Hardware, oriented as described above:
|
||||||
Channel 1 : roll rate, p
|
Chennel 0 : yaw rate, r
|
||||||
Channel 2 : pitch rate, q
|
Channel 1 : roll rate, p
|
||||||
Channel 3 : x / y gyro temperature
|
Channel 2 : pitch rate, q
|
||||||
Channel 4 : x acceleration, aX
|
Channel 3 : x / y gyro temperature
|
||||||
Channel 5 : y acceleration, aY
|
Channel 4 : x acceleration, aX
|
||||||
Channel 6 : z acceleration, aZ
|
Channel 5 : y acceleration, aY
|
||||||
Channel 7 : Differential pressure sensor port
|
Channel 6 : z acceleration, aZ
|
||||||
|
Channel 7 : Differential pressure sensor port
|
||||||
*/
|
|
||||||
extern "C" {
|
*/
|
||||||
// AVR LibC Includes
|
extern "C" {
|
||||||
#include <inttypes.h>
|
// AVR LibC Includes
|
||||||
#include <stdint.h>
|
#include <inttypes.h>
|
||||||
#include <avr/interrupt.h>
|
#include <stdint.h>
|
||||||
#include "WConstants.h"
|
#include <avr/interrupt.h>
|
||||||
}
|
#include "WConstants.h"
|
||||||
|
}
|
||||||
#include "AP_ADC_ADS7844.h"
|
|
||||||
|
#include "AP_ADC_ADS7844.h"
|
||||||
// Commands for reading ADC channels on ADS7844
|
|
||||||
static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 };
|
// Commands for reading ADC channels on ADS7844
|
||||||
|
static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 };
|
||||||
// the sum of the values since last read
|
|
||||||
static volatile uint32_t _sum[8];
|
// 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];
|
// how many values we've accumulated since last read
|
||||||
|
static volatile uint16_t _count[8];
|
||||||
static uint32_t last_ch6_micros;
|
|
||||||
|
static uint32_t last_ch6_micros;
|
||||||
// TCNT2 values for various interrupt rates,
|
|
||||||
// assuming 256 prescaler. Note that these values
|
// TCNT2 values for various interrupt rates,
|
||||||
// assume a zero-time ISR. The actual rate will be a
|
// assuming 256 prescaler. Note that these values
|
||||||
// bit lower than this
|
// assume a zero-time ISR. The actual rate will be a
|
||||||
#define TCNT2_781_HZ (256-80)
|
// bit lower than this
|
||||||
#define TCNT2_1008_HZ (256-62)
|
#define TCNT2_781_HZ (256-80)
|
||||||
#define TCNT2_1302_HZ (256-48)
|
#define TCNT2_1008_HZ (256-62)
|
||||||
|
#define TCNT2_1302_HZ (256-48)
|
||||||
static inline unsigned char ADC_SPI_transfer(unsigned char data)
|
|
||||||
{
|
static inline unsigned char ADC_SPI_transfer(unsigned char data)
|
||||||
/* Put data into buffer, sends the data */
|
{
|
||||||
UDR2 = data;
|
/* Put data into buffer, sends the data */
|
||||||
/* Wait for data to be received */
|
UDR2 = data;
|
||||||
while ( !(UCSR2A & (1 << RXC2)) );
|
/* Wait for data to be received */
|
||||||
/* Get and return received data from buffer */
|
while ( !(UCSR2A & (1 << RXC2)) );
|
||||||
return UDR2;
|
/* Get and return received data from buffer */
|
||||||
}
|
return UDR2;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void AP_ADC_ADS7844::read(void)
|
void AP_ADC_ADS7844::read(void)
|
||||||
{
|
{
|
||||||
uint8_t ch;
|
uint8_t ch;
|
||||||
|
|
||||||
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 v;
|
uint16_t v;
|
||||||
|
|
||||||
v = ADC_SPI_transfer(0) << 8; // Read first byte
|
v = ADC_SPI_transfer(0) << 8; // Read first byte
|
||||||
v |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
|
v |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
|
||||||
|
|
||||||
if (v & 0x8007) {
|
if (v & 0x8007) {
|
||||||
// this is a 12-bit ADC, shifted by 3 bits.
|
// this is a 12-bit ADC, shifted by 3 bits.
|
||||||
// if we get other bits set then the value is
|
// if we get other bits set then the value is
|
||||||
// bogus and should be ignored
|
// bogus and should be ignored
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (++_count[ch] == 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[ch] = 0;
|
||||||
_count[ch] = 1;
|
_count[ch] = 1;
|
||||||
last_ch6_micros = micros();
|
last_ch6_micros = micros();
|
||||||
}
|
}
|
||||||
_sum[ch] += (v >> 3);
|
_sum[ch] += (v >> 3);
|
||||||
}
|
}
|
||||||
|
|
||||||
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
|
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Constructors ////////////////////////////////////////////////////////////////
|
// Constructors ////////////////////////////////////////////////////////////////
|
||||||
AP_ADC_ADS7844::AP_ADC_ADS7844() :
|
AP_ADC_ADS7844::AP_ADC_ADS7844() :
|
||||||
_filter_index_accel(0),
|
_filter_index_accel(0),
|
||||||
filter_result(false)
|
filter_result(false)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
void AP_ADC_ADS7844::Init( AP_PeriodicProcess * scheduler )
|
void AP_ADC_ADS7844::Init( AP_PeriodicProcess * scheduler )
|
||||||
{
|
{
|
||||||
|
|
@ -160,105 +161,105 @@ void AP_ADC_ADS7844::Init( AP_PeriodicProcess * scheduler )
|
||||||
scheduler->register_process( AP_ADC_ADS7844::read );
|
scheduler->register_process( AP_ADC_ADS7844::read );
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read one channel value
|
// Read one channel value
|
||||||
float AP_ADC_ADS7844::Ch(uint8_t ch_num)
|
float AP_ADC_ADS7844::Ch(uint8_t ch_num)
|
||||||
{
|
{
|
||||||
uint16_t count;
|
uint16_t count;
|
||||||
uint32_t sum;
|
uint32_t sum;
|
||||||
|
|
||||||
// ensure we have at least one value
|
// ensure we have at least one value
|
||||||
while (_count[ch_num] == 0) /* noop */ ;
|
while (_count[ch_num] == 0) /* noop */ ;
|
||||||
|
|
||||||
// grab the value with interrupts disabled, and clear the count
|
// grab the value with interrupts disabled, and clear the count
|
||||||
cli();
|
cli();
|
||||||
count = _count[ch_num];
|
count = _count[ch_num];
|
||||||
sum = _sum[ch_num];
|
sum = _sum[ch_num];
|
||||||
_count[ch_num] = 0;
|
_count[ch_num] = 0;
|
||||||
_sum[ch_num] = 0;
|
_sum[ch_num] = 0;
|
||||||
sei();
|
sei();
|
||||||
|
|
||||||
return ((float)sum)/count;
|
return ((float)sum)/count;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read 6 channel values
|
// Read 6 channel values
|
||||||
// this assumes that the counts for all of the 6 channels are
|
// this assumes that the counts for all of the 6 channels are
|
||||||
// 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
|
||||||
uint32_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];
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
|
||||||
// ensure we have at least one value
|
// ensure we have at least one value
|
||||||
for (i=0; i<6; i++) {
|
for (i=0; i<6; i++) {
|
||||||
while (_count[channel_numbers[i]] == 0) /* noop */;
|
while (_count[channel_numbers[i]] == 0) /* noop */;
|
||||||
}
|
}
|
||||||
|
|
||||||
// grab the values with interrupts disabled, and clear the counts
|
// grab the values with interrupts disabled, and clear the counts
|
||||||
cli();
|
cli();
|
||||||
for (i=0; i<6; i++) {
|
for (i=0; i<6; i++) {
|
||||||
count[i] = _count[channel_numbers[i]];
|
count[i] = _count[channel_numbers[i]];
|
||||||
sum[i] = _sum[channel_numbers[i]];
|
sum[i] = _sum[channel_numbers[i]];
|
||||||
_count[channel_numbers[i]] = 0;
|
_count[channel_numbers[i]] = 0;
|
||||||
_sum[channel_numbers[i]] = 0;
|
_sum[channel_numbers[i]] = 0;
|
||||||
}
|
}
|
||||||
sei();
|
sei();
|
||||||
|
|
||||||
// calculate averages. We keep this out of the cli region
|
// calculate averages. We keep this out of the cli region
|
||||||
// to prevent us stalling the ISR while doing the
|
// to prevent us stalling the ISR while doing the
|
||||||
// division. That costs us 36 bytes of stack, but I think its
|
// division. That costs us 36 bytes of stack, but I think its
|
||||||
// worth it.
|
// worth it.
|
||||||
for (i = 0; i < 6; i++) {
|
for (i = 0; i < 6; i++) {
|
||||||
result[i] = (sum[i] + (count[i]/2)) / count[i];
|
result[i] = (sum[i] + (count[i]/2)) / count[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
if(filter_result){
|
if(filter_result){
|
||||||
uint32_t _sum_accel;
|
uint32_t _sum_accel;
|
||||||
|
|
||||||
// simple Gyro Filter
|
// simple Gyro Filter
|
||||||
for (i = 0; i < 3; i++) {
|
for (i = 0; i < 3; i++) {
|
||||||
// add prev filtered value to new raw value, divide by 2
|
// add prev filtered value to new raw value, divide by 2
|
||||||
result[i] = (_prev_gyro[i] + result[i]) >> 1;
|
result[i] = (_prev_gyro[i] + result[i]) >> 1;
|
||||||
|
|
||||||
// remember the filtered value
|
// remember the filtered value
|
||||||
_prev_gyro[i] = result[i];
|
_prev_gyro[i] = result[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Accel filter
|
// Accel filter
|
||||||
for (i = 0; i < 3; i++) {
|
for (i = 0; i < 3; i++) {
|
||||||
// move most recent result into filter
|
// move most recent result into filter
|
||||||
_filter_accel[i][_filter_index_accel] = result[i+3];
|
_filter_accel[i][_filter_index_accel] = result[i+3];
|
||||||
|
|
||||||
// clear the sum
|
// clear the sum
|
||||||
_sum_accel = 0;
|
_sum_accel = 0;
|
||||||
|
|
||||||
// sum the filter
|
// sum the filter
|
||||||
for (uint8_t n = 0; n < ADC_ACCEL_FILTER_SIZE; n++) {
|
for (uint8_t n = 0; n < ADC_ACCEL_FILTER_SIZE; n++) {
|
||||||
_sum_accel += _filter_accel[i][n];
|
_sum_accel += _filter_accel[i][n];
|
||||||
}
|
}
|
||||||
|
|
||||||
// filter does a moving average on last 8 reads, sums half with half of last filtered value
|
// filter does a moving average on last 8 reads, sums half with half of last filtered value
|
||||||
// save old result
|
// save old result
|
||||||
_prev_accel[i] = result[i+3] = (_sum_accel >> 4) + (_prev_accel[i] >> 1); // divide by 16, divide by 2
|
_prev_accel[i] = result[i+3] = (_sum_accel >> 4) + (_prev_accel[i] >> 1); // divide by 16, divide by 2
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// increment filter index
|
// increment filter index
|
||||||
_filter_index_accel++;
|
_filter_index_accel++;
|
||||||
|
|
||||||
// loop our filter
|
// loop our filter
|
||||||
if(_filter_index_accel == ADC_ACCEL_FILTER_SIZE)
|
if(_filter_index_accel == ADC_ACCEL_FILTER_SIZE)
|
||||||
_filter_index_accel = 0;
|
_filter_index_accel = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// return number of microseconds since last call
|
// return number of microseconds since last call
|
||||||
uint32_t us = micros();
|
uint32_t us = micros();
|
||||||
uint32_t ret = us - last_ch6_micros;
|
uint32_t ret = us - last_ch6_micros;
|
||||||
last_ch6_micros = us;
|
last_ch6_micros = us;
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,42 +1,43 @@
|
||||||
#ifndef AP_ADC_ADS7844_H
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
#define AP_ADC_ADS7844_H
|
#ifndef AP_ADC_ADS7844_H
|
||||||
|
#define AP_ADC_ADS7844_H
|
||||||
#define bit_set(p,m) ((p) |= ( 1<<m))
|
|
||||||
#define bit_clear(p,m) ((p) &= ~(1<<m))
|
#define bit_set(p,m) ((p) |= ( 1<<m))
|
||||||
|
#define bit_clear(p,m) ((p) &= ~(1<<m))
|
||||||
// We use Serial Port 2 in SPI Mode
|
|
||||||
#define ADC_DATAOUT 51 // MOSI
|
// We use Serial Port 2 in SPI Mode
|
||||||
#define ADC_DATAIN 50 // MISO
|
#define ADC_DATAOUT 51 // MOSI
|
||||||
#define ADC_SPICLOCK 52 // SCK
|
#define ADC_DATAIN 50 // MISO
|
||||||
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
|
#define ADC_SPICLOCK 52 // SCK
|
||||||
|
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
|
||||||
// DO NOT CHANGE FROM 8!!
|
|
||||||
#define ADC_ACCEL_FILTER_SIZE 8
|
// DO NOT CHANGE FROM 8!!
|
||||||
|
#define ADC_ACCEL_FILTER_SIZE 8
|
||||||
#include "AP_ADC.h"
|
|
||||||
|
#include "AP_ADC.h"
|
||||||
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
|
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
|
|
||||||
class AP_ADC_ADS7844 : public AP_ADC
|
class AP_ADC_ADS7844 : public AP_ADC
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
AP_ADC_ADS7844(); // Constructor
|
AP_ADC_ADS7844(); // Constructor
|
||||||
void Init( AP_PeriodicProcess * scheduler );
|
void Init( AP_PeriodicProcess * scheduler );
|
||||||
|
|
||||||
// Read 1 sensor value
|
// Read 1 sensor value
|
||||||
float Ch(unsigned char ch_num);
|
float Ch(unsigned char ch_num);
|
||||||
|
|
||||||
// Read 6 sensors at once
|
// Read 6 sensors at once
|
||||||
uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
||||||
bool filter_result;
|
bool filter_result;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint16_t _filter_accel[3][ADC_ACCEL_FILTER_SIZE];
|
uint16_t _filter_accel[3][ADC_ACCEL_FILTER_SIZE];
|
||||||
uint16_t _prev_gyro[3];
|
uint16_t _prev_gyro[3];
|
||||||
uint16_t _prev_accel[3];
|
uint16_t _prev_accel[3];
|
||||||
uint8_t _filter_index_accel;
|
uint8_t _filter_index_accel;
|
||||||
static void read( void );
|
static void read( void );
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue