fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 17:54:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Condition TECS properly on any altitude control mode

Browse Source
This commit is contained in:
Lorenz Meier 2015-07-27 10:16:48 +02:00
parent 09da389558
commit 75085dc5d6
1 changed files with 14 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1037,7 +1037,7 @@ bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
{
float dt = FLT_MIN; // Using non zero value to a avoid division by zero
float dt = 0.01; // Using non zero value to a avoid division by zero
if (_control_position_last_called > 0) {
dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
}
@ -1045,20 +1045,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
bool setpoint = true;
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
/* update TECS filters, but only if in auto mode. */
if (_control_mode.flag_control_auto_enabled && (_global_pos.timestamp > 0) && !_mTecs.getEnabled() && !_vehicle_status.condition_landed) {
_tecs.update_50hz(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
/* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_vehicle_status.condition_landed &&
(_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled));
/* update TECS filters */
if (!_mTecs.getEnabled()) {
_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
}
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
/* define altitude error */
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;