diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-03-27 22:57:29 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-03-27 22:57:29 +0100 |
commit | 3e9dfcb6f7bbda7653e6d8873b6273f2e9299d73 (patch) | |
tree | e441d825ebda02767a301e7c15f042abc30decdf /src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | |
parent | d3ca12f136c9a890f7289d69bd783840dc86cfba (diff) | |
download | px4-firmware-3e9dfcb6f7bbda7653e6d8873b6273f2e9299d73.tar.gz px4-firmware-3e9dfcb6f7bbda7653e6d8873b6273f2e9299d73.tar.bz2 px4-firmware-3e9dfcb6f7bbda7653e6d8873b6273f2e9299d73.zip |
mtecs: first rough version of takeoff mode
Diffstat (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 |
1 files changed, 9 insertions, 2 deletions
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 1e8447aed..44afa43d8 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 @@ -361,7 +361,7 @@ private: void reset_landing_state(); /* - * Call TECS + * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, @@ -1364,7 +1364,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp); + + if (!climbout_mode) { + /* Normal operation */ + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_NORMAL); + } else { + /* Climbout/Takeoff mode requested */ + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_TAKEOFF); + } } else { _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, |