ab_gp_proc_position.c

Go to the documentation of this file.

 
#include "ab_gp_proc_position.h"
#include "ab_siteGP.h"
#include "rte/utils/cast/rte_cast.h"
 
bool ab_gp_proc_position_update(ab_gp_ertms_train_context_t *ctx,
                                uint32_t nid_lrbg,
                                uint32_t d_lrbg_m,
                                rbc_q_dirlrbg_t q_dirlrbg,
                                uint32_t v_train_kmh,
                                uint32_t m_mode,
                                uint32_t m_level,
                                uint32_t cycle,
                                uint32_t timestamp_ms)
{
    const ab_site_balise_t *balise;
 
    if (ctx == NULL)
    {
        return false;
    }
 
    ctx->position.nid_lrbg = nid_lrbg;
    ctx->position.d_lrbg_m = d_lrbg_m;
    ctx->position.q_dirlrbg = q_dirlrbg;
    ctx->position.v_train_kmh = v_train_kmh;
    ctx->position.m_mode = m_mode;
    ctx->position.m_level = m_level;
    ctx->position.last_report_cycle = cycle;
    ctx->position.last_report_time_ms = timestamp_ms;
    ctx->contact_timer_ms = 0U; /* Reset communication contact timer */
 
    /* Correlate LRBG with static site topology */
    balise = ab_site_get_balise(nid_lrbg);
    if (balise != NULL)
    {
        ab_site_direction_t train_dir;
 
        ctx->position.current_segment_id = balise->segment_id;
        ctx->position.current_route_id = balise->route_id;
 
        /* Determine train travel direction relative to segment.
         * The balise direction indicates its nominal orientation on the segment:
         *   NOMINAL: increasing pos_m (pos 0 -> length)
         *   REVERSE: decreasing pos_m (length -> pos 0)
         * Packet 0 q_dirlrbg indicates train direction relative to LRBG orientation:
         *   NOMINAL: train moves in the balise's nominal direction
         *   REVERSE: train moves in the balise's reverse direction */
        if (balise->direction == AB_SITE_DIR_REVERSE)
        {
            if (q_dirlrbg == RBC_Q_DIRLRBG_REVERSE)
            {
                train_dir = AB_SITE_DIR_NOMINAL;
            }
            else
            {
                train_dir = AB_SITE_DIR_REVERSE;
            }
        }
        else
        {
            if (q_dirlrbg == RBC_Q_DIRLRBG_REVERSE)
            {
                train_dir = AB_SITE_DIR_REVERSE;
            }
            else
            {
                train_dir = AB_SITE_DIR_NOMINAL;
            }
        }
        ctx->position.direction_on_segment = train_dir;
 
        /* Calculate train coordinate along the segment (pos_on_segment_m) */
        if (train_dir == AB_SITE_DIR_REVERSE)
        {
            if (balise->pos_m >= d_lrbg_m)
            {
                ctx->position.pos_on_segment_m = balise->pos_m - d_lrbg_m;
            }
            else
            {
                ctx->position.pos_on_segment_m = 0U;
            }
        }
        else
        {
            (void)rte_cast_checked_add_u32(balise->pos_m, d_lrbg_m, &ctx->position.pos_on_segment_m);
        }
 
        (void)rte_cast_checked_add_u32(balise->pos_m, d_lrbg_m, &ctx->position.distance_in_route_m);
    }
    else
    {
        ctx->position.current_segment_id = 0U;
        ctx->position.current_route_id = 0U;
        ctx->position.pos_on_segment_m = d_lrbg_m;
        ctx->position.direction_on_segment = AB_SITE_DIR_NOMINAL;
        ctx->position.distance_in_route_m = d_lrbg_m;
    }
 
    return true;
}
 
bool ab_gp_proc_position_is_train_in_route(const ab_gp_ertms_train_context_t *ctx,
                                           uint16_t route_id)
{
    uint8_t i;
 
    if (ctx == NULL || route_id == 0U)
    {
        return false;
    }
 
    if (ctx->position.current_route_id == route_id)
    {
        return true;
    }
 
    /* Also check if route_id is one of the allocated routes in active MA */
    for (i = 0U; i < ctx->ma.route_count; i++)
    {
        if (ctx->ma.route_ids[i] == route_id)
        {
            return true;
        }
    }
 
    return false;
}
 
bool ab_gp_proc_position_has_exceeded_ma(const ab_gp_ertms_train_context_t *ctx)
{
    if (ctx == NULL || !ctx->ma.has_active_ma)
    {
        return false;
    }
 
    if (ctx->ma.ma_length_m > 0U && ctx->position.distance_in_route_m > ctx->ma.ma_length_m)
    {
        return true;
    }
 
    return false;
}