diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index 7a09ebf557..04b2862c89 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -3,7 +3,6 @@ #include #include -#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; diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 693f86eb1b..4c53abf19c 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -44,22 +44,20 @@ * Channel 7 : Differential pressure sensor port * */ + +#include +#include +#include + #include "AP_ADC_ADS7844.h" -extern "C" { -// AVR LibC Includes -#include -#include -#include -} -#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 diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index 71452161f4..e2fc7b0a8b 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -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< +#include "AP_ADC.h" +#include 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; }; diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index d3b04b5bc5..9a1cad507f 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -1,9 +1,7 @@ + #include "AP_ADC_HIL.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif +#include +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; -} \ No newline at end of file +} diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index 7cefcf90ae..a6285c6030 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -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 diff --git a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde index ad3e11ea19..f4445b32bf 100644 --- a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde +++ b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde @@ -5,32 +5,30 @@ */ #include -#include -#include -#include -#include // ArduPilot Mega ADC Library -FastSerialPort0(Serial); // FTDI/console +#include +#include +#include +#include +#include -Arduino_Mega_ISR_Registry isr_registry; -AP_TimerProcess adc_scheduler; +#include +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(); diff --git a/libraries/AP_ADC/examples/AP_ADC_test/Arduino.h b/libraries/AP_ADC/examples/AP_ADC_test/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_ADC/examples/AP_ADC_test/nocore.inoflag b/libraries/AP_ADC/examples/AP_ADC_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2