mirror of https://github.com/ArduPilot/ardupilot
AP_Frsky_Telem: Enforce the use of 0 when velocity_NED is not available
This commit is contained in:
parent
0743b979fa
commit
fcb54c2d68
|
@ -759,7 +759,9 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void)
|
|||
Vector3f velNED {};
|
||||
|
||||
// if we can't get velocity then we use zero for vertical velocity
|
||||
_ahrs.get_velocity_NED(velNED);
|
||||
if (!_ahrs.get_velocity_NED(velNED)) {
|
||||
velNED.zero();
|
||||
}
|
||||
// vertical velocity in dm/s
|
||||
velandyaw = prep_number(roundf(-velNED.z * 10), 2, 1);
|
||||
// horizontal velocity in dm/s (use airspeed if available and enabled - even if not used - otherwise use groundspeed)
|
||||
|
|
Loading…
Reference in New Issue