From 263c48d0895acde13081d9f990cacfe0c2cab1c7 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Feb 2016 17:33:18 -0800 Subject: [PATCH 1/8] EKF: remove dependecies and allow ekf to be built as standalone shared lib --- .gitmodules | 3 + EKF/CMakeLists.txt | 33 ++ EKF/covariance.cpp | 2 +- EKF/ekf.h | 1 + EKF/ekf_helper.cpp | 2 +- EKF/estimator_interface.cpp | 4 +- EKF/estimator_interface.h | 3 +- EKF/geo.cpp | 827 ++++++++++++++++++++++++++++++++++++ EKF/geo.h | 310 ++++++++++++++ EKF/gps_checks.cpp | 10 +- EKF/mag_fusion.cpp | 2 +- EKF/mathlib.cpp | 14 + EKF/mathlib.h | 19 + 13 files changed, 1218 insertions(+), 12 deletions(-) create mode 100644 .gitmodules create mode 100644 EKF/CMakeLists.txt create mode 100644 EKF/geo.cpp create mode 100644 EKF/geo.h create mode 100644 EKF/mathlib.cpp create mode 100644 EKF/mathlib.h diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000000..86336b59f7 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "Matrix"] + path = matrix + url = https://github.com/PX4/Matrix diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt new file mode 100644 index 0000000000..b9355b2ee5 --- /dev/null +++ b/EKF/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.8) + +project (ECL CXX) +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_CURRENT_SOURCE_DIR EKF) +set(CMAKE_CXX_FLAGS "-DPOSIX_SHARED") +set (EIGEN3_INCLUDE_DIR "/usr/local/include/eigen3/") + +IF( NOT EIGEN3_INCLUDE_DIR ) + MESSAGE( FATAL_ERROR "Please point the environment variable EIGEN3_INCLUDE_DIR to the include directory of your Eigen3 installation.") +ENDIF() +INCLUDE_DIRECTORIES ( "${EIGEN3_INCLUDE_DIR}" ) + +include_directories( + ./ + ../ + ../matrix + EIGEN3_INCLUDE_DIR +) +set(SRCS + estimator_interface.cpp + ekf.cpp + ekf_helper.cpp + covariance.cpp + vel_pos_fusion.cpp + mag_fusion.cpp + gps_checks.cpp + control.cpp + geo.cpp + mathlib.cpp +) +add_definitions(-std=c++11) +add_library(ecl SHARED ${SRCS}) \ No newline at end of file diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 105a692602..221d3c1d10 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -42,7 +42,7 @@ #include "ekf.h" #include -#include +#include "mathlib.h" void Ekf::initialiseCovariance() { diff --git a/EKF/ekf.h b/EKF/ekf.h index de693135e9..c381bea4b2 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -41,6 +41,7 @@ */ #include "estimator_interface.h" +#include "geo.h" class Ekf : public EstimatorInterface { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a799f6e5e3..e055021dff 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -45,7 +45,7 @@ #include #endif #include -#include +#include "mathlib.h" // Reset the velocity states. If we have a recent and valid // gps measurement then use for velocity initialisation diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index cf26c748d7..9720dbd554 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -44,7 +44,7 @@ #include #include #include "estimator_interface.h" -#include +#include "mathlib.h" EstimatorInterface::EstimatorInterface() @@ -216,7 +216,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && _flow_buffer.allocate(OBS_BUFFER_LENGTH) && _output_buffer.allocate(IMU_BUFFER_LENGTH))) { - PX4_WARN("Estimator Buffer Allocation failed!"); + printf("Estimator Buffer Allocation failed!"); unallocate_buffers(); return false; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 5e240178c5..6d558d5212 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -41,9 +41,8 @@ #include #include -#include #include "RingBuffer.h" - +#include "geo.h" #include "common.h" using namespace estimator; diff --git a/EKF/geo.cpp b/EKF/geo.cpp new file mode 100644 index 0000000000..d422d20817 --- /dev/null +++ b/EKF/geo.cpp @@ -0,0 +1,827 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * @author Anton Babushkin + */ +#ifdef POSIX_SHARED +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. + * + * 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 MAVGEO 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. + * + ****************************************************************************/ + +/** +* @file geo_mag_declination.c +* +* Calculation / lookup table for earth magnetic field declination. +* +* Lookup table from Scott Ferguson +* +* XXX Lookup table currently too coarse in resolution (only full degrees) +* and lat/lon res - needs extension medium term. +* +*/ + +#include "geo.h" + +/** set this always to the sampling in degrees for the table below */ +#define SAMPLING_RES 10.0f +#define SAMPLING_MIN_LAT -60.0f +#define SAMPLING_MAX_LAT 60.0f +#define SAMPLING_MIN_LON -180.0f +#define SAMPLING_MAX_LON 180.0f + +static const int8_t declination_table[13][37] = \ +{ + { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, +}; + +static float get_lookup_table_val(unsigned lat, unsigned lon); + +float get_mag_declination(float lat, float lon) +{ + /* + * If the values exceed valid ranges, return zero as default + * as we have no way of knowing what the closest real value + * would be. + */ + if (lat < -90.0f || lat > 90.0f || + lon < -180.0f || lon > 180.0f) { + return 0.0f; + } + + /* round down to nearest sampling resolution */ + int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; + + /* for the rare case of hitting the bounds exactly + * the rounding logic wouldn't fit, so enforce it. + */ + + /* limit to table bounds - required for maxima even when table spans full globe range */ + if (lat <= SAMPLING_MIN_LAT) { + min_lat = SAMPLING_MIN_LAT; + } + + if (lat >= SAMPLING_MAX_LAT) { + min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + if (lon <= SAMPLING_MIN_LON) { + min_lon = SAMPLING_MIN_LON; + } + + if (lon >= SAMPLING_MAX_LON) { + min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + /* find index of nearest low sampling point */ + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; + + float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); + float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); + float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1); + float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); + + /* perform bilinear interpolation on the four grid corners */ + + float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; + float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; + + return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; +} + +float get_lookup_table_val(unsigned lat_index, unsigned lon_index) +{ + return declination_table[lat_index][lon_index]; +} + +/* + * Azimuthal Equidistant Projection + * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html + */ + +static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0}; +static struct globallocal_converter_reference_s gl_ref = {0.0f, false}; + +bool map_projection_global_initialized() +{ + return map_projection_initialized(&mp_ref); +} + +bool map_projection_initialized(const struct map_projection_reference_s *ref) +{ + return ref->init_done; +} + +uint64_t map_projection_global_timestamp() +{ + return map_projection_timestamp(&mp_ref); +} + +uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} + +int map_projection_global_init(double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); +} + +int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + + ref->lat_rad = lat_0 * M_DEG_TO_RAD; + ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->sin_lat = sin(ref->lat_rad); + ref->cos_lat = cos(ref->lat_rad); + + ref->timestamp = timestamp; + ref->init_done = true; + + return 0; +} + +int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad) +{ + return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad); +} + +int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; + + return 0; +} + +int map_projection_global_project(double lat, double lon, float *x, float *y) +{ + return map_projection_project(&mp_ref, lat, lon, x, y); + +} + +int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double lat_rad = lat * M_DEG_TO_RAD; + double lon_rad = lon * M_DEG_TO_RAD; + + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref->lon_rad); + + double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon; + + if (arg > 1.0) { + arg = 1.0; + + } else if (arg < -1.0) { + arg = -1.0; + } + + double c = acos(arg); + double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); + + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; + + return 0; +} + +int map_projection_global_reproject(float x, float y, double *lat, double *lon) +{ + return map_projection_reproject(&mp_ref, x, y, lat, lon); +} + +int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double c = sqrtf(x_rad * x_rad + y_rad * y_rad); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_rad; + double lon_rad; + + if (fabs(c) > DBL_EPSILON) { + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + + } else { + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; + } + + *lat = lat_rad * 180.0 / M_PI; + *lon = lon_rad * 180.0 / M_PI; + + return 0; +} + +int map_projection_global_getref(double *lat_0, double *lon_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (lat_0 != NULL) { + *lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad; + } + + if (lon_0 != NULL) { + *lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad; + } + + return 0; + +} +int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) +{ + gl_ref.alt = alt_0; + + if (!map_projection_global_init(lat_0, lon_0, timestamp)) { + gl_ref.init_done = true; + return 0; + + } else { + gl_ref.init_done = false; + return -1; + } +} + +bool globallocalconverter_initialized() +{ + return gl_ref.init_done && map_projection_global_initialized(); +} + +int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + map_projection_global_project(lat, lon, x, y); + *z = gl_ref.alt - alt; + + return 0; +} + +int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + map_projection_global_reproject(x, y, lat, lon); + *alt = gl_ref.alt - z; + + return 0; +} + +int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (map_projection_global_getref(lat_0, lon_0)) { + return -1; + } + + if (alt_0 != NULL) { + *alt_0 = gl_ref.alt; + } + + return 0; +} + +float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + 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 / (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; +} + +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) +{ + if (fabsf(dist) < FLT_EPSILON) { + *lat_target = lat_A; + *lon_target = lon_A; + + } else if (dist >= FLT_EPSILON) { + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + + } else { + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + heading = _wrap_2pi(heading + M_PI_F); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } +} + +void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_target, double *lon_target) +{ + bearing = _wrap_2pi(bearing); + double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); + + double lat_start_rad = lat_start * M_DEG_TO_RAD; + double lon_start_rad = lon_start * M_DEG_TO_RAD; + + *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 *= M_RAD_TO_DEG; + *lon_target *= M_RAD_TO_DEG; +} +float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + 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) , + cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + theta = _wrap_pi(theta); + + return theta; +} + +void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, + float *v_e) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos( + d_lon)); + *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); +} + +void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, + float *v_n, float *v_e) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat; + *v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad); +} + +void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, + double *lon_res) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + + *lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; +} + +// Additional functions - @author Doug Weibel + +int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_start, double lon_start, double lat_end, double lon_end) +{ +// This function returns the distance to the nearest point on the track line. Distance is positive if current +// position is right of the track and negative if left of the track as seen from a point on the track line +// headed towards the end point. + + float dist_to_end; + float bearing_end; + float bearing_track; + float bearing_diff; + + int return_value = ERROR; // Set error flag, cleared when valid result calculated. + crosstrack_error->past_end = false; + crosstrack_error->distance = 0.0f; + crosstrack_error->bearing = 0.0f; + + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + + // Return error if arguments are bad + if (dist_to_end < 0.1f) { + return ERROR; + } + + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); + bearing_diff = bearing_track - bearing_end; + bearing_diff = _wrap_pi(bearing_diff); + + // Return past_end = true if past end point of line + if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + crosstrack_error->past_end = true; + return_value = OK; + return return_value; + } + + crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); + + if (sin(bearing_diff) >= 0) { + crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); + + } else { + crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); + } + + return_value = OK; + + return return_value; + +} + + +int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) +{ + // This function returns the distance to the nearest point on the track arc. Distance is positive if current + // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and + // headed towards the end point. + + // Determine if the current position is inside or outside the sector between the line from the center + // to the arc start and the line from the center to the arc end + float bearing_sector_start; + float bearing_sector_end; + float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + bool in_sector; + + int return_value = ERROR; // Set error flag, cleared when valid result calculated. + crosstrack_error->past_end = false; + crosstrack_error->distance = 0.0f; + crosstrack_error->bearing = 0.0f; + + // Return error if arguments are bad + if (radius < 0.1f) { return return_value; } + + + if (arc_sweep >= 0.0f) { + bearing_sector_start = arc_start_bearing; + bearing_sector_end = arc_start_bearing + arc_sweep; + + if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; } + + } else { + bearing_sector_end = arc_start_bearing; + bearing_sector_start = arc_start_bearing - arc_sweep; + + if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; } + } + + in_sector = false; + + // Case where sector does not span zero + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start + && bearing_now <= bearing_sector_end) { in_sector = true; } + + // Case where sector does span zero + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start + || bearing_now < bearing_sector_end)) { in_sector = true; } + + // If in the sector then calculate distance and bearing to closest point + if (in_sector) { + crosstrack_error->past_end = false; + float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + + if (dist_to_center <= radius) { + crosstrack_error->distance = radius - dist_to_center; + crosstrack_error->bearing = bearing_now + M_PI_F; + + } else { + crosstrack_error->distance = dist_to_center - radius; + crosstrack_error->bearing = bearing_now; + } + + // If out of the sector then calculate dist and bearing to start or end point + + } else { + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + double start_disp_x = (double)radius * sin(arc_start_bearing); + double start_disp_y = (double)radius * cos(arc_start_bearing); + double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep))); + double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep))); + double lon_start = lon_now + start_disp_x / 111111.0; + double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0; + double lon_end = lon_now + end_disp_x / 111111.0; + double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0; + double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + + + if (dist_to_start < dist_to_end) { + crosstrack_error->distance = dist_to_start; + crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + + } else { + crosstrack_error->past_end = true; + crosstrack_error->distance = dist_to_end; + crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + } + + } + + crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing); + return_value = OK; + return return_value; +} + +float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z) +{ + double current_x_rad = lat_next / 180.0 * M_PI; + double current_y_rad = lon_next / 180.0 * M_PI; + double x_rad = lat_now / 180.0 * M_PI; + double y_rad = lon_now / 180.0 * M_PI; + + double d_lat = x_rad - current_x_rad; + double d_lon = y_rad - current_y_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + float dxy = CONSTANTS_RADIUS_OF_EARTH * c; + float dz = alt_now - alt_next; + + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + + return sqrtf(dxy * dxy + dz * dz); +} + + +float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z) +{ + float dx = x_now - x_next; + float dy = y_now - y_next; + float dz = z_now - z_next; + + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + + return sqrtf(dx * dx + dy * dy + dz * dz); +} + +float _wrap_pi(float bearing) +{ + /* value is inf or NaN */ + if (!math::isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= M_PI_F) { + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < -M_PI_F) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +float _wrap_2pi(float bearing) +{ + /* value is inf or NaN */ + if (!math::isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= M_TWOPI_F) { + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < 0.0f) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +float _wrap_180(float bearing) +{ + /* value is inf or NaN */ + if (!math::isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= 180.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < -180.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +float _wrap_360(float bearing) +{ + /* value is inf or NaN */ + if (!math::isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= 360.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < 0.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} +#endif //POSIX_SHARED \ No newline at end of file diff --git a/EKF/geo.h b/EKF/geo.h new file mode 100644 index 0000000000..e20c440583 --- /dev/null +++ b/EKF/geo.h @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file geo.h + * + * Definition of geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * @author Anton Babushkin + * Additional functions - @author Doug Weibel + */ +#ifndef GEO_H +#define GEO_H +#ifdef POSIX_SHARED +#include +#include "mathlib.h" + +#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ +#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ +#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ +#define M_TWOPI_F 6.28318530717958647692f +#define M_PI_2_F 1.57079632679489661923f +#define M_RAD_TO_DEG 0.01745329251994329576f +#define M_DEG_TO_RAD 57.29577951308232087679f +#define OK 0 +#define ERROR -1 +// XXX remove +struct crosstrack_error_s { + bool past_end; // Flag indicating we are past the end of the line/arc segment + float distance; // Distance in meters to closest point on line/arc + float bearing; // Bearing in radians to closest point on line/arc +} ; + +/* lat/lon are in radians */ +struct map_projection_reference_s { + double lat_rad; + double lon_rad; + double sin_lat; + double cos_lat; + bool init_done; + uint64_t timestamp; +}; + +struct globallocal_converter_reference_s { + float alt; + bool init_done; +}; + +/** + * Checks if global projection was initialized + * @return true if map was initialized before, false else + */ +bool map_projection_global_initialized(void); + +/** + * Checks if projection given as argument was initialized + * @return true if map was initialized before, false else + */ +bool map_projection_initialized(const struct map_projection_reference_s *ref); + +/** + * Get the timestamp of the global map projection + * @return the timestamp of the map_projection + */ +uint64_t map_projection_global_timestamp(void); + +/** + * Get the timestamp of the map projection given by the argument + * @return the timestamp of the map_projection + */ +uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref); + +/** + * Writes the reference values of the global projection to ref_lat and ref_lon + * @return 0 if map_projection_init was called before, -1 else + */ +int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad); + +/** + * Writes the reference values of the projection given by the argument to ref_lat and ref_lon + * @return 0 if map_projection_init was called before, -1 else + */ +int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad); + + +/** + * Initializes the map transformation given by the argument. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +int map_projection_init_timestamped(struct map_projection_reference_s *ref, + double lat_0, double lon_0, uint64_t timestamp); + +/** + * Initializes the map transformation given by the argument and sets the timestamp to now. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); + +/** + * Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the global projection + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +int map_projection_global_project(double lat, double lon, float *x, float *y); + + +/* Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the projection given by the argument +* @param x north +* @param y east +* @param lat in degrees (47.1234567°, not 471234567°) +* @param lon in degrees (8.1234567°, not 81234567°) +* @return 0 if map_projection_init was called before, -1 else +*/ +int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y); + +/** + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the global projection + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +int map_projection_global_reproject(float x, float y, double *lat, double *lon); + +/** + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the projection given by the argument + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon); + +/** + * Get reference position of the global map projection + */ +int map_projection_global_getref(double *lat_0, double *lon_0); + +/** + * Initialize the global mapping between global position (spherical) and local position (NED). + */ +int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp); + +/** + * Checks if globallocalconverter was initialized + * @return true if map was initialized before, false else + */ +bool globallocalconverter_initialized(void); + +/** + * Convert from global position coordinates to local position coordinates using the global reference + */ +int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z); + +/** + * Convert from local position coordinates to global position coordinates using the global reference + */ +int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt); + +/** + * Get reference position of the global to local converter + */ +int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0); + +/** + * Returns the distance to the next waypoint in meters. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ +float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + + +/** + * 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 in meters (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°) + */ +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 + * see http://www.movable-type.co.uk/scripts/latlong.html + * + * @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 in rad + * @param distance in meters + * @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°) + */ +void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_target, double *lon_target); + +/** + * Returns the bearing to the next waypoint in radians. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ +float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, + float *v_e); + +void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, + float *v_n, float *v_e); + +void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, + double *lon_res); + +int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_start, double lon_start, double lat_end, double lon_end); + +int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + +/* + * Calculate distance in global frame + */ +float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z); + +/* + * Calculate distance in local frame (NED) + */ +float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z); + +float _wrap_180(float bearing); +float _wrap_360(float bearing); +float _wrap_pi(float bearing); +float _wrap_2pi(float bearing); +float get_mag_declination(float lat, float lon); +#else +#include +#endif //POSIX_SHARED +#endif //GEO_H \ No newline at end of file diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 5cb52924f5..fb00841941 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -40,8 +40,8 @@ */ #include "ekf.h" -#include - +#include "mathlib.h" +#include "geo.h" // GPS pre-flight check bit locations #define MASK_GPS_NSATS (1<<0) #define MASK_GPS_GDOP (1<<1) @@ -63,7 +63,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) // Initialise projection double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; - map_projection_init(&_pos_ref, lat, lon); + map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin _gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2); _NED_origin_initialised = true; @@ -121,7 +121,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE); } else { - map_projection_init(&_pos_ref, lat, lon); + map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); _gps_alt_ref = gps->alt * 1e-3f; } @@ -150,7 +150,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) } // Save current position as the reference for next time - map_projection_init(&_pos_ref, lat, lon); + map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); _last_gps_origin_time_us = _time_last_imu; // Calculate the vertical drift velocity and limit to 10x the threshold diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index e1f7857063..9133998173 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -40,7 +40,7 @@ * */ #include "ekf.h" -#include +#include "mathlib.h" void Ekf::fuseMag() { diff --git a/EKF/mathlib.cpp b/EKF/mathlib.cpp new file mode 100644 index 0000000000..717520e489 --- /dev/null +++ b/EKF/mathlib.cpp @@ -0,0 +1,14 @@ +#include "mathlib.h" + +float math::constrain(float &val, float min, float max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} +float math::radians(float degrees) +{ + return (degrees / 180.0f) * M_PI_F; +} +float math::degrees(float radians) +{ + return (radians * 180.0f) / M_PI_F; +} \ No newline at end of file diff --git a/EKF/mathlib.h b/EKF/mathlib.h new file mode 100644 index 0000000000..3b7baa6b7e --- /dev/null +++ b/EKF/mathlib.h @@ -0,0 +1,19 @@ +#ifndef MATHLIB_H +#define MATHLIB_H +#ifdef POSIX_SHARED +#include +#include +#define M_PI_F 3.14159265358979323846f + +namespace math { + using namespace Eigen; + using namespace std; + + float constrain(float &val, float min, float max); + float radians(float degrees); + float degrees(float radians); +} +#else +#include +#endif //POSIX_SHARED +#endif //MATHLIB_H \ No newline at end of file From 2f05f778ded0b6712cd63da2e15e616280c19dcf Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Feb 2016 17:34:28 -0800 Subject: [PATCH 2/8] ignore build directory --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 99a241d6b8..e5122b0688 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ *.DS_Store +Build/ *~ From cba5271785009add12768d791b5f8d2f916f245e Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Feb 2016 17:38:21 -0800 Subject: [PATCH 3/8] add matrix submodule --- .gitmodules | 2 +- matrix | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) create mode 160000 matrix diff --git a/.gitmodules b/.gitmodules index 86336b59f7..88c937c27f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "Matrix"] +[submodule "matrix"] path = matrix url = https://github.com/PX4/Matrix diff --git a/matrix b/matrix new file mode 160000 index 0000000000..95e3d7d6ce --- /dev/null +++ b/matrix @@ -0,0 +1 @@ +Subproject commit 95e3d7d6ce5274d9b7fec76c0093dc5eaa381abf From 64a7b14171cc8606c1d7d12cb9b06a4a6c8cbc8c Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Wed, 17 Feb 2016 17:46:48 -0800 Subject: [PATCH 4/8] update readme to include steps to build EKF shared object --- README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/README.md b/README.md index 45d31df98b..fdf6c0b97f 100644 --- a/README.md +++ b/README.md @@ -5,3 +5,12 @@ Very lightweight Estimation & Control Library. This library solves the estimation & control problems of a number of robots and drones. It accepts GPS, vision and inertial sensor inputs. It is extremely lightweight and efficient and yet has the rugged field-proven performance. The library is currently BSD licensed, but might move to Apache 2.0. +## Building EKF Library +Prerequisite: + * Eigen3: http://eigen.tuxfamily.org/index.php installed + +By following the steps mentioned below you can create a shared library which can be included in projects using `-l` flag of gcc: + * mkdir Build/ + * cd Build/ + * cmake ../EKF + * make From a40eb7cf3799d251a8fef7a7132b2c4a13a06c11 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Feb 2016 17:50:24 -0800 Subject: [PATCH 5/8] EKF: add check for existence of matrix submodule --- EKF/CMakeLists.txt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index b9355b2ee5..0bfd5a54d5 100644 --- a/EKF/CMakeLists.txt +++ b/EKF/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8) project (ECL CXX) set(CMAKE_BUILD_TYPE Release) -set(CMAKE_CURRENT_SOURCE_DIR EKF) +set(CMAKE_CURRENT_SOURCE_DIR ./) set(CMAKE_CXX_FLAGS "-DPOSIX_SHARED") set (EIGEN3_INCLUDE_DIR "/usr/local/include/eigen3/") @@ -11,6 +11,12 @@ IF( NOT EIGEN3_INCLUDE_DIR ) ENDIF() INCLUDE_DIRECTORIES ( "${EIGEN3_INCLUDE_DIR}" ) +if( NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/../matrix/.git" ) +message( SEND_ERROR "The git submodules are not available. Please run +git submodule update --init --recursive" +) +endif() + include_directories( ./ ../ From e65c3489054241bcde0358ac1a000948f7223274 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Feb 2016 18:04:15 -0800 Subject: [PATCH 6/8] EKF: add travis build --- .travis.yml | 16 ++++++++++++++++ build.sh | 13 +++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 .travis.yml create mode 100755 build.sh diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000000..4832bc6e9d --- /dev/null +++ b/.travis.yml @@ -0,0 +1,16 @@ +language: cpp +sudo: required +compiler: g++ + +before_install: +- sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test +- sudo apt-get update -qq + +install: +- sudo apt-get install -qq g++-4.8 +- export CXX="g++-4.8" + +before_script: + - chmod +x build.sh + +script: ./build.sh \ No newline at end of file diff --git a/build.sh b/build.sh new file mode 100755 index 0000000000..7cd7025af6 --- /dev/null +++ b/build.sh @@ -0,0 +1,13 @@ +wget -O eigen.tar.bz2 http://bitbucket.org/eigen/eigen/get/3.2.8.tar.bz2 +mkdir eigen +tar -xvjf eigen.tar.bz2 -C eigen --strip-components=1 +mkdir eigen-build +cd eigen-build +cmake ../eigen +make +sudo make install +cd .. +mkdir Build +cd Build +cmake ../EKF +make From 80632cc12cd7b988eceff332165e486632fcbe88 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Feb 2016 19:54:12 -0800 Subject: [PATCH 7/8] EKF: add comments and licensing information to builder files --- EKF/CMakeLists.txt | 35 ++++++++++++++++++++++++++++++++++- build.sh | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 70 insertions(+), 1 deletion(-) diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index 0bfd5a54d5..eee1af9037 100644 --- a/EKF/CMakeLists.txt +++ b/EKF/CMakeLists.txt @@ -1,3 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015 ECL Development Team. All rights reserved. +# +# 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 ECL 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. +# +############################################################################ + cmake_minimum_required(VERSION 2.8) project (ECL CXX) @@ -7,7 +40,7 @@ set(CMAKE_CXX_FLAGS "-DPOSIX_SHARED") set (EIGEN3_INCLUDE_DIR "/usr/local/include/eigen3/") IF( NOT EIGEN3_INCLUDE_DIR ) - MESSAGE( FATAL_ERROR "Please point the environment variable EIGEN3_INCLUDE_DIR to the include directory of your Eigen3 installation.") + MESSAGE( FATAL_ERROR "Please point the environment variable EIGEN3_INCLUDE_DIR to the include directory of your Eigen3 installation.") ENDIF() INCLUDE_DIRECTORIES ( "${EIGEN3_INCLUDE_DIR}" ) diff --git a/build.sh b/build.sh index 7cd7025af6..4b45e5592d 100755 --- a/build.sh +++ b/build.sh @@ -1,3 +1,37 @@ +############################################################################ +# +# Copyright (c) 2015 ECL Development Team. All rights reserved. +# +# 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 ECL 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. +# +############################################################################ + +#download, build and install eigen wget -O eigen.tar.bz2 http://bitbucket.org/eigen/eigen/get/3.2.8.tar.bz2 mkdir eigen tar -xvjf eigen.tar.bz2 -C eigen --strip-components=1 @@ -6,6 +40,8 @@ cd eigen-build cmake ../eigen make sudo make install + +#build EKF shared library cd .. mkdir Build cd Build From ba7f0fc9ffc028a8420cfaa1f8cc2b3cdf043e70 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Feb 2016 20:02:08 -0800 Subject: [PATCH 8/8] EKF: add licensing information for mathlib header and src --- EKF/mathlib.cpp | 45 ++++++++++++++++++++++++++++++++++++++++++++- EKF/mathlib.h | 40 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 84 insertions(+), 1 deletion(-) diff --git a/EKF/mathlib.cpp b/EKF/mathlib.cpp index 717520e489..7db1730553 100644 --- a/EKF/mathlib.cpp +++ b/EKF/mathlib.cpp @@ -1,5 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file mathlib.cpp + * + * Definition of math namespace function for POSIX SHARED + * + * @author Siddharth Bharat Purohit + */ #include "mathlib.h" +#ifdef POSIX_SHARED + float math::constrain(float &val, float min, float max) { return (val < min) ? min : ((val > max) ? max : val); @@ -11,4 +53,5 @@ float math::radians(float degrees) float math::degrees(float radians) { return (radians * 180.0f) / M_PI_F; -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/EKF/mathlib.h b/EKF/mathlib.h index 3b7baa6b7e..f20ce65111 100644 --- a/EKF/mathlib.h +++ b/EKF/mathlib.h @@ -1,3 +1,43 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file mathlib.h + * + * Target specific math functions and definitions + * + * @author Siddharth Bharat Purohit + */ #ifndef MATHLIB_H #define MATHLIB_H #ifdef POSIX_SHARED