Linux kernel modules for robotic devices
UPDATE 1: I received the important comment about priorities in Linux kernel space. The OS can choose what should be done right now and choose the process with higher priority. Or kernel might be busy for a long time. So in situation that someone is using kernel for a very long time, these programs might not work in appropriate way. But if you are developing Linux kernel module, the best practice is using interruptions and not to use in these processes complex algorithms or operations.
UPDATE 2: The value in servo should not be zeroed. I found out that after the publishing story. In case when we set it to zero, the servo can be late to turn the whole angle.
As everything in 2022 — my plans were a little ruined. Well, ok, not so dramatic — I should correct my previous plans about developing self-driving system.
I told you about my global and local tasks here. And I stopped on the part, where I talked about communication with devices using WiringPi library. I had planned to go further with building map from sonar and odometry data. At that moment I was sure, that all I need is to code the other conversations with devices like that.
But I faced several problems which became a reason for delay new story and changed my plans.
So to decide some of these problems I used Linux kernel modules development. And I would like to focus on it in this chapter.
The problems and solutions.
The first thing that went wrong — you cannot be sure that defined quantity of microseconds would be counted precisely because Linux is not real-time operating system, so it has interruptions and inter-applications switches. But for working with servo the precise quantity of microseconds are critical, because it depends on angle you want to turn linearly. You can take a look on the Arduino code here.
Let’s remember what we should do to turn the servo in defined angled. First, we should count delay in microseconds which is needed to turn the right angle. This delay is set between HIGH and LOW signal. The delay is laid between 500 and approximate 2500 microseconds. Here is the formula from the first story: Time = k * angle + minimum_range where k is 11 here.
The same thing is with Ultrasonic Sensor, for counting precise distance, you should be sure in sequence of signals and delays between them.
I could build the ROS-Arduino connection, but I am not searching for easy ways. And I decided to work with servo and sonar with building kernel modules.
Linux Kernel Modules.
So let’s go to Linux kernel development.
All Linux OS consists of many kernel modules, so you have possibility to add, edit or remove something is OS code and develop your own Linux kernel modules. The example of Linux kernel diagram is on Image 1.
As a Software Engineer you are coding app that communicate to OS through special API which is hidden. This API can translate our code to kernel modules and than they translate it to hardware.
Using Linux kernel module developing we are building our app near to hardware, so it allows us to do more things in more direct ways: like to set entire quantity of microseconds or to communicate with device directly without using driver or use our own serialization interface etc. To be more precise, we are developing our own device’s driver.
The main task and subtasks.
So, our main task here is to write the two kernel modules for communication with sonar and servo.
What we need for that is:
- Struct file_operations which contains callbacks for different operation: read, write, open, release. As you can remember, in Linux OS everything is a file, which can be opened, read etc. So you should define all that operations;
- For precise time counter, we should define struct hrtimer. It is high-resolution kernel timer, which can count precise time without delays, because it is done from kernel;
- Also we need a structure for saving intermediate data;
- We should write init function and exit function for kernel module.
Servo kernel Module.
We begin with servo. So let’s define file operations and callbacks to them. We do not need to read anything, so read = 0.
// The prototype functions for the character driver -- must come before the struct definition
static int dev_open(struct inode *, struct file *);
static int dev_release(struct inode *, struct file *);
static ssize_t dev_write(struct file *, const char *, size_t, loff_t *);
// static loff_t dev_llseek(struct file *file,loff_t offset, int orig);
static struct file_operations fops =
{
.open = dev_open,
.read = 0,
.write = dev_write,
.release = dev_release,
.llseek = 0
};Then let’s define the structure for saving data:
struct servo_file {
u16 read_pos;
char buffer[4];
};typedef struct servo_file servo_file_t;
We are going to get int values in bytes in little endian, so we have the buffer of size 4. Also we save the read_pos in case we are receiving the data partially for some reason.
In dev_open we just allocate the servo_file structure. The important difference in memory allocation here — the function kzalloc instead of malloc.
We are most interested in dev_write function. With it help we will receive the angle value and do turn to defined angle. Here we are reading char by char the value from user space (you cannot interact with user space directly from Linux kernel). And than just assign the byte array value to int value. Here value is a global variable.
static ssize_t dev_write(struct file *filep, const char *buffer, size_t len, loff_t *offset)
{
ssize_t r;
char c;
servo_file_t *sf = (servo_file_t*)filep->private_data;for (r = 0; r < len; r++, buffer++) {
if (get_user(c, buffer) || r > 4) {
return -EFAULT;
}
sf->buffer[sf->read_pos] = c;
sf->read_pos += 1;
}
if (r == 4) sf->read_pos = 0;
value = *(u32 *)sf->buffer;
printk(KERN_INFO "value changed to %d", value);
return r;
}
We also have two structs hrtimer and their callbacks defined in member function. Structs hrtimer are defined in init function. Also there we assign the pin number for interaction with servo. The pin number should be defined by user during the kernel module loading, it would be showed later.
* Function to init the module */
static __init int servo_init(void)
{
if (pin < 0) {
printk(KERN_ERR "servo: You must specify servo pin\n");
return -EINVAL;
} // register character device and request major number
majorNumber = register_chrdev(0, DEVICE_BUS, &fops);
if (majorNumber < 0) {
printk(KERN_ERR
"servo: failed to register a major number\n");
servo_free();
return -EPERM;
}
// register the device class
servo_class = class_create(THIS_MODULE, CLASS_NAME);
if (IS_ERR(servo_class)) {
printk(KERN_ERR
"servo: failed to register device class: %ld\n",
PTR_ERR(servo_class));
servo_free();
return -EPERM;
}
// register the device driver
rc_device = device_create(servo_class, NULL, MKDEV(majorNumber, DEV_MINOR), NULL, DEVICE_NAME);
if (IS_ERR(rc_device)) {
printk(KERN_ERR
"servo: failed to create the TX device: %ld\n",
PTR_ERR(rc_device));
servo_free();
return -EPERM;
} // prepare pin for the receiver
pin_desc = gpio_to_desc(pin);
if (IS_ERR(pin_desc)) {
printk(KERN_ERR "servo: pin gpiod_request error: %ld\n",
PTR_ERR(pin_desc));
servo_free();
return -EPERM;
}
// output
gpiod_direction_output(pin_desc, 0); // allocate and init timers
tx_low_timer = kzalloc(sizeof(struct hrtimer), GFP_KERNEL);
if (!tx_low_timer) {
printk(KERN_ERR "servo: can't allocate memory for timer\n");
servo_free();
return -ENOMEM;
}
hrtimer_init(tx_low_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
tx_low_timer->function = tx_low_callback;
// and second one
tx_high_timer = kzalloc(sizeof(struct hrtimer), GFP_KERNEL);
if (!tx_low_timer) {
printk(KERN_ERR "servo: can't allocate memory for timer\n");
servo_free();
return -ENOMEM;
}
hrtimer_init(tx_high_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
tx_high_timer->function = tx_high_callback;
hrtimer_start(tx_high_timer,
ktime_set(0, PERIOD * 1000UL),
HRTIMER_MODE_REL); printk(KERN_INFO "servo: driver started\n");
return 0;
}
As you could notice, the pin is parameter. So we should define it as parameter:
module_param(pin, int, S_IRUGO);
MODULE_PARM_DESC(pin,"Servo pin number");Let’s take a look to callbacks. Here we have timer which count every 20ms — it is our cycle. If value is more than zero, than we send logic 1 to pin, than wait for our defined quantity of microseconds, send logic 0 and change value to 0.
/* TX high timer callback */
static enum hrtimer_restart tx_high_callback(struct hrtimer *timer)
{
if (value) gpiod_set_value(pin_desc, 1);
hrtimer_try_to_cancel(tx_low_timer);
if (value) hrtimer_start(tx_low_timer,
ktime_set(0, value * 1000UL), HRTIMER_MODE_REL);
hrtimer_start(tx_high_timer,
ktime_set(0, PERIOD * 1000UL), HRTIMER_MODE_REL);
return HRTIMER_NORESTART;
}/* TX low timer callback */
static enum hrtimer_restart tx_low_callback(struct hrtimer *timer)
{
gpiod_set_value(pin_desc, 0);
value = 0;
return HRTIMER_NORESTART;
}
So after the kernel module is ready, we call make. The Makefile for kernel modules looks little different:
If everything went well, we should receive three new files with extensions .ko, .mod and .o .
Then, to add the device to /dev/ — we call insmod servo.ko pin=17 — where pin=17 is parameter.
Now we can control the servo with simple Python script:
Here we are using not the angle value, but the delay value. This value is between 500 ms (0 degrees) and 2500 ms (180 degrees). Values are less or more will be celled or floored to this interval.
The tough thing about kernel module development is debugging. All you can use for debugging — is logs. You can see logs of your kernel module with command dmesg. Here is my logs:
[27587.094752] value changed to 2400
[27604.612502] value changed to 300Sonar Kernel Module.
Let’s now take a look to the code of kernel module for communication with Ultrasonic sensor. Here the things are quite similar so I am not going to repeat the all structure. Let’s just take a look to what is different here. I will leave the link to my repo with code in the end.
To refresh the memory: we should send 1–0–1 to trig pin with delays 2–10 microseconds between sends. Then receive the value from echo pin. Here the same thing, here we should define only dev_read. And global variables where we are going to save our difference in values of sending and receiving the signals.
// The prototype functions for the character driver -- must come before the struct definition
static int dev_open(struct inode *, struct file *);
static int dev_release(struct inode *, struct file *);
static ssize_t dev_read(struct file *, char *, size_t, loff_t *);
static u64 trig_time = 0;
static int bus_number_opens = 0;
static struct file* opened_files[MAX_OPENED_FILES];
static u64 echo_delay = 0;static struct file_operations fops =
{
.open = dev_open,
.read = dev_read,
.write = 0,
.release = dev_release,
.llseek = 0
};
And here is the dev_read. I have done it as char buffer because on that step it is more useful for me. You can read the /dev/sonar like text file and use, for example, reading line by line. Or you can read only the latest received value.
static ssize_t dev_read(struct file *filep, char *buffer, size_t len, loff_t *offset)
{
int numlen = 0;
int i, j;
char c;
ssize_t r;
sonar_file_t *sf = (sonar_file_t*)filep->private_data; // no data yet
if (!sf->read_pending) {
if (filep->f_flags & O_NONBLOCK)
return -EAGAIN;
if (wait_event_interruptible(rx_wq, sf->read_pending))
return -ERESTARTSYS;
} // remember current value
if (sf->read_pos == 0) sf->read_value = echo_delay; // get length of value in chars
i = sf->read_value;
do {
i /= 10;
numlen++;
} while (i);for (r = 0; sf->read_pos <= numlen && r < len; r++, sf->read_pos++, buffer++) {
if (sf->read_pos < numlen) {
j = sf->read_value;
for (i = 1; i < numlen - sf->read_pos; i++)
{
j /= 10;
}
c = '0' + (j % 10);
} else {
sf->read_pending = 0;
c = '\n';
}
put_user(c, buffer);
}
if (sf->read_pos >= numlen) sf->read_pos = 0;return r;
}
And we also have hrtimer structures and callbacks. Here we have a timer for trig_duration, in which we are sending the sequence of 1–0–1 to trig pin.
/* Trigger high timer callback */
static enum hrtimer_restart tx_high_callback(struct hrtimer *timer)
{
trig_time = ktime_to_us(ktime_get_boottime());
if (trig_duration) gpiod_set_value(trig_pin_desc, 0);
hrtimer_try_to_cancel(tx_low_timer);
if (trig_duration) hrtimer_start(tx_low_timer,
ktime_set(0, trig_duration * 1000UL),
HRTIMER_MODE_REL);
hrtimer_start(tx_high_timer, ktime_set(0,
trig_interval * 1000UL),
HRTIMER_MODE_REL);
return HRTIMER_NORESTART;
}/* Trigger low timer callback */
static enum hrtimer_restart tx_low_callback(struct hrtimer *timer)
{
gpiod_set_value(trig_pin_desc, 1);
return HRTIMER_NORESTART;
}/* IRQ fired every falling edge of echo pin */
static irq_handler_t echo_irq_handler(unsigned int irq,
void *dev_id, struct pt_regs *regs)
{
int i;
u64 now = ktime_to_us(ktime_get_boottime());
if (trig_time) {
echo_delay = now - trig_time;
// notify clients
for (i = 0; i < bus_number_opens; i++) {
sonar_file_t* rcf = (sonar_file_t*)opened_files[i]->private_data;
rcf->read_pending = 1;
wake_up_interruptible(&rx_wq);
}
}return (irq_handler_t) IRQ_HANDLED;
}
Here we have interrupt handler, which is used for saving echo values. This callback is called every time we have something to read from echo_pin. We have trig_time which we have received before. echo_delay is difference between current time and trig_time. It is our value from sonar which we are going to translate to distance later.
Now we can read from /dev/sonar values in time delays.
cat /dev/sonar
2364
2404
2357
2333
2357
2428
2354
2354
2332
2355
2334
2354
2331
2333
2356
2333
2356
2377
2431
2356
2355So this is how we work with servo and sonar by kernel modules.
Here is the link to repo: https://github.com/olesyaksyon/my_awesome_hell_machine
I hope it was not boring:) Thanks to Cluster who helped me to go deeper in embedding software.
In the next chapter I will tell you about hardware improvement. Also I have received the IMU sensor. I will use it data for receiving odometry and transform information. I am going to tell about I2C interface and how to translate IMU data to odometry.
Thank you and see you next chapter!