ab_gp_proc_ma.c

Go to the documentation of this file.

 
#include "ab_gp_proc_ma.h"
#include "../state_machines/ab_gp_dispatch.h"
#include "../state_machines/session/ab_gp_session_fsm.h" /* ab_gp_session_fsm_can_handle() -
                                                          * a pure legality query, not an
                                                          * event dispatch (see line ~205). */
#include "rte/oal/memory/rte_mem_util.h"
#include "rte/utils/cast/rte_cast.h"

static bool distance_from_lrbg_to_route_end(const ab_site_ga_route_layout_t *route, uint32_t nid_lrbg,
                                            uint32_t *out_dist_m)
{
    const ab_site_balise_t *balise = ab_site_get_balise(nid_lrbg);
    const ab_site_signal_t *end_sig = ab_site_get_signal(route->end_signal);
 
    if ((balise == NULL) || (end_sig == NULL) || (out_dist_m == NULL))
    {
        return false;
    }
 
    if (end_sig->direction == AB_SITE_DIR_REVERSE)
    {
        *out_dist_m = (balise->chainage_m > end_sig->chainage_m) ? (balise->chainage_m - end_sig->chainage_m) : 0U;
    }
    else
    {
        *out_dist_m = (end_sig->chainage_m > balise->chainage_m) ? (end_sig->chainage_m - balise->chainage_m) : 0U;
    }
    return true;
}
 
uint32_t ab_gp_proc_ma_remaining_length_m(const ab_site_ga_route_layout_t *route, uint32_t nid_lrbg, uint32_t d_lrbg_m)
{
    uint32_t remaining_m = 0U;
 
    /* @p d_lrbg_m (distance since the train's own last-reported balise)
     * is deliberately NOT part of this computation, on direct
     * instruction: MA length is measured from the train's last KNOWN
     * balise - a discrete, site-data-backed reference point - not from a
     * continuously self-reported odometer offset past it. Kept as a
     * parameter (unused) rather than dropped from the signature - both
     * call sites below already resolve it from ctx->position/session
     * state and passing it costs nothing; a future revision may want it
     * back for a tighter bound. */
    (void)d_lrbg_m;
 
    if (route == NULL)
    {
        return 0U;
    }
 
    if (!distance_from_lrbg_to_route_end(route, nid_lrbg, &remaining_m))
    {
        return route->length_m; /* cannot resolve position within this route - see this function's own doc */
    }
 
    /* Not clamped to route->length_m: a balise before the start signal legitimately has more than the authored
     * route length to the end signal (see distance_from_lrbg_to_route_end()). */
    return remaining_m;
}
 
