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)
{
    /**
     * We use magic numbers here, but each message ID (such as 0x401) should be
     * part of an enumeration that is shared between different controllers.
     */
    can_msg_t msg = { 0 };
    msg.msg_id = make_id(cid_geographical_controller, 0x401);

    /**
     * Send the message to the geographical controller to subscribe to
     * GPS data to be sent at 5Hz:
     */
    msg.frame_fields.data_len = 1;
    msg.frame_fields.is_29bit = 1;
    msg.data.bytes[0] = 5;
    CAN_tx(can1, &msg, portMAX_DELAY);

    /**
     * Now, we should be able to retrieve our data from the geographical controller at 5Hz.
     * Since we may have subscribed to many messages, you will need to see pCid->src to find out
     * where the message came from, and then also check the pCid->id to detect what message it is,
     * and then parse the data bytes into our variable types.
     */
    if (CAN_rx(can1, &msg, portMAX_DELAY)) {
        controller_id_t *pCid = &(msg.msg_id);

        /**
         * Check if we got the response from the GPS controller
         * If you subscribe to a lot of messages, you might want to put this 
         * in a large switch statement that calls functions.
         */
        if (pCid->src == cid_geographical_controller)
        {
            if (pCid->msg_num== 0x501) /* 0x501 is a magic num, should be an enum */
            {
                float longitude = * (float*) &(msg.data.bytes[0]);
                float latitude  = * (float*) &(msg.data.bytes[4]);
            }
        }
    }

    return 0;
}

/* Either a "plain vanilla" FreeRTOS task, or run() method of scheduler_task */
void main_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("I don't expect this message!");
            }
        }
    }
}
void subscription_task(void *p)
{
    /* Subscription message loop at 20Hz
     * This should send all messages ranging from 1-20Hz
     */
    while (1) {
        subscription_send();
        vTaskDelay(OS_MS(50));
    }
}

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

#include <stdint.h>

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



/**
 * 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,

    /// 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()))) {
            (*vectorPtr) += subsMsg;
        }
        else {
            LOG_ERROR("List capacity for %uHz task has exceeded maximum subscriptions",
                      msgRateToFreqMs(rate));
        }
    }

    return ok;
}