From d45cc69d1d62a2ccac00757ab02a731fca6d6ece Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 27 Jun 2014 21:44:21 +0200 Subject: mtecs/wind: store wind variance --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 93ed18b8d..272ad5344 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1345,8 +1345,8 @@ FixedwingEstimator::task_main() _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { -- cgit v1.2.3 From 503ded05394767d58359834e73bc63672b701dbe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Jun 2014 12:35:04 +0200 Subject: Remove old TECS implementation - we can really only decently flight-test and support one. --- makefiles/config_aerocore_default.mk | 1 - makefiles/config_px4fmu-v1_default.mk | 1 - makefiles/config_px4fmu-v2_default.mk | 1 - .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 72 +++++----------------- 4 files changed, 16 insertions(+), 59 deletions(-) (limited to 'src') diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index d6902c4ee..fd112b32d 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -80,7 +80,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion MODULES += lib/launchdetection diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e4bc6fecf..f943f62f2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -107,7 +107,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion MODULES += lib/launchdetection diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a68cc32f4..83dd85adb 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -120,7 +120,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion MODULES += lib/launchdetection diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5df17e2ec..f2c5db806 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -43,8 +43,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Original implementation for total energy control class: - * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * Implementation for total energy control class: + * Thomas Gubler * * More details and acknowledgements in the referenced library headers. * @@ -88,7 +88,6 @@ #include #include #include -#include #include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -201,7 +200,6 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; - TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -568,23 +566,6 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); - _tecs.set_time_const(_parameters.time_const); - _tecs.set_min_sink_rate(_parameters.min_sink_rate); - _tecs.set_max_sink_rate(_parameters.max_sink_rate); - _tecs.set_throttle_damp(_parameters.throttle_damp); - _tecs.set_integrator_gain(_parameters.integrator_gain); - _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); - _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); - _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); - _tecs.set_speed_weight(_parameters.speed_weight); - _tecs.set_pitch_damping(_parameters.pitch_damping); - _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); - _tecs.set_max_climb_rate(_parameters.max_climb_rate); - _tecs.set_heightrate_p(_parameters.heightrate_p); - _tecs.set_speedrate_p(_parameters.speedrate_p); - /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -656,9 +637,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - return airspeed_updated; } @@ -839,10 +817,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { - _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - } - float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -867,9 +841,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); - /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ - _tecs.set_speed_weight(_parameters.speed_weight); - /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -1222,8 +1193,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* user switched off throttle */ if (_manual.z < 0.1f) { throttle_max = 0.0f; - /* switch to pure pitch based altitude control, give up speed */ - _tecs.set_speed_weight(0.0f); } /* climb out control */ @@ -1263,9 +1232,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getPitchSetpoint(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1449,29 +1418,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - if (_mTecs.getEnabled()) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - } else { - limitOverride.disablePitchMinOverride(); - } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); } else { - /* Using tecs library */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, - climbout_mode, climbout_pitch_min_rad, - throttle_min, throttle_max, throttle_cruise, - pitch_min_rad, pitch_max_rad); + limitOverride.disablePitchMinOverride(); } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } int -- cgit v1.2.3 From 2bd5511d096c3351bf74c81072e2cc00b344ef79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Jun 2014 12:37:26 +0200 Subject: geo lookup lib: Moved to separate module and compiling with -Os to save some precious flash --- src/lib/geo/geo.h | 2 +- src/lib/geo/geo_mag_declination.c | 136 ------------------------------- src/lib/geo/geo_mag_declination.h | 47 ----------- src/lib/geo/module.mk | 5 +- src/lib/geo_lookup/geo_mag_declination.c | 136 +++++++++++++++++++++++++++++++ src/lib/geo_lookup/geo_mag_declination.h | 47 +++++++++++ src/lib/geo_lookup/module.mk | 40 +++++++++ 7 files changed, 226 insertions(+), 187 deletions(-) delete mode 100644 src/lib/geo/geo_mag_declination.c delete mode 100644 src/lib/geo/geo_mag_declination.h create mode 100644 src/lib/geo_lookup/geo_mag_declination.c create mode 100644 src/lib/geo_lookup/geo_mag_declination.h create mode 100644 src/lib/geo_lookup/module.mk (limited to 'src') diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 87c1cf460..8b286af36 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -50,7 +50,7 @@ __BEGIN_DECLS -#include "geo_mag_declination.h" +#include "geo_lookup/geo_mag_declination.h" #include diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c deleted file mode 100644 index 09eac38f4..000000000 --- a/src/lib/geo/geo_mag_declination.c +++ /dev/null @@ -1,136 +0,0 @@ -/**************************************************************************** - * - * 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 - -/** 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); - -__EXPORT 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]; -} \ No newline at end of file diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo/geo_mag_declination.h deleted file mode 100644 index 0ac062d6d..000000000 --- a/src/lib/geo/geo_mag_declination.h +++ /dev/null @@ -1,47 +0,0 @@ -/**************************************************************************** - * - * 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 - -__EXPORT float get_mag_declination(float lat, float lon); - -__END_DECLS diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk index 9500a2bcc..d08ca4532 100644 --- a/src/lib/geo/module.mk +++ b/src/lib/geo/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,5 +35,4 @@ # Geo library # -SRCS = geo.c \ - geo_mag_declination.c +SRCS = geo.c diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c new file mode 100644 index 000000000..09eac38f4 --- /dev/null +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 + +/** 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); + +__EXPORT 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]; +} \ No newline at end of file diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h new file mode 100644 index 000000000..0ac062d6d --- /dev/null +++ b/src/lib/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 + +__EXPORT float get_mag_declination(float lat, float lon); + +__END_DECLS diff --git a/src/lib/geo_lookup/module.mk b/src/lib/geo_lookup/module.mk new file mode 100644 index 000000000..d7a10df2d --- /dev/null +++ b/src/lib/geo_lookup/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Geo lookup table / data library +# + +SRCS = geo_mag_declination.c + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 13f9ce9ddd0f6ab4b8fd4f7be3234e6989da8ea9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Jun 2014 12:38:31 +0200 Subject: Comment fix in perf counter header, no code changes. --- src/modules/systemlib/perf_counter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 6835ee4a2..668d9dfdf 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -75,7 +75,7 @@ __EXPORT extern void perf_free(perf_counter_t handle); /** * Count a performance event. * - * This call only affects counters that take single events; PC_COUNT etc. + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. * * @param handle The handle returned from perf_alloc. */ -- cgit v1.2.3