RoboMaster electronic control framework based on RT-Thread (6)

RoboMaster electronic control framework based on RT-Thread (6)

Because of RT-Thread’s stable and efficient kernel, rich documentation and tutorials, active community atmosphere, as well as device driver framework, Kconfig, Scons, log system, and massive software packages… it is difficult not to choose RT-Thread for project development. . But it is precisely because of the wide coverage of these advantages that many beginners will find it difficult to start, but as soon as you step into the door of RT-Thread, you will discover its beauty. This series of documents will be used as a record and sharing of my development of the RoboMaster electronic control framework based on RT-Thread. I hope it can help more friends who are new to RT-Thread. Everyone is welcome to communicate and share, correct shortcomings, and make progress together.

Background

The development board used is DJI’s RoboMaster-C development board, and the basic project is rt-thread>bsp>stm32f407-robomaster-c

MSG module development

The MSG module is mainly used for communication between application layer threads, implementing a communication method in which publishers publish topics and subscribers subscribe to topics. Next, we mainly discuss the original intention of developing the MSG module and its comparison with other inter-thread communication methods.

The original intention of the development was to develop a set of simple and easy to use inspired by ROS and other schools such as the soft bus center of Jilin University and the message center module of Hunan University (mainly because of the lack of global variables). , Thread safety, Module decoupling, and Clear logic inter-thread communication mechanism are particularly important.

But RTOS already provides inter-thread communication mechanisms such as mailboxes, message queues, and signals (soft interrupts, which will not be discussed in detail here). Why not use them directly? Next, we will conduct a comparative analysis of each communication method:

• Mailbox: Each email in the mailbox can only hold a fixed 4-byte content. When larger data needs to be transmitted, the pointer can be transmitted as the content of the email. But there is a problem with using the mailbox: when receiving the mail, the mail will be taken out. In other words, there is only one copy of the sent letter. After it is received by one thread, other threads will not be able to receive the letter. Therefore, faced with multiple Problems arise when there is a consumer situation. Moreover, the mailbox mechanism requires an existing producer to send an email before the corresponding consumer can read the email. In other words, each read is actually blocking. Before the producer’s email is sent, the consumer is If you can’t read the data, you can’t read the historical data from the last time it was used. Therefore, as long as the generator doesn’t send the email in time, problems will easily occur. However, in the development of robots, we often face multiple consumers, and when the data is not updated in time, the last data is used first in order to maintain the control frequency (of course, if the data has not been updated for a long time, the historical data is used) This is absolutely not possible! It can range from jittering to loss of control! For example, the remote control and motor are offline, etc. There are corresponding offline detection and processing mechanisms for these situations).
• Message queue: The message queue is an extension of the mailbox. The transmitted data includes the pointer to the message and the length of the message, which facilitates data analysis and acquisition. However, the disadvantages of the above-mentioned email communication method still exist.
• Global variables: Global variables are direct and fast to read and write, and are simple to use. However, if they are abused in RTOS, the harm will be terrible. We should try to avoid using global variables. The specific effects and harms will not be discussed here.
• MSG module: The msg module mainly solves the above-mentioned problems of mailboxes and message queues facing multiple consumers, and avoids the situation of global variables flying all over the sky. The msg module is relatively lightweight and is a rough imitation of the ROS communication mechanism, imitating the communication mechanism between topics, publishers, and subscribers. When a subscriber reads a topic, it will not retrieve or change the topic data, and it will not affect other subscribers’ reading of the topic. And the subscriber is not blocked when reading the topic, and the publisher does not need to update the topic first. There is no order between the subscriber and the publisher. Moreover, it is thread-safe and uses semaphores for protection. However, the reading and writing speed is definitely not as fast as using global variables directly, but it can basically be ignored. The advantages far outweigh the disadvantages.

Code implementation

This is the definition of several important objects such as topic, subscriber, and publisher. Topic objects include names (subscription and publishing topics are mainly judged and linked by names), pointers to data, and semaphores that provide security; publishers and subscribers include topic names, pointers to topics, and message lengths. It should be noted here that the data format of the msg example in the topic needs to be known by both the publisher and the subscriber in order to parse it correctly, which is similar to msg in ROS.

/**
 * @brief topic type
 *
 */
typedef struct topic
{<!-- -->
    char name[MSG_NAME_MAX];
    void *msg; // Pointer to msg instance
    rt_sem_t sem;
} topic_t;

/**
 * @brief Subscriber type. Each publisher has a publisher instance and can access all subscribers to the topics it publishes through a linked list
 *
 */
typedef struct sublisher
{<!-- -->
    const char *topic_name;
    topic_t *tp; // topic pointer
    uint8_t len; // Message type length
} subscriber_t;

/**
 * @brief publisher type. Each publisher has a publisher instance and can access all subscribers to the topics it publishes through a linked list
 *
 */
typedef struct publisher
{<!-- -->
    const char *topic_name;
    topic_t *tp; // topic pointer
    uint8_t len; // Message type length
} publisher_t;

The following are the functions for subscriber and publisher registration. Their registration does not need to be in order:

