diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-28 17:20:44 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-28 17:20:44 +0200 |
commit | 045ee8c7c7c4618a8a03f9c9342f53d2fc7333c9 (patch) | |
tree | e04acc5d91d2ff1cb36d97370e3897d7caf9f9ea /src/modules | |
parent | cc8f7f4c97de923f60f9469aa2847e6e1474d52d (diff) | |
parent | 7b15a424f06074c37ade650c5fd2661495b06e74 (diff) | |
download | px4-firmware-045ee8c7c7c4618a8a03f9c9342f53d2fc7333c9.tar.gz px4-firmware-045ee8c7c7c4618a8a03f9c9342f53d2fc7333c9.tar.bz2 px4-firmware-045ee8c7c7c4618a8a03f9c9342f53d2fc7333c9.zip |
Merge remote-tracking branch 'upstream/mtecs' into navigator_rewrite
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 72 | ||||
-rw-r--r-- | src/modules/systemlib/perf_counter.h | 2 |
3 files changed, 19 insertions, 59 deletions
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 abe272e56..ac99e658d 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 @@ -1368,8 +1368,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) { 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 49d812e7e..000c02e3d 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 <mavlink/mavlink_log.h> #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> -#include <external_lgpl/tecs/tecs.h> #include <drivers/drv_range_finder.h> #include "landingslope.h" #include "mtecs/mTecs.h" @@ -199,7 +198,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; @@ -565,23 +563,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 || @@ -653,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - 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_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 */ @@ -863,9 +837,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); @@ -1228,8 +1199,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 */ @@ -1269,9 +1238,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; @@ -1455,29 +1424,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 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. */ |