diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index fc5d7639c7..172565b320 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -45,11 +45,10 @@ # # Libraries # -#MODULES += lib/mathlib -#MODULES += lib/geo -#MODULES += lib/geo_lookup +MODULES += lib/mathlib +MODULES += lib/geo +MODULES += lib/geo_lookup #MODULES += lib/conversion -#MODULES += lib/utils # # Linux port diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 08405dd655..30b9d9ec28 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -274,17 +274,17 @@ __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) { - double lat_now_rad = lat_now / 180.0d * M_PI; - double lon_now_rad = lon_now / 180.0d * M_PI; - double lat_next_rad = lat_next / 180.0d * M_PI; - double lon_next_rad = lon_next / 180.0d * M_PI; + double lat_now_rad = lat_now / (double)180.0 * M_PI; + double lon_now_rad = lon_now / (double)180.0 * M_PI; + double lat_next_rad = lat_next / (double)180.0 * M_PI; + double lon_next_rad = lon_next / (double)180.0 * M_PI; double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); + double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon / (double)2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double c = (double)2.0 * atan2(sqrt(a), sqrt((double)1.0 - a)); return CONSTANTS_RADIUS_OF_EARTH * c; } diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index 0ae545f1a5..dffa213e05 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -47,7 +47,7 @@ namespace math { -#ifndef CONFIG_ARCH_ARM +#if !defined(CONFIG_ARCH_ARM) && !defined(__PX4_LINUX) #define M_PI_F 3.14159265358979323846f #endif diff --git a/src/platforms/linux/include/queue.h b/src/platforms/linux/include/queue.h new file mode 100644 index 0000000000..c65f760a2d --- /dev/null +++ b/src/platforms/linux/include/queue.h @@ -0,0 +1,143 @@ +/************************************************************************ + * include/queue.h + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************/ + +#ifndef __INCLUDE_QUEUE_H +#define __INCLUDE_QUEUE_H + +/************************************************************************ + * Included Files + ************************************************************************/ + +#include + +/************************************************************************ + * Pre-processor Definitions + ************************************************************************/ + +#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) +#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) + +#define sq_next(p) ((p)->flink) +#define dq_next(p) ((p)->flink) +#define dq_prev(p) ((p)->blink) + +#define sq_empty(q) ((q)->head == NULL) +#define dq_empty(q) ((q)->head == NULL) + +#define sq_peek(q) ((q)->head) +#define dq_peek(q) ((q)->head) + +// Required for Linux +#define FAR +#ifndef NULL +#define NULL (void *)0 +#endif + +/************************************************************************ + * Global Type Declarations + ************************************************************************/ + +struct sq_entry_s +{ + FAR struct sq_entry_s *flink; +}; +typedef struct sq_entry_s sq_entry_t; + +struct dq_entry_s +{ + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; +}; +typedef struct dq_entry_s dq_entry_t; + +struct sq_queue_s +{ + FAR sq_entry_t *head; + FAR sq_entry_t *tail; +}; +typedef struct sq_queue_s sq_queue_t; + +struct dq_queue_s +{ + FAR dq_entry_t *head; + FAR dq_entry_t *tail; +}; +typedef struct dq_queue_s dq_queue_t; + +// FIXME - hack for mavlink until kernel work queue can be removed +#define LPWORK 1 +typedef struct { + dq_entry_t dq; +} work_s; +inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2); +inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2) {} + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue); +EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, + dq_queue_t *queue); +EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, + dq_queue_t *queue); + +EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_QUEUE_H_ */ + diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 996e56efb9..6cb7f3f42c 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -65,41 +65,7 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) -#define OK 0 -#define ERROR -1 - -//XXX hack to be able to use isfinte from math.h, -D_GLIBCXX_USE_C99_MATH seems not to work -#define isfinite(_value) std::isfinite(_value) - -/* Useful constants. */ -#define M_E_F 2.7182818284590452354f -#define M_LOG2E_F 1.4426950408889634074f -#define M_LOG10E_F 0.43429448190325182765f -#define M_LN2_F _M_LN2_F -#define M_LN10_F 2.30258509299404568402f -#define M_PI_F 3.14159265358979323846f -#define M_TWOPI_F (M_PI_F * 2.0f) -#define M_PI_2_F 1.57079632679489661923f -#define M_PI_4_F 0.78539816339744830962f -#define M_3PI_4_F 2.3561944901923448370E0f -#define M_SQRTPI_F 1.77245385090551602792981f -#define M_1_PI_F 0.31830988618379067154f -#define M_2_PI_F 0.63661977236758134308f -#define M_2_SQRTPI_F 1.12837916709551257390f -#define M_DEG_TO_RAD_F 0.01745329251994f -#define M_RAD_TO_DEG_F 57.2957795130823f -#define M_SQRT2_F 1.41421356237309504880f -#define M_SQRT1_2_F 0.70710678118654752440f -#define M_LN2LO_F 1.9082149292705877000E-10f -#define M_LN2HI_F 6.9314718036912381649E-1f -#define M_SQRT3_F 1.73205080756887719000f -#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ -#define M_LOG2_E_F _M_LN2_F -#define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */ -#define M_DEG_TO_RAD 0.01745329251994 -#define M_RAD_TO_DEG 57.2957795130823 - -#else +#elif defined(__PX4_NUTTX) || defined(__PX4_LINUX) /* * Building for NuttX */ @@ -118,6 +84,13 @@ typedef param_t px4_param_t; /* Get value of parameter by name */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt) +#else +#error "No target OS defined" +#endif + +/* NuttX Specific defines */ +#if defined(__PX4_NUTTX) + /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) #define isspace(c) \ @@ -125,6 +98,49 @@ typedef param_t px4_param_t; (c) == '\r' || (c) == '\f' || c== '\v') #endif +/* Linux Specific defines */ +#elif defined(__PX4_LINUX) + +// Flag is meaningless on Linux +#define O_BINARY 0 + +#endif + + +/* Defines for ROS and Linux */ +#if defined(__PX4_ROS) || defined(__PX4_LINUX) +#define OK 0 +#define ERROR -1 + +#include +/* Float defines of the standard double length constants */ +#define M_E_F (float)M_E +#define M_LOG2E_F (float)M_LOG2E +#define M_LOG10E_F (float)M_LOG10E +#define M_LN2_F (float)M_LN2 +#define M_LN10_F (float)M_LN10 +#define M_PI_F (float)M_PI +#define M_TWOPI_F (M_PI_F * 2.0f) +#define M_PI_2_F (float)M_PI_2 +#define M_PI_4_F (float)M_PI_4 +#define M_3PI_4_F (float)2.3561944901923448370E0f +#define M_SQRTPI_F (float)1.77245385090551602792981f +#define M_1_PI_F (float)M_1_PI +#define M_2_PI_F (float)M_2_PI +#define M_2_SQRTPI_F 1.12837916709551257390f +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f +#define M_SQRT2_F (float)M_SQRT2 +#define M_SQRT1_2_F (float)M_SQRT1_2 +#define M_LN2LO_F 1.9082149292705877000E-10f +#define M_LN2HI_F 6.9314718036912381649E-1f +#define M_SQRT3_F 1.73205080756887719000f +#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ +#define M_LOG2_E_F _M_LN2_F +#define M_INVLN2_F 1.4426950408889633870E0f/* 1 / log(2) */ +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 + #endif /* Defines for all platforms */