Trajectory tracker

trajectories.c

#include <string.h>
#include "trajectories.h"

void trajectory_init(trajectory_t *traj,
                     float *buffer, int len, int dimension,
                     uint64_t sampling_time_us)
{
    traj->buffer = buffer;
    traj->length = len;
    traj->dimension = dimension;
    traj->sampling_time_us = sampling_time_us;
    traj->read_pointer = 0;
    traj->read_time_us = traj->last_defined_time_us = 0;
}

void trajectory_chunk_init(trajectory_chunk_t *chunk, float *buffer, int length,
                           int dimension, uint64_t start_time_us,
                           uint64_t sampling_time_us)
{
    chunk->buffer = buffer;
    chunk->length = length;
    chunk->dimension = dimension;
    chunk->sampling_time_us = sampling_time_us;
    chunk->start_time_us = start_time_us;
}

int trajectory_apply_chunk(trajectory_t *traj, trajectory_chunk_t *chunk)
{
    int start_index, i = 0, write_index;
    int64_t start_time_us;
    float *src, *dst;

    if (chunk->sampling_time_us != traj->sampling_time_us) {
        return TRAJECTORY_ERROR_TIMESTEP_MISMATCH;
    }

    if (chunk->dimension != traj->dimension) {
        return TRAJECTORY_ERROR_DIMENSION_MISMATCH;
    }

    /* Check if the end of the chunk is past the end of the trajectory. */
    if (chunk->start_time_us + chunk->length * chunk->sampling_time_us >=
        traj->read_time_us + traj->length * traj->sampling_time_us) {
        return TRAJECTORY_ERROR_CHUNK_TOO_LONG;
    }

    start_index = (chunk->start_time_us - traj->read_time_us) / (traj->sampling_time_us);
    start_index += traj -> read_pointer;

    start_time_us = chunk->start_time_us;

    /* Do not copy points before the read pointer. */
    i = 1 + ((traj->read_time_us - chunk->start_time_us) / traj->sampling_time_us);

    while (i < chunk->length) {
        write_index = (i + start_index) % traj->length;

        memcpy(&traj->buffer[write_index * traj->dimension],
               &chunk->buffer[i * traj->dimension],
               traj->dimension * sizeof (float));

        i ++;
    }
    traj->last_defined_time_us = chunk->start_time_us +
                          (chunk->length - 1) * chunk->sampling_time_us;
    return 0;
}

float* trajectory_read(trajectory_t *traj, int64_t time)
{
    if (time > traj->last_defined_time_us) {
        return NULL;
    }

    if (time < traj->read_time_us) {
        return NULL;
    }
    traj->read_pointer += (time - traj->read_time_us) / traj->sampling_time_us;
    traj->read_pointer = traj->read_pointer % traj->length;

    traj->read_time_us = time;
    return &traj->buffer[traj->read_pointer * traj->dimension];
}

trajectories.h

#ifndef TRAJECTORIES_H
#define TRAJECTORIES_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>

#define TRAJECTORY_ERROR_TIMESTEP_MISMATCH -1
#define TRAJECTORY_ERROR_CHUNK_TOO_LONG -2
#define TRAJECTORY_ERROR_DIMENSION_MISMATCH -3

typedef struct {
    float *buffer;
    int length;
    int dimension;
    int64_t sampling_time_us;
    int read_pointer;
    int64_t read_time_us;
    int64_t last_defined_time_us;
} trajectory_t;

typedef struct {
    float *buffer;
    int length;
    int dimension;
    int64_t sampling_time_us;
    int64_t start_time_us;

} trajectory_chunk_t;


/** Inits a trajectory structure.
 *
 * @param [in] traj The trajectory to initialize.
 * @param [in] buffer The buffer to use for the trajectory.
 * @param [in] len The max number of points in the trajectory.
 * @param [in] dimension The dimension of a point in the trajectory.
 * @param [in] sampling_time_us Time between 2 samples in us.
 * @param [in] start_time_us Starting time in us.
 */
void trajectory_init(trajectory_t *traj,
                     float *buffer, int len, int dimension,
                     uint64_t sampling_time_us);

