ardupilot/libraries/AP_NavEKF/AP_NavEKF_Source.h

129 lines
3.9 KiB
C++

#pragma once
#include <AP_Param/AP_Param.h>
#define AP_NAKEKF_SOURCE_SET_MAX 3 // three sets of sources
class AP_NavEKF_Source
{
public:
// Constructor
AP_NavEKF_Source();
/* Do not allow copies */
AP_NavEKF_Source(const AP_NavEKF_Source &other) = delete;
AP_NavEKF_Source &operator=(const AP_NavEKF_Source&) = delete;
enum class SourceXY {
NONE = 0,
// BARO = 1 (not applicable)
// RANGEFINDER = 2 (not applicable)
GPS = 3,
BEACON = 4,
OPTFLOW = 5,
EXTNAV = 6,
WHEEL_ENCODER = 7
};
enum class SourceZ {
NONE = 0,
BARO = 1,
RANGEFINDER = 2,
GPS = 3,
BEACON = 4,
// OPTFLOW = 5 (not applicable, optical flow can be used for terrain alt but not relative or absolute alt)
EXTNAV = 6
// WHEEL_ENCODER = 7 (not applicable)
};
enum class SourceYaw {
NONE = 0,
COMPASS = 1,
EXTERNAL = 2,
EXTERNAL_COMPASS_FALLBACK = 3
};
// enum for OPTIONS parameter
enum class SourceOptions {
FUSE_ALL_VELOCITIES = (1 << 0) // fuse all velocities configured in source sets
};
// initialisation
void init();
// get current position source
SourceXY getPosXYSource() const { return _active_source_set.posxy; }
SourceZ getPosZSource() const { return _active_source_set.posz; }
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void setPosVelYawSourceSet(uint8_t source_set_idx);
// get/set velocity source
SourceXY getVelXYSource() const { return _active_source_set.velxy; }
SourceZ getVelZSource() const { return _active_source_set.velz; }
void setVelZSource(SourceZ source) { _active_source_set.velz = source; }
// true/false of whether velocity source should be used
bool useVelXYSource(SourceXY velxy_source) const;
bool useVelZSource(SourceZ velz_source) const;
// true if a velocity source is configured
bool haveVelZSource() const;
// get yaw source
SourceYaw getYawSource() const { return _active_source_set.yaw; }
// align position of inactive sources to ahrs
void align_inactive_sources();
// sensor-specific helper functions
// true if any source is GPS
bool usingGPS() const;
// true if source parameters have been configured (used for parameter conversion)
bool configured_in_storage();
// mark parameters as configured in storage (used to ensure parameter conversion is only done once)
void mark_configured_in_storage();
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
static const struct AP_Param::GroupInfo var_info[];
// return true if ext nav is enabled on any source
bool ext_nav_enabled(void) const;
// return true if ext yaw is enabled on any source
bool ext_yaw_enabled(void) const;
// return true if wheel encoder is enabled on any source
bool wheel_encoder_enabled(void) const;
private:
// Parameters
struct SourceSet {
AP_Int8 posxy; // xy position source
AP_Int8 velxy; // xy velocity source
AP_Int8 posz; // position z (aka altitude or height) source
AP_Int8 velz; // velocity z source
AP_Int8 yaw; // yaw source
} _source_set[AP_NAKEKF_SOURCE_SET_MAX];
AP_Int16 _options; // source options bitmask
// active sources
struct {
SourceXY posxy; // current xy position source
SourceZ posz; // current z position source
SourceXY velxy; // current xy velocity source
SourceZ velz; // current z velocity source
SourceYaw yaw; // current yaw source
} _active_source_set;
bool initialised; // true once init has been run
bool config_in_storage; // true once configured in storage has returned true
};