From ba7abd62d8f9f31405aa237e9c4099f50b48c7cf Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 23 Sep 2011 13:52:10 -0700 Subject: [PATCH] Adjusted DCM Gains to normal levels. Added a check to stop navigating when landing and to land faster. --- ArduCopter/APM_Config.h | 2 +- ArduCopter/commands_logic.pde | 4 ++++ ArduCopter/motors.pde | 8 ++++---- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index a7d000c483..82e597d06c 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -29,7 +29,7 @@ */ -# define CH7_OPTION CH7_RTL +# define CH7_OPTION CH7_DO_NOTHING /* CH7_DO_NOTHING CH7_SET_HOVER diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 6adaed22f2..85fbf34213 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -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"); diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 948d91cedf..d03710efd5 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -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