bool ab_gp_proc_ma_compute(ab_gp_ertms_train_context_t *ctx,
                           const ab_site_ga_route_layout_t *route,
                           uint32_t ma_seq)
{
    uint8_t i;
    const ab_site_segment_t *seg;
    const ab_site_balise_t *balise;
    uint32_t cum_dist = 0U;
    uint32_t remaining_m;
 
    if (ctx == NULL || route == NULL)
    {
        return false;
    }
 
    /* Checked FIRST, before any of the packet-15/21/27/5 computation below
     * mutates ctx->ma - a rejected grant must leave ctx untouched, not
     * half-populate it then discover the event has no outcome. MA_GRANTED
     * is handled from ESTABLISHED/DATA_VALIDATED/STANDBY/FS
     * (ab_gp_session_fsm.c's own table, FS itself included since
     * ab_gp_proc_ma_compute() can legitimately run again while already FS).
     * ESTABLISHED is included deliberately, not left off "for strictness":
     * this codebase's own real connect path (ab_gp_proc_dispatch.c's
     * handle_msg_155()) drives straight to ESTABLISHED and never validates
     * train data at all (see that function's own doc) - requiring
     * DATA_VALIDATED here would make every real MA grant on that path
     * unreachable. Still a real tightening versus the old unconditional
     * assignment: IDLE/INITIATED/SHUNTING/EMERGENCY_STOP/TERMINATING can no
     * longer jump a train straight to Full Supervision. A query only
     * (can_handle, not handle_event) - the actual transition is applied
     * below, once this function's own work has succeeded, not here. */
    if (!ab_gp_session_fsm_can_handle(ctx->session_state, AB_GP_SESSION_EVENT_MA_GRANTED, NULL))
    {
        return false;
    }
 
    remaining_m = ab_gp_proc_ma_remaining_length_m(route, ctx->position.nid_lrbg, ctx->position.d_lrbg_m);
 
    /* remaining_m is the distance to the end signal (the Supervised
     * Location). The granted End/Limit of Authority sits one overlap
     * margin short of it - see RTE_EXAMPLE_MA_OVERLAP_M's own doc.
     * Subtracted here once; ab_gp_proc_ma_extend() then just adds each
     * further route's own length, so the single margin carries to
     * whatever the new last end signal is. */
    if (remaining_m > RTE_EXAMPLE_MA_OVERLAP_M)
    {
        remaining_m -= RTE_EXAMPLE_MA_OVERLAP_M;
    }
    else
    {
        remaining_m = 0U;
    }
 
    rte_mem_set(&ctx->ma, 0, sizeof(ctx->ma));
 
    ctx->ma.has_active_ma = true;
    ctx->ma.ma_seq = ma_seq;
    ctx->ma.ma_length_m = remaining_m;
    ctx->ma.v_loa_kmh = 0U;
    ctx->ma.t_loa_s = 60U;
    ctx->ma.start_signal = route->start_signal;
    ctx->ma.end_signal = route->end_signal;
    ctx->ma.route_count = 1U;
    ctx->ma.route_ids[0] = route->id;
 
    /* 1. Packet 15 (Movement Authority) */
    ctx->ma.p15.ma_seq = ma_seq;
    ctx->ma.p15.ma_length = remaining_m;
    ctx->ma.p15.v_loa = 0U;
    ctx->ma.p15.t_loa_s = 60U;
    ctx->ma.p15.has_section_timer = false;
 
    /* 2. Packet 21 (Gradient Profile along constituent segments) */
    ctx->ma.p21.segment_count = 0U;
    cum_dist = 0U;
    for (i = 0U; (i < route->segment_count) && (i < (uint8_t)RTE_EXAMPLE_MAX_SEGMENTS_PER_ROUTE) && (i < (uint8_t)RBC_MAX_GRADIENT_SEGMENTS); i++)
    {
        seg = ab_site_get_segment(route->segment_ids[i]);
        if (seg != NULL)
        {
            int8_t g_a = 0;
            ctx->ma.p21.segments[ctx->ma.p21.segment_count].d_gradient = cum_dist;
            (void)rte_cast_i32_to_i8((int32_t)seg->gradient_permil, &g_a);
            ctx->ma.p21.segments[ctx->ma.p21.segment_count].g_a = g_a;
            ctx->ma.p21.segment_count++;
            (void)rte_cast_checked_add_u32(cum_dist, seg->length_m, &cum_dist);
        }
    }
 
    /* 3. Packet 27 (Static Speed Profile along constituent segments) */
    ctx->ma.p27.segment_count = 0U;
    cum_dist = 0U;
    for (i = 0U; (i < route->segment_count) && (i < (uint8_t)RTE_EXAMPLE_MAX_SEGMENTS_PER_ROUTE) && (i < (uint8_t)RBC_MAX_SPEED_SEGMENTS); i++)
    {
        seg = ab_site_get_segment(route->segment_ids[i]);
        if (seg != NULL)
        {
            uint8_t v_static = 0U;
            ctx->ma.p27.segments[ctx->ma.p27.segment_count].d_static = cum_dist;
            (void)rte_cast_u32_to_u8(seg->v_max_kmh / 5U, &v_static);
            ctx->ma.p27.segments[ctx->ma.p27.segment_count].v_static = v_static;
            ctx->ma.p27.segment_count++;
            (void)rte_cast_checked_add_u32(cum_dist, seg->length_m, &cum_dist);
        }
    }
 
    /* 4. Packet 5 (Balise Linking Information) */
    ctx->ma.p5.link_count = 0U;
    for (i = 0U; (i < route->balise_count) && (i < (uint8_t)RTE_EXAMPLE_MAX_BALISES_PER_ROUTE) && (i < (uint8_t)RBC_MAX_LINKING_ITEMS); i++)
    {
        balise = ab_site_get_balise(route->balise_nid_lrbgs[i]);
        if (balise != NULL)
        {
            uint16_t nid_bg = 0U;
            (void)rte_cast_u32_to_u16(balise->nid_lrbg & 0x3FFFU, &nid_bg);
            ctx->ma.p5.links[ctx->ma.p5.link_count].nid_bg = nid_bg;
            ctx->ma.p5.links[ctx->ma.p5.link_count].d_link = balise->pos_m;
            ctx->ma.p5.links[ctx->ma.p5.link_count].q_linkdir = RBC_Q_DIR_NOMINAL;
            ctx->ma.p5.link_count++;
        }
    }
 
    /* Advance train operational state to Full Supervision. Already
     * confirmed handleable above (can_handle, no side effect) before any
     * of this function's own work started - this call can therefore only
     * fail on ctx == NULL, already excluded at function entry. */
    (void)ab_gp_dispatch(ctx, AB_GP_SESSION_EVENT_MA_GRANTED);
 
    return true;
}
 