/** Inits a trajectory chunk. A chunk is a part of a trajectory that can later
 * be merged to a trajectory.
 *
 * @param [in] chunk The chunk to initialize.
 * @param [in] buffer The buffer to use for the chunk.
 * @param [in] len The max number of points in the chunk.
 * @param [in] dimension The dimension of a point in the chunk.
 * @param [in] sampling_time_us Time between 2 samples in us.
 * @param [in] start_time_us Starting time in us.
 */
void trajectory_chunk_init(trajectory_chunk_t *chunk, float *buffer, int length,
                           int dimension, uint64_t start_time_us,
                           uint64_t sampling_time_us);


/** Merges the given trajectory with the given chunk.
 *
 * @returns 0 if everything was OK.
 * @returns TRAJECTORY_ERROR_TIMESTEP_MISMATCH If the timestep from the
 * trajectory is not the same as the chunk's one.
 * @returns TRAJECTORY_ERROR_CHUNK_TOO_LONG if the chunk is too long or too far
 * in the future to be applied.
 * @returns TRAJECTORY_ERROR_DIMENSION_MISMATCH if the chunk's points do not
 * have the same dimesnion as the trajectory's ones.
 *
 * @note If an error occurs, the trajectory is left unchanged.
 */
int trajectory_apply_chunk(trajectory_t *traj, trajectory_chunk_t *chunk);

/** Reads the current point of the trajectory.
 *
 * @returns A pointer to the first field of the current point.
 * @note This updates the trajectory, settting the read pointer and read time.
 */
float* trajectory_read(trajectory_t *traj, int64_t time);


#ifdef __cplusplus
}
#endif

#endif

trajectories_test.cpp

#include <iostream>
#include "trajectories.h"
#include "CppUTest/TestHarness.h"

TEST_GROUP(TrajectoryInitTestGroup)
{
    float buffer[3][2];
};


TEST(TrajectoryInitTestGroup, CanInitTraj)
{
    trajectory_t traj;

    trajectory_init(&traj, (float *)buffer, 3, 2, 1000);

    POINTERS_EQUAL((float *)buffer, traj.buffer);
    CHECK_EQUAL(3, traj.length);
    CHECK_EQUAL(2, traj.dimension);
    CHECK_EQUAL(1000, (int)traj.sampling_time_us);
    CHECK_EQUAL(0, (int)traj.read_pointer);
}

TEST(TrajectoryInitTestGroup, CanInitChunk)
{
    trajectory_chunk_t chunk;
    trajectory_chunk_init(&chunk, (float *)buffer, 3, 2, 100, 10);
    POINTERS_EQUAL((float *)buffer, chunk.buffer);
    CHECK_EQUAL(3, chunk.length);
    CHECK_EQUAL(2, chunk.dimension);
    CHECK_EQUAL(100, (int)chunk.start_time_us);
    CHECK_EQUAL(10, (int)chunk.sampling_time_us);
}

TEST_GROUP(TrajectoriesMergingTestGroup)
{
    trajectory_t traj;
    float traj_buffer[100];

    trajectory_chunk_t chunk;
    float chunk_buffer[10];


    const uint64_t dt = 100;
    void setup(void)
    {
        memset(traj_buffer, 0, sizeof traj_buffer);
        memset(chunk_buffer, 0, sizeof chunk_buffer);

        for (int i = 0; i < 10; ++i) {
            chunk_buffer[i] = (float)i + 1;
        }

        trajectory_init(&traj, (float *)traj_buffer, 100, 1, dt);
        trajectory_chunk_init(&chunk, (float *)chunk_buffer, 10, 1, 0, dt);
    }
};

TEST(TrajectoriesMergingTestGroup, SimpleMerge)
{
    chunk.start_time_us = 20 * dt;

    trajectory_apply_chunk(&traj, &chunk);

    CHECK_EQUAL(traj_buffer[20], 1.);
    CHECK_EQUAL(traj_buffer[21], 2.);
    CHECK_EQUAL(traj_buffer[29], 10.);
}

TEST(TrajectoriesMergingTestGroup, ReturnCode)
{
    int ret;
    chunk.start_time_us = 20 * dt;

    ret = trajectory_apply_chunk(&traj, &chunk);
    CHECK_EQUAL(0, ret);
}

