diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index e95fdddeb1..eae295514f 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -37,8 +37,13 @@ CH7_SIMPLE_MODE CH7_RTL 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! // diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0e9316927c..ba70d3791a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -244,7 +244,8 @@ static const char* flight_mode_strings[] = { "GUIDED", "LOITER", "RTL", - "CIRCLE"}; + "CIRCLE", + "POSITION"}; /* Radio values Channel assignments @@ -259,8 +260,8 @@ static const char* flight_mode_strings[] = { */ // test -//Vector3f accels_rot; -//float accel_gain = 20; +Vector3f accels_rot; +//float accel_gain = 12; // temp 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 bool GPS_enabled = false; -static byte loop_step; static bool new_radio_frame; //////////////////////////////////////////////////////////////////////////////// @@ -597,8 +597,6 @@ static void medium_loop() // This case deals with the GPS and Compass //----------------------------------------- case 0: - loop_step = 1; - medium_loopCounter++; #ifdef OPTFLOW_ENABLED @@ -638,12 +636,10 @@ static void medium_loop() // This case performs some navigation computations //------------------------------------------------ case 1: - loop_step = 2; medium_loopCounter++; // Auto control modes: if(g_gps->new_data && g_gps->fix){ - loop_step = 11; // invalidate GPS data g_gps->new_data = false; @@ -674,7 +670,6 @@ static void medium_loop() // command processing //------------------- case 2: - loop_step = 3; medium_loopCounter++; // Read altitude from sensors @@ -690,7 +685,6 @@ static void medium_loop() // This case deals with sending high rate telemetry //------------------------------------------------- case 3: - loop_step = 4; medium_loopCounter++; // perform next command @@ -729,7 +723,6 @@ static void medium_loop() // This case controls the slow loop //--------------------------------- case 4: - loop_step = 5; medium_loopCounter = 0; if (g.battery_monitoring != 0){ @@ -823,7 +816,6 @@ static void slow_loop() //---------------------------------------- switch (slow_loopCounter){ case 0: - loop_step = 6; slow_loopCounter++; superslow_loopCounter++; @@ -838,7 +830,6 @@ static void slow_loop() break; case 1: - loop_step = 7; slow_loopCounter++; // Read 3-position switch on radio @@ -863,7 +854,6 @@ static void slow_loop() break; case 2: - loop_step = 8; slow_loopCounter = 0; update_events(); @@ -902,7 +892,6 @@ static void slow_loop() // 1Hz loop static void super_slow_loop() { - loop_step = 9; if (g.log_bitmask & MASK_LOG_CUR) Log_Write_Current(); @@ -915,7 +904,6 @@ static void super_slow_loop() static void update_GPS(void) { - loop_step = 10; g_gps->update(); update_GPS_light(); @@ -1098,7 +1086,7 @@ void update_throttle_mode(void) } // 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; } } @@ -1147,6 +1135,7 @@ static void update_navigation() // switch passthrough to LOITER case LOITER: + case POSITION: wp_control = LOITER_MODE; // calculates the desired Roll and Pitch @@ -1212,8 +1201,8 @@ static void update_trig(void){ // 270 = cos_yaw: -1.00, sin_yaw: 0.00, - //Vector3f accel_filt = imu.get_accel_filtered(); - //accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); + Vector3f accel_filt = imu.get_accel_filtered(); + accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); } // updated at 10hz diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 02b3ab7205..9942a2dda8 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -189,16 +189,20 @@ get_nav_yaw_offset(int yaw_input, int reset) } } -/* +///* static int alt_hold_velocity() { - // subtract filtered Accel - float error = abs(next_WP.alt - current_loc.alt); - error = min(error, 200); - error = 1 - (error/ 200.0); - return (accels_rot.z + 9.81) * accel_gain * error; + #if ACCEL_ALT_HOLD == 1 + // subtract filtered Accel + float error = abs(next_WP.alt - current_loc.alt); + error = min(error, 200.0); + 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() { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3039ca197a..6671526176 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -480,7 +480,7 @@ # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch #endif #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 #ifndef NAV_IMAX # define NAV_IMAX 16 // degrees diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index af83f046a0..394fc404d5 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -90,6 +90,12 @@ static void read_trim_switch() 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 if (g.rc_7.control_in > 800){ auto_level_counter = 10; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 5be7d03247..9408dea354 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -33,6 +33,7 @@ #define CH7_SIMPLE_MODE 3 #define CH7_RTL 4 #define CH7_AUTO_TRIM 5 +#define CH7_ADC_FILTER 6 // Frame types #define QUAD_FRAME 0 @@ -123,7 +124,8 @@ #define LOITER 5 // Hold a single location #define RTL 6 // 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_2 2 diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 5ba03676fe..30a6f9c70f 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -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 = 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 = 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 @@ -156,29 +158,11 @@ static void calc_nav_pitch_roll() nav_pitch);*/ } - static long get_altitude_error() { 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() { float power; @@ -197,7 +181,6 @@ static int get_loiter_angle() return angle; } - static long wrap_360(long error) { if (error > 36000) error -= 36000; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 8812079962..094e1edf98 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -414,6 +414,14 @@ static void set_mode(byte mode) next_WP = current_loc; break; + case POSITION: + yaw_mode = YAW_HOLD; + roll_pitch_mode = ROLL_PITCH_AUTO; + throttle_mode = THROTTLE_MANUAL; + + next_WP = current_loc; + break; + case GUIDED: yaw_mode = YAW_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO; diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index b3ef6cbb76..f0fc048886 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -1,232 +1,258 @@ -/* - 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 - 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 - #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 }; - -// 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]; - -static uint32_t last_ch6_micros; - -// 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; -} - - -ISR (TIMER2_OVF_vect) -{ - uint8_t ch; - static uint8_t timer_offset; - - bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) - ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel - - 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 - - if (v & 0x8007) { - // this is a 12-bit ADC, shifted by 3 bits. - // if we get other bits set then the value is - // bogus and should be ignored - continue; - } - - if (++_count[ch] == 0) { - // overflow ... shouldn't happen too often - // unless we're just not using the - // channel. Notice that we overflow the count - // to 1 here, not zero, as otherwise the - // reader below could get a division by zero - _sum[ch] = 0; - _count[ch] = 1; - last_ch6_micros = micros(); - } - _sum[ch] += (v >> 3); - } - - bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) - - // this gives us a sample rate between 781Hz and 1302Hz. We - // randomise it to try to minimise aliasing effects - timer_offset = (timer_offset + 49) % 32; - TCNT2 = TCNT2_781_HZ + timer_offset; -} - - -// Constructors //////////////////////////////////////////////////////////////// -AP_ADC_ADS7844::AP_ADC_ADS7844() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void AP_ADC_ADS7844::Init(void) -{ - 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 - - // 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]); - _count[i] = 1; - _sum[i] = adc_tmp; - } - - last_ch6_micros = micros(); - - // Enable Timer2 Overflow interrupt to capture ADC data - TIMSK2 = 0; // Disable interrupts - TCCR2A = 0; // normal counting mode - TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256 - 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) -{ - 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(); - count = _count[ch_num]; - sum = _sum[ch_num]; - _count[ch_num] = 0; - _sum[ch_num] = 0; - 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 -// sensor by either Ch6() or Ch() and never mix them. If you mix them -// then you will get very strange results -uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) -{ - uint16_t count[6]; - uint32_t sum[6]; - uint8_t i; - - // 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++) { - count[i] = _count[channel_numbers[i]]; - sum[i] = _sum[channel_numbers[i]]; - _count[channel_numbers[i]] = 0; - _sum[channel_numbers[i]] = 0; - } - 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 - // 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; - last_ch6_micros = us; - return ret; -} +/* + 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 + 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 + #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 }; + +// 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]; + +static uint32_t last_ch6_micros; + +// 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; +} + + +ISR (TIMER2_OVF_vect) +{ + uint8_t ch; + static uint8_t timer_offset; + + bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) + ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel + + 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 + + if (v & 0x8007) { + // this is a 12-bit ADC, shifted by 3 bits. + // if we get other bits set then the value is + // bogus and should be ignored + continue; + } + + if (++_count[ch] == 0) { + // overflow ... shouldn't happen too often + // unless we're just not using the + // channel. Notice that we overflow the count + // to 1 here, not zero, as otherwise the + // reader below could get a division by zero + _sum[ch] = 0; + _count[ch] = 1; + last_ch6_micros = micros(); + } + _sum[ch] += (v >> 3); + } + + bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) + + // this gives us a sample rate between 781Hz and 1302Hz. We + // randomise it to try to minimise aliasing effects + timer_offset = (timer_offset + 49) % 32; + TCNT2 = TCNT2_781_HZ + timer_offset; +} + + +// Constructors //////////////////////////////////////////////////////////////// +AP_ADC_ADS7844::AP_ADC_ADS7844() : + _filter_index(0), + filter_result(false) +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void AP_ADC_ADS7844::Init(void) +{ + 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 + + // 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]); + _count[i] = 1; + _sum[i] = adc_tmp; + } + + last_ch6_micros = micros(); + _filter_index = 0; + + // Enable Timer2 Overflow interrupt to capture ADC data + TIMSK2 = 0; // Disable interrupts + TCCR2A = 0; // normal counting mode + TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256 + 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) +{ + 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(); + count = _count[ch_num]; + sum = _sum[ch_num]; + _count[ch_num] = 0; + _sum[ch_num] = 0; + 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 +// sensor by either Ch6() or Ch() and never mix them. If you mix them +// then you will get very strange results +uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) +{ + uint16_t count[6]; + uint32_t sum[6]; + uint8_t i; + + // 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++) { + count[i] = _count[channel_numbers[i]]; + sum[i] = _sum[channel_numbers[i]]; + _count[channel_numbers[i]] = 0; + _sum[channel_numbers[i]] = 0; + } + 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 + // worth it. + for (i = 0; i < 6; i++) { + result[i] = sum[i] / count[i]; + } + + // filter ch 0,1,2 for smoother Gyro output. + + 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; +} diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index 2b1314ea1c..e33f157f92 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -1,32 +1,36 @@ -#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); - - // Read 6 sensors at once - uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); - - private: -}; - -#endif +#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); + + // Read 6 sensors at once + uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + bool filter_result; + + private: + uint16_t _filter[3][ADC_FILTER_SIZE]; + uint8_t _filter_index; + +}; + +#endif