66 lines
2.1 KiB
C
66 lines
2.1 KiB
C
#ifndef NAVIGATION_FUSION_H
|
|
#define NAVIGATION_FUSION_H
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#include "fusion/fusion_engine.h"
|
|
#include "fusion/fusion_measurement.h"
|
|
#include "fusion/fusion_motion.h"
|
|
#include "imu/imu_sample.h"
|
|
|
|
typedef struct {
|
|
uint64_t timestamp_ticks;
|
|
uint64_t last_accepted_gps_ticks;
|
|
double position_enu_m[3];
|
|
double velocity_enu_mps[3];
|
|
double latitude_deg;
|
|
double longitude_deg;
|
|
double height_m;
|
|
double position_std_m[3];
|
|
double speed_mps;
|
|
double last_position_nis;
|
|
fusion_motion_state_t motion_state;
|
|
uint64_t gps_accepted;
|
|
uint64_t gps_nis_rejected;
|
|
uint64_t gps_position_recoveries;
|
|
uint64_t zero_update_count;
|
|
uint64_t replay_count;
|
|
uint64_t stale_gps_count;
|
|
uint64_t imu_processed_count;
|
|
unsigned int calibration_sample_count;
|
|
fusion_engine_state_t engine_state;
|
|
bool local_valid;
|
|
bool global_valid;
|
|
bool vehicle_at_rest;
|
|
/* Compatibility alias for UART publishers: valid means global LLH valid. */
|
|
bool valid;
|
|
} navigation_fusion_output_t;
|
|
|
|
/* Initialize the sensor-independent 15-state ESKF and GPS-to-ENU adapter. */
|
|
bool navigation_fusion_init(
|
|
uint64_t local_timebase_hz,
|
|
const fusion_imu_quality_profile_t* imu_quality,
|
|
bool platform_landed);
|
|
|
|
/* PX4-style external platform fact. For this bench prototype `true` means
|
|
* unarmed/on-ground; a mobile integration must drive it from its own
|
|
* arm/contact/land supervisor. */
|
|
void navigation_fusion_set_platform_landed(
|
|
bool platform_landed,
|
|
uint64_t timestamp_ticks);
|
|
|
|
/* Prediction input: every timestamped IMU sample is integrated; it is not an
|
|
* ESKF observation and is never rate-selected in this layer. */
|
|
bool navigation_fusion_push_imu(const imu_sample_t* measurement);
|
|
|
|
/* Observation input: convert a valid LLH fix to local ENU, apply it at the
|
|
* captured GPS epoch, then replay newer IMU history to current time. */
|
|
bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement);
|
|
|
|
/* local_valid can exist without GPS. global_valid/valid require a GPS origin. */
|
|
bool navigation_fusion_get_output(navigation_fusion_output_t* output);
|
|
const char* navigation_fusion_state_name(fusion_engine_state_t state);
|
|
|
|
#endif
|