PX4 Firmware
PX4 Autopilot Software http://px4.io
terrain_estimator.h
Go to the documentation of this file.
1
/****************************************************************************
2
*
3
* Copyright (c) 2015 Roman Bapst. All rights reserved.
4
*
5
* Redistribution and use in source and binary forms, with or without
6
* modification, are permitted provided that the following conditions
7
* are met:
8
*
9
* 1. Redistributions of source code must retain the above copyright
10
* notice, this list of conditions and the following disclaimer.
11
* 2. Redistributions in binary form must reproduce the above copyright
12
* notice, this list of conditions and the following disclaimer in
13
* the documentation and/or other materials provided with the
14
* distribution.
15
* 3. Neither the name PX4 nor the names of its contributors may be
16
* used to endorse or promote products derived from this software
17
* without specific prior written permission.
18
*
19
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30
* POSSIBILITY OF SUCH DAMAGE.
31
*
32
****************************************************************************/
33
34
/**
35
* @file terrain_estimator.h
36
*/
37
38
#pragma once
39
40
#include <
lib/mathlib/mathlib.h
>
41
#include <
matrix/math.hpp
>
42
#include <
uORB/topics/sensor_combined.h
>
43
#include <
uORB/topics/vehicle_gps_position.h
>
44
#include <
uORB/topics/vehicle_attitude.h
>
45
#include <
uORB/topics/distance_sensor.h
>
46
47
48
/*
49
* This class can be used to estimate distance to the ground using a laser range finder.
50
* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose.
51
* The predict(...) function will do a state prediciton based on accelerometer inputs. It also
52
* considers accelerometer bias.
53
* The measurement_update(...) function does a measurement update based on range finder and gps
54
* velocity measurements. Both functions should always be called together when there is new
55
* acceleration data available.
56
* The is_valid() function provides information whether the estimate is valid.
57
*/
58
59
class
__EXPORT
TerrainEstimator
60
{
61
public
:
62
TerrainEstimator
();
63
~
TerrainEstimator
() =
default
;
64
65
bool
is_valid
() {
return
_terrain_valid;}
66
float
get_distance_to_ground
() {
return
-_x(0);}
67
float
get_velocity
() {
return
_x(1);}
68
69
void
predict(
float
dt
,
const
struct
vehicle_attitude_s
*
attitude
,
const
struct
sensor_combined_s
*sensor,
70
const
struct
distance_sensor_s
*distance);
71
void
measurement_update(uint64_t time_ref,
const
struct
vehicle_gps_position_s
*
gps
,
72
const
struct
distance_sensor_s
*distance,
73
const
struct
vehicle_attitude_s
*attitude);
74
75
private
:
76
enum
{n_x = 3};
77
78
float
_distance_last
;
79
bool
_terrain_valid
;
80
81
// kalman filter variables
82
matrix::Vector<float, n_x>
_x
;
// state: ground distance, velocity, accel bias in z direction
83
float
_u_z
;
// acceleration in earth z direction
84
matrix::Matrix<float, 3, 3>
_P
;
// covariance matrix
85
86
// timestamps
87
uint64_t
_time_last_distance
;
88
uint64_t
_time_last_gps
;
89
90
/*
91
struct {
92
float var_acc_z;
93
float var_p_z;
94
float var_p_vz;
95
float var_lidar;
96
float var_gps_vz;
97
} _params;
98
*/
99
100
bool
is_distance_valid(
float
distance);
101
102
};
TerrainEstimator::_time_last_gps
uint64_t _time_last_gps
Definition:
terrain_estimator.h:88
matrix::Vector< float, n_x >
TerrainEstimator::_x
matrix::Vector< float, n_x > _x
Definition:
terrain_estimator.h:82
distance_sensor_s
Definition:
distance_sensor.h:70
vehicle_attitude_s
Definition:
vehicle_attitude.h:51
TerrainEstimator::is_valid
bool is_valid()
Definition:
terrain_estimator.h:65
__EXPORT
Definition:
I2C.hpp:51
TerrainEstimator::_time_last_distance
uint64_t _time_last_distance
Definition:
terrain_estimator.h:87
TerrainEstimator::get_distance_to_ground
float get_distance_to_ground()
Definition:
terrain_estimator.h:66
vehicle_attitude.h
crsf_frame_type_t::gps
crsf_frame_type_t::attitude
TerrainEstimator::_P
matrix::Matrix< float, 3, 3 > _P
Definition:
terrain_estimator.h:84
math.hpp
TerrainEstimator::_u_z
float _u_z
Definition:
terrain_estimator.h:83
vehicle_gps_position.h
TerrainEstimator::_terrain_valid
bool _terrain_valid
Definition:
terrain_estimator.h:79
TerrainEstimator
Definition:
terrain_estimator.h:59
sensor_combined_s
Definition:
sensor_combined.h:52
vehicle_gps_position_s
Definition:
vehicle_gps_position.h:51
TerrainEstimator::_distance_last
float _distance_last
Definition:
terrain_estimator.h:78
mathlib.h
matrix::Matrix< float, 3, 3 >
distance_sensor.h
dt
float dt
Definition:
px4io.c:73
TerrainEstimator::get_velocity
float get_velocity()
Definition:
terrain_estimator.h:67
sensor_combined.h
src
lib
terrain_estimation
terrain_estimator.h
Generated by
1.8.13