Difference between revisions of "Self-driving Car"

/**
 * Have an enumeration of controller IDs
 */
typedef enum {
    cid_geographical_controller = 50,
    cid_master_controller = 60,
} cid_t;

/**
 * Each controller shall then set its own ID
 */
const cid_t our_controller_id = cid_master_controller;

/**
 * Create a "union" whose struct overlaps with the uint32_t
 */
typedef union {
    struct {
        uint32_t msg_num : 12; ///< Message number
        uint32_t src : 8;      ///< Source ID
        uint32_t dst : 8;      ///< Destination ID
        uint32_t : 1;          ///< Unused (reserved 29th bit)
    };
    /// This "raw" overlaps with <DST> <SRC> <ID>
    uint32_t raw;

} __attribute__((packed)) controller_id_t;

/**
 * Creates a message ID based on the message ID protocol
 * @param [in] dst  The destination controller ID
 * @param [in] msg_num  The message number to send to the dst controller
 *
 * @returns  The 32-bit message ID created by the input parameters
 */
extern "C" uint32_t make_id(uint8_t dst, uint16_t msg_num)
{
    controller_id_t cid;
    cid.raw = 0;
    cid.msg_num  = msg_num;
    cid.src = our_controller_id;
    cid.dst = dst;
    return cid.raw;
}

int main(void)
{
    /* Initialize CAN Bus and set the acceptance filter(s) */
    CAN_init(can1, 100, 10, 10, NULL, NULL);
    CAN_reset_bus(can1);

    /* Create the subscription lists.
     * Set an initial capacity, and we disallow the capacity to grow such that the tasks can
     * access the vector without having to lock/unlock the interrupts or use a semaphore
     */
    const int maxMsgsPerList = 50;
    VECTOR<subscribedMsg_t> *list1Hz  = new VECTOR<subscribedMsg_t> (maxMsgsPerList);
    VECTOR<subscribedMsg_t> *list20Hz = new VECTOR<subscribedMsg_t> (maxMsgsPerList);

    /* Create the subscription handler tasks, and give them the rate, and our list pointers */
    scheduler_add_task(new canSubscribedMsgTask(msgRate1Hz,  list1Hz,  1));
    scheduler_add_task(new canSubscribedMsgTask(msgRate20Hz, list20Hz, 2));

    /* Add more tasks... */

    /* Start the scheduler to run all the tasks */
    scheduler_start(); 

    return 0;
}

/* Either a "plain vanilla" FreeRTOS task, or run() method of scheduler_task */
void rx_fanout_task(void *p)
{
    can_msg_t msg;
    while(1) {
        if (CAN_rx(can1, &msg, portMAX_DELAY)) {
            /* This is psuedocode, so add your real logic here */
            if (!dst_is_not_us) {
                log_error("HW CAN Filter must be incorrect!");
            }
            else if (command_message) {  /* 0x100 - 0x1FF */
                handle_common_cmd(msg);  /* 0x200 - 0x2FF */
            }
            else if (our_command) {      /* 0x300 - 0x3FF */
                handle_our_cmd(msg);
            }
            else if (new_subscribe) {    /* 0x400 - 0x4FF */
                subscription_add(msg);
            }
            else if (subscribed_msg) {   /* 0x500 - 0x5FF */
                handle_subscribed_msg(msg);
            }
            else if (cmd_response_msg) { /* 0x200 - 0x2FF */
                handle_cmd_rsp_msg(msg);
            }
            else {
                LOG_ERROR("Unexpected Message ID: 0x%08X", msg.msg_id);
            }
        }
    }
}

/* Granted that the canSubscribedMsgTask tasks are added, a function foo can add subscription like this */
void foo()
{
    /* Static function, so need to use :: */
    can_msg_t canMsg = { 0 };
    canSubscribedMsgTask::addSubscribedMsg(msgRate1Hz, dst, msgNum, &canMsg);
}

/*** subscription_task.hpp ***/
#ifndef SUBSCRIPTION_TASK_HPP_
#define SUBSCRIPTION_TASK_HPP_

#include <stdint.h>

#include "can.h"
#include "vector.hpp"
#include "scheduler_task.hpp"



/**
 * Encoded enumeration of message rate and the index.
 * We need the message rate in ms to set the task's run() method call frequency, but
 * we also need an index to be able to add subscription messages to the correct list.
 */
typedef enum {
    msgRate1Hz  = (1000 << 4) | 0,
    msgRate5Hz  = ( 200 << 4) | 1,
    msgRate10Hz = ( 100 << 4) | 2,
    msgRate20Hz = (  50 << 4) | 3,
    msgRate50Hz = (  20 << 4) | 4,

    /// Marks the last entry; do not use!
    msgRateLast = (0 << 4) | 5,
} msgRate_t;

/** @{ Converts msgRate_t to frequency in milliseconds and its index */
static inline uint8_t msgRateToIndex(msgRate_t mr)   { return (mr & 0x0F); }
static inline uint16_t msgRateToFreqMs(msgRate_t mr) { return (mr >> 4);   }
/** @} */

/**
 * Structure of a subscribed message
 */
