AP_ADC: ported to AP_HAL

AP_ADC_test run on bench with APM1, looks ok.
This commit is contained in:
Pat Hickey 2012-10-09 15:39:43 -07:00 committed by Andrew Tridgell
parent 050a878935
commit 30deb76ea3
8 changed files with 107 additions and 124 deletions

View File

@ -3,7 +3,6 @@
#include <stdint.h>
#include <stdlib.h>
#include <../AP_PeriodicProcess/AP_PeriodicProcess.h>
/*
* AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
@ -25,7 +24,7 @@ class AP_ADC
public:
AP_ADC() {
}; // Constructor
virtual void Init(AP_PeriodicProcess * scheduler = NULL) = 0;
virtual void Init() = 0;
/* read one channel value */
virtual float Ch(uint8_t ch_num) = 0;

View File

@ -44,22 +44,20 @@
* Channel 7 : Differential pressure sensor port
*
*/
#include <AP_Progmem.h>
#include <AP_Common.h>
#include <AP_HAL.h>
#include "AP_ADC_ADS7844.h"
extern "C" {
// AVR LibC Includes
#include <inttypes.h>
#include <stdint.h>
#include <avr/interrupt.h>
}
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WConstants.h"
#endif
extern const AP_HAL::HAL& hal;
// DO NOT CHANGE FROM 8!!
#define ADC_ACCEL_FILTER_SIZE 8
// Commands for reading ADC channels on ADS7844
static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 };
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];
@ -67,41 +65,36 @@ static volatile uint32_t _sum[8];
// how many values we've accumulated since last read
static volatile uint16_t _count[8];
// variables to calculate time period over which a group of samples were collected
static volatile uint32_t _ch6_delta_time_start_micros = 0; // time we start collecting sample (reset on update)
static volatile uint32_t _ch6_last_sample_time_micros = 0; // time latest sample was collected
// TCNT2 values for various interrupt rates,
// assuming 256 prescaler. Note that these values
// assume a zero-time ISR. The actual rate will be a
// bit lower than this
#define TCNT2_781_HZ (256-80)
#define TCNT2_1008_HZ (256-62)
#define TCNT2_1302_HZ (256-48)
static inline unsigned char ADC_SPI_transfer(unsigned char data)
{
/* Put data into buffer, sends the data */
UDR2 = data;
/* Wait for data to be received */
while ( !(UCSR2A & (1 << RXC2)) ) ;
/* Get and return received data from buffer */
return UDR2;
}
// 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;
AP_HAL::SPIDeviceDriver* AP_ADC_ADS7844::_spi = NULL;
AP_HAL::Semaphore* AP_ADC_ADS7844::_spi_sem = NULL;
void AP_ADC_ADS7844::read(uint32_t tnow)
{
uint8_t ch;
bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
if (_spi_sem) {
bool got = _spi_sem->get((void*)&_spi_sem);
if (!got) return;
}
_spi->cs_assert();
// Command to read the first channel
_spi->transfer(adc_cmd[0]);
for (ch = 0; ch < 8; ch++) {
uint16_t v;
v = ADC_SPI_transfer(0) << 8; // Read first byte
v |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
// Read first byte
v = _spi->transfer(0) << 8;
// Read second byte and send next command
v |= _spi->transfer(adc_cmd[ch + 1]);
if (v & 0x8007) {
// this is a 12-bit ADC, shifted by 3 bits.
@ -122,50 +115,55 @@ void AP_ADC_ADS7844::read(uint32_t tnow)
_sum[ch] += (v >> 3);
}
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
_spi->cs_release();
if (_spi_sem) {
_spi_sem->release((void*)&_spi_sem);
}
// record time of this sample
_ch6_last_sample_time_micros = micros();
_ch6_last_sample_time_micros = hal.scheduler->micros();
}
// Constructors ////////////////////////////////////////////////////////////////
AP_ADC_ADS7844::AP_ADC_ADS7844()
{
}
AP_ADC_ADS7844::AP_ADC_ADS7844() { }
// Public Methods //////////////////////////////////////////////////////////////
void AP_ADC_ADS7844::Init( AP_PeriodicProcess * scheduler )
void AP_ADC_ADS7844::Init()
{
scheduler->suspend_timer();
pinMode(ADC_CHIP_SELECT, OUTPUT);
digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low)
// Setup Serial Port2 in SPI mode
UBRR2 = 0;
DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
// Set MSPI mode of operation and SPI data mode 0.
UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2);
// Enable receiver and transmitter.
UCSR2B = (1 << RXEN2) | (1 << TXEN2);
// Set Baud rate
UBRR2 = 2; // SPI clock running at 2.6MHz
hal.scheduler->suspend_timer_procs();
_spi = hal.spi->device(AP_HAL::SPIDevice_ADS7844);
if (_spi == NULL) {
hal.console->printf_P(PSTR("PANIC: AP_ADC_ADS7844 missing SPI device driver\n"));
}
_spi_sem = _spi->get_semaphore();
if (_spi_sem) {
while (!_spi_sem->get((void*)&_spi_sem));
}
_spi->cs_assert();
// 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]);
adc_tmp = _spi->transfer(0) << 8;
adc_tmp |= _spi->transfer(adc_cmd[i + 1]);
_count[i] = 1;
_sum[i] = adc_tmp;
}
_spi->cs_release();
_ch6_last_sample_time_micros = micros();
if (_spi_sem) {
_spi_sem->release((void*)&_spi_sem);
}
scheduler->resume_timer();
scheduler->register_process( AP_ADC_ADS7844::read );
_ch6_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->resume_timer_procs();
hal.scheduler->register_timer_process( AP_ADC_ADS7844::read, 1, 0);
}
@ -179,14 +177,12 @@ float AP_ADC_ADS7844::Ch(uint8_t ch_num)
while (_count[ch_num] == 0) /* noop */;
// grab the value with interrupts disabled, and clear the count
uint8_t oldSREG = SREG;
cli();
hal.scheduler->begin_atomic();
count = _count[ch_num];
sum = _sum[ch_num];
_count[ch_num] = 0;
_sum[ch_num] = 0;
SREG = oldSREG;
hal.scheduler->end_atomic();
return ((float)sum)/count;
}
@ -222,8 +218,7 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, float *result)
}
// grab the values with interrupts disabled, and clear the counts
uint8_t oldSREG = SREG;
cli();
hal.scheduler->begin_atomic();
for (i=0; i<6; i++) {
count[i] = _count[channel_numbers[i]];
sum[i] = _sum[channel_numbers[i]];
@ -236,7 +231,7 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, float *result)
uint32_t ret = _ch6_last_sample_time_micros - _ch6_delta_time_start_micros;
_ch6_delta_time_start_micros = _ch6_last_sample_time_micros;
SREG = oldSREG;
hal.scheduler->end_atomic();
// calculate averages. We keep this out of the cli region
// to prevent us stalling the ISR while doing the

