mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 20:48:33 -04:00
Copter: precision landing uses sonar alt if available
This commit is contained in:
parent
58b7bf1588
commit
9fe3abf5ca
@ -15,10 +15,16 @@ void Copter::init_precland()
|
|||||||
|
|
||||||
void Copter::update_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 output
|
||||||
Log_Write_Precland();
|
Log_Write_Precland();
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
Loading…
Reference in New Issue
Block a user