2014-01-03 07:57:50 -04:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
*/
|
2016-02-17 21:25:30 -04:00
|
|
|
#pragma once
|
2014-01-03 07:57:50 -04:00
|
|
|
|
2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
|
2015-01-02 04:06:22 -04:00
|
|
|
|
2021-10-18 23:15:00 -03:00
|
|
|
class AP_InertialNav
|
2014-01-03 07:57:50 -04:00
|
|
|
{
|
|
|
|
public:
|
|
|
|
// Constructor
|
2024-08-30 23:01:02 -03:00
|
|
|
AP_InertialNav(class AP_AHRS &ahrs) :
|
2014-10-09 04:30:20 -03:00
|
|
|
_ahrs_ekf(ahrs)
|
2015-03-12 10:22:54 -03:00
|
|
|
{}
|
2014-01-03 07:57:50 -04:00
|
|
|
|
2014-01-03 20:16:07 -04:00
|
|
|
/**
|
|
|
|
update internal state
|
|
|
|
*/
|
2021-10-18 23:15:00 -03:00
|
|
|
void update(bool high_vibes = false);
|
2014-01-03 20:16:07 -04:00
|
|
|
|
2014-01-03 07:57:50 -04:00
|
|
|
/**
|
2015-01-02 04:06:22 -04:00
|
|
|
* get_filter_status - returns filter status as a series of flags
|
2014-01-03 07:57:50 -04:00
|
|
|
*/
|
2021-10-18 23:15:00 -03:00
|
|
|
nav_filter_status get_filter_status() const;
|
2014-01-03 07:57:50 -04:00
|
|
|
|
|
|
|
/**
|
2022-02-02 01:26:14 -04:00
|
|
|
* get_position_neu_cm - returns the current position relative to the EKF origin in cm.
|
2014-01-03 07:57:50 -04:00
|
|
|
*
|
|
|
|
* @return
|
|
|
|
*/
|
2021-10-20 05:33:14 -03:00
|
|
|
const Vector3f& get_position_neu_cm() const;
|
2014-01-03 07:57:50 -04:00
|
|
|
|
2021-09-11 05:40:29 -03:00
|
|
|
/**
|
2022-02-02 01:26:14 -04:00
|
|
|
* get_position_xy_cm - returns the current x-y position relative to the EKF origin in cm.
|
2021-09-11 05:40:29 -03:00
|
|
|
*
|
|
|
|
* @return
|
|
|
|
*/
|
2021-10-20 05:33:14 -03:00
|
|
|
const Vector2f& get_position_xy_cm() const;
|
2021-09-11 05:40:29 -03:00
|
|
|
|
2014-01-03 07:57:50 -04:00
|
|
|
/**
|
2022-02-02 01:26:14 -04:00
|
|
|
* get_position_z_up_cm - returns the current z position relative to the EKF origin, frame z up, in cm.
|
2021-10-20 05:33:14 -03:00
|
|
|
* @return
|
|
|
|
*/
|
|
|
|
float get_position_z_up_cm() const;
|
|
|
|
|
|
|
|
/**
|
|
|
|
* get_velocity_neu_cms - returns the current velocity in cm/s
|
2014-01-03 07:57:50 -04:00
|
|
|
*
|
|
|
|
* @return velocity vector:
|
|
|
|
* .x : latitude velocity in cm/s
|
|
|
|
* .y : longitude velocity in cm/s
|
|
|
|
* .z : vertical velocity in cm/s
|
|
|
|
*/
|
2021-10-20 05:33:14 -03:00
|
|
|
const Vector3f& get_velocity_neu_cms() const;
|
2014-01-03 07:57:50 -04:00
|
|
|
|
2021-09-11 05:40:29 -03:00
|
|
|
/**
|
2022-02-02 01:26:14 -04:00
|
|
|
* get_velocity_xy_cms - returns the current x-y velocity relative to the EKF origin in cm.
|
2021-09-11 05:40:29 -03:00
|
|
|
*
|
|
|
|
* @return
|
|
|
|
*/
|
2021-10-20 05:33:14 -03:00
|
|
|
const Vector2f& get_velocity_xy_cms() const;
|
2021-09-11 05:40:29 -03:00
|
|
|
|
2014-01-03 07:57:50 -04:00
|
|
|
/**
|
2021-10-20 05:33:14 -03:00
|
|
|
* get_speed_xy_cms - returns the current horizontal speed in cm/s
|
2014-01-03 07:57:50 -04:00
|
|
|
*
|
2017-10-13 00:42:49 -03:00
|
|
|
* @returns the current horizontal speed in cm/s
|
2014-01-03 07:57:50 -04:00
|
|
|
*/
|
2021-10-20 05:33:14 -03:00
|
|
|
float get_speed_xy_cms() const;
|
2014-01-03 07:57:50 -04:00
|
|
|
|
|
|
|
/**
|
2021-10-20 05:33:14 -03:00
|
|
|
* get_velocity_z_up_cms - returns the current z-axis velocity, frame z-axis up, in cm/s
|
2014-01-03 07:57:50 -04:00
|
|
|
*
|
2021-10-20 05:33:14 -03:00
|
|
|
* @return z-axis velocity, frame z-axis up, in cm/s
|
2014-01-03 07:57:50 -04:00
|
|
|
*/
|
2021-10-20 05:33:14 -03:00
|
|
|
float get_velocity_z_up_cms() const;
|
2014-01-03 20:16:07 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
Vector3f _relpos_cm; // NEU
|
|
|
|
Vector3f _velocity_cm; // NEU
|
2021-07-20 09:16:30 -03:00
|
|
|
AP_AHRS &_ahrs_ekf;
|
2014-01-03 07:57:50 -04:00
|
|
|
};
|