fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 13:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF trivial code style cleanup

Browse Source
This commit is contained in:
Daniel Agar
2017-08-25 10:09:11 -04:00
parent 89236ef275
commit 99ba1c3745
12 changed files with 295 additions and 289 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/geo.cpp
+4 -12
View File
@@ -42,23 +42,15 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifdef POSIX_SHARED
//#include <unistd.h>
//#include <pthread.h>
#include "ecl.h"
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
#ifndef __PX4_QURT
#if defined(__cplusplus) && !defined(__PX4_NUTTX)
#include <cmath>
#define ISFINITE(x) std::isfinite(x)
#else
#define ISFINITE(x) isfinite(x)
#endif
#endif
/****************************************************************************
*
* Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
@@ -473,7 +465,7 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
float theta = atan2f(sin(d_lon) * cos(lat_next_rad) ,
float theta = atan2f(sin(d_lon) * cos(lat_next_rad),
cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
theta = _wrap_pi(theta);