Browse Source

AP_Frsky_Telem: airspeed scaling factor fix

mission-4.1.18
floaledm 8 years ago committed by Tom Pittenger
parent
commit
49e47fd679
  1. 2
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

2
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@ -742,7 +742,7 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void) @@ -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;
}

Loading…
Cancel
Save