bool Ekf::init(uint64_t timestamp)
{
bool ret = initialise_interface(timestamp);
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = 1.0f;
_state.gyro_scale(1) = 1.0f;
_state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
_state.quat_nominal.setZero();
_state.quat_nominal(0) = 1.0f;
_output_new.vel.setZero();
_output_new.pos.setZero();
_output_new.quat_nominal = matrix::Quaternion<float>();
_delta_angle_corr.setZero();
_delta_vel_corr.setZero();
_vel_corr.setZero();
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
_imu_down_sampled.delta_vel_dt = 0.0f;
_imu_down_sampled.time_us = timestamp;
_q_down_sampled(0) = 1.0f;
_q_down_sampled(1) = 0.0f;
_q_down_sampled(2) = 0.0f;
_q_down_sampled(3) = 0.0f;
_imu_updated = false;
_NED_origin_initialised = false;
_gps_speed_valid = false;
_mag_healthy = false;
_filter_initialised = false;
_terrain_initialised = false;
_control_status.value = 0;
_control_status_prev.value = 0;
return ret;
}
//=============================================================================
bool LocationModule::query(double& latitude, double& longitude, double& speed)
{
if (initialise_interface()) {
// Attempt to get a fix.
if (m_interface->waiting(m_timeout_us)) {
const struct gps_data_t* data = m_interface->read();
if (NULL != data) {
if (data->set & MODE_SET) {
const gps_fix_t fix = data->fix;
switch (fix.mode) {
case MODE_NOT_SEEN:
DEBUG_LOG("MODE_NOT_SEEN");
break;
case MODE_NO_FIX:
DEBUG_LOG("MODE_NO_FIX");
break;
case MODE_2D:
case MODE_3D:
if ((data->set & LATLON_SET) && (data->set & SPEED_SET)) {
latitude = fix.latitude;
longitude = fix.longitude;
speed = fix.speed;
return true;
}
DEBUG_LOG("Incomplete fix");
break;
}
} else {
DEBUG_LOG("No fix mode");
}
} else {
DEBUG_LOG("No data");
}
}
} else {
DEBUG_LOG("No interface");
}
return false;
}
