aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added ADC gyro Filtering for quads - this fixes a noise issue introduced into the controller

Browse Source 
added Position mode
removed
Added back in the accelerometer experiment
Added filter_result boolean to enable filter on the fly
This commit is contained in:
Jason Short 2011-09-29 23:27:23 -07:00
parent 7b08185d83
commit df1a39f650
10 changed files with 340 additions and 313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/APM_Config.h
View File

@ -37,8 +37,13 @@
CH7_SIMPLE_MODE CH7_SIMPLE_MODE
CH7_RTL CH7_RTL
CH7_AUTO_TRIM CH7_AUTO_TRIM
CH7_ADC_FILTER (experimental)
*/ */
#define ACCEL_ALT_HOLD 0
#define ACCEL_ALT_HOLD_GAIN 12.0
// ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode
// See the config.h and defines.h files for how to set this up! // See the config.h and defines.h files for how to set this up!
// //

27
ArduCopter/ArduCopter.pde
View File

@ -244,7 +244,8 @@ static const char* flight_mode_strings[] = {
"GUIDED", "GUIDED",
"LOITER", "LOITER",
"RTL", "RTL",
"CIRCLE"}; "CIRCLE",
"POSITION"};
/* Radio values /* Radio values
Channel assignments Channel assignments
@ -259,8 +260,8 @@ static const char* flight_mode_strings[] = {
*/ */
// test // test
//Vector3f accels_rot; Vector3f accels_rot;
//float accel_gain = 20; //float accel_gain = 12;
// temp // temp
int y_actual_speed; int y_actual_speed;
@ -495,7 +496,6 @@ static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS
static byte counter_one_herz; static byte counter_one_herz;
static bool GPS_enabled = false; static bool GPS_enabled = false;
static byte loop_step;
static bool new_radio_frame; static bool new_radio_frame;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -597,8 +597,6 @@ static void medium_loop()
// This case deals with the GPS and Compass // This case deals with the GPS and Compass
//----------------------------------------- //-----------------------------------------
case 0: case 0:
loop_step = 1;
medium_loopCounter++; medium_loopCounter++;
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
@ -638,12 +636,10 @@ static void medium_loop()
// This case performs some navigation computations // This case performs some navigation computations
//------------------------------------------------ //------------------------------------------------
case 1: case 1:
loop_step = 2;
medium_loopCounter++; medium_loopCounter++;
// Auto control modes: // Auto control modes:
if(g_gps->new_data && g_gps->fix){ if(g_gps->new_data && g_gps->fix){
loop_step = 11;
// invalidate GPS data // invalidate GPS data
g_gps->new_data = false; g_gps->new_data = false;
@ -674,7 +670,6 @@ static void medium_loop()
// command processing // command processing
//------------------- //-------------------
case 2: case 2:
loop_step = 3;
medium_loopCounter++; medium_loopCounter++;
// Read altitude from sensors // Read altitude from sensors
@ -690,7 +685,6 @@ static void medium_loop()
// This case deals with sending high rate telemetry // This case deals with sending high rate telemetry
//------------------------------------------------- //-------------------------------------------------
case 3: case 3:
loop_step = 4;
medium_loopCounter++; medium_loopCounter++;
// perform next command // perform next command
@ -729,7 +723,6 @@ static void medium_loop()
// This case controls the slow loop // This case controls the slow loop
//--------------------------------- //---------------------------------
case 4: case 4:
loop_step = 5;
medium_loopCounter = 0; medium_loopCounter = 0;
if (g.battery_monitoring != 0){ if (g.battery_monitoring != 0){
@ -823,7 +816,6 @@ static void slow_loop()
//---------------------------------------- //----------------------------------------
switch (slow_loopCounter){ switch (slow_loopCounter){
case 0: case 0:
loop_step = 6;
slow_loopCounter++; slow_loopCounter++;
superslow_loopCounter++; superslow_loopCounter++;
@ -838,7 +830,6 @@ static void slow_loop()
break; break;
case 1: case 1:
loop_step = 7;
slow_loopCounter++; slow_loopCounter++;
// Read 3-position switch on radio // Read 3-position switch on radio
@ -863,7 +854,6 @@ static void slow_loop()
break; break;
case 2: case 2:
loop_step = 8;
slow_loopCounter = 0; slow_loopCounter = 0;
update_events(); update_events();
@ -902,7 +892,6 @@ static void slow_loop()
// 1Hz loop // 1Hz loop
static void super_slow_loop() static void super_slow_loop()
{ {
loop_step = 9;
if (g.log_bitmask & MASK_LOG_CUR) if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current(); Log_Write_Current();
@ -915,7 +904,6 @@ static void super_slow_loop()
static void update_GPS(void) static void update_GPS(void)
{ {
loop_step = 10;
g_gps->update(); g_gps->update();
update_GPS_light(); update_GPS_light();
@ -1098,7 +1086,7 @@ void update_throttle_mode(void)
} }
// apply throttle control at 200 hz // apply throttle control at 200 hz
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(); g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost() + alt_hold_velocity();
break; break;
} }
} }
@ -1147,6 +1135,7 @@ static void update_navigation()
// switch passthrough to LOITER // switch passthrough to LOITER
case LOITER: case LOITER:
case POSITION:
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
@ -1212,8 +1201,8 @@ static void update_trig(void){
// 270 = cos_yaw: -1.00, sin_yaw: 0.00, // 270 = cos_yaw: -1.00, sin_yaw: 0.00,
//Vector3f accel_filt = imu.get_accel_filtered(); Vector3f accel_filt = imu.get_accel_filtered();
//accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
} }
// updated at 10hz // updated at 10hz

18
ArduCopter/Attitude.pde
View File

@ -189,16 +189,20 @@ get_nav_yaw_offset(int yaw_input, int reset)
} }
} }
/* ///*
static int alt_hold_velocity() static int alt_hold_velocity()
{ {
// subtract filtered Accel #if ACCEL_ALT_HOLD == 1
float error = abs(next_WP.alt - current_loc.alt); // subtract filtered Accel
error = min(error, 200); float error = abs(next_WP.alt - current_loc.alt);
error = 1 - (error/ 200.0); error = min(error, 200.0);
return (accels_rot.z + 9.81) * accel_gain * error; error = 1 - (error/ 200.0);
return (accels_rot.z + 9.81) * ACCEL_ALT_HOLD_GAIN * error;
#else
return 0;
#endif
} }
*/ //*/
static int get_angle_boost() static int get_angle_boost()
{ {

2
ArduCopter/config.h
View File

@ -480,7 +480,7 @@
# define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch
#endif #endif
#ifndef NAV_I #ifndef NAV_I
# define NAV_I 0.10 // this # define NAV_I 0.10 // this feels really low, 4s to move 1 degree pitch...
#endif #endif
#ifndef NAV_IMAX #ifndef NAV_IMAX
# define NAV_IMAX 16 // degrees # define NAV_IMAX 16 // degrees

6
ArduCopter/control_modes.pde
View File

@ -90,6 +90,12 @@ static void read_trim_switch()
trim_flag = false; trim_flag = false;
} }
} }
#elif CH7_OPTION == CH7_ADC_FILTER
if (g.rc_7.control_in > 800){
adc.filter_result = true;
}else{
adc.filter_result = false;
}
#elif CH7_OPTION == CH7_AUTO_TRIM #elif CH7_OPTION == CH7_AUTO_TRIM
if (g.rc_7.control_in > 800){ if (g.rc_7.control_in > 800){
auto_level_counter = 10; auto_level_counter = 10;

4
ArduCopter/defines.h
View File

@ -33,6 +33,7 @@
#define CH7_SIMPLE_MODE 3 #define CH7_SIMPLE_MODE 3
#define CH7_RTL 4 #define CH7_RTL 4
#define CH7_AUTO_TRIM 5 #define CH7_AUTO_TRIM 5
#define CH7_ADC_FILTER 6
// Frame types // Frame types
#define QUAD_FRAME 0 #define QUAD_FRAME 0
@ -123,7 +124,8 @@
#define LOITER 5 // Hold a single location #define LOITER 5 // Hold a single location
#define RTL 6 // AUTO control #define RTL 6 // AUTO control
#define CIRCLE 7 // AUTO control #define CIRCLE 7 // AUTO control
#define NUM_MODES 8 #define POSITION 8 // AUTO control
#define NUM_MODES 9
#define SIMPLE_1 1 #define SIMPLE_1 1
#define SIMPLE_2 2 #define SIMPLE_2 2

25
ArduCopter/navigation.pde
View File

@ -83,11 +83,13 @@ static void calc_loiter(int x_error, int y_error)
y_rate_error = y_target_speed - y_actual_speed; // 413 y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum
nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav);
nav_lat = constrain(nav_lat, -3500, 3500);
x_rate_error = x_target_speed - x_actual_speed; x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -250, 250); x_rate_error = constrain(x_rate_error, -250, 250);
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav);
nav_lon = constrain(nav_lon, -3500, 3500);
} }
// nav_roll, nav_pitch // nav_roll, nav_pitch
@ -156,29 +158,11 @@ static void calc_nav_pitch_roll()
nav_pitch);*/ nav_pitch);*/
} }
static long get_altitude_error() static long get_altitude_error()
{ {
return next_WP.alt - current_loc.alt; return next_WP.alt - current_loc.alt;
} }
/*
static void calc_altitude_smoothing_error()
{
// limit climb rates - we draw a straight line between first location and edge of waypoint_radius
target_altitude = next_WP.alt - ((float)(wp_distance * (next_WP.alt - prev_WP.alt)) / (float)(wp_totalDistance - g.waypoint_radius));
// stay within a certain range
if(prev_WP.alt > next_WP.alt){
target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
}else{
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
}
altitude_error = target_altitude - current_loc.alt;
}
*/
static int get_loiter_angle() static int get_loiter_angle()
{ {
float power; float power;
@ -197,7 +181,6 @@ static int get_loiter_angle()
return angle; return angle;
} }
static long wrap_360(long error) static long wrap_360(long error)
{ {
if (error > 36000) error -= 36000; if (error > 36000) error -= 36000;

8
ArduCopter/system.pde
View File

@ -414,6 +414,14 @@ static void set_mode(byte mode)
next_WP = current_loc; next_WP = current_loc;
break; break;
case POSITION:
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_MANUAL;
next_WP = current_loc;
break;
case GUIDED: case GUIDED:
yaw_mode = YAW_AUTO; yaw_mode = YAW_AUTO;
roll_pitch_mode = ROLL_PITCH_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO;

490
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -1,232 +1,258 @@
/* /*
AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega
Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
Modified by John Ihlein 6 / 19 / 2010 to: Modified by John Ihlein 6 / 19 / 2010 to:
1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably 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. only an issue on initial read of ADC at program start.
2)Reorder analog read order as follows: 2)Reorder analog read order as follows:
p, q, r, ax, ay, az p, q, r, ax, ay, az
This library is free software; you can redistribute it and / or This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. 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) External ADC ADS7844 is connected via Serial port 2 (in SPI mode)
TXD2 = MOSI = pin PH1 TXD2 = MOSI = pin PH1
RXD2 = MISO = pin PH0 RXD2 = MISO = pin PH0
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 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 So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
we have an 10x 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)
Ch(ch_num) : Return the ADC channel value Ch(ch_num) : Return the ADC channel value
// HJI - Input definitions. USB connector assumed to be on the left, Rx and servo // 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 // connector pins to the rear. IMU shield components facing up. These are board
// referenced sensor inputs, not device referenced. // referenced sensor inputs, not device referenced.
On Ardupilot Mega Hardware, oriented as described above: On Ardupilot Mega Hardware, oriented as described above:
Chennel 0 : yaw rate, r Chennel 0 : yaw rate, r
Channel 1 : roll rate, p Channel 1 : roll rate, p
Channel 2 : pitch rate, q Channel 2 : pitch rate, q
Channel 3 : x / y gyro temperature Channel 3 : x / y gyro temperature
Channel 4 : x acceleration, aX Channel 4 : x acceleration, aX
Channel 5 : y acceleration, aY Channel 5 : y acceleration, aY
Channel 6 : z acceleration, aZ Channel 6 : z acceleration, aZ
Channel 7 : Differential pressure sensor port Channel 7 : Differential pressure sensor port
*/ */
extern "C" { extern "C" {
// AVR LibC Includes // AVR LibC Includes
#include <inttypes.h> #include <inttypes.h>
#include <stdint.h> #include <stdint.h>
#include <avr/interrupt.h> #include <avr/interrupt.h>
#include "WConstants.h" #include "WConstants.h"
} }
#include "AP_ADC_ADS7844.h" #include "AP_ADC_ADS7844.h"
// Commands for reading ADC channels on ADS7844
// 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
// the sum of the values since last read static volatile uint32_t _sum[8];
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 uint32_t last_ch6_micros;
static uint32_t last_ch6_micros;
// TCNT2 values for various interrupt rates,
// TCNT2 values for various interrupt rates, // assuming 256 prescaler. Note that these values
// assuming 256 prescaler. Note that these values // assume a zero-time ISR. The actual rate will be a
// assume a zero-time ISR. The actual rate will be a // bit lower than this
// bit lower than this #define TCNT2_781_HZ (256-80)
#define TCNT2_781_HZ (256-80) #define TCNT2_1008_HZ (256-62)
#define TCNT2_1008_HZ (256-62) #define TCNT2_1302_HZ (256-48)
#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 */
/* Put data into buffer, sends the data */ UDR2 = data;
UDR2 = data; /* Wait for data to be received */
/* Wait for data to be received */ while ( !(UCSR2A & (1 << RXC2)) );
while ( !(UCSR2A & (1 << RXC2)) ); /* Get and return received data from buffer */
/* Get and return received data from buffer */ return UDR2;
return UDR2; }
}
ISR (TIMER2_OVF_vect)
ISR (TIMER2_OVF_vect) {
{ uint8_t ch;
uint8_t ch; static uint8_t timer_offset;
static uint8_t timer_offset;
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)
// this gives us a sample rate between 781Hz and 1302Hz. We
// this gives us a sample rate between 781Hz and 1302Hz. We // randomise it to try to minimise aliasing effects
// randomise it to try to minimise aliasing effects timer_offset = (timer_offset + 49) % 32;
timer_offset = (timer_offset + 49) % 32; TCNT2 = TCNT2_781_HZ + timer_offset;
TCNT2 = TCNT2_781_HZ + timer_offset; }
}
// Constructors ////////////////////////////////////////////////////////////////
// Constructors //////////////////////////////////////////////////////////////// AP_ADC_ADS7844::AP_ADC_ADS7844() :
AP_ADC_ADS7844::AP_ADC_ADS7844() _filter_index(0),
{ filter_result(false)
} {
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_ADC_ADS7844::Init(void) // Public Methods //////////////////////////////////////////////////////////////
{ void AP_ADC_ADS7844::Init(void)
pinMode(ADC_CHIP_SELECT, OUTPUT); {
pinMode(ADC_CHIP_SELECT, OUTPUT);
digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low)
digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low)
// Setup Serial Port2 in SPI mode
UBRR2 = 0; // Setup Serial Port2 in SPI mode
DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode UBRR2 = 0;
// Set MSPI mode of operation and SPI data mode 0. DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2); // Set MSPI mode of operation and SPI data mode 0.
// Enable receiver and transmitter. UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2);
UCSR2B = (1 << RXEN2) | (1 << TXEN2); // Enable receiver and transmitter.
// Set Baud rate UCSR2B = (1 << RXEN2) | (1 << TXEN2);
UBRR2 = 2; // SPI clock running at 2.6MHz // Set Baud rate
UBRR2 = 2; // SPI clock running at 2.6MHz
// get an initial value for each channel. This ensures
// _count[] is never zero // get an initial value for each channel. This ensures
for (uint8_t i=0; i<8; i++) { // _count[] is never zero
uint16_t adc_tmp; for (uint8_t i=0; i<8; i++) {
adc_tmp = ADC_SPI_transfer(0) << 8; uint16_t adc_tmp;
adc_tmp |= ADC_SPI_transfer(adc_cmd[i + 1]); adc_tmp = ADC_SPI_transfer(0) << 8;
_count[i] = 1; adc_tmp |= ADC_SPI_transfer(adc_cmd[i + 1]);
_sum[i] = adc_tmp; _count[i] = 1;
} _sum[i] = adc_tmp;
}
last_ch6_micros = micros();
last_ch6_micros = micros();
// Enable Timer2 Overflow interrupt to capture ADC data _filter_index = 0;
TIMSK2 = 0; // Disable interrupts
TCCR2A = 0; // normal counting mode // Enable Timer2 Overflow interrupt to capture ADC data
TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256 TIMSK2 = 0; // Disable interrupts
TCNT2 = 0; TCCR2A = 0; // normal counting mode
TIFR2 = _BV(TOV2); // clear pending interrupts; TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256
TIMSK2 = _BV(TOIE2); // enable the overflow interrupt TCNT2 = 0;
} TIFR2 = _BV(TOV2); // clear pending interrupts;
TIMSK2 = _BV(TOIE2); // enable the overflow interrupt
// Read one channel value }
uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num)
{ // Read one channel value
uint16_t count; uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num)
uint32_t sum; {
uint16_t count;
// ensure we have at least one value uint32_t sum;
while (_count[ch_num] == 0) /* noop */ ;
// ensure we have at least one value
// grab the value with interrupts disabled, and clear the count while (_count[ch_num] == 0) /* noop */ ;
cli();
count = _count[ch_num]; // grab the value with interrupts disabled, and clear the count
sum = _sum[ch_num]; cli();
_count[ch_num] = 0; count = _count[ch_num];
_sum[ch_num] = 0; sum = _sum[ch_num];
sei(); _count[ch_num] = 0;
_sum[ch_num] = 0;
return sum/count; sei();
}
return sum/count;
// Read 6 channel values }
// this assumes that the counts for all of the 6 channels are
// equal. This will only be true if we always consistently access a // Read 6 channel values
// sensor by either Ch6() or Ch() and never mix them. If you mix them // this assumes that the counts for all of the 6 channels are
// then you will get very strange results // equal. This will only be true if we always consistently access a
uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) // sensor by either Ch6() or Ch() and never mix them. If you mix them
{ // then you will get very strange results
uint16_t count[6]; uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
uint32_t sum[6]; {
uint8_t i; uint16_t count[6];
uint32_t sum[6];
// ensure we have at least one value uint8_t i;
for (i=0; i<6; i++) {
while (_count[channel_numbers[i]] == 0) /* noop */; // 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++) { // grab the values with interrupts disabled, and clear the counts
count[i] = _count[channel_numbers[i]]; cli();
sum[i] = _sum[channel_numbers[i]]; for (i=0; i<6; i++) {
_count[channel_numbers[i]] = 0; count[i] = _count[channel_numbers[i]];
_sum[channel_numbers[i]] = 0; sum[i] = _sum[channel_numbers[i]];
} _count[channel_numbers[i]] = 0;
sei(); _sum[channel_numbers[i]] = 0;
}
// calculate averages. We keep this out of the cli region sei();
// to prevent us stalling the ISR while doing the
// division. That costs us 36 bytes of stack, but I think its // calculate averages. We keep this out of the cli region
// worth it. // to prevent us stalling the ISR while doing the
for (i=0; i<6; i++) { // division. That costs us 36 bytes of stack, but I think its
result[i] = sum[i] / count[i]; // worth it.
} for (i = 0; i < 6; i++) {
result[i] = sum[i] / count[i];
// return number of microseconds since last call }
uint32_t us = micros();
uint32_t ret = us - last_ch6_micros; // filter ch 0,1,2 for smoother Gyro output.
last_ch6_micros = us;
return ret; if(filter_result){
} uint32_t _filter_sum;
for (i = 0; i < 3; i++) {
// move most recent result into filter
_filter[i][_filter_index] = result[i];
_filter_sum = 0;
// sum the filter
for (uint8_t n = 0; n < 8; n++) {
_filter_sum += _filter[i][n];
}
result[i] = _filter_sum / 8;
}
// increment filter index
_filter_index++;
// loop our filter
if(_filter_index == 8)
_filter_index = 0;
}
// return number of microseconds since last call
uint32_t us = micros();
uint32_t ret = us - last_ch6_micros;
last_ch6_micros = us;
return ret;
}

68
libraries/AP_ADC/AP_ADC_ADS7844.h
View File

@ -1,32 +1,36 @@
#ifndef AP_ADC_ADS7844_H #ifndef AP_ADC_ADS7844_H
#define AP_ADC_ADS7844_H #define AP_ADC_ADS7844_H
#define bit_set(p,m) ((p) |= ( 1<<m)) #define bit_set(p,m) ((p) |= ( 1<<m))
#define bit_clear(p,m) ((p) &= ~(1<<m)) #define bit_clear(p,m) ((p) &= ~(1<<m))
// We use Serial Port 2 in SPI Mode // We use Serial Port 2 in SPI Mode
#define ADC_DATAOUT 51 // MOSI #define ADC_DATAOUT 51 // MOSI
#define ADC_DATAIN 50 // MISO #define ADC_DATAIN 50 // MISO
#define ADC_SPICLOCK 52 // SCK #define ADC_SPICLOCK 52 // SCK
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40 #define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
#define ADC_FILTER_SIZE 3 #define ADC_FILTER_SIZE 8
#include "AP_ADC.h" #include "AP_ADC.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(); void Init();
// Read 1 sensor value // Read 1 sensor value
uint16_t Ch(unsigned char ch_num); uint16_t 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;
private:
}; private:
uint16_t _filter[3][ADC_FILTER_SIZE];
#endif uint8_t _filter_index;
};
#endif