dRehmFlight VTOL - Teensy (Arduino) Flight Controller and Stabilization

[Mohamed]

Did you use interrupt-capable pins to read the pwm channels? You can comment out the failsafe() function in the void loop to see what, if any, values are coming through to the pins

so I connected ch5 of the receiver to ch5 and ch6 on the flight controller and uncommented the failsafe function so it can detect a failsafe normally. As expected all the channel were displaying the output values of the receiver and not the failsafe values, and ch5 and ch6 had the same values.(because I connected them together)

the values seems to jump around a bit

the output frequency of my receiver is variable. do you thing that would change something, like make a bit stable?

other issues is that the servos move further when the IMU is moved than when there is a stick input (here I have : s2_command_scaled = 0.5 - roll_PID; ) , i assigned thro_des to another servo and that one has full range. ( s1_command_scaled = thro_des ; )

does it have anything to do with the stabilized axis variables?

I'm going to put this flight controller on the ft simple scout for a test, it's a 4 channel, plane

 

[mjetski]

Hi Nick. I'm new here and been playing with the FC on Teensy 4.1. I have hooked it all up and have it running. However sometimes the loop rate goes from 500 to over 10000 and then stays there until rebooted. Sometimes there are also looprate spikes?

Not sure what would be causing this? It happens when `i just let it sit on the workbench after about 10 min or sometimes 15 min.

I'm attaching some screenshots to show the issue.

M

Hi all,

I'm sharing my custom flight controller code written for the Teensy 4.0 and MPU6050 IMU. The best summary I've been able to prepare thus far:

"dRehmFlight VTOL is a bare-bones flight controller for hobbyists or researchers wanting to get something flying fast with the ability to quickly and intuitively adapt to unique configurations. The Arduino environment makes it easy to make your additions--from basic functionality to sensor integration to custom inner-loop control--without the hassle of dealing with spaghetti code cluttered by features you'll never use."

Download: https://github.com/nickrehm/dRehmFlight

Features:

• Code modifications and compiling done within the Arduino IDE with Teensyduino add-on.
• Default code supports 6 ESC outputs using OneShot125 Protocol, and 7 conventional PWM outputs for ESCs or servos, with the ability to modify the code for extra outputs for custom setups.
• Support for conventional PWM, PPM, or SBUS receivers.
• MPU6050 and MPY9250 IMUs supported.
• Easy to use control mixer with stabilized axis variables and ability to pass direct, unstabilized commands to the motors or servos direct from the transmitter.
• Three PID controller types including rate and angle-based setpoint.
• Simple variable fading, with support for more advanced options planned in the future.
• Default hardware setup (Teensy 4.0 and MPU6050 IMU) costs less than $30 and weighs less than 15 grams.
• Comprehensive documentation with explanation of every function and variable, as well as tutorials for setting up the hardware and modifying the code for your application.

This is not just meant to stabilize multirotors (though it does that really well). Here is an example of what you can do with the stock code after adding ~20 additional lines of code in the provided control mixer:

So, if you know a little bit about Arduino, this project may be for you to get practically any vehicle type stabilized in the air. If you don't know about Arduino--don't worry. Check out the provided documentation (GitHub download) which includes a complete overview of the code as well as detailed tutorials for setting up the hardware and modifying the code for your application. I'm also releasing a video series covering everything (+more) in the documentation:

Code Walkthrough Video: https://www.youtube.com/watch?v=_n5GBudUf5Q&feature=youtu.be

Hope some of you tinkerers find this project useful,

Nick

 

[NickRehm]

is there a possibility that the failsafe values are displayed because I'm not using ch6
I am using a 5ch receiver and when the failsafe function is commented I can see that my values are within the range of 800 to 2200 that is set in the code

Yes this is tripping failsafe because it is detecting that channel 6 is not hooked up

 

[NickRehm]

other issues is that the servos move further when the IMU is moved than when there is a stick input (here I have : s2_command_scaled = 0.5 - roll_PID; ) , i assigned thro_des to another servo and that one has full range. ( s1_command_scaled = thro_des ; )

does it have anything to do with the stabilized axis variables?

I'm going to put this flight controller on the ft simple scout for a test, it's a 4 channel, plane

Servo travel depends entirely on the controller gains and your maximum rates/angles you set. High rates = large deflection (large error in the controller in response to large stick input). High controller gains = large deflection (more aggressive response to error in the controller). Your stick input in not passed directly through to the motors/servos but rather through the PID controller which interprets the command and moved actuators accordingly

 

[NickRehm]

Hi Nick. I'm new here and been playing with the FC on Teensy 4.1. I have hooked it all up and have it running. However sometimes the loop rate goes from 500 to over 10000 and then stays there until rebooted. Sometimes there are also looprate spikes?

Not sure what would be causing this? It happens when `i just let it sit on the workbench after about 10 min or sometimes 15 min.

