|
|
|
@ -642,8 +642,18 @@ void Mode::land_run_horizontal_control()
@@ -642,8 +642,18 @@ void Mode::land_run_horizontal_control()
|
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
static uint8_t prt_once; |
|
|
|
|
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && copter.rangefinder_alt_ok() && g.land_lock_alt && (copter.control_mode == Mode::Number::LAND); |
|
|
|
|
// bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 4) && g.land_lock_alt && (copter.control_mode == Mode::Number::LAND);
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
|
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 4) && \
|
|
|
|
|
g.land_lock_alt && \
|
|
|
|
|
(copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO); |
|
|
|
|
if(!prt_once && land_lock_attitude) |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "land lock sitl mode"); |
|
|
|
|
#else |
|
|
|
|
|
|
|
|
|
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \
|
|
|
|
|
copter.rangefinder_alt_ok() && g.land_lock_alt && \
|
|
|
|
|
(copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO); |
|
|
|
|
#endif |
|
|
|
|
if(land_lock_attitude) |
|
|
|
|
{ |
|
|
|
|
if(prt_once == 0) |
|
|
|
|