Autonomous parafoil flight controller - UART VS interrupts

Status
Not open for further replies.

YohanH

Member
Hi there,

I'm a french student working on an autonomous parafoil recovery system, (https://www.youtube.com/watch?v=vNYQW6N5U98 https://hackaday.io/project/176621-r2home) designed for weather balloons or model rockets. I'm working on this project for a little while now, but I'm only really starting to work on the "flight controller" part of the project.

Here is a picture of the system :

r2homelowres.jpg

I already have all the hardware ready for further software development, but I'm starting to reach my (very low) limit in terms of programming knowledge, so I'm looking for advices or ideas, to get me closer to my goal. It mainly concerns issues about serial communication, and time management in a loop.

Here is a picture of the latest version of my flight controller :

DSC_5418.jpg

As you can see, it's mainly just a Teensy 4.1, on a fancy PCB with fancy connectors. There are just also two different powering circuits on it, one for the Teensy, with a buckconverter, and then a linear regulator. And another powering circuit for the servos.

This flight controller need to communicate continuously with multiples devices, like for example a GNSS receiver (SAM M8Q), a barometer (BMP280), an SBUS receiver, a telemetry transceiver (SiK Radio), while also controlling two PWM signals for servos, reading one PWM signal coming as a feedback from one of the servos. The Teensy is also recording ~40 importants flight variables on a µSD card onboard at a rate of 20Hz. Finally the Teensy is managing a buzzer (passive buzzer) and an Adafruit RGB led.

Here is the actual version of the software, it is running OK, but I'm encountering many problems that I'm going to explain just a bit later : https://github.com/YohanHadji/R2Home/tree/main/R2Home_rls_V0.96_DEBUG

If you don't want to read the full code, which I would fully understand, here is the short version with only the main loop :

Code:
 void loop() {
  
  tloop = millis(); 
  
  getdata();
  datacmpt(); 
  
  flight_state(); 
  
  applycmd();
  updatecmd(100);
  
  userinter(); 
 
  loop_time = (millis()-tloop); 
  if (loop_time<100) {watchdog.reset();}

}

What getdata() does is checking :

1. Is there any new char from the GPS ?
2. Is it time to poll the barometer ? If yes -> poll the barometer
3. Get RC commands
4. Check battery voltage

What datacmpt() does is "computing" all the data :

1. Based on GPS data, check if the GPS is healthy or not
2. If GPS has been updated, compute navigation data again
3. Compute vertical speed, from barometric data
4. Run the altitude fusion between barometer and GPS
5. Get all the data into strings, to prepare them for single shot SD datalog, and telemetry send.
6. Check if it's time to log data or send it via telemetry

flight_state() is the flight mode state machine, it contains all the conditions to transition from one flight mode, manual for example, to another, autopilot for example

What applycmd() does is deciding based on the flight mode what goes into the servo command variable, either the manual command, or the autopilot command

What updatecmd(100) does, is updating the servo command at a given rate, 100Hz in that case. Why do I need such a function ? Because one of the servo I'm using, is a, continuous rotation, digital feedback servo. So it means I have to read a feedback at 100Hz or less or more, and update the command I'm giving to this servo with a PID, to be able to control it, not in speed, but by rotation angle, like a normal servo, but on a wider angle range.

Finally what userinter() does is :

1. Feeding the EasyBuzzer lib, this lib is very useful to offload the work to get buzzer alerts on flight mode transition etc.
2. Monitor the battery voltage, and if critical, trigger another buzzer alert. A
3. Changing and updating the state of the Adafruit RGB led. This RGB led is describing the flight mode, and helps me to know if the system is ready for flight.

Now let's talk about the problems I'm facing. The first problem I have, is that I'm having troubles, getting a stable UART communication between the GNSS receiver I'm using, and the Teensy 4.1.

Here is the situation : the GNSS receiver, is sending at a given rate navigation messages trough UART, and I have a small function that is counting how much time has passed since the last UART message has been received correctly by the Teensy 4.1. Now if I run the GPS code part alone, everything is working great, and in a very logical way, this time is oscillating between 0 and 100ms (if for example the GNSS receiver is set to send a message at 10Hz).

