Copter: precision landing uses sonar alt if available

This commit is contained in:
Daniel Nugent 2015-09-11 18:15:02 +09:00 committed by Randy Mackay
parent 58b7bf1588
commit 9fe3abf5ca

View File

@ -15,10 +15,16 @@ void Copter::init_precland()
void Copter::update_precland()
{
copter.precland.update(current_loc.alt);
float final_alt = current_loc.alt;
// use range finder altitude if it is valid
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
final_alt = sonar_alt;
}
copter.precland.update(final_alt);
// log output
Log_Write_Precland();
}
#endif