diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 023337ff04..a8eee50435 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -319,14 +319,21 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou
 }
 
 __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
-		double *lat_end, double *lon_end)
+		double *lat_target, double *lon_target)
 {
 	bearing = _wrap_2pi(bearing);
 	double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
 
-	*lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing));
-	*lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start),
-				     cos(radius_ratio) - sin(lat_start) * sin(*lat_end));
+	double lat_start_rad = lat_start / (double)180.0 * M_PI;
+	double lon_start_rad = lon_start / (double)180.0 * M_PI;
+
+	*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
+				   double)bearing));
+	*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
+					    cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
+
+	*lat_target *= (double)180.0 / M_PI;
+	*lon_target *= (double)180.0 / M_PI;
 }
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
 {
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 8dc0836e34..0aa6cf03a8 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -237,12 +237,33 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
 
-// TODO put description for both functions and improve naming
+/**
+ * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
+ * from waypoint A
+ *
+ * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
+ * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
+ * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
+ * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
+ * @param dist distance of target waypoint from waypoint A (can be negative)
+ * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
+ * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
+ */
 __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
 		double *lat_target, double *lon_target);
 
+/**
+ * Creates a waypoint from given waypoint, distance and bearing
+ *
+ * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
+ * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
+ * @param bearing
+ * @param distance
+ * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
+ * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
+ */
 __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
-		double *end_lat, double *end_lon);
+		double *lat_target, double *lon_target);
 
 /**
  * Returns the bearing to the next waypoint in radians.