View File

@ -1,28 +1,17 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef AP_ADC_ADS7844_H
#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))
// We use Serial Port 2 in SPI Mode
#define ADC_DATAOUT 51 // MOSI
#define ADC_DATAIN 50 // MISO
#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
#include "AP_ADC.h"
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
#include <inttypes.h>
#include "AP_ADC.h"
#include <AP_HAL.h>
class AP_ADC_ADS7844 : public AP_ADC
{
public:
AP_ADC_ADS7844(); // Constructor
void Init( AP_PeriodicProcess * scheduler );
void Init();
// Read 1 sensor value
float Ch(unsigned char ch_num);
@ -38,6 +27,8 @@ public:
private:
static void read(uint32_t);
static AP_HAL::SPIDeviceDriver *_spi;
static AP_HAL::Semaphore *_spi_sem;
};

View File

@ -1,9 +1,7 @@
#include "AP_ADC_HIL.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
/*
* AP_ADC_HIL.cpp
@ -42,12 +40,12 @@ AP_ADC_HIL::AP_ADC_HIL()
setGyroTemp(0);
setPressure(0);
last_hil_time = millis();
last_hil_time = hal.scheduler->millis();
}
void AP_ADC_HIL::Init( AP_PeriodicProcess * scheduler )
void AP_ADC_HIL::Init()
{
scheduler->register_process( AP_ADC_HIL::read );
hal.scheduler->register_timer_process( AP_ADC_HIL::read );
}
// Read one channel value
@ -64,7 +62,7 @@ uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, float *result)
for (uint8_t i=0; i<6; i++) {
result[i] = Ch(channel_numbers[i]);
}
return ((millis() - last_hil_time)*2)/5;
return ((hal.scheduler->millis() - last_hil_time)*2)/5;
}
// Set one channel value
@ -98,4 +96,4 @@ bool AP_ADC_HIL::new_data_available(const uint8_t *channel_numbers)
uint16_t AP_ADC_HIL::num_samples_available(const uint8_t *channel_numbers)
{
return _count;
}
}

View File

@ -28,7 +28,7 @@ public:
///
// Initializes sensor, part of public AP_ADC interface
void Init(AP_PeriodicProcess*);
void Init();
///
// Read the sensor, part of public AP_ADC interface

View File

@ -5,32 +5,30 @@
*/
#include <math.h>
#include <FastSerial.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_ADC.h> // ArduPilot Mega ADC Library
FastSerialPort0(Serial); // FTDI/console
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess adc_scheduler;
#include <AP_ADC.h>
const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
unsigned long timer;
AP_ADC_ADS7844 adc;
uint32_t timer;
void setup()
{
Serial.begin(115200, 128, 128);
Serial.println("ArduPilot Mega ADC library test");
delay(1000);
hal.uart0->begin(115200, 128, 128);
hal.console->println("ArduPilot Mega ADC library test");
hal.scheduler->delay(1000);
isr_registry.init();
adc_scheduler.init(&isr_registry);
adc.Init(); // APM ADC initialization
adc.Init(&adc_scheduler); // APM ADC initialization
delay(1000);
timer = millis();
hal.scheduler->delay(1000);
timer = hal.scheduler->millis();
}
static const uint8_t channel_map[6] = { 1, 2, 0, 4, 5, 6};
@ -40,11 +38,11 @@ uint32_t last_usec = 0;
static void show_timing()
{
uint32_t mint = (uint32_t)-1, maxt = 0, totalt=0;
uint32_t start_time = millis();
uint32_t start_time = hal.scheduler->millis();
float result[6];
uint32_t count = 0;
Serial.println("Starting timing test");
hal.console->println("Starting timing test");
adc.Ch6(channel_map, result);
@ -54,9 +52,9 @@ static void show_timing()
if (deltat < mint) mint = deltat;
totalt += deltat;
count++;
} while ((millis() - start_time) < 5000);
} while ((hal.scheduler->millis() - start_time) < 5000);
Serial.printf("timing: mint=%lu maxt=%lu avg=%lu\n", mint, maxt, totalt/count);
hal.console->printf("timing: mint=%lu maxt=%lu avg=%lu\n", mint, maxt, totalt/count);
}
static void show_data()
@ -81,13 +79,13 @@ static void show_data()
if (result[i] < min[i]) min[i] = result[i];
if (result[i] > max[i]) max[i] = result[i];
if (fabs(result[i]) > 0x8000) {
Serial.printf("result[%u]=%f\n", (unsigned)i, result[i]);
hal.console->printf("result[%u]=%f\n", (unsigned)i, result[i]);
}
}
} while ((millis() - timer) < 200);
} while ((hal.scheduler->millis() - timer) < 200);
timer = millis();
Serial.printf("g=(%f,%f,%f) a=(%f,%f,%f) +/-(%u,%u,%u,%u,%u,%u) gt=%u dt=%u\n",
timer = hal.scheduler->millis();
hal.console->printf("g=(%f,%f,%f) a=(%f,%f,%f) +/-(%u,%u,%u,%u,%u,%u) gt=%u dt=%u\n",
result[0], result[1], result[2],
result[3], result[4], result[5],
(max[0]-min[0]), (max[1]-min[1]), (max[2]-min[2]),
@ -98,9 +96,11 @@ static void show_data()
void loop()
{
if (millis() < 5000) {
if (hal.scheduler->millis() < 5000) {
show_timing();
} else {
show_data();
}
}
AP_HAL_MAIN();