dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

move geo and geo_lookup from PX4 Firmware to ECL

This commit is contained in:
Daniel Agar 2018-03-22 14:10:30 -04:00
parent 1bb4c17c0e
commit cd12f049fe
25 changed files with 392 additions and 343 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
CMakeLists.txt
View File

@ -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 :

5
EKF/CMakeLists.txt
View File

@ -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

2
EKF/airspeed_fusion.cpp
View File

@ -42,7 +42,7 @@
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <mathlib/mathlib.h>
void Ekf::fuseAirspeed()
{

2
EKF/common.h
View File

@ -40,6 +40,8 @@
*
*/
#include <matrix/matrix/math.hpp>
namespace estimator
{

2
EKF/control.cpp
View File

@ -41,7 +41,7 @@
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <mathlib/mathlib.h>
void Ekf::controlFusionModes()
{

8
EKF/covariance.cpp
View File

@ -40,10 +40,11 @@
*
*/
#include "../ecl.h"
#include "ekf.h"
#include <ecl.h>
#include <math.h>
#include "mathlib.h"
#include <mathlib/mathlib.h>
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

5
EKF/drag_fusion.cpp
View File

@ -38,9 +38,10 @@
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
void Ekf::fuseDrag()
{

9
EKF/ekf.cpp
View File

@ -39,9 +39,10 @@
* @author Paul Riseborough <p_riseborough@live.com.au>
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
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) {

2
EKF/ekf.h
View File

@ -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)

5
EKF/ekf_helper.cpp
View File

@ -39,9 +39,10 @@
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
#include <cstdlib>
// Reset the velocity states. If we have a recent and valid

5
EKF/estimator_interface.cpp
View File

@ -42,9 +42,8 @@
#include "estimator_interface.h"
#include "../ecl.h"
#include <math.h>
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
// 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,

11
EKF/estimator_interface.h
View File

@ -39,11 +39,12 @@
*
*/
#include <matrix/matrix/math.hpp>
#include "RingBuffer.h"
#include "geo.h"
#include "common.h"
#include "mathlib.h"
#include "RingBuffer.h"
#include <geo/geo.h>
#include <matrix/matrix/math.hpp>
#include <mathlib/mathlib.h>
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

7
EKF/gps_checks.cpp
View File

@ -39,10 +39,11 @@
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include "geo.h"
#include <ecl.h>
#include <geo_lookup/geo_mag_declination.h>
#include <mathlib/mathlib.h>
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)

5
EKF/mag_fusion.cpp
View File

@ -39,9 +39,10 @@
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
void Ekf::fuseMag()
{

3
EKF/optflow_fusion.cpp
View File

@ -41,7 +41,8 @@
*/
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
void Ekf::fuseOptFlow()
{

5
EKF/sideslip_fusion.cpp
View File

@ -39,9 +39,10 @@
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
void Ekf::fuseSideslip()
{

3
EKF/terrain_estimator.cpp
View File

@ -40,7 +40,8 @@
*/
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
bool Ekf::initHagl()
{

3
EKF/vel_pos_fusion.cpp
View File

@ -42,7 +42,8 @@
*/
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
void Ekf::fuseVelPosHeight()
{

2
ecl.h
View File

@ -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

335
EKF/geo.cpp → geo/geo.cpp
View File

@ -41,155 +41,19 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifdef POSIX_SHARED
#include "ecl.h"
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
/****************************************************************************
*
* 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 <scottfromscott@gmail.com>
*
* XXX Lookup table currently too coarse in resolution (only full degrees)
* and lat/lon res - needs extension medium term.
*
*/
#include <stdint.h>
#include "geo.h"
#include <ecl.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];
}
#include <mathlib/mathlib.h>
#include <cfloat>
/*
* 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 <douglas.weibel@colorado.edu>
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

68
EKF/geo.h → geo/geo.h
View File

@ -42,23 +42,18 @@
* @author Anton Babushkin <anton.babushkin@me.com>
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/
#ifndef GEO_H
#define GEO_H
#ifdef POSIX_SHARED
#pragma once
#include <stdbool.h>
#include "mathlib.h"
#include <stdint.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 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 <lib/geo/geo.h>
#endif //POSIX_SHARED
#endif //GEO_H
__END_DECLS

135
geo_lookup/geo_mag_declination.cpp Normal file
View File

@ -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 <scottfromscott@gmail.com> and
* Stephan Brown <stephan.brown.07@gmail.com>
*
* XXX Lookup table currently too coarse in resolution (only full degrees)
* and lat/lon res - needs extension medium term.
*
*/
#include <stdint.h>
#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];
}

47
geo_lookup/geo_mag_declination.h Normal file
View File

@ -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

0
EKF/mathlib.cpp → mathlib/mathlib.cpp
View File

6
EKF/mathlib.h → mathlib/mathlib.h
View File

@ -43,12 +43,18 @@
#ifdef POSIX_SHARED
// #include <Eigen/Dense>
// #include <algorithm>
#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;