TEST(TrajectoriesMergingTestGroup, MergeWrapAround)
{
    // Leave enough room to wrap around
    traj.read_pointer = 50;
    traj.read_time_us = 50 * dt;

    // The chunk arrives at the end of the buffer
    chunk.start_time_us = 95 * dt;

    trajectory_apply_chunk(&traj, &chunk);

    CHECK_EQUAL(1., traj_buffer[95])
    CHECK_EQUAL(5., traj_buffer[99])
    CHECK_EQUAL(6., traj_buffer[0])
    CHECK_EQUAL(7., traj_buffer[1])
}

TEST(TrajectoriesMergingTestGroup, MergeWithNonZeroStartTime)
{
    traj.read_time_us = 100;
    chunk.start_time_us = 100 + 2 * dt;
    trajectory_apply_chunk(&traj, &chunk);
    CHECK_EQUAL(1., traj_buffer[2]);
}

TEST(TrajectoriesMergingTestGroup, MergeWithNonZeroReadPointer)
{
    traj.read_pointer = 4;
    traj.read_time_us = 100;
    chunk.start_time_us = 100 + 2 * dt;

    trajectory_apply_chunk(&traj, &chunk);

    CHECK_EQUAL(1., traj_buffer[2 + 4]);
}

TEST(TrajectoriesMergingTestGroup, MergeFromPast)
{
    traj.read_time_us = 100;
    traj.read_pointer = 10;
    chunk.start_time_us = 100 - 2 * dt;

    trajectory_apply_chunk(&traj, &chunk);

    CHECK_EQUAL(4., traj_buffer[11]);

    // Check that we did not touch the points before the read pointer
    CHECK_EQUAL(0., traj_buffer[10]);
}

TEST_GROUP(TrajectoriesReadTestGroup)
{
    trajectory_t traj;
    const uint64_t dt = 100;

    void setup()
    {
        trajectory_init(&traj, NULL, 100, 1, dt);
    }
};

TEST(TrajectoriesReadTestGroup, ReadChangesTheReadPointer)
{
    int64_t time = traj.read_time_us + 4 * dt;

    // Trajectory is defined up to this point
    traj.last_defined_time_us = time;


    CHECK_EQUAL(0, traj.read_pointer);

    trajectory_read(&traj, time);

    CHECK_EQUAL(4, traj.read_pointer);
    LONGS_EQUAL(time, traj.read_time_us);
}

TEST(TrajectoriesReadTestGroup, ReadWrapsAround)
{
    // Wrap around 3 times
    int64_t time = traj.read_time_us + 4 * dt + 3 * 100 * dt;

    // Trajectory is defined up to this point
    traj.last_defined_time_us = time;

    trajectory_read(&traj, time);

    CHECK_EQUAL(4, traj.read_pointer);
    LONGS_EQUAL(time, traj.read_time_us);
}

TEST(TrajectoriesReadTestGroup, ReadReturnsPointerToCorrectZone)
{
    int64_t time = traj.read_time_us + 4 * dt;

    // Trajectory is defined at this point
    traj.last_defined_time_us = time;

    float *res;

    res = trajectory_read(&traj, time);

    POINTERS_EQUAL(res, &traj.buffer[4]);
}

TEST_GROUP(TrajectoriesMultipleDimensionTestGroup)
{
    const uint64_t dt = 100;

    trajectory_t traj;
    float traj_buffer[100][3];

    trajectory_chunk_t chunk;
    float chunk_buffer[10][3];

    void setup()
    {
        memset(traj_buffer, 0, sizeof traj_buffer);
        trajectory_init(&traj, (float *)traj_buffer, 100, 3, dt);

        memset(chunk_buffer, 0, sizeof chunk_buffer);
        trajectory_chunk_init(&chunk, (float *)chunk_buffer, 10, 3, 0, dt);
    }
};

