fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 12:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib fixes

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-19 14:10:25 +04:00
parent e3a5a384d7
commit ba612c3ee8
22 changed files with 513 additions and 1191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/external_lgpl/tecs/tecs.cpp
+3 -3
View File
@@ -29,7 +29,7 @@ using namespace math;
*
*/
void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
{
// Implement third order complementary filter for height and height rate
// estimted height rate = _integ2_state
@@ -257,7 +257,7 @@ void TECS::_update_energies(void)
_SKEdot = _integ5_state * _vel_dot;
}
void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat)
{
// Calculate total energy values
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
@@ -468,7 +468,7 @@ void TECS::_update_STE_rate_lim(void)
_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
}
void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
float throttle_min, float throttle_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max)
{
src/lib/external_lgpl/tecs/tecs.h
+3 -3
View File
@@ -71,10 +71,10 @@ public:
// Update of the estimated height and height rate internal state
// Update of the inertial speed rate internal state
// Should be called at 50Hz or greater
void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
// Update the control loop calculations
void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
float throttle_min, float throttle_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max);
// demanded throttle in percentage
@@ -338,7 +338,7 @@ private:
void _update_energies(void);
// Update Demanded Throttle
void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat);
// Detect Bad Descent
void _detect_bad_descent(void);