Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 03:20:00 +03:00
Code Issues Wiki Activity

Merge pull request #11049 from tbs-fpv/crsf_yaw_value_fix

Browse source
This commit is contained in:
Michael Keller 2021-11-10 09:08:48 +13:00
parent 86e16f29be
commit 948ba63397
1 changed files with 15 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

19
src/main/telemetry/crsf.c
View file

@ -248,15 +248,26 @@ int16_t Roll angle ( rad / 10000 )
int16_t Yaw angle ( rad / 10000 ) int16_t Yaw angle ( rad / 10000 )
*/ */
#define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD)) // convert andgle in decidegree to radians/10000 with reducing angle to +/-180 degree range
static int16_t decidegrees2Radians10000(int16_t angle_decidegree)
{
while (angle_decidegree > 1800) {
angle_decidegree -= 3600;
}
while (angle_decidegree < -1800) {
angle_decidegree += 3600;
}
return (int16_t)(RAD * 1000.0f * angle_decidegree);
}
// fill dst buffer with crsf-attitude telemetry frame
void crsfFrameAttitude(sbuf_t *dst) void crsfFrameAttitude(sbuf_t *dst)
{ {
sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE); sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE);
sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch)); sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.pitch));
sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll)); sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.roll));
sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw)); sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.yaw));
} }
/* /*