I'm attaching some screenshots to show the issue.

M

Might be bad IMU connection causing the code to hang. Good continuity between IMU and teensy?

 

[Mohamed]

Servo travel depends entirely on the controller gains and your maximum rates/angles you set. High rates = large deflection (large error in the controller in response to large stick input). High controller gains = large deflection (more aggressive response to error in the controller). Your stick input in not passed directly through to the motors/servos but rather through the PID controller which interprets the command and moved actuators accordingly

what values do you suggest I should start with for a normal fixed 4 channel airplane?

 

[NickRehm]

Rate or angle mode? Desired flying characteristics? Your experience flying?

 

[Mohamed]

Rate or angle mode? Desired flying characteristics? Your experience flying?

I have no issues flying a 4ch plane and I also fly freestyle drones.
let's try angle mode on the plane first.

 

[NickRehm]

These gains and max roll pitch yaw values worked well on my ekranoplan:

//Controller parameters (take note of defaults before modifying!):
float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0)
float maxRoll = 40.0; //Max roll angle in degrees for angle mode (maximum 60 degrees), deg/sec for rate mode
float maxPitch = 45.0; //Max pitch angle in degrees for angle mode (maximum 60 degrees), deg/sec for rate mode
float maxYaw = 180.0; //Max yaw rate in deg/sec

float Kp_roll_angle = 0.8; //Roll P-gain - angle mode
float Ki_roll_angle = 0.22; //Roll I-gain - angle mode
float Kd_roll_angle = 0.3; //Roll D-gain - angle mode (if using controlANGLE2(), set to 0.0)
float B_loop_roll = 0.9; //Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1)
float Kp_pitch_angle = 0.8; //Pitch P-gain - angle mode
float Ki_pitch_angle = 0.3; //Pitch I-gain - angle mode
float Kd_pitch_angle = 0.3; //Pitch D-gain - angle mode (if using controlANGLE2(), set to 0.0)
float B_loop_pitch = 0.9; //Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1)

float Kp_yaw = 0.4; //Yaw P-gain
float Ki_yaw = 0.05; //Yaw I-gain
float Kd_yaw = 0.00012; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!)


Note my yaw control was through differential thrust, not a control surface. So you may need to increase Kp_yaw

 

[Mohamed]

Rate or angle mode? Desired flying characteristics? Your experience flying?

did you experience any problems with the radiodata not being stable? if yes how did you solve it?

 

[Mohamed]

These gains and max roll pitch yaw values worked well on my ekranoplan:

//Controller parameters (take note of defaults before modifying!):
float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0)
float maxRoll = 40.0; //Max roll angle in degrees for angle mode (maximum 60 degrees), deg/sec for rate mode
float maxPitch = 45.0; //Max pitch angle in degrees for angle mode (maximum 60 degrees), deg/sec for rate mode
float maxYaw = 180.0; //Max yaw rate in deg/sec

float Kp_roll_angle = 0.8; //Roll P-gain - angle mode
float Ki_roll_angle = 0.22; //Roll I-gain - angle mode
float Kd_roll_angle = 0.3; //Roll D-gain - angle mode (if using controlANGLE2(), set to 0.0)
float B_loop_roll = 0.9; //Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1)
float Kp_pitch_angle = 0.8; //Pitch P-gain - angle mode
float Ki_pitch_angle = 0.3; //Pitch I-gain - angle mode
float Kd_pitch_angle = 0.3; //Pitch D-gain - angle mode (if using controlANGLE2(), set to 0.0)
float B_loop_pitch = 0.9; //Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1)

float Kp_yaw = 0.4; //Yaw P-gain
float Ki_yaw = 0.05; //Yaw I-gain
float Kd_yaw = 0.00012; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!)


Note my yaw control was through differential thrust, not a control surface. So you may need to increase Kp_yaw

ok I'll try that

 

[NickRehm]

did you experience any problems with the radiodata not being stable? if yes how did you solve it?

View attachment 230582

I've never worked with the pico so I don't know. Might be worth trying different interrupt pins. Could also simply be that the pico isn't fast enough and can't read the pwm values correctly.