However as soon as I run the complete code, the communication isn't stable anymore, and sometime, the Teensy keeps missing GNSS messages for >10s, while the looptime is of course staying well under 1ms. Fortunately, I think I've found a first part of the problem: it becomes way less frequent when I stop executing this line of code that allows to read the pwm feedback value from the servo :

Code:
 pulseIn(feedback_pin, HIGH, 1200)

And this is where I started to learn about interrupts. Here is my understanding, please tell me if I'm wrong: to execute this function, the Teensy needs to stop everything else, wait for the pulse to go high, then for the pulse to go low, and then start everything again. So if the GNSS receiver was transmitting anything while the Teensy was measuring the length of the pulse, then this message is lost (?)

And this is also where I started realizing that maybe this function isn't the only one in my code to be able to freeze the UART communication. The main problem here, is that there are no ways to really precisely know when the GNSS will send a new message to make sure I'm not reading a pulse at the same time, or doing anything else that would also freeze the code.

Is there a generic way to "protect" the UART communication and give it "admin roles" over everything else? In my case getting those GNSS messages is one of the most important thing the µC has to do. I have found nothing so far, but I'm surely just looking for the wrong thing or with the right words. Otherwise are there maybe generic rules for the Teensy to make sure I'm not using a function that can prevent other functions from working properly?

I also have other similar problems on this project, but I think I should solve this one first.

Thank you in advance for your ideas or suggestions.

Yohan
 
There was the "eventResponder" developed to do exactly this, a few years ago.
It is still part of Teensy-Core, and in every program you write.
I don't know much about it, I hope the other users or Paul can give hints and tips how to use it.

That would be the best way.

I'm not aware of a documentation.
Perhaps use Google..

If not, you could just add a line to the serial code and call a callback. Or use a technique like interrupt chaining, which was standard long years back and is a forgotten technique. It is very simple to use.
 
Last edited:
However, I'd take a close look at pulseIn- not sure how it is implemented but I can imagine it is unreliable when longish interrupts happen. Again, best it to a look at its code. If it uses micros(), a hardware timer or such, its ok.
 
Thanks, I'm going to take a look at it, I can see many thing poping on google about it.

About pulseIn, what I can say, is that a timeout is implemented, and in my case, the timeout is set at 1200 µs, so in principe, pulseIn won't interrupt the code for more than 1.2ms, but apparently it is already way more than enough to freeze the UART communication.
 
Depends. The UART code of course already uses an interrupt and buffers all incoming data as long it fits into the buffer... the buffer size can be increased. But I'm not sure if you want this.. it still would wait for your timeout (worst case).
So you may really want to use an Interrupt.

I hope someone chimes in and tells us details about the event responder. It's one of the few things I've never used... :) But I can imagine it can be useful here.
 
My current fear and understanding of the problem is that for some reason this pulseIn is preventing the Teensy from storing anything in the buffer. If despite pulseIn, or any other interrupt being executed, it was still able to read the message at the end of the pulseIn it would be already very good.

But actually, I have the impression if I understand the problem correctly, that the pulseIn and other interrupts happening during the transmission of the UART message, totally prevent to store the message in the buffer when it arrives. But maybe I'm wrong?

Do you mean that the problem would be that, by the time the pulseIn was done, too many characters arrived in the buffer, and it had to delete the first ones from the list?

That would be a very good news. Maybe I could try to set a bigger buffer size ?
 
Yes I'm pretty sure, that the default buffer is just not large enough.
It will receive during pulseIn(). PulseIn does not prevent the RX of data.

Hi, i digged a bit and found the used code of pulsIn:
Code:
uint32_t pulseIn_high(uint8_t pin, uint32_t timeout)
{
    const struct digital_pin_bitband_and_config_table_struct *p;
    p = digital_pin_to_info_PGM + pin;

    uint32_t usec_start, usec_stop;

    // wait for any previous pulse to end
    usec_start = micros();
    while ((*(p->reg + 2) & p->mask)) {
        if (micros()-usec_start > timeout) return 0;
    }
    // wait for the pulse to start
    usec_start = micros();
    while (!(*(p->reg + 2) & p->mask)) {
        if (micros()-usec_start > timeout) return 0;
    }
    usec_start = micros();
    // wait for the pulse to stop
    while ((*(p->reg + 2) & p->mask)) {
        if (micros()-usec_start > timeout) return 0;
    }
    usec_stop = micros();
    return usec_stop - usec_start;
}


