fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 20:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Disable mTECS until runtime error is better understood

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-30 17:41:45 +02:00
parent fa2e3bc902
commit 1dc23d0c49
2 changed files with 29 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+28 -28
View File
@@ -194,7 +194,7 @@ private:
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
// fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
struct {
@@ -443,7 +443,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_groundspeed_undershoot(0.0f),
_global_pos_valid(false),
_l1_control(),
_mTecs(),
// _mTecs(),
_was_pos_control_mode(false)
{
_nav_capabilities.turn_distance = 0.0f;
@@ -603,7 +603,7 @@ FixedwingPositionControl::parameters_update()
launchDetector.updateParams();
/* Update the mTecs */
_mTecs.updateParams();
// _mTecs.updateParams();
return OK;
}
@@ -820,9 +820,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
if (!_mTecs.getEnabled()) {
// if (!_mTecs.getEnabled()) {
_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
// }
/* define altitude error */
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
@@ -838,10 +838,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (!_was_pos_control_mode) {
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
// if (_mTecs.getEnabled()) {
// _mTecs.resetIntegrators();
// _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
// }
}
_was_pos_control_mode = true;
@@ -1160,9 +1160,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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.getEnabled() ? _mTecs.getThrottleSetpoint() : */_tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
_att_sp.pitch_body = /*_mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : */_tecs.get_pitch_demand();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1342,29 +1342,29 @@ 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);
} else {
// 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);
// } 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);
}
// }
}
int
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+1
View File
@@ -76,6 +76,7 @@ mTecs::mTecs() :
_counter(0),
_debug(false)
{
warnx("starting");
}
mTecs::~mTecs()