forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 1
/
AP_InertialNav_NavEKF.h
122 lines (102 loc) · 3.4 KB
/
AP_InertialNav_NavEKF.h
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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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
*/
#ifndef __AP_INERTIALNAV_NAVEKF_H__
#define __AP_INERTIALNAV_NAVEKF_H__
#include <AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
class AP_InertialNav_NavEKF : public AP_InertialNav
{
public:
// Constructor
AP_InertialNav_NavEKF(AP_AHRS_NavEKF &ahrs) :
AP_InertialNav(),
_haveabspos(false),
_ahrs_ekf(ahrs)
{}
/**
update internal state
*/
void update(float dt);
/**
* get_filter_status - returns filter status as a series of flags
*/
nav_filter_status get_filter_status() const;
/**
* get_origin - returns the inertial navigation origin in lat/lon/alt
*
* @return origin Location
*/
struct Location get_origin() const;
/**
* get_position - returns the current position relative to the home location in cm.
*
* the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t)
*
* @return
*/
const Vector3f& get_position() const;
/**
* get_llh - updates the provided location with the latest calculated location including absolute altitude
* returns true on success (i.e. the EKF knows it's latest position), false on failure
*/
bool get_location(struct Location &loc) const;
/**
* get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
*/
int32_t get_latitude() const;
/**
* get_longitude - returns the longitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
* @return
*/
int32_t get_longitude() const;
/**
* 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& get_velocity() const;
/**
* get_velocity_xy - returns the current horizontal velocity in cm/s
*
* @returns the current horizontal velocity in cm/s
*/
float get_velocity_xy() const;
/**
* get_altitude - get latest altitude estimate in cm
* @return
*/
float get_altitude() const;
/**
* getHgtAboveGnd - get latest altitude estimate above ground level in centimetres and validity flag
* @return
*/
bool get_hagl(float &hagl) const;
/**
* get_hgt_ctrl_limit - get maximum height to be observed by the control loops in cm and a validity flag
* this is used to limit height during optical flow navigation
* it will return invalid when no limiting is required
* @return
*/
bool get_hgt_ctrl_limit(float& limit) const;
/**
* get_velocity_z - returns the current climbrate.
*
* @see get_velocity().z
*
* @return climbrate in cm/s
*/
float get_velocity_z() const;
private:
Vector3f _relpos_cm; // NEU
Vector3f _velocity_cm; // NEU
struct Location _abspos;
bool _haveabspos;
AP_AHRS_NavEKF &_ahrs_ekf;
};
#endif // __AP_INERTIALNAV_NAVEKF_H__