// TODO: an inline version should handle the common case where state is const
uint32_t pulseIn(uint8_t pin, uint8_t state, uint32_t timeout)
{
    if (pin >= CORE_NUM_DIGITAL) return 0;
    if (state) return pulseIn_high(pin, timeout);
    return pulseIn_low(pin, timeout);
}

So, yes, it uses micros() - which is good.
Now, let's see how to increase the buffer..
 
SerialX.addMemoryForRead(void *buffer, size_t length) should do it.
Use a long buffer.. :)

perhaps

char myRXBuffer[1024];

...

SerialX.addMemoryForRead(myRXBuffer, sizeof(myRXBuffer) );

(untested)
 
I was testing the exact same thing at the exact same time haha! Apparently the function "availableForRead" doesn't exist though, so atm I have no way to see if there is more memory after adding some..
 
it's just available() - it returns the # of bytes in the rx buffer.
I'm not aware of a function that tells you the size of the buffer. If I remember correctly, the default was 64 (can have changed..)
 
So.. if I want to know the maximum number of bytes in the rx buffer the only thing I can do is fill it up to the maximum right ?
 
Well, what I'm doing is that I'm slowly filling the buffer with another serial port and a wire between the Tx from one, and the Rx of the one I'm monitoring, and I see up to which point SerialX.available() is incrementing. And in my understanding the maximum number given by SerialX.available is the current maximum size of the buffer, isn't it ?
 
yup.

Not sure why you need to find the size of the buffer... you've set it yourself.. isn't the fillgrade more interesting? :)
 
I just wanted to make sure the function changing the size of the buffer was well working on my teensy :)

But the good news are :

1. You had the right idea, it was indeed a buffer size problem

2. Now it is working perfectly

Here is a small before / after, the interesting data is in red, it is the time in ms since the last GNSS UART message, before, you can see that there were many many data stales, and now everything is staying under 250ms, and the update rate is set at 5Hz, so this is perfectly normal.

Before :

Capture d’écran 2021-08-20 à 16.07.13.png

After : Capture d’écran 2021-08-20 à 16.03.57.png

(Make sure to check the scale on the left to really see the difference between both plot)
 