Could you comment out the loopRate() function at the end of the void loop and uncomment the printLoopRate() function...what speed are you getting unthrottled?

 

[Mohamed]

I've never worked with the pico so I don't know. Might be worth trying different interrupt pins. Could also simply be that the pico isn't fast enough and can't read the pwm values correctly.

Could you comment out the loopRate() function at the end of the void loop and uncomment the printLoopRate() function...what speed are you getting unthrottled?

here

 

[NickRehm]

Pico isn't fast enough, should be less than 500us between loops, you are seeing 3x that. Sorry

 

[Mohamed]

Pico isn't fast enough, should be less than 500us between loops, you are seeing 3x that. Sorry

alright thanks for all the help!!!!

the raspberry pi pico has two cores that each run at 133mhz but all the code that does that so far has been with sdk on some other platform maybe I'll try writing something with that in the future

thanks again !!!

 

[NickRehm]

One last thing you could try is removing the commandMotors() call in the void loop. That takes up quite a bit of time since its just bit banging the digital output pins and idling the code. Downside is you no longer have oneshot125 motor outputs, but you can always use the spare servo outputs for motors. Try that and report back your timing

 

[Mohamed]

One last thing you could try is removing the commandMotors() call in the void loop. That takes up quite a bit of time since its just bit banging the digital output pins and idling the code. Downside is you no longer have oneshot125 motor outputs, but you can always use the spare servo outputs for motors. Try that and report back your timing

 

[Mohamed]

One last thing you could try is removing the commandMotors() call in the void loop. That takes up quite a bit of time since its just bit banging the digital output pins and idling the code. Downside is you no longer have oneshot125 motor outputs, but you can always use the spare servo outputs for motors. Try that and report back your timing

radio data is still the same

 

[Scotto]

Hi Nick, thank you for providing this incredible resource! I've never really seen code before so I'm pretty impressed you got all that to work and make it easy enough for me to modify.
I've almost got my project all programmed, but I am going to ask for help before I break something. I'm trying to put flaps on a slider on channel 6. I declared auxl in a few places and got an output but its just on/off/-on. I could get by with that but I would like to know how to make it smooth. I'm guessing I need to add a float somewhere. On the next project, I would like to try a little pitch_IMU on the flaps, too. Would I need to constrain auxl to pitch somewhere?

 

[tamuct01]

Hi Nick, thank you for providing this incredible resource! I've never really seen code before so I'm pretty impressed you got all that to work and make it easy enough for me to modify.
I've almost got my project all programmed, but I am going to ask for help before I break something. I'm trying to put flaps on a slider on channel 6. I declared auxl in a few places and got an output but its just on/off/-on. I could get by with that but I would like to know how to make it smooth. I'm guessing I need to add a float somewhere. On the next project, I would like to try a little pitch_IMU on the flaps, too. Would I need to constrain auxl to pitch somewhere?

To add channels requires a lot of extra variables, but they follow the same pattern as the existing channels. It helps to really understand how the code works by watching the videos that Nick has published.

You will need to add (to the best of my knowledge):

[user-specified variables]
unsigned long channel_XX_fs = 1500; //failsafe value
const int servoXXPin = YY; // if you will map a channel to a servo - flaps?
PWMServo servoXX;
unsigned long channel_XX_pwm; // PWM value for new channel
float flaps_des; // or some name to represent your desired state of flaps
float sXX_command_scaled; // for your flaps servo
float sXX_command_PWM;

[void setup]
servoXX.attach(servoXXPin, 900, 2100);
channel_XX_pwm = channel_XX_fs;
servoXX.write(90); // startup servo position

//Command actuators
servoXX.write(sXX_command_PWM);

[controlMixer]
sXX_command_scaled = 0.5 + flaps_des;

[getDesState()]
flaps_des = (channel_XX_pwm - 1000.0)/1000.0; //Between 0 and 1
flaps_des = constrain(flaps_des, 0.0, 1.0); //between 0 and 1

[scaleCommands()]
sXX_command_PWM = sXX_command_scaled*180;
sXX_command_PWM = constrain(sXX_command_PWM, 0, 180);

[getCommands()]
// I use SBUS, so the PWM value is pulled form the SBUS section
channel_XX_pwm = sbusChannels[XX] * scale + bias;

[failSafe()]
channel_XX_pwm = channel_XX_fs;

I'm sure I missed a few, but follow through the code and it should be pretty understandable. Good luck!