/**
 * @brief Register as a news publisher
 *
 * @param name The topic name (topic) published by the publisher
 * @param len message type length, obtained through sizeof()
 * @return publisher_t* Returns the publisher instance
 */
publisher_t *pub_register(char *name, uint8_t len){<!-- -->
    uint8_t check_num = check_topic_name(name);
    publisher_t *pub = (publisher_t *)rt_malloc(sizeof(publisher_t));

    if(!check_num){<!-- --> // If there is no topic instance with the same name
        topic_obj[idx] = (topic_t *)rt_malloc(sizeof(topic_t));
        rt_memset(topic_obj[idx], 0, sizeof(topic_t));
        if(topic_obj[idx] == NULL){<!-- -->
            LOG_E("malloc failed!");
            return NULL;
        }
        topic_obj[idx]->msg = (void *)rt_malloc(len);
        if(topic_obj[idx]->msg == NULL){<!-- -->
            LOG_E("malloc failed!");
            return NULL;
        }
        topic_obj[idx]->sem = rt_sem_create(name, 1, RT_IPC_FLAG_PRIO);
        rt_strcpy(topic_obj[idx]->name, name);
        pub->tp = topic_obj[idx];
        pub->topic_name = topic_obj[idx]->name;
        pub->len = len;
        idx++;
    }
    else{<!-- -->
        pub->tp = topic_obj[check_num - 1];
        pub->topic_name = topic_obj[check_num - 1]->name;
        pub->len = len;
    }

    return pub;
}

/**
 * @brief Subscribe to name's topic messages
 *
 * @param name topic name
 * @param data message length, obtained through sizeof()
 * @return subscriber_t* Returns the subscriber instance
 */
subscriber_t *sub_register(char *name, uint8_t len){<!-- -->
    //The same process as the publisher, directly call the publisher's registration function
    subscriber_t *sub = (subscriber_t *)pub_register(name, len);

    return sub;
}

When registering, create a corresponding topic based on the incoming topic name and message length. It will first check whether there is a topic with the same name among the existing topics. If so, the existing topic address will be returned. If not, a new topic will be created. . Therefore, the name of the topic is very important. If the names of subscribers and publishers are different by one character when they are created, they will not match. It is recommended that the name of the topic be managed directly using macro definitions to avoid errors (otherwise an error will occur It’s hard to check)

The following is a simple implementation of subscribers and publishers handling topics:

/**
 * @brief Post a message
 *
 * @param pub publisher instance pointer
 * @param data data pointer, put the message to be published here
 * @return uint8_t A return value of 0 indicates that the release failed, and a return value of 1 indicates that the release was successful.
 */
uint8_t pub_push_msg(publisher_t *pub, void *data){<!-- -->
    if(rt_sem_take(pub->tp->sem, RT_WAITING_NO)){<!-- -->
        LOG_W("take sem failed!");
        return 0;
    }
    rt_memcpy(pub->tp->msg, data, pub->len);
    rt_sem_release(pub->tp->sem);
    return 1;
}

/**
 * @brief Get the message
 *
 * @param sub subscriber instance pointer
 * @param data data pointer, the received message will be placed here
 * @return uint8_t A return value of 1 indicates that a new message has been obtained
 */
uint8_t sub_get_msg(subscriber_t *sub, void *data){<!-- -->
    rt_memcpy(data, sub->tp->msg, sub->len);
    return 1;
}

It can be seen that it is currently very simple (lightweight (not)). It is only protected by using a semaphore when the publisher updates the topic, thereby ensuring the integrity of the write. However, there is no protection for the subscriber to obtain the message, because as long as it is guaranteed In order to ensure the integrity of the publisher’s updated data, there is no problem in subscribing to read topics. But there are still many areas that need to be improved, and the robustness still needs to be strengthened. For example, rt_memcpy does not guarantee atomicity. The main purpose here is to provide a solution Ideas and suggestions.

Usage examples

/* First, you need to define the data format of the topic in the .h file */
struct msg_test{<!-- -->
    uint8_t id;
    uint8_t data[5];
}

/* In the publisher's .c file */
publisher_t *pub;
stuct msg_test msg_p;
pub = pub_register("msg_test", sizeof(struct msg_test));
msg_p.id = 1;
for(uint8_t i = 0, i < 5, i + + ){<!-- -->
    msg_p.data[0] = i;
}
pub_push_msg(pub, & amp;msg_p);

/* In the subscriber's .c file */
subscriber_t *sub;
stuct msg_test msg_s;
sub = sub_register("msg_test", sizeof(struct msg_test));
sub_get_msg(sub, & amp;msg_s);

Existing problems

It can be seen that the current msg module is still very crude, and there are still many areas that can be improved. And it is also possible that I am not familiar enough with the existing inter-thread communication mechanism of RTOS and cannot apply it skillfully, so I need to magically modify a set of such communication mechanism by myself. (I wonder if RT-Thread will provide a communication mechanism for subscribers, publishers, and topics in the future? Or a software package or the like) You are welcome to provide suggestions for improving and optimizing this module, and everyone is welcome to communicate!