Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues 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_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!
//

27
ArduCopter/ArduCopter.pde
View File

@ -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

12
ArduCopter/Attitude.pde
View File

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

6
ArduCopter/control_modes.pde
View File

@ -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;

4
ArduCopter/defines.h
View File

@ -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

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 = 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;

8
ArduCopter/system.pde
View File

@ -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;

32
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -53,7 +53,6 @@ extern "C" {
#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 };
@ -128,7 +127,9 @@ ISR (TIMER2_OVF_vect)
// Constructors ////////////////////////////////////////////////////////////////
AP_ADC_ADS7844::AP_ADC_ADS7844()
AP_ADC_ADS7844::AP_ADC_ADS7844() :
_filter_index(0),
filter_result(false)
{
}
@ -160,6 +161,7 @@ void AP_ADC_ADS7844::Init(void)
}
last_ch6_micros = micros();
_filter_index = 0;
// Enable Timer2 Overflow interrupt to capture ADC data
TIMSK2 = 0; // Disable interrupts
@ -220,10 +222,34 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
// 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++) {
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;

6
libraries/AP_ADC/AP_ADC_ADS7844.h
View File

@ -9,7 +9,7 @@
#define ADC_DATAIN 50 // MISO
#define ADC_SPICLOCK 52 // SCK
#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 <inttypes.h>
@ -25,8 +25,12 @@ class AP_ADC_ADS7844 : public AP_ADC
// 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