// return the smoothed gyro vector corrected for drift
const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const
{
if (!using_EKF()) {
return AP_AHRS_DCM::get_gyro();
}
return _gyro_estimate;
}
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const
{
if(!using_EKF()) {
return AP_AHRS_DCM::get_accel_ef_blended();
}
return _accel_ef_ekf_blended;
}
// dead-reckoning support
bool AP_AHRS_NavEKF::get_position(struct Location &loc)
{
if (using_EKF() && EKF.getLLH(loc)) {
return true;
}
return AP_AHRS_DCM::get_position(loc);
}
// return a relative ground position in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
{
if (using_EKF()) {
return EKF.getPosNED(vec);
}
return false;
}
// accelerometer values in the earth frame in m/s/s
const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const
{
if(!using_EKF()) {
return AP_AHRS_DCM::get_accel_ef(i);
}
return _accel_ef_ekf[i];
}
const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const
{
if (!using_EKF()) {
return AP_AHRS_DCM::get_dcm_matrix();
}
return _dcm_matrix;
}
const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
{
if (!using_EKF()) {
return AP_AHRS_DCM::get_gyro_drift();
}
return _gyro_bias;
}
// true if compass is being used
bool AP_AHRS_NavEKF::use_compass(void)
{
if (using_EKF()) {
return EKF.use_compass();
}
return AP_AHRS_DCM::use_compass();
}
// return a ground velocity in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
{
if (using_EKF()) {
EKF.getVelNED(vec);
return true;
}
return false;
}
// return a wind estimation vector, in m/s
Vector3f AP_AHRS_NavEKF::wind_estimate(void)
{
if (!using_EKF()) {
return AP_AHRS_DCM::wind_estimate();
}
Vector3f wind;
EKF.getWind(wind);
return wind;
}
// EKF has a better ground speed vector estimate
Vector2f AP_AHRS_NavEKF::groundspeed_vector(void)
{
if (!using_EKF()) {
return AP_AHRS_DCM::groundspeed_vector();
}
Vector3f vec;
EKF.getVelNED(vec);
return Vector2f(vec.x, vec.y);
}
// return a wind estimation vector, in m/s
Vector3f AP_AHRS_NavEKF::wind_estimate(void)
{
if (!using_EKF()) {
// EKF does not estimate wind speed when there is no airspeed
// sensor active
return AP_AHRS_DCM::wind_estimate();
}
Vector3f wind;
EKF.getWind(wind);
return wind;
}
// dead-reckoning support
bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
{
Vector3f ned_pos;
if (using_EKF() && EKF.getLLH(loc) && EKF.getPosNED(ned_pos)) {
// fixup altitude using relative position from AHRS home, not
// EKF origin
loc.alt = get_home().alt - ned_pos.z*100;
return true;
}
return AP_AHRS_DCM::get_position(loc);
}
float AP_AHRS_NavEKF::get_yaw_for_control_cd(void) const
{
if (!using_EKF()) {
return AP_AHRS_DCM::get_yaw_for_control_cd();
}
float ret = degrees(attitude_for_control.get_euler_yaw())*100.0f;
if (ret < 0) {
ret += 36000;
}
return ret;
}
// return secondary attitude solution if available, as eulers in radians
bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
{
if (using_EKF()) {
// return DCM attitude
eulers = _dcm_attitude;
return true;
}
if (ekf_started) {
// EKF is secondary
EKF.getEulerAngles(eulers);
return true;
}
// no secondary available
return false;
}
// return secondary position solution if available
bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc)
{
if (using_EKF()) {
// return DCM position
AP_AHRS_DCM::get_position(loc);
return true;
}
if (ekf_started) {
// EKF is secondary
EKF.getLLH(loc);
return true;
}
// no secondary available
return false;
}
/*
check if the AHRS subsystem is healthy
*/
bool AP_AHRS_NavEKF::healthy(void) const
{
// If EKF is started we switch away if it reports unhealthy. This could be due to bad
// sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters
// an internal processing error, but not for bad sensor data.
if (_ekf_use != EKF_DO_NOT_USE) {
bool ret = ekf_started && EKF.healthy();
if (!ret) {
return false;
}
if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND) &&
!using_EKF()) {
// on fixed wing we want to be using EKF to be considered
// healthy if EKF is enabled
return false;
}
return true;
}
return AP_AHRS_DCM::healthy();
}
void AP_AHRS_NavEKF::update(void)
{
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
// if we have a compass set and GPS lock we can start the EKF
if (get_compass() && get_gps()->status() >= GPS::GPS_OK_FIX_3D) {
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = true;
EKF.InitialiseFilterDynamic();
}
}
}
if (ekf_started) {
EKF.UpdateFilter();
EKF.getRotationBodyToNED(_dcm_matrix);
if (using_EKF()) {
Vector3f eulers;
EKF.getEulerAngles(eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;
yaw_sensor = degrees(yaw) * 100;
if (yaw_sensor < 0)
yaw_sensor += 36000;
update_trig();
}
}
}
void AP_AHRS_NavEKF::update(void)
{
// we need to restore the old DCM attitude values as these are
// used internally in DCM to calculate error values for gyro drift
// correction
roll = _dcm_attitude.x;
pitch = _dcm_attitude.y;
yaw = _dcm_attitude.z;
update_cd_values();
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
// wait 10 seconds
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = true;
EKF.InitialiseFilterDynamic();
}
}
if (ekf_started) {
EKF.UpdateFilter();
EKF.getRotationBodyToNED(_dcm_matrix);
if (using_EKF()) {
Vector3f eulers;
EKF.getEulerAngles(eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
// keep _gyro_bias for get_gyro_drift()
EKF.getGyroBias(_gyro_bias);
_gyro_bias = -_gyro_bias;
// calculate corrected gryo estimate for get_gyro()
_gyro_estimate.zero();
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i)) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}
}
if (healthy_count > 1) {
_gyro_estimate /= healthy_count;
}
_gyro_estimate += _gyro_bias;
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * _ins.get_accel(i);
}
}
// update _accel_ef_ekf_blended
EKF.getAccelNED(_accel_ef_ekf_blended);
}
}
}
void AP_AHRS_NavEKF::update(void)
{
// we need to restore the old DCM attitude values as these are
// used internally in DCM to calculate error values for gyro drift
// correction
roll = _dcm_attitude.x;
pitch = _dcm_attitude.y;
yaw = _dcm_attitude.z;
update_cd_values();
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
// wait 1 second for DCM to output a valid tilt error estimate
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = EKF.InitialiseFilterDynamic();
}
}
if (ekf_started) {
EKF.UpdateFilter();
EKF.getRotationBodyToNED(_dcm_matrix);
if (using_EKF()) {
Vector3f eulers;
EKF.getEulerAngles(eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
// keep _gyro_bias for get_gyro_drift()
EKF.getGyroBias(_gyro_bias);
_gyro_bias = -_gyro_bias;
// calculate corrected gryo estimate for get_gyro()
_gyro_estimate.zero();
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i)) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}
}
if (healthy_count > 1) {
_gyro_estimate /= healthy_count;
}
_gyro_estimate += _gyro_bias;
float abias1, abias2;
EKF.getAccelZBias(abias1, abias2);
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i==0) {
accel.z -= abias1;
} else if (i==1) {
accel.z -= abias2;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * accel;
}
}
if(_ins.get_accel_health(0) && _ins.get_accel_health(1)) {
float IMU1_weighting;
EKF.getIMU1Weighting(IMU1_weighting);
_accel_ef_ekf_blended = _accel_ef_ekf[0] * IMU1_weighting + _accel_ef_ekf[1] * (1.0f-IMU1_weighting);
} else {
_accel_ef_ekf_blended = _accel_ef_ekf[0];
}
}
}
}
// return true if inertial navigation is active
bool AP_AHRS_NavEKF::have_inertial_nav(void) const
{
return using_EKF();
}
