From cd12f049fe4d5455235321c48079ea7c089f8d84 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 22 Mar 2018 14:10:30 -0400 Subject: [PATCH] move geo and geo_lookup from PX4 Firmware to ECL --- CMakeLists.txt | 60 +++--- EKF/CMakeLists.txt | 5 +- EKF/airspeed_fusion.cpp | 2 +- EKF/common.h | 2 + EKF/control.cpp | 2 +- EKF/covariance.cpp | 8 +- EKF/drag_fusion.cpp | 5 +- EKF/ekf.cpp | 9 +- EKF/ekf.h | 2 - EKF/ekf_helper.cpp | 5 +- EKF/estimator_interface.cpp | 5 +- EKF/estimator_interface.h | 11 +- EKF/gps_checks.cpp | 7 +- EKF/mag_fusion.cpp | 5 +- EKF/optflow_fusion.cpp | 3 +- EKF/sideslip_fusion.cpp | 5 +- EKF/terrain_estimator.cpp | 3 +- EKF/vel_pos_fusion.cpp | 3 +- ecl.h | 2 + {EKF => geo}/geo.cpp | 335 +++++++++-------------------- {EKF => geo}/geo.h | 68 +++--- geo_lookup/geo_mag_declination.cpp | 135 ++++++++++++ geo_lookup/geo_mag_declination.h | 47 ++++ {EKF => mathlib}/mathlib.cpp | 0 {EKF => mathlib}/mathlib.h | 6 + 25 files changed, 392 insertions(+), 343 deletions(-) rename {EKF => geo}/geo.cpp (58%) rename {EKF => geo}/geo.h (90%) create mode 100644 geo_lookup/geo_mag_declination.cpp create mode 100644 geo_lookup/geo_mag_declination.h rename {EKF => mathlib}/mathlib.cpp (100%) rename {EKF => mathlib}/mathlib.h (96%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 93a686c2ba..626f2d6e42 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,37 +30,31 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -px4_add_module( - MODULE lib__ecl - STACK_MAIN 6000 - STACK_MAX 6000 - COMPILE_FLAGS - SRCS - airdata/WindEstimator.cpp - attitude_fw/ecl_controller.cpp - attitude_fw/ecl_pitch_controller.cpp - attitude_fw/ecl_roll_controller.cpp - attitude_fw/ecl_wheel_controller.cpp - attitude_fw/ecl_yaw_controller.cpp - EKF/airspeed_fusion.cpp - EKF/control.cpp - EKF/covariance.cpp - EKF/ekf.cpp - EKF/ekf_helper.cpp - EKF/estimator_interface.cpp - EKF/gps_checks.cpp - EKF/mag_fusion.cpp - EKF/optflow_fusion.cpp - EKF/sideslip_fusion.cpp - EKF/terrain_estimator.cpp - EKF/vel_pos_fusion.cpp - EKF/drag_fusion.cpp - l1/ecl_l1_pos_controller.cpp - tecs/tecs.cpp - validation/data_validator.cpp - validation/data_validator_group.cpp - DEPENDS - platforms__common +include_directories(.) +px4_add_library(ecl + airdata/WindEstimator.cpp + attitude_fw/ecl_controller.cpp + attitude_fw/ecl_pitch_controller.cpp + attitude_fw/ecl_roll_controller.cpp + attitude_fw/ecl_wheel_controller.cpp + attitude_fw/ecl_yaw_controller.cpp + EKF/airspeed_fusion.cpp + EKF/control.cpp + EKF/covariance.cpp + EKF/drag_fusion.cpp + EKF/ekf.cpp + EKF/ekf_helper.cpp + EKF/estimator_interface.cpp + EKF/gps_checks.cpp + EKF/mag_fusion.cpp + EKF/optflow_fusion.cpp + EKF/sideslip_fusion.cpp + EKF/terrain_estimator.cpp + EKF/vel_pos_fusion.cpp + geo/geo.cpp + geo_lookup/geo_mag_declination.cpp + l1/ecl_l1_pos_controller.cpp + tecs/tecs.cpp + validation/data_validator.cpp + validation/data_validator_group.cpp ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index 91429ac640..207febef4e 100644 --- a/EKF/CMakeLists.txt +++ b/EKF/CMakeLists.txt @@ -49,6 +49,9 @@ include_directories( ) set(SRCS + ../geo/geo.cpp + ../geo_lookup/geo_mag_declination.cpp + ../mathlib/mathlib.cpp airspeed_fusion.cpp control.cpp covariance.cpp @@ -56,10 +59,8 @@ set(SRCS ekf.cpp ekf_helper.cpp estimator_interface.cpp - geo.cpp gps_checks.cpp mag_fusion.cpp - mathlib.cpp optflow_fusion.cpp sideslip_fusion.cpp terrain_estimator.cpp diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index fb9d53dee3..a8b127bc27 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -42,7 +42,7 @@ */ #include "../ecl.h" #include "ekf.h" -#include "mathlib.h" +#include void Ekf::fuseAirspeed() { diff --git a/EKF/common.h b/EKF/common.h index 3eadc087fc..29181cbb51 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -40,6 +40,8 @@ * */ +#include + namespace estimator { diff --git a/EKF/control.cpp b/EKF/control.cpp index 133a460b29..02dc2c67c4 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -41,7 +41,7 @@ #include "../ecl.h" #include "ekf.h" -#include "mathlib.h" +#include void Ekf::controlFusionModes() { diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 2f7c06ef5c..647aca9b7b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -40,10 +40,11 @@ * */ -#include "../ecl.h" #include "ekf.h" + +#include #include -#include "mathlib.h" +#include void Ekf::initialiseCovariance() { @@ -753,8 +754,7 @@ void Ekf::fixCovarianceErrors() float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { - P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, - sq(_gravity_mss * _dt_ekf_avg)); + P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(CONSTANTS_ONE_G * _dt_ekf_avg)); } // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index a7daf26525..3e744e5f3d 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -38,9 +38,10 @@ * @author Paul Riseborough * */ -#include "../ecl.h" + #include "ekf.h" -#include "mathlib.h" +#include +#include void Ekf::fuseDrag() { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 6da8f2b767..bedad0f263 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -39,9 +39,10 @@ * @author Paul Riseborough */ -#include "../ecl.h" #include "ekf.h" -#include "mathlib.h" + +#include +#include bool Ekf::init(uint64_t timestamp) { @@ -359,7 +360,7 @@ void Ekf::predictState() _state.vel += corrected_delta_vel_ef; // compensate for acceleration due to gravity - _state.vel(2) += _gravity_mss * _imu_sample_delayed.delta_vel_dt; + _state.vel(2) += CONSTANTS_ONE_G * _imu_sample_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity _state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f; @@ -485,7 +486,7 @@ void Ekf::calculateOutputStates() Vector3f delta_vel_NED = _R_to_earth_now * delta_vel; // corrrect for measured accceleration due to gravity - delta_vel_NED(2) += _gravity_mss * imu_new.delta_vel_dt; + delta_vel_NED(2) += CONSTANTS_ONE_G * imu_new.delta_vel_dt; // calculate the earth frame velocity derivatives if (imu_new.delta_vel_dt > 1e-4f) { diff --git a/EKF/ekf.h b/EKF/ekf.h index 6e223a6bfe..34979c3983 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -41,7 +41,6 @@ */ #include "estimator_interface.h" -#include "geo.h" class Ekf : public EstimatorInterface { @@ -236,7 +235,6 @@ private: static constexpr uint8_t _k_num_states{24}; ///< number of EKF states static constexpr float _k_earth_rate{0.000072921f}; ///< earth spin rate (rad/sec) - static constexpr float _gravity_mss{9.80665f}; ///< average earth gravity at sea level (m/sec**2) struct { uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a33b5ea7fd..716d0290af 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -39,9 +39,10 @@ * */ -#include "../ecl.h" #include "ekf.h" -#include "mathlib.h" + +#include +#include #include // Reset the velocity states. If we have a recent and valid diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 049ea86a16..5dbbec9108 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -42,9 +42,8 @@ #include "estimator_interface.h" -#include "../ecl.h" -#include -#include "mathlib.h" +#include +#include // Accumulate imu data and store to buffer at desired rate void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index eefdee15bb..26f119d9f5 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -39,11 +39,12 @@ * */ -#include -#include "RingBuffer.h" -#include "geo.h" #include "common.h" -#include "mathlib.h" +#include "RingBuffer.h" + +#include +#include +#include using namespace estimator; @@ -416,7 +417,7 @@ protected: // Used by the multi-rotor specific drag force fusion uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) - float _air_density{1.225f}; // air density (kg/m**3) + float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) // Output Predictor outputSample _output_sample_delayed{}; // filter output on the delayed time horizon diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index e43f17dc49..015393d22b 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -39,10 +39,11 @@ * */ -#include "../ecl.h" #include "ekf.h" -#include "mathlib.h" -#include "geo.h" + +#include +#include +#include // GPS pre-flight check bit locations #define MASK_GPS_NSATS (1<<0) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 632bda0c67..7606a47e3c 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -39,9 +39,10 @@ * @author Paul Riseborough * */ -#include "../ecl.h" + #include "ekf.h" -#include "mathlib.h" +#include +#include void Ekf::fuseMag() { diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index df0dd3163d..eaae6a91d3 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -41,7 +41,8 @@ */ #include "ekf.h" -#include "mathlib.h" +#include +#include void Ekf::fuseOptFlow() { diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index a3eed52e35..2a0c69f863 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -39,9 +39,10 @@ * @author Paul Riseborough * */ -#include "../ecl.h" + #include "ekf.h" -#include "mathlib.h" +#include +#include void Ekf::fuseSideslip() { diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index b54fd9c6ff..82918eeea3 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -40,7 +40,8 @@ */ #include "ekf.h" -#include "mathlib.h" +#include +#include bool Ekf::initHagl() { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 86a640131f..c7ac014604 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -42,7 +42,8 @@ */ #include "ekf.h" -#include "mathlib.h" +#include +#include void Ekf::fuseVelPosHeight() { diff --git a/ecl.h b/ecl.h index cf9c32572b..0f87d43392 100644 --- a/ecl.h +++ b/ecl.h @@ -51,6 +51,8 @@ #else +#define ecl_absolute_time() (0) +#define ecl_elapsed_time (0) #define ECL_INFO printf #define ECL_WARN printf #define ECL_ERR printf diff --git a/EKF/geo.cpp b/geo/geo.cpp similarity index 58% rename from EKF/geo.cpp rename to geo/geo.cpp index 7fca38888c..c2ab3cee82 100644 --- a/EKF/geo.cpp +++ b/geo/geo.cpp @@ -41,155 +41,19 @@ * @author Lorenz Meier * @author Anton Babushkin */ -#ifdef POSIX_SHARED -#include "ecl.h" - -#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 #include "geo.h" +#include -/** 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]; -} +#include +#include /* * 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 map_projection_reference_s mp_ref = {}; static struct globallocal_converter_reference_s gl_ref = {0.0f, false}; bool map_projection_global_initialized() @@ -212,18 +76,18 @@ 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 +// lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) { 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 +// lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) { - ref->lat_rad = lat_0 * M_DEG_TO_RAD; - ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->lat_rad = math::radians(lat_0); + ref->lon_rad = math::radians(lon_0); ref->sin_lat = sin(ref->lat_rad); ref->cos_lat = cos(ref->lat_rad); @@ -233,13 +97,18 @@ int map_projection_init_timestamped(struct map_projection_reference_s *ref, doub return 0; } +//lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) +{ + return map_projection_init_timestamped(ref, lat_0, lon_0, ecl_absolute_time()); +} + 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) +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; @@ -254,18 +123,16 @@ int map_projection_reference(const struct map_projection_reference_s *ref, doubl 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) +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 lat_rad = math::radians(lat); + double lon_rad = math::radians(lon); double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); @@ -294,15 +161,14 @@ 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) +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 x_rad = (double)x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = (double)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); @@ -332,11 +198,11 @@ int map_projection_global_getref(double *lat_0, double *lon_0) } if (lat_0 != nullptr) { - *lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad; + *lat_0 = math::degrees(mp_ref.lat_rad); } if (lon_0 != nullptr) { - *lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad; + *lon_0 = math::degrees(mp_ref.lon_rad); } return 0; @@ -421,7 +287,7 @@ float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_n } 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) + double *lat_target, double *lon_target) { if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; @@ -439,28 +305,28 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B } void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, - double *lat_target, double *lon_target) + double *lat_target, double *lon_target) { bearing = _wrap_2pi(bearing); - double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); + double radius_ratio = fabs((double)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; + double lat_start_rad = math::radians(lat_start); + double lon_start_rad = math::radians(lon_start); - *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( - double)bearing)); + *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; + *lat_target = math::degrees(*lat_target); + *lon_target = math::degrees(*lon_target); } + 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 lat_now_rad = math::radians(lat_now); + double lon_now_rad = math::radians(lon_now); + double lat_next_rad = math::radians(lat_next); + double lon_next_rad = math::radians(lon_next); double d_lon = lon_next_rad - lon_now_rad; @@ -473,13 +339,13 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne 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) +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 lat_now_rad = math::radians(lat_now); + double lon_now_rad = math::radians(lon_now); + double lat_next_rad = math::radians(lat_next); + double lon_next_rad = math::radians(lon_next); double d_lon = lon_next_rad - lon_now_rad; @@ -489,13 +355,13 @@ void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next *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) +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 lat_now_rad = math::radians(lat_now); + double lon_now_rad = math::radians(lon_now); + double lat_next_rad = math::radians(lat_next); + double lon_next_rad = math::radians(lon_next); double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; @@ -505,73 +371,64 @@ void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double 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) +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; + double lat_now_rad = math::radians(lat_now); + double lon_now_rad = math::radians(lon_now); - *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; + *lat_res = math::degrees(lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH); + *lon_res = math::degrees(lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))); } // 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) + 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. + // 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. + int return_value = -1; // 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); + float 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; + return -1; } - 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); + float bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + float bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); + float bearing_diff = _wrap_pi(bearing_track - bearing_end); // 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_value = 0; return return_value; } crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); - if (sin(bearing_diff) >= 0) { + if (sinf(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_value = 0; 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) + 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 @@ -579,42 +436,49 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_ // 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; + 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. + int return_value = -1; // 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 (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; } + if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= (2 * M_PI_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; } + if (bearing_sector_start < 0.0f) { bearing_sector_start += (2 * M_PI_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; } + && 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; } + || bearing_now < bearing_sector_end)) { + + in_sector = true; + } // If in the sector then calculate distance and bearing to closest point if (in_sector) { @@ -639,10 +503,10 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_ // 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 start_disp_x = (double)radius * sin((double)arc_start_bearing); + double start_disp_y = (double)radius * cos((double)arc_start_bearing); + double end_disp_x = (double)radius * sin((double)_wrap_pi((double)(arc_start_bearing + arc_sweep))); + double end_disp_y = (double)radius * cos((double)_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; @@ -660,11 +524,10 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_ 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_value = 0; return return_value; } @@ -692,7 +555,6 @@ float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float a 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) @@ -717,7 +579,7 @@ float _wrap_pi(float bearing) int c = 0; while (bearing >= M_PI_F) { - bearing -= M_TWOPI_F; + bearing -= (2 * M_PI_F); if (c++ > 3) { return NAN; @@ -727,7 +589,7 @@ float _wrap_pi(float bearing) c = 0; while (bearing < -M_PI_F) { - bearing += M_TWOPI_F; + bearing += (2 * M_PI_F); if (c++ > 3) { return NAN; @@ -746,8 +608,8 @@ float _wrap_2pi(float bearing) int c = 0; - while (bearing >= M_TWOPI_F) { - bearing -= M_TWOPI_F; + while (bearing >= (2 * M_PI_F)) { + bearing -= (2 * M_PI_F); if (c++ > 3) { return NAN; @@ -757,7 +619,7 @@ float _wrap_2pi(float bearing) c = 0; while (bearing < 0.0f) { - bearing += M_TWOPI_F; + bearing += (2 * M_PI_F); if (c++ > 3) { return NAN; @@ -826,4 +688,3 @@ float _wrap_360(float bearing) return bearing; } -#endif //POSIX_SHARED \ No newline at end of file diff --git a/EKF/geo.h b/geo/geo.h similarity index 90% rename from EKF/geo.h rename to geo/geo.h index a4f6ca2c51..dd5069fb7b 100644 --- a/EKF/geo.h +++ b/geo/geo.h @@ -42,23 +42,18 @@ * @author Anton Babushkin * Additional functions - @author Doug Weibel */ -#ifndef GEO_H -#define GEO_H -#ifdef POSIX_SHARED + +#pragma once + #include -#include "mathlib.h" +#include #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 57.29577951308232087679f -#define M_DEG_TO_RAD 0.01745329251994329576f -#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 @@ -68,12 +63,12 @@ struct crosstrack_error_s { /* lat/lon are in radians */ struct map_projection_reference_s { + uint64_t timestamp; double lat_rad; double lon_rad; double sin_lat; double cos_lat; bool init_done; - uint64_t timestamp; }; struct globallocal_converter_reference_s { @@ -81,6 +76,8 @@ struct globallocal_converter_reference_s { bool init_done; }; +__BEGIN_DECLS + /** * Checks if global projection was initialized * @return true if map was initialized before, false else @@ -115,9 +112,17 @@ 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); +int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad); +/** + * Initializes the global map transformation. + * + * 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_global_init(double lat_0, double lon_0, uint64_t timestamp); /** * Initializes the map transformation given by the argument. @@ -127,8 +132,7 @@ int map_projection_reference(const struct map_projection_reference_s *ref, doubl * @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); +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. @@ -151,7 +155,6 @@ int map_projection_init(struct map_projection_reference_s *ref, double lat_0, do */ 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 @@ -160,8 +163,7 @@ int map_projection_global_project(double lat, double lon, float *x, float *y); * @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); +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 @@ -185,8 +187,7 @@ int map_projection_global_reproject(float x, float y, double *lat, double *lon); * @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); +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 @@ -229,7 +230,6 @@ int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0); */ 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 @@ -243,7 +243,7 @@ float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_n * @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); + double *lat_target, double *lon_target); /** * Creates a waypoint from given waypoint, distance and bearing @@ -257,7 +257,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B * @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); + double *lat_target, double *lon_target); /** * Returns the bearing to the next waypoint in radians. @@ -269,21 +269,18 @@ void waypoint_from_heading_and_distance(double lat_start, double lon_start, floa */ 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(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 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); +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); + 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); + double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); /* * Calculate distance in global frame @@ -303,8 +300,5 @@ 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 + +__END_DECLS diff --git a/geo_lookup/geo_mag_declination.cpp b/geo_lookup/geo_mag_declination.cpp new file mode 100644 index 0000000000..c5514341b2 --- /dev/null +++ b/geo_lookup/geo_mag_declination.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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's magnetic field declination. +* +* Lookup table from Scott Ferguson and +* Stephan Brown +* +* XXX Lookup table currently too coarse in resolution (only full degrees) +* and lat/lon res - needs extension medium term. +* +*/ + +#include +#include "geo_mag_declination.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 + +#define constrain(val, min, max) (val < min) ? min : ((val > max) ? max : val) + +static const int8_t declination_table[13][37] = \ +{ + { 47, 45, 44, 43, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -41, -48, -55, -61, -67, -71, -74, -75, -72, -61, -23, 23, 41, 46, 47, 47 }, + { 30, 30, 30, 30, 30, 29, 29, 29, 27, 23, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -46, -51, -55, -57, -56, -52, -44, -31, -14, 1, 13, 21, 26, 29, 30 }, + { 22, 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, -39, -31, -21, -11, -3, 3, 9, 14, 18, 20, 22 }, + { 16, 17, 17, 17, 17, 16, 16, 16, 15, 13, 8, 0, -9, -17, -22, -24, -25, -24, -22, -20, -21, -24, -29, -31, -31, -28, -23, -16, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -13, -19, -23, -24, -24, -21, -17, -12, -9, -10, -14, -17, -18, -16, -12, -8, -3, 0, 1, 3, 5, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 8, 5, 0, -7, -15, -20, -22, -22, -19, -15, -10, -5, -2, -1, -4, -7, -8, -8, -6, -3, 0, 0, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 3, -2, -9, -15, -19, -20, -17, -13, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 2, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 7, 5, 1, -3, -10, -15, -17, -17, -14, -10, -5, -2, 0, 1, 2, 2, 0, -1, -1, -1, -1, 0, 0, 0, 0, 0, 3, 5, 7, 8 }, + { 8, 8, 9, 9, 10, 10, 9, 8, 5, 0, -5, -11, -15, -16, -15, -11, -7, -3, -1, 0, 2, 3, 3, 1, 0, 0, 0, 0, 0, -1, -2, -3, -2, 0, 3, 6, 8 }, + { 6, 8, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -6, -3, 0, 1, 3, 4, 4, 3, 2, 1, 1, 0, -1, -3, -5, -6, -5, -3, 0, 3, 6 }, + { 5, 8, 11, 13, 14, 15, 13, 10, 5, -1, -9, -14, -16, -16, -13, -10, -6, -3, 0, 2, 3, 5, 5, 5, 5, 4, 3, 1, -1, -4, -7, -9, -8, -6, -2, 1, 5 }, + { 3, 8, 12, 15, 17, 17, 16, 12, 5, -3, -12, -18, -20, -19, -16, -12, -8, -4, 0, 2, 4, 7, 8, 9, 10, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 3 }, + { 3, 8, 13, 16, 19, 20, 19, 14, 4, -7, -18, -24, -26, -24, -21, -16, -11, -6, -2, 2, 6, 10, 13, 15, 17, 16, 13, 7, 0, -7, -13, -15, -14, -11, -6, -1, 3 }, +}; + +static float get_lookup_table_val(unsigned lat, unsigned lon); +static unsigned get_lookup_table_index(float *val, float min, float max); + +unsigned get_lookup_table_index(float *val, float min, float max) +{ + /* 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 (*val < min) { + *val = min; + } + + /* limit to (table bounds - 1) because bilinear interpolation requires checking (index + 1) */ + if (*val > max) { + *val = max - SAMPLING_RES; + } + + return (-(min) + *val) / SAMPLING_RES; +} + +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 */ + float min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; + float min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; + + /* find index of nearest low sampling point */ + unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT); + unsigned min_lon_index = get_lookup_table_index(&min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON); + + 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 lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.0f, 1.0f); + float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.0f, 1.0f); + + float declination_min = lon_scale * (declination_se - declination_sw) + declination_sw; + float declination_max = lon_scale * (declination_ne - declination_nw) + declination_nw; + + return lat_scale * (declination_max - declination_min) + declination_min; +} + +float get_lookup_table_val(unsigned lat_index, unsigned lon_index) +{ + return declination_table[lat_index][lon_index]; +} diff --git a/geo_lookup/geo_mag_declination.h b/geo_lookup/geo_mag_declination.h new file mode 100644 index 0000000000..c799e05bf9 --- /dev/null +++ b/geo_lookup/geo_mag_declination.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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.h +* +* Calculation / lookup table for earth magnetic field declination. +* +*/ + +#pragma once + +__BEGIN_DECLS + +float get_mag_declination(float lat, float lon); + +__END_DECLS diff --git a/EKF/mathlib.cpp b/mathlib/mathlib.cpp similarity index 100% rename from EKF/mathlib.cpp rename to mathlib/mathlib.cpp diff --git a/EKF/mathlib.h b/mathlib/mathlib.h similarity index 96% rename from EKF/mathlib.h rename to mathlib/mathlib.h index f84fedfb9c..4656e874c1 100644 --- a/EKF/mathlib.h +++ b/mathlib/mathlib.h @@ -43,12 +43,18 @@ #ifdef POSIX_SHARED // #include // #include +#ifndef M_PI_F #define M_PI_F 3.14159265358979323846f +#endif #ifndef M_PI #define M_PI (3.14159265358979323846f) #endif +#ifndef M_PI_2_F +#define M_PI_2_F (M_PI / 2.0f) +#endif + namespace math { // using namespace Eigen;