controlador de atitude
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?
User | Revision | Line number | New 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 | } |