forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
AP_InertialNav_NavEKF.cpp
106 lines (96 loc) · 2.59 KB
/
AP_InertialNav_NavEKF.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
#include <AP_HAL/AP_HAL.h>
#include <AP_Baro/AP_Baro.h>
#include "AP_InertialNav_NavEKF.h"
/*
A wrapper around the AP_InertialNav class which uses the NavEKF
filter if available, and falls back to the AP_InertialNav filter
when EKF is not available
*/
/**
update internal state
*/
void AP_InertialNav_NavEKF::update(bool high_vibes)
{
// get the NE position relative to the local earth frame origin
Vector2f posNE;
if (_ahrs_ekf.get_relative_position_NE_origin(posNE)) {
_relpos_cm.x = posNE.x * 100; // convert from m to cm
_relpos_cm.y = posNE.y * 100; // convert from m to cm
}
// get the D position relative to the local earth frame origin
float posD;
if (_ahrs_ekf.get_relative_position_D_origin(posD)) {
_relpos_cm.z = - posD * 100; // convert from m in NED to cm in NEU
}
// get the velocity relative to the local earth frame
Vector3f velNED;
if (_ahrs_ekf.get_velocity_NED(velNED)) {
// during high vibration events use vertical position change
if (high_vibes) {
float rate_z;
if (_ahrs_ekf.get_vert_pos_rate(rate_z)) {
velNED.z = rate_z;
}
}
_velocity_cm = velNED * 100; // convert to cm/s
_velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
}
}
/**
* get_filter_status : returns filter status as a series of flags
*/
nav_filter_status AP_InertialNav_NavEKF::get_filter_status() const
{
nav_filter_status status;
_ahrs_ekf.get_filter_status(status);
return status;
}
/**
* get_position - returns the current position relative to the home location in cm.
*
* @return
*/
const Vector3f &AP_InertialNav_NavEKF::get_position(void) const
{
return _relpos_cm;
}
/**
* get_velocity - returns the current velocity in cm/s
*
* @return velocity vector:
* .x : latitude velocity in cm/s
* .y : longitude velocity in cm/s
* .z : vertical velocity in cm/s
*/
const Vector3f &AP_InertialNav_NavEKF::get_velocity() const
{
return _velocity_cm;
}
/**
* get_speed_xy - returns the current horizontal speed in cm/s
*
* @returns the current horizontal speed in cm/s
*/
float AP_InertialNav_NavEKF::get_speed_xy() const
{
return _velocity_cm.xy().length();
}
/**
* get_altitude - get latest altitude estimate in cm
* @return
*/
float AP_InertialNav_NavEKF::get_altitude() const
{
return _relpos_cm.z;
}
/**
* get_velocity_z - returns the current climbrate.
*
* @see get_velocity().z
*
* @return climbrate in cm/s
*/
float AP_InertialNav_NavEKF::get_velocity_z() const
{
return _velocity_cm.z;
}