From 4e5c6fc80503e436a04c8f718a8f774ae427df5a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 31 Jul 2017 15:29:16 +0200 Subject: [PATCH] frsky_telemetry: fix heading for D protocol --- src/drivers/frsky_telemetry/frsky_data.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index cfff8cfac0..5e67d41969 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -248,7 +248,12 @@ void frsky_send_frame2(int uart) time_t time_gps = global_pos->time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); - course = (global_pos->yaw + M_PI_F) / M_PI_F * 180.0f; + course = global_pos->yaw / M_PI_F * 180.0f; + + if (course < 0.f) { // course is in range [0, 360], 0=north, CW + course += 360.f; + } + lat = frsky_format_gps(fabsf(global_pos->lat)); lat_ns = (global_pos->lat < 0) ? 'S' : 'N'; lon = frsky_format_gps(fabsf(global_pos->lon));