diff --git a/libraries/AP_ADC/AP_ADC.cpp b/libraries/AP_ADC/AP_ADC.cpp index d673d4504a..dff8c08a40 100644 --- a/libraries/AP_ADC/AP_ADC.cpp +++ b/libraries/AP_ADC/AP_ADC.cpp @@ -1,11 +1,11 @@ -/* - ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega - Code by James Goppert. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. -*/ - -#include "AP_ADC.h" +/* + ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega + Code by James Goppert. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. +*/ + +#include "AP_ADC.h" diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index 5d749c9ae9..b2838befa4 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -1,28 +1,28 @@ -#ifndef AP_ADC_H -#define AP_ADC_H - +#ifndef AP_ADC_H +#define AP_ADC_H + #include -/* - AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega - Code by James Goppert. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Methods: - Init() : Initialization of ADC. (interrupts etc) - Ch(ch_num) : Return the ADC channel value +/* + AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega + Code by James Goppert. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + 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 -{ - public: - AP_ADC() {}; // Constructor - virtual void Init() {}; +*/ + +class AP_ADC +{ + public: + AP_ADC() {}; // Constructor + virtual void Init() {}; /* read one channel value */ virtual uint16_t Ch(uint8_t ch_num) = 0; @@ -41,7 +41,7 @@ class AP_ADC private: }; -#include "AP_ADC_ADS7844.h" -#include "AP_ADC_HIL.h" - -#endif +#include "AP_ADC_ADS7844.h" +#include "AP_ADC_HIL.h" + +#endif diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index be90bba00a..b3ef6cbb76 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -1,61 +1,61 @@ -/* - AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega - Code by Jordi Mu�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 - only an issue on initial read of ADC at program start. - 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 - 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 - RXD2 = MISO = pin PH0 - XCK2 = SCK = pin PH2 - Chip Select pin is PC4 (33) [PH6 (9)] - We are using the 16 clocks per conversion timming to increase efficiency (fast) +/* + AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega + Code by Jordi Mu�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 + only an issue on initial read of ADC at program start. + 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 + 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 + RXD2 = MISO = pin PH0 + XCK2 = SCK = pin PH2 + 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) - 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 10x oversampling and averaging. - - Methods: - 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 - // referenced sensor inputs, not device referenced. - On Ardupilot Mega Hardware, oriented as described above: - Chennel 0 : yaw rate, r - Channel 1 : roll rate, p - Channel 2 : pitch rate, q - Channel 3 : x / y gyro temperature - Channel 4 : x acceleration, aX - Channel 5 : y acceleration, aY - Channel 6 : z acceleration, aZ - Channel 7 : Differential pressure sensor port - -*/ -extern "C" { - // AVR LibC Includes - #include + + Methods: + 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 + // referenced sensor inputs, not device referenced. + On Ardupilot Mega Hardware, oriented as described above: + Chennel 0 : yaw rate, r + Channel 1 : roll rate, p + Channel 2 : pitch rate, q + Channel 3 : x / y gyro temperature + Channel 4 : x acceleration, aX + Channel 5 : y acceleration, aY + Channel 6 : z acceleration, aZ + Channel 7 : Differential pressure sensor port + +*/ +extern "C" { + // AVR LibC Includes + #include #include - #include - #include "WConstants.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 }; + #include + #include "WConstants.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 }; // the sum of the values since last read static volatile uint32_t _sum[8]; @@ -78,12 +78,12 @@ 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; -} - - + while ( !(UCSR2A & (1 << RXC2)) ); + /* Get and return received data from buffer */ + return UDR2; +} + + ISR (TIMER2_OVF_vect) { uint8_t ch; @@ -127,13 +127,13 @@ ISR (TIMER2_OVF_vect) } -// Constructors //////////////////////////////////////////////////////////////// -AP_ADC_ADS7844::AP_ADC_ADS7844() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void AP_ADC_ADS7844::Init(void) +// Constructors //////////////////////////////////////////////////////////////// +AP_ADC_ADS7844::AP_ADC_ADS7844() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void AP_ADC_ADS7844::Init(void) { pinMode(ADC_CHIP_SELECT, OUTPUT); @@ -172,15 +172,15 @@ void AP_ADC_ADS7844::Init(void) // Read one channel value uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num) -{ +{ uint16_t count; uint32_t sum; - + // ensure we have at least one value while (_count[ch_num] == 0) /* noop */ ; - + // grab the value with interrupts disabled, and clear the count - cli(); + cli(); count = _count[ch_num]; sum = _sum[ch_num]; _count[ch_num] = 0; @@ -204,8 +204,8 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) // ensure we have at least one value for (i=0; i<6; i++) { while (_count[channel_numbers[i]] == 0) /* noop */; - } - + } + // grab the values with interrupts disabled, and clear the counts cli(); for (i=0; i<6; i++) { @@ -214,8 +214,8 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) _count[channel_numbers[i]] = 0; _sum[channel_numbers[i]] = 0; } - sei(); - + sei(); + // 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 diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index 35f72d36db..2b1314ea1c 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -1,24 +1,24 @@ -#ifndef AP_ADC_ADS7844_H -#define AP_ADC_ADS7844_H - -#define bit_set(p,m) ((p) |= ( 1< - -class AP_ADC_ADS7844 : public AP_ADC -{ - public: - AP_ADC_ADS7844(); // Constructor - void Init(); +#ifndef AP_ADC_ADS7844_H +#define AP_ADC_ADS7844_H + +#define bit_set(p,m) ((p) |= ( 1< + +class AP_ADC_ADS7844 : public AP_ADC +{ + public: + AP_ADC_ADS7844(); // Constructor + void Init(); // Read 1 sensor value uint16_t Ch(unsigned char ch_num); @@ -28,5 +28,5 @@ class AP_ADC_ADS7844 : public AP_ADC private: }; - -#endif + +#endif diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index 108bb1120f..e5e698ca0c 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -1,51 +1,51 @@ -#include "AP_ADC_HIL.h" -#include "WProgram.h" - -/* - AP_ADC_HIL.cpp - Author: James Goppert - - License: - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. -*/ - -const uint8_t AP_ADC_HIL::sensors[6] = {1,2,0,4,5,6}; -const int8_t AP_ADC_HIL::sensorSign[6] = { 1, -1, -1,-1, 1, 1}; -const float AP_ADC_HIL::gyroBias[3] = {1665,1665,1665}; -const float AP_ADC_HIL::accelBias[3] = {2025,2025,2025}; -// gyroScale = 1/[GyroGain*pi/180] GyroGains (0.4,0.41,0.41) -const float AP_ADC_HIL::gyroScale[3] = {143.239, 139.746, 139.746}; -const float AP_ADC_HIL::accelScale[3] = {418,418,418}; // adcPerG - -AP_ADC_HIL::AP_ADC_HIL() -{ - // gyros set to zero for calibration - setGyro(0,0); - setGyro(1,0); - setGyro(2,0); - - // accels set to zero for calibration - setAccel(0,0); - setAccel(1,0); - setAccel(2,0); - - // set diff press and temp to zero - setGyroTemp(0); - setPressure(0); +#include "AP_ADC_HIL.h" +#include "WProgram.h" + +/* + AP_ADC_HIL.cpp + Author: James Goppert + + License: + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. +*/ + +const uint8_t AP_ADC_HIL::sensors[6] = {1,2,0,4,5,6}; +const int8_t AP_ADC_HIL::sensorSign[6] = { 1, -1, -1,-1, 1, 1}; +const float AP_ADC_HIL::gyroBias[3] = {1665,1665,1665}; +const float AP_ADC_HIL::accelBias[3] = {2025,2025,2025}; +// gyroScale = 1/[GyroGain*pi/180] GyroGains (0.4,0.41,0.41) +const float AP_ADC_HIL::gyroScale[3] = {143.239, 139.746, 139.746}; +const float AP_ADC_HIL::accelScale[3] = {418,418,418}; // adcPerG + +AP_ADC_HIL::AP_ADC_HIL() +{ + // gyros set to zero for calibration + setGyro(0,0); + setGyro(1,0); + setGyro(2,0); + + // accels set to zero for calibration + setAccel(0,0); + setAccel(1,0); + setAccel(2,0); + + // set diff press and temp to zero + setGyroTemp(0); + setPressure(0); last_hil_time = millis(); -} - -void AP_ADC_HIL::Init(void) -{ -} - -// Read one channel value +} + +void AP_ADC_HIL::Init(void) +{ +} + +// Read one channel value uint16_t AP_ADC_HIL::Ch(unsigned char ch_num) -{ +{ return adcValue[ch_num]; } @@ -56,25 +56,25 @@ uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) result[i] = Ch(channel_numbers[i]); } return ((millis() - last_hil_time)*2)/5; -} - -// Set one channel value -void AP_ADC_HIL::setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, - int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress) -{ - // gyros - setGyro(0,p); - setGyro(1,q); - setGyro(2,r); - - // temp - setGyroTemp(gyroTemp); - - // accel - setAccel(0,aX); - setAccel(1,aY); - setAccel(2,aZ); - - // differential pressure - setPressure(diffPress); -} +} + +// Set one channel value +void AP_ADC_HIL::setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, + int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress) +{ + // gyros + setGyro(0,p); + setGyro(1,q); + setGyro(2,r); + + // temp + setGyroTemp(gyroTemp); + + // accel + setAccel(0,aX); + setAccel(1,aY); + setAccel(2,aZ); + + // differential pressure + setPressure(diffPress); +} diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index 1794da8eaf..83908f0538 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -1,37 +1,37 @@ -#ifndef AP_ADC_HIL_H -#define AP_ADC_HIL_H - -/* - AP_ADC_HIL.h - Author: James Goppert - - License: - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. -*/ - -#include -#include "AP_ADC.h" - -/// -// A hardware in the loop model of the ADS7844 analog to digital converter -// @author James Goppert DIYDrones.com -class AP_ADC_HIL : public AP_ADC -{ - public: - - /// - // Constructor - AP_ADC_HIL(); // Constructor - - /// - // Initializes sensor, part of public AP_ADC interface - void Init(); - - /// - // Read the sensor, part of public AP_ADC interface +#ifndef AP_ADC_HIL_H +#define AP_ADC_HIL_H + +/* + AP_ADC_HIL.h + Author: James Goppert + + License: + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. +*/ + +#include +#include "AP_ADC.h" + +/// +// A hardware in the loop model of the ADS7844 analog to digital converter +// @author James Goppert DIYDrones.com +class AP_ADC_HIL : public AP_ADC +{ + public: + + /// + // Constructor + AP_ADC_HIL(); // Constructor + + /// + // Initializes sensor, part of public AP_ADC interface + void Init(); + + /// + // Read the sensor, part of public AP_ADC interface uint16_t Ch(unsigned char ch_num); /// @@ -40,57 +40,57 @@ class AP_ADC_HIL : public AP_ADC /// // Set the adc raw values given the current rotations rates, - // temps, accels, and pressures - void setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, - int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress); - - private: - - /// - // The raw adc array - uint16_t adcValue[8]; - + // temps, accels, and pressures + void setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, + int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress); + + private: + + /// + // 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 + /// + // 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 float accelBias[3]; + static const float accelBias[3]; static const float accelScale[3]; static const int8_t sensorSign[6]; - - /// - // gyro set function - // @param val the value of the gyro in milli rad/s - // @param index the axis for the gyro(0-x,1-y,2-z) - inline void setGyro(uint8_t index, int16_t val) { - int16_t temp = val * gyroScale[index] / 1000 + gyroBias[index]; - adcValue[sensors[index]] = (sensorSign[index] < 0) ? -temp : temp; - } - - /// - // accel set function - // @param val the value of the accel in milli g's - // @param index the axis for the accelerometer(0-x,1-y,2-z) - inline void setAccel(uint8_t index, int16_t val) { - int16_t temp = val * accelScale[index] / 1000 + accelBias[index]; - adcValue[sensors[index+3]] = (sensors[index+3] < 0) ? -temp : temp; - } - - /// - // Sets the differential pressure adc channel - // TODO: implement - void setPressure(int16_t val) {} - - /// - // Sets the gyro temp adc channel - // TODO: implement - void setGyroTemp(int16_t val) {} -}; - -#endif + + /// + // gyro set function + // @param val the value of the gyro in milli rad/s + // @param index the axis for the gyro(0-x,1-y,2-z) + inline void setGyro(uint8_t index, int16_t val) { + int16_t temp = val * gyroScale[index] / 1000 + gyroBias[index]; + adcValue[sensors[index]] = (sensorSign[index] < 0) ? -temp : temp; + } + + /// + // accel set function + // @param val the value of the accel in milli g's + // @param index the axis for the accelerometer(0-x,1-y,2-z) + inline void setAccel(uint8_t index, int16_t val) { + int16_t temp = val * accelScale[index] / 1000 + accelBias[index]; + adcValue[sensors[index+3]] = (sensors[index+3] < 0) ? -temp : temp; + } + + /// + // Sets the differential pressure adc channel + // TODO: implement + void setPressure(int16_t val) {} + + /// + // Sets the gyro temp adc channel + // TODO: implement + void setGyroTemp(int16_t val) {} +}; + +#endif