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; }