forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/navigator_rewrite' into navigator_rewrite_estimator
Conflicts: src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
This commit is contained in:
commit
f0a21c4f5f
|
@ -80,8 +80,8 @@ LIBRARIES += lib/mathlib/CMSIS
|
||||||
MODULES += lib/mathlib
|
MODULES += lib/mathlib
|
||||||
MODULES += lib/mathlib/math/filter
|
MODULES += lib/mathlib/math/filter
|
||||||
MODULES += lib/ecl
|
MODULES += lib/ecl
|
||||||
MODULES += lib/external_lgpl
|
|
||||||
MODULES += lib/geo
|
MODULES += lib/geo
|
||||||
|
MODULES += lib/geo_lookup
|
||||||
MODULES += lib/conversion
|
MODULES += lib/conversion
|
||||||
MODULES += lib/launchdetection
|
MODULES += lib/launchdetection
|
||||||
|
|
||||||
|
|
|
@ -107,8 +107,8 @@ LIBRARIES += lib/mathlib/CMSIS
|
||||||
MODULES += lib/mathlib
|
MODULES += lib/mathlib
|
||||||
MODULES += lib/mathlib/math/filter
|
MODULES += lib/mathlib/math/filter
|
||||||
MODULES += lib/ecl
|
MODULES += lib/ecl
|
||||||
MODULES += lib/external_lgpl
|
|
||||||
MODULES += lib/geo
|
MODULES += lib/geo
|
||||||
|
MODULES += lib/geo_lookup
|
||||||
MODULES += lib/conversion
|
MODULES += lib/conversion
|
||||||
MODULES += lib/launchdetection
|
MODULES += lib/launchdetection
|
||||||
|
|
||||||
|
|
|
@ -120,8 +120,8 @@ LIBRARIES += lib/mathlib/CMSIS
|
||||||
MODULES += lib/mathlib
|
MODULES += lib/mathlib
|
||||||
MODULES += lib/mathlib/math/filter
|
MODULES += lib/mathlib/math/filter
|
||||||
MODULES += lib/ecl
|
MODULES += lib/ecl
|
||||||
MODULES += lib/external_lgpl
|
|
||||||
MODULES += lib/geo
|
MODULES += lib/geo
|
||||||
|
MODULES += lib/geo_lookup
|
||||||
MODULES += lib/conversion
|
MODULES += lib/conversion
|
||||||
MODULES += lib/launchdetection
|
MODULES += lib/launchdetection
|
||||||
|
|
||||||
|
|
|
@ -50,7 +50,7 @@
|
||||||
|
|
||||||
__BEGIN_DECLS
|
__BEGIN_DECLS
|
||||||
|
|
||||||
#include "geo_mag_declination.h"
|
#include "geo_lookup/geo_mag_declination.h"
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
|
|
@ -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
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
@ -35,5 +35,4 @@
|
||||||
# Geo library
|
# Geo library
|
||||||
#
|
#
|
||||||
|
|
||||||
SRCS = geo.c \
|
SRCS = geo.c
|
||||||
geo_mag_declination.c
|
|
||||||
|
|
|
@ -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
|
|
@ -1494,6 +1494,23 @@ FixedwingEstimator::task_main()
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||||
|
_wind.timestamp = _global_pos.timestamp;
|
||||||
|
_wind.windspeed_north = _ekf->states[14];
|
||||||
|
_wind.windspeed_east = _ekf->states[15];
|
||||||
|
_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) {
|
||||||
|
/* publish the wind estimate */
|
||||||
|
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
/* advertise and publish */
|
||||||
|
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -43,8 +43,8 @@
|
||||||
* Proceedings of the AIAA Guidance, Navigation and Control
|
* Proceedings of the AIAA Guidance, Navigation and Control
|
||||||
* Conference, Aug 2004. AIAA-2004-4900.
|
* Conference, Aug 2004. AIAA-2004-4900.
|
||||||
*
|
*
|
||||||
* Original implementation for total energy control class:
|
* Implementation for total energy control class:
|
||||||
* Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
|
* Thomas Gubler
|
||||||
*
|
*
|
||||||
* More details and acknowledgements in the referenced library headers.
|
* More details and acknowledgements in the referenced library headers.
|
||||||
*
|
*
|
||||||
|
@ -88,7 +88,6 @@
|
||||||
#include <mavlink/mavlink_log.h>
|
#include <mavlink/mavlink_log.h>
|
||||||
#include <launchdetection/LaunchDetector.h>
|
#include <launchdetection/LaunchDetector.h>
|
||||||
#include <ecl/l1/ecl_l1_pos_controller.h>
|
#include <ecl/l1/ecl_l1_pos_controller.h>
|
||||||
#include <external_lgpl/tecs/tecs.h>
|
|
||||||
#include <drivers/drv_range_finder.h>
|
#include <drivers/drv_range_finder.h>
|
||||||
#include "landingslope.h"
|
#include "landingslope.h"
|
||||||
#include "mtecs/mTecs.h"
|
#include "mtecs/mTecs.h"
|
||||||
|
@ -199,7 +198,6 @@ private:
|
||||||
math::Matrix<3, 3> _R_nb; ///< current attitude
|
math::Matrix<3, 3> _R_nb; ///< current attitude
|
||||||
|
|
||||||
ECL_L1_Pos_Controller _l1_control;
|
ECL_L1_Pos_Controller _l1_control;
|
||||||
TECS _tecs;
|
|
||||||
fwPosctrl::mTecs _mTecs;
|
fwPosctrl::mTecs _mTecs;
|
||||||
bool _was_pos_control_mode;
|
bool _was_pos_control_mode;
|
||||||
|
|
||||||
|
@ -565,23 +563,6 @@ FixedwingPositionControl::parameters_update()
|
||||||
_l1_control.set_l1_period(_parameters.l1_period);
|
_l1_control.set_l1_period(_parameters.l1_period);
|
||||||
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
|
_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 */
|
/* sanity check parameters */
|
||||||
if (_parameters.airspeed_max < _parameters.airspeed_min ||
|
if (_parameters.airspeed_max < _parameters.airspeed_min ||
|
||||||
_parameters.airspeed_max < 5.0f ||
|
_parameters.airspeed_max < 5.0f ||
|
||||||
|
@ -653,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* update TECS state */
|
|
||||||
_tecs.enable_airspeed(_airspeed_valid);
|
|
||||||
|
|
||||||
return airspeed_updated;
|
return airspeed_updated;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -835,10 +813,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
|
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
|
||||||
math::Vector<3> accel_earth = _R_nb * accel_body;
|
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;
|
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
|
||||||
|
|
||||||
/* no throttle limit as default */
|
/* no throttle limit as default */
|
||||||
|
@ -863,9 +837,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
/* get circle mode */
|
/* get circle mode */
|
||||||
bool was_circle_mode = _l1_control.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) */
|
/* current waypoint (the one currently heading for) */
|
||||||
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
|
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
|
||||||
|
|
||||||
|
@ -1228,8 +1199,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
/* user switched off throttle */
|
/* user switched off throttle */
|
||||||
if (_manual.z < 0.1f) {
|
if (_manual.z < 0.1f) {
|
||||||
throttle_max = 0.0f;
|
throttle_max = 0.0f;
|
||||||
/* switch to pure pitch based altitude control, give up speed */
|
|
||||||
_tecs.set_speed_weight(0.0f);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* climb out control */
|
/* climb out control */
|
||||||
|
@ -1269,9 +1238,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
|
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
|
||||||
}
|
}
|
||||||
else {
|
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) {
|
if (_control_mode.flag_control_position_enabled) {
|
||||||
last_manual = false;
|
last_manual = false;
|
||||||
|
@ -1455,29 +1424,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
|
||||||
const math::Vector<3> &ground_speed,
|
const math::Vector<3> &ground_speed,
|
||||||
tecs_mode mode)
|
tecs_mode mode)
|
||||||
{
|
{
|
||||||
if (_mTecs.getEnabled()) {
|
/* Using mtecs library: prepare arguments for mtecs call */
|
||||||
/* Using mtecs library: prepare arguments for mtecs call */
|
float flightPathAngle = 0.0f;
|
||||||
float flightPathAngle = 0.0f;
|
float ground_speed_length = ground_speed.length();
|
||||||
float ground_speed_length = ground_speed.length();
|
if (ground_speed_length > FLT_EPSILON) {
|
||||||
if (ground_speed_length > FLT_EPSILON) {
|
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
|
||||||
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);
|
|
||||||
} 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);
|
|
||||||
}
|
}
|
||||||
|
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);
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
|
|
@ -75,7 +75,7 @@ __EXPORT extern void perf_free(perf_counter_t handle);
|
||||||
/**
|
/**
|
||||||
* Count a performance event.
|
* 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.
|
* @param handle The handle returned from perf_alloc.
|
||||||
*/
|
*/
|
||||||
|
|
Loading…
Reference in New Issue