diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 794ae15a13..6e480a5bfd 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2177,6 +2177,10 @@ static void tuning(){ g.pid_optflow_roll.kD(tuning_value); g.pid_optflow_pitch.kD(tuning_value); break; + + case CH6_AHRS_YAW_KP: + ahrs._kp_yaw.set(tuning_value); + break; } } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 1b9282104a..5360f7bd60 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -100,9 +100,9 @@ public: k_param_sonar_type, k_param_super_simple, k_param_rtl_land_enabled, - k_param_rtl_approach_alt, k_param_axis_enabled, - k_param_copter_leds_mode, //159 + k_param_copter_leds_mode, //158 + k_param_ahrs, // AHRS group // // 160: Navigation parameters @@ -110,6 +110,7 @@ public: k_param_RTL_altitude = 160, k_param_crosstrack_gain, k_param_auto_land_timeout, + k_param_rtl_approach_alt, // diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 9f4f5dd770..02fae1e2c0 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -136,12 +136,13 @@ static const AP_Param::Info var_info[] PROGMEM = { GOBJECT(compass, "COMPASS_", Compass), GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK), - GOBJECT(imu, "IMU_", IMU) + GOBJECT(imu, "IMU_", IMU), + GOBJECT(ahrs, "AHRS_", AP_AHRS), #if FRAME_CONFIG == HELI_FRAME - ,GOBJECT(motors, "H_", AP_MotorsHeli) + GOBJECT(motors, "H_", AP_MotorsHeli), #else - ,GOBJECT(motors, "MOT_", AP_Motors) + GOBJECT(motors, "MOT_", AP_Motors), #endif }; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 97847bb4f6..3f4487609a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -185,6 +185,8 @@ #define CH6_LOITER_RATE_KI 28 #define CH6_LOITER_RATE_KD 23 +#define CH6_AHRS_YAW_KP 30 + // nav byte mask // ------------- diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 5519f0cd22..6125550fe0 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -84,6 +84,7 @@ public: k_param_airspeed_offset, k_param_sonar_enabled, k_param_airspeed_enabled, + k_param_ahrs, // AHRS group // // 150: Navigation parameters diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 3619095c05..0603b47f6c 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -125,7 +125,8 @@ static const AP_Param::Info var_info[] PROGMEM = { GOBJECT(compass, "COMPASS_", Compass), GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK), - GOBJECT(imu, "IMU_", IMU) + GOBJECT(imu, "IMU_", IMU), + GOBJECT(ahrs, "AHRS_", AP_AHRS) }; diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index 4bb1685ce8..a4d60e15ae 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -5,3 +5,6 @@ list below ---------------- Test Developer ... + +Chris Anderson +Phil Cole put this here after rebasing an old branch to master. diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 883c158a53..10e2b90381 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -82,6 +82,8 @@ public: // attitude virtual Matrix3f get_dcm_matrix(void) = 0; + static const struct AP_Param::GroupInfo var_info[]; + protected: // pointer to compass object, if enabled Compass * _compass; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index b6fa2e1f45..2cf29b3249 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -23,6 +23,12 @@ // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN #define GPS_SPEED_RESET 100 +// table of user settable parameters +const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { + AP_GROUPINFO("YAW_P", 0, AP_AHRS_DCM, _kp_yaw), + AP_GROUPEND +}; + // run a full DCM update round void AP_AHRS_DCM::update(void) @@ -453,7 +459,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // allow the yaw reference source to affect all 3 components // of _omega_yaw_P as we need to be able to correctly hold a // heading when roll and pitch are non-zero - _omega_yaw_P = error * _kp_yaw; + _omega_yaw_P = error * _kp_yaw.get(); // add yaw correction to integrator correction vector, but // only for the z gyro. We rely on the accelerometers for x diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 5952380c7e..600592d8e5 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -17,7 +17,7 @@ public: AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps) { _kp_roll_pitch = 0.13; - _kp_yaw = 0.4; + _kp_yaw.set(0.4); _dcm_matrix(Vector3f(1, 0, 0), Vector3f(0, 1, 0), Vector3f(0, 0, 1)); @@ -42,10 +42,12 @@ public: float get_error_rp(void); float get_error_yaw(void); + // settable parameters + AP_Float _kp_yaw; + private: float _kp_roll_pitch; float _ki_roll_pitch; - float _kp_yaw; float _ki_yaw; bool _have_initial_yaw; diff --git a/libraries/AP_AHRS/AP_AHRS_HIL.h b/libraries/AP_AHRS/AP_AHRS_HIL.h index ea46cbe92a..20d903ffa6 100644 --- a/libraries/AP_AHRS/AP_AHRS_HIL.h +++ b/libraries/AP_AHRS/AP_AHRS_HIL.h @@ -32,6 +32,9 @@ public: float get_error_rp(void) { return 0; } float get_error_yaw(void) { return 0; } + // settable parameters + AP_Float _kp_yaw; + private: Vector3f _omega; };