mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-08 17:03:57 -04:00
Adjusted DCM Gains to normal levels. Added a check to stop navigating when landing and to land faster.
This commit is contained in:
parent
f65390920a
commit
7c72f13702
@ -29,7 +29,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
# define CH7_OPTION CH7_RTL
|
# define CH7_OPTION CH7_DO_NOTHING
|
||||||
/*
|
/*
|
||||||
CH7_DO_NOTHING
|
CH7_DO_NOTHING
|
||||||
CH7_SET_HOVER
|
CH7_SET_HOVER
|
||||||
|
@ -350,6 +350,10 @@ static bool verify_land()
|
|||||||
|
|
||||||
if(g.sonar_enabled){
|
if(g.sonar_enabled){
|
||||||
// decide which sensor we're using
|
// 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){
|
if(sonar_alt < 40){
|
||||||
land_complete = true;
|
land_complete = true;
|
||||||
//Serial.println("Y");
|
//Serial.println("Y");
|
||||||
|
@ -39,7 +39,7 @@ static void arm_motors()
|
|||||||
// Tune down DCM
|
// Tune down DCM
|
||||||
// -------------------
|
// -------------------
|
||||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
dcm.kp_roll_pitch(0.030000);
|
dcm.kp_roll_pitch(0.120000);
|
||||||
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
|
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
|
||||||
//dcm.ki_roll_pitch(0.000006);
|
//dcm.ki_roll_pitch(0.000006);
|
||||||
#endif
|
#endif
|
||||||
@ -104,7 +104,7 @@ static void arm_motors()
|
|||||||
// Tune down DCM
|
// Tune down DCM
|
||||||
// -------------------
|
// -------------------
|
||||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
#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
|
//dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user