Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
Diff: HorizontaEstimator/HorizontalEstimator.cpp
- Revision:
- 15:155ca63b7884
- Parent:
- 12:9fed6f656f88
- Child:
- 17:f682b4a5686d
diff -r 7439203a9e44 -r 155ca63b7884 HorizontaEstimator/HorizontalEstimator.cpp --- a/HorizontaEstimator/HorizontalEstimator.cpp Wed Oct 03 19:39:48 2018 +0000 +++ b/HorizontaEstimator/HorizontalEstimator.cpp Wed Oct 03 22:12:38 2018 +0000 @@ -4,6 +4,8 @@ // Class constructor HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4) { + u = 0.0f; + v = 0.0f; } // Initialize class @@ -25,9 +27,13 @@ { // flow.read(); - float d = z/(cos(phi)*cos(theta)); - float u_m = (flow.x+q)*d; - float v_m = (flow.y-p)*d; - u = (1-rho_hor)*u+rho_hor*u_m; - v = (1-rho_hor)*v+rho_hor*v_m; + float div = (cos(phi)*cos(theta)); + if (div>0.5f) + { + float d = z/div; + float u_m = (flow.x+q)*d; + float v_m = (flow.y-p)*d; + u = (1-rho_hor)*u+rho_hor*u_m; + v = (1-rho_hor)*v+rho_hor*v_m; + } } \ No newline at end of file