86 lines
2.4 KiB
C
86 lines
2.4 KiB
C
#ifndef RADICK_TARGET_TRACKER_H
|
|
#define RADICK_TARGET_TRACKER_H
|
|
|
|
#include "radar_protocol.h"
|
|
|
|
#include <stdbool.h>
|
|
#include <stddef.h>
|
|
#include <stdint.h>
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
#define TARGET_TRACKER_CAPACITY RD03D_MAX_TARGETS
|
|
|
|
typedef struct {
|
|
float position_ema_alpha;
|
|
float speed_ema_alpha;
|
|
float motion_ema_alpha;
|
|
float max_association_distance_mm;
|
|
uint16_t max_missed_frames;
|
|
} target_tracker_config_t;
|
|
|
|
typedef struct {
|
|
uint32_t id;
|
|
float x_mm;
|
|
float y_mm;
|
|
float speed_cm_s;
|
|
uint16_t resolution_mm;
|
|
uint32_t age_frames;
|
|
uint16_t last_seen_age_frames;
|
|
bool visible;
|
|
bool observed_this_frame;
|
|
} target_snapshot_t;
|
|
|
|
/* Public state keeps the module allocator-free; consumers should use snapshot. */
|
|
typedef struct {
|
|
target_snapshot_t snapshot;
|
|
float last_observed_x_mm;
|
|
float last_observed_y_mm;
|
|
float velocity_x_mm_per_frame;
|
|
float velocity_y_mm_per_frame;
|
|
bool active;
|
|
} target_track_state_t;
|
|
|
|
typedef struct {
|
|
target_tracker_config_t config;
|
|
target_track_state_t tracks[TARGET_TRACKER_CAPACITY];
|
|
uint32_t next_id;
|
|
} target_tracker_t;
|
|
|
|
void target_tracker_default_config(target_tracker_config_t *config);
|
|
|
|
/* A NULL config selects conservative defaults suitable for the 10 Hz stream. */
|
|
void target_tracker_init(target_tracker_t *tracker,
|
|
const target_tracker_config_t *config);
|
|
|
|
/* Clear tracks and restart IDs at one while preserving the current config. */
|
|
void target_tracker_reset(target_tracker_t *tracker);
|
|
|
|
/*
|
|
* Advance the tracker by exactly one radar frame. Only detections with valid
|
|
* set participate, and at most the first three valid detections are used.
|
|
* Returns the number of tracks retained after the update.
|
|
*/
|
|
size_t target_tracker_update(target_tracker_t *tracker,
|
|
const rd03d_target_t *detections,
|
|
size_t detection_count);
|
|
|
|
size_t target_tracker_update_frame(target_tracker_t *tracker,
|
|
const rd03d_frame_t *frame);
|
|
|
|
/*
|
|
* Copy visible tracks ordered by stable ID. Passing NULL/zero capacity is a
|
|
* valid way to query the number of visible tracks.
|
|
*/
|
|
size_t target_tracker_snapshot(const target_tracker_t *tracker,
|
|
target_snapshot_t *out,
|
|
size_t capacity);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif /* RADICK_TARGET_TRACKER_H */
|