|
|
|
@ -693,7 +693,7 @@ void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
@@ -693,7 +693,7 @@ void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
|
|
|
|
|
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) && \
|
|
|
|
|
bool land_lock_attitude = (motors->armed() && get_alt_above_ground_cm() < g.land_lock_alt ) && \
|
|
|
|
|
copter.rangefinder_alt_ok() && g.land_lock_alt && \
|
|
|
|
|
(copter.control_mode == Mode::Number::LOITER || copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO); |
|
|
|
|
#endif |
|
|
|
@ -702,22 +702,16 @@ void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
@@ -702,22 +702,16 @@ void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
|
|
|
|
|
float *target_roll = p_roll; |
|
|
|
|
float *target_pitch = p_pitch; |
|
|
|
|
float *target_yaw_rate = p_yaw_rate; |
|
|
|
|
if(prt_once == 0) |
|
|
|
|
{ |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Land ready to enter attitude lock mode"); |
|
|
|
|
copter.Log_Write_Event(DATA_LAND_READY_LOCK); |
|
|
|
|
prt_once = 1; |
|
|
|
|
} |
|
|
|
|
if(copter.rangefinder_state.alt_cm_filt.get() < g.land_lock_alt){ |
|
|
|
|
*target_roll = 0; |
|
|
|
|
*target_pitch = 0; |
|
|
|
|
*target_yaw_rate = 0; |
|
|
|
|
loiter_nav->soften_for_landing(); |
|
|
|
|
if(prt_once == 1) |
|
|
|
|
if(prt_once == 0) |
|
|
|
|
{ |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Land Lock attitude!"); |
|
|
|
|
copter.Log_Write_Event(DATA_LAND_LOCK_ATT); |
|
|
|
|
prt_once = 2; |
|
|
|
|
prt_once = 1; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
}else{ |
|
|
|
|