Adjusted DCM Gains to normal levels. Added a check to stop navigating when landing and to land faster.

This commit is contained in:
Jason Short 2011-09-23 13:52:10 -07:00
parent 25b0a3c43b
commit ba7abd62d8
3 changed files with 9 additions and 5 deletions

View File

@ -29,7 +29,7 @@
*/
# define CH7_OPTION CH7_RTL
# define CH7_OPTION CH7_DO_NOTHING
/*
CH7_DO_NOTHING
CH7_SET_HOVER

View File

@ -350,6 +350,10 @@ static bool verify_land()
if(g.sonar_enabled){
// decide which sensor we're using
if(sonar_alt < 300){
next_WP = current_loc; // don't pitch or roll
next_WP.alt = -200; // force us down
}
if(sonar_alt < 40){
land_complete = true;
//Serial.println("Y");

View File

@ -39,9 +39,9 @@ static void arm_motors()
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000);
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
//dcm.ki_roll_pitch(0.000006);
dcm.kp_roll_pitch(0.120000);
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
//dcm.ki_roll_pitch(0.000006);
#endif
// tune down compass
@ -104,7 +104,7 @@ static void arm_motors()
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.12); // higher for fast recovery
//dcm.kp_roll_pitch(0.12); // higher for fast recovery
//dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
#endif