aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Frsky_Telem: transmit airspeed instead of groundspeed, if available

Browse Source
This commit is contained in:
floaledm 2016-09-01 16:27:48 -05:00 committed by Andrew Tridgell
parent 8deba69715
commit 4dc98faa48
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -690,8 +690,13 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void)
// vertical velocity in dm/s
velandyaw = prep_number(roundf(velNED.z * 10), 2, 1);
// horizontal velocity in dm/s
velandyaw |= prep_number(roundf(_ahrs.groundspeed_vector().length() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
// 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;
} else {
velandyaw |= prep_number(roundf(_ahrs.groundspeed_vector().length() * 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)
velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET;
return velandyaw;