bool ab_gp_proc_ma_extend(ab_gp_ertms_train_context_t *ctx,
                          const ab_site_ga_route_layout_t *additional_route)
{
    if (ctx == NULL || additional_route == NULL || !ctx->ma.has_active_ma)
    {
        return false;
    }
 
    if (ctx->ma.route_count >= (uint8_t)RTE_EXAMPLE_MAX_ROUTES_PER_TRAIN)
    {
        return false;
    }
 
    /* The extended MA is measured from the same LRBG as the first grant: distance(LRBG -> the new last end signal)
     * minus one overlap, not the old length plus the added route's authored length (which drifts whenever the
     * authored lengthM differs from the chainage). It replaces the old value: that one was relative to an earlier
     * LRBG, and keeping it (a max) would overshoot the new end signal once the train has moved on. */
    {
        uint32_t total_m = ab_gp_proc_ma_remaining_length_m(additional_route, ctx->position.nid_lrbg,
                                                            ctx->position.d_lrbg_m);
        total_m = (total_m > RTE_EXAMPLE_MA_OVERLAP_M) ? (total_m - RTE_EXAMPLE_MA_OVERLAP_M) : 0U;
        ctx->ma.ma_length_m = total_m;
    }
    ctx->ma.ma_seq++;
    ctx->ma.end_signal = additional_route->end_signal;
    ctx->ma.route_ids[ctx->ma.route_count] = additional_route->id;
    ctx->ma.route_count++;
 
    ctx->ma.p15.ma_seq = ctx->ma.ma_seq;
    ctx->ma.p15.ma_length = ctx->ma.ma_length_m;
 
    return true;
}
 
bool ab_gp_proc_ma_shorten(ab_gp_ertms_train_context_t *ctx,
                           uint32_t new_ma_length_m)
{
    if (ctx == NULL || !ctx->ma.has_active_ma)
    {
        return false;
    }
 
    if (new_ma_length_m < ctx->ma.ma_length_m)
    {
        ctx->ma.ma_length_m = new_ma_length_m;
        ctx->ma.p15.ma_length = new_ma_length_m;
        return true;
    }
 
    return false;
}
 
bool ab_gp_proc_ma_handle_ack(ab_gp_ertms_train_context_t *ctx, uint32_t ma_seq)
{
    if (ctx == NULL || !ctx->ma.has_active_ma)
    {
        return false;
    }
 
    return (ma_seq == ctx->ma.ma_seq);
}
 
void ab_gp_proc_ma_revoke(ab_gp_ertms_train_context_t *ctx)
{
    if (ctx == NULL)
    {
        return;
    }
 
    ctx->ma.has_active_ma = false;
    ctx->ma.ma_length_m = 0U;
    ctx->ma.route_count = 0U;
    /* No outer guard needed - the table only defines an outcome for
     * MA_REVOKED from FS, so it is a harmless no-op from anywhere else
     * (e.g. an already-STANDBY train with no MA to begin with). */
    (void)ab_gp_dispatch(ctx, AB_GP_SESSION_EVENT_MA_REVOKED);
}