TEST(TrajectoriesMultipleDimensionTestGroup, CanMerge)
{
    chunk.start_time_us = 20 * dt;

    for (int i = 0; i < 10; ++i) {
        chunk_buffer[i][0] = (float)i;
        chunk_buffer[i][1] = (float)10 * i;
        chunk_buffer[i][2] = (float)100 * i;
    }

    trajectory_apply_chunk(&traj, &chunk);

    CHECK_EQUAL(1., traj_buffer[21][0]);
    CHECK_EQUAL(10., traj_buffer[21][1]);
    CHECK_EQUAL(100., traj_buffer[21][2]);

    CHECK_EQUAL(2., traj_buffer[22][0]);
    CHECK_EQUAL(20., traj_buffer[22][1]);
    CHECK_EQUAL(200., traj_buffer[22][2]);

    CHECK_EQUAL(9., traj_buffer[29][0]);
    CHECK_EQUAL(90., traj_buffer[29][1]);
    CHECK_EQUAL(900., traj_buffer[29][2]);
}

TEST(TrajectoriesMultipleDimensionTestGroup, CanRead)
{
    traj.last_defined_time_us = traj.read_time_us + 2 * dt;
    float *res = trajectory_read(&traj, traj.read_time_us + 2 * dt);
    POINTERS_EQUAL(&traj_buffer[2][0], res);
}

TEST_GROUP(TrajectoriesErrorTestGroup)
{
    trajectory_t traj;
    float traj_buffer[100];

    trajectory_chunk_t chunk;
    float chunk_buffer[10];

    const uint64_t dt = 100;
    int ret;
    void setup(void)
    {
        memset(traj_buffer, 0, sizeof traj_buffer);
        memset(chunk_buffer, 0, sizeof chunk_buffer);

        trajectory_init(&traj, (float *)traj_buffer, 100, 1, dt);
        trajectory_chunk_init(&chunk, (float *)chunk_buffer, 10, 1, 0, dt);
    }
};

TEST(TrajectoriesErrorTestGroup, CheckSampleRate)
{
    // Sample rates don't match
    chunk.sampling_time_us = traj.sampling_time_us + 10;

    ret = trajectory_apply_chunk(&traj, &chunk);
    CHECK_EQUAL(TRAJECTORY_ERROR_TIMESTEP_MISMATCH, ret);
}

TEST(TrajectoriesErrorTestGroup, CheckTooFarInTheFuture)
{
    // Last point of chunk will override the trajectory
    chunk.start_time_us = 90 * dt;

    ret = trajectory_apply_chunk(&traj, &chunk);
    CHECK_EQUAL(TRAJECTORY_ERROR_CHUNK_TOO_LONG, ret);
}

TEST(TrajectoriesErrorTestGroup, CheckDimension)
{
    chunk.dimension = traj.dimension - 1;
    ret = trajectory_apply_chunk(&traj, &chunk);
    CHECK_EQUAL(TRAJECTORY_ERROR_DIMENSION_MISMATCH, ret);
}

TEST(TrajectoriesErrorTestGroup, ReadAfterBufferEnd)
{
    float *res;

    chunk.start_time_us = 20 * dt;

    trajectory_apply_chunk(&traj, &chunk);

    res = trajectory_read(&traj, 30 * dt);
    POINTERS_EQUAL(NULL, res);
}

TEST(TrajectoriesErrorTestGroup, ReadBeforeReadPointer)
{
    // Reading before read pointer is undefined, as data may have been
    // overwritten
    float *res;

    chunk.start_time_us = 20 * dt;
    trajectory_apply_chunk(&traj, &chunk);
    res = trajectory_read(&traj, 10 * dt);
    CHECK_TRUE(res != NULL);

    // Now check before the read pointer
    res = trajectory_read(&traj, 5 * dt);
    CHECK_TRUE(res == NULL);
}

in void trajectory_apply_chunk(trajectory_t *traj, trajectory_chunk_t *chunk) what should be the desired/correct behaviour when traj and chunk don't have the same sampling time?

• the read function should find the closest or only allow multiples of the sample time (TBD)
• the while loops should be replaced by an explicit computation which is easier to understand and faster.
• use memcpy to copy memory
• A few checks are needed:
  • verify the sampling time is the same when merging
  • the read function should return an error (NULL) when the requested sample doesn't exist
  • the merge function should check that the chunk isn't too long

Fixed everything, except for

the read function should find the closest or only allow multiples of the sample time (TBD)

Should I mod it to find the closest ? Or do we stay with the current "find next point" behavior ?