so I managed to resolve my issue by combining different answers from the website: How to use google test for C++ to run through combinations of data & Can gmock be used for stubbing C functions?.
My answer is a bit big but like that if you want to use it, you can simply use copy & past.
To mock freeRTOS elements, in my test-folder:
FreeRTOS_mock.hpp
/* Include freeRTOS headers */
#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
/* Include gTest mockup functionality */
#include "gmock/gmock.h"
/* Mock all functions needed from FreeRTOS */
namespace freertos {
class FreeRTOSInterface
{
public:
    virtual ~FreeRTOSInterface() {}
    virtual QueueHandle_t xQueueGenericCreate(const UBaseType_t uxQueueLength, const UBaseType_t uxItemSize, const uint8_t ucQueueType) = 0;
    /* define other freeRTOS elements the same way */
};
class FreeRTOSMock : public FreeRTOSInterface
{
public:
    virtual ~FreeRTOSMock() {}
    MOCK_METHOD3(xQueueGenericCreate, QueueHandle_t(const UBaseType_t uxQueueLength, const UBaseType_t uxItemSize, const uint8_t ucQueueType));
    /* Align with what was defined above */
};
} /* namespace freertos */
FreeRTOS_mock.cpp
#include "FreeRTOS_mock.hpp"
freertos::FreeRTOSMock FreeRTOSMockObj;
QueueHandle_t xQueueGenericCreate(const UBaseType_t uxQueueLength, const UBaseType_t uxItemSize, const uint8_t ucQueueType)
{
    return FreeRTOSMockObj.xQueueGenericCreate(uxQueueLength, uxItemSize, ucQueueType);
}
/* Align with what is in the .hpp */
TestSuiteXXX_unittest.cpp
#include "FreeRTOS_mock.hpp"
extern freertos::FreeRTOSMock FreeRTOSMockObj;
/* Write my TCs by using the FreeRTOS functions*/
What is also important is that you must have defined a valid FreeRTOSConfig.h and in the makefile:
INCLUDE_DIRS = \
        -I$(FREERTOS_DIR)/Source/include \
        -I$(FREERTOS_DIR)/Source/portable/GCC/ARM_CM4F \
        -I$(PRJ_FREERTOS_CFG) \
        -I$(UNITTEST_INCLUDE_DIR)
SRC_FILES = \
    ./test/FreeRTOS_mock.cpp \
    ./src/XXX.cpp
#Specify all unittest files
UNITTEST_SRC_FILES = \
    ./test/TestSuiteXXX_unittest.cpp
To Emulate the sensoric-data:
TestSuiteXXX_unittest.cpp
#include "FreeRTOS_mock.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include <vector>
#include "Algo.hpp"
extern freertos::FreeRTOSMock FreeRTOSMockObj;
/* A sensor measurement */
std::vector<int32_t> input1 { 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
std::vector<int32_t> output1 { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
/* Not very pretty adaptation function but it does the job */
std::vector<std::tuple<int32_t, int32_t>> genSet(std::vector<int32_t> a, std::vector<int32_t> b)
{
    uint32_t i(0);
    std::vector<std::tuple<int32_t int32_t>> vectorToReturn(a.size());
    for (i = 0 ; i < a.size(); i++)
    {
        vectorToReturn[i] = std::make_tuple(a[i], b[i]);
    }
    return vectorToReturn;
}
/** Define the Value-Parameterized Tests class */
class AlgoToTest: public ::testing::TestWithParam<std::tuple<int32_t, int32_t>>
{
public:
    /* SetUp() is run immediately before a test starts. */
    virtual void SetUp()
    {
        algo = new Algo::Algo();
    }
    /* TearDown() is invoked immediately after a test finishes. */
    virtual void TearDown()
    {
        delete algo;
    }
    Algo::Algorithm* algo = NULL;
};
/* The test-case used to loop on */
TEST_P(AlgoToTest, AlgoTestCase1)
{
    int32_t outputValue(0);
    outputValue = algo->run(std::get<0>(GetParam()), std::get<1>(GetParam()));
    ASSERT_EQ(outputValue, std::get<3>(GetParam()));
}
INSTANTIATE_TEST_CASE_P(AlgoTestRun1, AlgoToTest, ::testing::ValuesIn(genSet(input1, output1)));
If you have improvement propositions, please comment!