typedef struct {
    uint8_t dstAddr;        ///< The destination address of a node
    uint16_t msgNum;        ///< The message number for the node
    can_msg_t *canMsgPtr;   ///< Pointer to the actual CAN message pointer
} subscribedMsg_t;

/**
 * The task that sends out the subscription messages of a list at the defined frequency.
 */
class canSubscribedMsgTask : public scheduler_task
{
    public:
        canSubscribedMsgTask(msgRate_t rate,                ///< The message rate of this task
                             VECTOR<subscribedMsg_t> *ptr,  ///< The subscription message list to send
                             uint8_t priority               ///< The priority of this task
                             );

        /// FreeRTOS task method
        bool run(void *p);

        /// Add a subscription message to be sent at the given rate
        static bool addSubscribedMsg(msgRate_t rate,        ///< The message rate
                                     uint8_t dst,           ///< Destination address
                                     uint16_t msgNum,       ///< Message number
                                     can_msg_t *pCanMsg     ///< Actual CAN message pointer
                                     );

    private:
        canSubscribedMsgTask();                 ///< Private default constructor, do not use.
        const msgRate_t mMsgRate;               ///< This tasks' message rate
        VECTOR<subscribedMsg_t> *mpSubsMsgList; ///< The pointer to the subscription list

        /// The list of subscription message list
        static VECTOR<subscribedMsg_t> *mpSubsList[msgRateLast];
};



#endif /* SUBSCRIPTION_TASK_HPP_ */

/*** subscription_task.cpp ***/
#include <string.h>

#include "subscription_task.hpp"
#include "file_logger.h"



/// Shared list of vector pointers between all of the instances of canSubsribedMsgTask (s)
VECTOR<subscribedMsg_t> *canSubscribedMsgTask::mpSubsList[msgRateLast] = { 0 };

/// Extern methods (defined at another *.c file)
extern "C" uint32_t make_id(uint8_t dst, uint16_t msg_num);



canSubscribedMsgTask::
canSubscribedMsgTask(msgRate_t rate,                ///< The message rate of this task
                     VECTOR<subscribedMsg_t> *ptr,  ///< The subscription message list to send
                     uint8_t priority               ///< The priority of this task
                     ) :
    scheduler_task("sendMsg", 3 * 512, priority),   ///< Default constructor calls
    mMsgRate(msgRate1Hz),                           ///< Store our message rate
    mpSubsMsgList(ptr)                              ///< Store our subscription list pointer
{
    /* Set the task rate to call the run() method */
    setRunDuration(msgRateToFreqMs(mMsgRate));

    /* Add our subscribed message list pointer to the list such that
     * addSubscribedMsg() method can add subscribed messages to our list.
     */
    mpSubsList[msgRateToIndex(mMsgRate)] = mpSubsMsgList;
}

bool canSubscribedMsgTask::run(void *p)
{
    can_msg_t msg;
    subscribedMsg_t subsMsg;
    const can_t canbusNum = can1;  /* CAN Bus to use */
    const uint32_t timeoutMs = 50; /* Some reasonable time */

    for (unsigned int i = 0; i < mpSubsMsgList->size(); i++)
    {
        /* Get the item pointer from the list */
        subsMsg = (*mpSubsMsgList) [i];

        /* Copy the CAN message from the CAN message pointer */
        memset(&msg, 0, sizeof(msg));
        msg = *(subsMsg.canMsgPtr);

        /* Form the message ID that we need to use */
        msg.msg_id = make_id(subsMsg.dstAddr, subsMsg.msgNum);

        /* We must be able to at least queue the message without a timeout otherwise
         * either the CAN Bus is over-utilized or our queue sizes are too small.
         */
        if (!CAN_tx(canbusNum, &msg, timeoutMs)) {
            LOG_ERROR("Error sending message from %uHz task within %u ms",
                      msgRateToFreqMs(mMsgRate), timeoutMs);
        }
    }

    return true;
}

bool canSubscribedMsgTask::addSubscribedMsg(msgRate_t rate, uint8_t dst, uint16_t msgNum, can_msg_t *pCanMsg)
{
    bool ok = false;

    /* Populate the fields of the subscribed message */
    subscribedMsg_t subsMsg;
    subsMsg.dstAddr = dst;
    subsMsg.msgNum = msgNum;
    subsMsg.canMsgPtr = pCanMsg;

    /* Look up the vector for this msgRate_t(rate), and check if it has capacity */
    VECTOR<subscribedMsg_t> *vectorPtr =  mpSubsList[msgRateToIndex(rate)];

    if (! (ok = (NULL != vectorPtr))) {
        LOG_ERROR("Vector pointer for %uHz task was NULL, was the task created?",
                   msgRateToFreqMs(rate));
    }
    else {
        /* Add the subscription message to our list or if no capacity, log an error*/
        if ((ok = (vectorPtr->size() < vectorPtr->capacity()))) {
            /* TODO: Check the vector to avoid double subscription, so make sure the
             *       vector doesn't contain the same message number and destination addr
             */
            (*vectorPtr).push_back(subsMsg);
        }
        else {
            LOG_ERROR("List capacity for %uHz task has exceeded maximum subscriptions",
                      msgRateToFreqMs(rate));
        }
    }

    return ok;
}