diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-14 09:44:58 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-14 09:44:58 +0200 |
| commit | 0b12efc8400519724a2884e20bbb44d8caf7629d (patch) | |
| tree | c8eda622ffbbd473cf98e4c6162ef043ec633735 /src/modules | |
| parent | 6901402853e213f75530b7a46505da2b1c5e8d1d (diff) | |
| download | px4-firmware-0b12efc8400519724a2884e20bbb44d8caf7629d.tar.gz px4-firmware-0b12efc8400519724a2884e20bbb44d8caf7629d.tar.bz2 px4-firmware-0b12efc8400519724a2884e20bbb44d8caf7629d.zip | |
Checked in lift compensation based on Dcm from Paul Riseborough
Diffstat (limited to 'src/modules')
| -rw-r--r-- | src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp | 4 |
1 files changed, 2 insertions, 2 deletions
diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp index 0f1a355d7..b72edfefa 100644 --- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp +++ b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp @@ -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); |