diff --git a/libraries/AP_DCM/AP_DCM_FW.cpp b/libraries/AP_DCM/AP_DCM_FW.cpp index bf8d678e7b..c66f9f7225 100644 --- a/libraries/AP_DCM/AP_DCM_FW.cpp +++ b/libraries/AP_DCM/AP_DCM_FW.cpp @@ -1,19 +1,43 @@ +/* + APM_DCM_FW.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega + Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com + This library works with the ArduPilot Mega and "Oilpan" + + 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: + quick_init() : For air restart + init() : For ground start. Calibrates the IMU + update_DCM(_G_Dt) : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data + get_roll_sensor() : Returns roll in degrees * 100 + get_roll() : Returns roll in radians + get_pitch_sensor() : Returns pitch in degrees * 100 + get_pitch() : Returns pitch in radians + get_yaw_sensor() : Returns yaw in degrees * 100 + get_yaw() : Returns yaw in radians + +*/ #include #define OUTPUTMODE 1 // This is just used for debugging, remove later +#define TRUE 1 +#define FALSE 0 #define ToRad(x) (x*0.01745329252) // *pi/180 #define ToDeg(x) (x*57.2957795131) // *180/pi -#define Kp_ROLLPITCH 0.5852 // .0014 * 418 Pitch&Roll Drift Correction Proportional Gain -#define Ki_ROLLPITCH 0.0001254 // 0.0000003 * 418 Pitch&Roll Drift Correction Integrator Gain +#define Kp_ROLLPITCH 0.05967 // .0014 * 418/9.81 Pitch&Roll Drift Correction Proportional Gain +#define Ki_ROLLPITCH 0.00001278 // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain #define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain #define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain -#define SPEEDFILT 400 // centimeters/second +#define SPEEDFILT 300 // centimeters/second #define ADC_CONSTRAINT 900 @@ -154,11 +178,11 @@ AP_DCM_FW::_accel_adjust(void) Vector3f _veloc, _temp; float _vel; - _veloc.x = (_gps->ground_speed / 100) / 9.81; // We are working with acceleration in g units + _veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units //_accel_vector += _omega_integ_corr % _veloc; // Equation 26 This line is giving the compiler a problem so we break it up below _temp.y = _omega_integ_corr.z * _veloc.x; // only computing the non-zero terms - _temp.z = -1.0f * _omega_integ_corr.y * _veloc.x; + _temp.z = -1.0f * _omega_integ_corr.y * _veloc.x; // After looking at the compiler issue lets remove _veloc and simlify _accel_vector -= _temp; @@ -234,11 +258,12 @@ AP_DCM_FW::drift_correction(void) float accel_magnitude; float accel_weight; float integrator_magnitude; + static bool last_speed_below_thresh = TRUE; //*****Roll and Pitch*************** // Calculate the magnitude of the accelerometer vector - accel_magnitude = _accel_vector.length(); + accel_magnitude = _accel_vector.length() / 9.80665f; // Dynamic weighting of accelerometer info (reliability filter) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) @@ -268,7 +293,11 @@ AP_DCM_FW::drift_correction(void) error_course= (_dcm_matrix.a.x * _compass->Heading_Y) - (_dcm_matrix.b.x * _compass->Heading_X); // Equation 23, Calculating YAW error } else { // Use GPS Ground course to correct yaw gyro drift - if (_gps->ground_speed >= SPEEDFILT) { + if (_gps->ground_speed >= SPEEDFILT && last_speed_below_thresh) { + last_speed_below_thresh = FALSE; + // *** Need to put code here to compute the rotation matrix to update the DCM matrix to the correct yaw value now that we have a reference. + // *** Not having that code at present doesn't really hurt anything. It just delays the beginning of yaw drift correction by 1 gps sample in time. + } else if (_gps->ground_speed >= SPEEDFILT) { _course_over_ground_x = cos(ToRad(_gps->ground_course/100.0)); _course_over_ground_y = sin(ToRad(_gps->ground_course/100.0)); // Optimization: Pass these in as arguments to update so they don't have to be calculated here and the AP code @@ -276,6 +305,7 @@ AP_DCM_FW::drift_correction(void) error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x); // Equation 23, Calculating YAW error } else { error_course = 0; + last_speed_below_thresh = TRUE; } } diff --git a/libraries/AP_DCM/AP_DCM_FW.h b/libraries/AP_DCM/AP_DCM_FW.h index 73675fe693..6faf664d56 100644 --- a/libraries/AP_DCM/AP_DCM_FW.h +++ b/libraries/AP_DCM/AP_DCM_FW.h @@ -64,8 +64,6 @@ private: Matrix3f _dcm_matrix; - float _adc_in[6]; // array that store the 6 ADC channels used by IMU - float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers Vector3f _accel_vector; // Store the acceleration in a vector Vector3f _gyro_vector; //Store the gyros turn rate in a vector Vector3f _omega_P; //Omega Proportional correction @@ -78,11 +76,6 @@ private: float _course_over_ground_x; //Course overground X axis float _course_over_ground_y; //Course overground Y axis - // constants - static const uint8_t _sensors[6]; - static const int _sensor_signs[9]; - static const uint8_t _gyro_temp_ch = 3; // The ADC channel reading the gyro temperature - static const float _gyro_temp_curve[3][3]; }; #endif diff --git a/libraries/AP_IMU/AP_IMU.cpp b/libraries/AP_IMU/AP_IMU.cpp index b5e79591ba..90bfdd2f84 100644 --- a/libraries/AP_IMU/AP_IMU.cpp +++ b/libraries/AP_IMU/AP_IMU.cpp @@ -1,3 +1,21 @@ +/* + APM_IMU.cpp - IMU Sensor Library for Ardupilot Mega + Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com + + This library works with the ArduPilot Mega and "Oilpan" + + 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: + quick_init() : For air restart + init() : For ground start. Calibration + get_gyro() : Returns gyro vector. Elements in radians/second + get_accel() : Returns acceleration vector. Elements in meters/seconds squared + +*/ #include @@ -8,7 +26,7 @@ // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 // Tested value : 418 #define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer -#define accel_scale(x) (x/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared +#define accel_scale(x) (x*9.80665/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared #define ToRad(x) (x*0.01745329252) // *pi/180 #define ToDeg(x) (x*57.2957795131) // *180/pi diff --git a/libraries/AP_IMU/AP_IMU.h b/libraries/AP_IMU/AP_IMU.h index 3da188f7c7..6a5c438b9a 100644 --- a/libraries/AP_IMU/AP_IMU.h +++ b/libraries/AP_IMU/AP_IMU.h @@ -18,7 +18,7 @@ public: void quick_init(void); // For air start void init(void); // For ground start Vector3f get_gyro(void); // Radians/second - Vector3f get_accel(void); // G units + Vector3f get_accel(void); // meters/seconds squared // Members uint8_t gyro_sat_count; @@ -34,7 +34,7 @@ private: float _adc_in[6]; // array that store the 6 ADC channels used by IMU float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers Vector3f _accel_vector; // Store the acceleration in a vector - Vector3f _gyro_vector; //Store the gyros turn rate in a vector + Vector3f _gyro_vector; // Store the gyros turn rate in a vector // constants static const uint8_t _sensors[6];