Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: fixed airspeed; now same as VFR_HUD

Browse Source
This commit is contained in:
floaledm 2016-10-25 20:50:02 -05:00 committed by Tom Pittenger
parent 5e42800b5e
commit 56432f04d3
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -735,15 +735,14 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void)
// if we can't get velocity then we use zero for vertical velocity // if we can't get velocity then we use zero for vertical velocity
_ahrs.get_velocity_NED(velNED); _ahrs.get_velocity_NED(velNED);
// vertical velocity in dm/s // vertical velocity in dm/s
velandyaw = prep_number(roundf(velNED.z * 10), 2, 1); velandyaw = prep_number(roundf(velNED.z * 10), 2, 1);
// horizontal velocity in dm/s (use airspeed if available, otherwise use groundspeed) // horizontal velocity in dm/s (use airspeed if available, otherwise use groundspeed)
float airspeed; float aspeed;
if (_ahrs.airspeed_estimate_true(&airspeed)) { if (_ahrs.airspeed_estimate(&aspeed)) { // send airspeed estimate from ahrs if available
velandyaw |= prep_number(roundf(airspeed * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET; velandyaw |= prep_number(roundf(aspeed * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
} else { } else { // otherwise send groundspeed estimate from ahrs
velandyaw |= prep_number(roundf(_ahrs.groundspeed_vector().length() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET; velandyaw |= prep_number(roundf(_ahrs.groundspeed() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
} }
// yaw from [0;36000] centidegrees to .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits) // yaw from [0;36000] centidegrees to .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET; velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET;