Yuri De Stefani / CrazyflieController_final

programa final

VerticalEstimator/VerticalEstimator.cpp@5:b1f5ea192d12, 2018-10-17 (annotated)

Committer:
yurindes
Date:
Wed Oct 17 12:13:33 2018 +0000
Revision:
5:b1f5ea192d12
Child:
6:7a447d4ae677
yuri

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yurindes 5:b1f5ea192d12 1 #include "mbed.h"
yurindes 5:b1f5ea192d12 2 #include "VerticalEstimator.h"
yurindes 5:b1f5ea192d12 3 // Class constructor
yurindes 5:b1f5ea192d12 4 VerticalEstimator :: VerticalEstimator() : range (PB_7 , PB_6 )
yurindes 5:b1f5ea192d12 5 {
yurindes 5:b1f5ea192d12 6 z=0;
yurindes 5:b1f5ea192d12 7 w=0;
yurindes 5:b1f5ea192d12 8 z_m_last=0;
yurindes 5:b1f5ea192d12 9 }
yurindes 5:b1f5ea192d12 10 // Initialize class
yurindes 5:b1f5ea192d12 11 void VerticalEstimator :: init()
yurindes 5:b1f5ea192d12 12 {
yurindes 5:b1f5ea192d12 13 range.init();
yurindes 5:b1f5ea192d12 14 }
yurindes 5:b1f5ea192d12 15 // Predict vertical position and velocity from model
yurindes 5:b1f5ea192d12 16 void VerticalEstimator :: predict()
yurindes 5:b1f5ea192d12 17 {
yurindes 5:b1f5ea192d12 18 z=z+0.05f*w;
yurindes 5:b1f5ea192d12 19 w=w;
yurindes 5:b1f5ea192d12 20 }
yurindes 5:b1f5ea192d12 21 // Correct vertical position and velocity with measurement
yurindes 5:b1f5ea192d12 22 void VerticalEstimator :: correct ( float phi , float theta )
yurindes 5:b1f5ea192d12 23 {
yurindes 5:b1f5ea192d12 24 p=0.5f;
yurindes 5:b1f5ea192d12 25 range.read;
yurindes 5:b1f5ea192d12 26 float z_m = range.z;
yurindes 5:b1f5ea192d12 27 float w_m = (z-z_m)/0.05f;
yurindes 5:b1f5ea192d12 28 z=(1-p)*z+p*z_m;
yurindes 5:b1f5ea192d12 29 w=(1-p)*w+p*w_m;
yurindes 5:b1f5ea192d12 30 z_m_last = z_m;
yurindes 5:b1f5ea192d12 31 }