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

Checked in lift compensation based on Dcm from Paul Riseborough

Browse Source
This commit is contained in:
Lorenz Meier
2013-07-14 09:44:58 +02:00
parent 6901402853
commit 0b12efc840
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp
+2 -2
View File
@@ -83,8 +83,8 @@ void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_sc
// Establish actuator signs and lift compensation
float lift_sign = (R_bn(3, 3) >= 0) ? 1.0f : -1.0f;
// XXX temporary workaround
float lift_comp = fabsf((CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min)) * tanf(roll) * sinf(roll))) * _p_roll_ffd;
float lift_comp = CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min) * (R_nb(2,3) * R_nb(2,3)) / (R_nb(3,3) * sqrtf((R_nb(2,3) * R_nb(2,3)) + (R_nb(3,3) * R_nb(3,3)));
//float lift_comp = fabsf((CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min)) * tanf(roll) * sinf(roll))) * _p_roll_ffd;
float cy = cosf(yaw);
float sy = sinf(yaw);