AP_Frsky_Telem: airspeed scaling factor fix

This commit is contained in:
floaledm 2016-10-10 17:19:15 -05:00 committed by Tom Pittenger
parent 7041d2587b
commit 49e47fd679
1 changed files with 1 additions and 1 deletions

View File

@ -742,7 +742,7 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void)
// horizontal velocity in dm/s (use airspeed if available, otherwise use groundspeed)
float airspeed;
if (_ahrs.airspeed_estimate_true(&airspeed)) {
velandyaw |= prep_number(roundf(airspeed * 0.1f), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
velandyaw |= prep_number(roundf(airspeed * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
} else {
velandyaw |= prep_number(roundf(_ahrs.groundspeed_vector().length() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
}