As someone who develops flight controllers around the Teensy series of microcontrollers (https://github.com/bolderflight/spaaro), what I generally do, in pseudo code is:

Code:
void daq() {
  GetImuData();
  ReadSbus();
  GetPressureData();
  GetGnssData();
  RealTimeEstimation();  // apply low pass filters and estimate all the vehicle states I care about (i.e. attitude, altitude, airspeed, flight path angle, etc).
  ControlLaws();
  AddToDataLogBuffer();
  SendTelemetry();
}

void loop() {
  WriteDatalog();
}

void setup() {
  InitEverything();
  AttatchInterrupt(daq, IMU_DRDY, RISING);
}

So I tend to run my data acquisition, real-time filtering and estimation, and control laws in an interrupt. The interrupt is triggered from the IMU data ready interrupt. Datalog data is added to a buffer and then written to disk in a lower priority loop. For serial data, like SBUS, telemetry, and GNSS, I simply increase the Serial buffer sizes to hold everything until the next loop (SBUS and uBlox data are well defined rate and size, so I can compute the appropriate size Serial buffers to use) and my SBUS and uBlox drivers are written to read through all of the packets in the buffer and use the newest one. Works great for me and this setup means that I don't miss data frames.

It is similar to how event responder is supposed to work (I think), but I haven't been able to understand it well enough.
 
Have you tried the library I mentioned?

I have started to take a look at it, but I'm not sure it will work, because the signal I'm trying to monitor is a PWM signal and not a PPM signal, but the idea is still the same, you want to measure the length of a pulse HIGH, or LOW.
 
As someone who develops flight controllers around the Teensy series of microcontrollers (https://github.com/bolderflight/spaaro),

I have to say I'm really impressed by your work!

I think my code is actually following a bit the same idea, the only difference is maybe that my control loop is only based at the moment on the GNSS course over ground, which isn't really a high rate data ([1Hz ; 10Hz]), in comparison to data that would coming from an IMU at > 1kHz for example.

For example my autopilot algorithm is only executed just after the GNSS message being received, decoded and validated.

For the SD datalogging what I'm doing is that I update the buffer every loop, because some flags monitored can possibly be changed at every loop, and then I check if either one of this important flag has been changed, of if enough time has passed to get the data recording rate that I want, to trigger the copy of the buffer on the SD card.
 
I have to say I'm really impressed by your work!

I think my code is actually following a bit the same idea, the only difference is maybe that my control loop is only based at the moment on the GNSS course over ground, which isn't really a high rate data ([1Hz ; 10Hz]), in comparison to data that would coming from an IMU at > 1kHz for example.

For example my autopilot algorithm is only executed just after the GNSS message being received, decoded and validated.

For the SD datalogging what I'm doing is that I update the buffer every loop, because some flags monitored can possibly be changed at every loop, and then I check if either one of this important flag has been changed, of if enough time has passed to get the data recording rate that I want, to trigger the copy of the buffer on the SD card.

Aircraft, both fixed-wing and multirotor, can be well controlled at much lower rates. We used to run at a 50Hz loop rate and were even successfully controlling vehicles that had flexible structures (with structural modes up to 10Hz) that needed active control to suppress flutter. We currently run at 100Hz - I don't really understand why some projects choose to run in the kHz range. Most vehicle dynamics are in the 1 - 5 Hz range and many actuators have a bandwidth around 10 Hz (i.e. can't effectively control the vehicle higher than that).

In any case, I suspect that you might end up needing near a 25 Hz to 50 Hz system. You'll want to collect data and compute control commands at a rate several times higher than the dynamics. Besides, how do you control the ground track? For fixed-wing aircraft, ground track is controlled by bank angle and I suspect for parafoils it is the same. So even though ultimately we're trying to control ground track, we need to measure and control the higher rate dynamics of bank angle. We have an outer loop navigation filter and control law to estimate the ground track error and compute a bank angle command to correct the error. That feeds into an inner loop navigation filter and control law estimating the current bank angle and the actuator command necessary to reduce the bank angle error.

Even though your GNSS is outputting ground track, you might want to look into an Extended Kalman Filter (EKF) with an IMU. Dan Simon has an excellent book ("Optimal State Estimation"). The EKF outputs will be more accurate since you'll be adding information from other sensors, such as the yaw gyro. You'll also be able to get state data at a high rate and estimate other values (like pitch and roll angles).

We update our logging buffer every loop too. It's dumped into a circular buffer and then written to disk as a lower priority task. That task split means that we don't have to worry about latency in writing the data to the SD card.
 
Agreed, even my balancing robot did not need more than a few Hertz. I think at the end I used 10 Hz or such..
 
Depends on the performance you want - even a very drunk person can keep their balance, just don't expect it to be as solid
as someone with fast reactions! Same with any control loop - latency will reduce possible performance.
 
The by far heaviest brake is physics of movements. Its not that fast. Does not matter if you use 100Hz or 1000.
Might be counterintuitive- but it is as it is. You can't move a mass with the speed you *want*. With the "performance" you *want*

Try to stop your car in 1/1000 second. You really really want this.
Try to stop much less - say 1kG in 1/1000 second. It's not possible, too.

It really is meaningless if you use 10 Hz, 100 Hz or 1000Hz. The speedup is very very close to zero.

Sure, if you feel better, just use 1000Hz. But the result will be not better.
 
Last edited:
Hi All!

After a week full of testing and improvements I can tell you that a 5Hz control loop is more than enough for the control I want to have, all this using only the GPS course over ground.

Here is an example of the result I got today, every color is a different autonomous flight back to home.

this_is_r2home.jpg

The controller is a PD controller (PID without I) and I had to add an attenuation function based on the rotation speed, if the rotation speed is higher than x, the command is gradually attenuated. This allows to "erase" the pendulum swing of the wing after a turn, and also to get out of a death spiral automatically. It also ensures that the PD controller is correct for a wide range of wing loading and flight altitudes.
 
Status
Not open for further replies.
Back
Top