Where (in the source files) can I invert/reverse the PWM outputs?

I want to reverse/invert the motor outputs of my board, so that if px4 tells the motor to run at full speed, the output is no voltage and if px4 tells the motor not to run, the output is a “full” PWM signal. It would be nice but not necessary, if this would occur to the motors only and only in brushed_esc mode.

Setup/Goal:
The PWM motor outputs go into a inverting driver circuit, which drives a N-Channel MOSFET which drives the Motor.

I don’t want to alter my circuit if this is avoidable.

Question:
Which file and what lines in the file are responsible for the PWM output and thus I have to alter?
If you are very motivated, I would be very pleased if you also gave me a hint in what I have to change the code :blush:

Hello Zec.
Can you please, tell me which vehicle are using in your project?
I believe it is possible to work around with the *.mix files instead of changing your code.
It would be easier to give a opinion if tell us what is your vehicle.

Paulo

My vehicle is a X-Quadcopter

Thanks Zec.

I am quite sure the *.mix files changing will solve your problem.

If you take a look at MultiRotorMixer.cpp you will find the following:

float roll = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f) (eq 1)

Where _roll_scale is the value that you have in your *.mix file, in your case 10000:

R: 4x 10000 10000 10000 0

The meaning of each value/letter is:

R-> Multirotor

4x-> X Quadcopter

First 10000 roll scale

Second 10000 pitch scale

Second 10000 yaw scale

0 idle speed.

After (eq 1) above, the result will be weighted by multiplying the values by a control matrix that will take into account the drone geometry.

After all these calcs had been performed, a final value ranging from -1 to + 1 is available, and by using some other equations, these values will be transformed in a pulse width (PWM) that will be put in the output pin to feed your ESC.

All above is to state my point: The values ranging from -1 to + 1 will be translate to PWM duty cicle of 1000(-1), 1500 (0), 2000(+1) etc. Thus, if you change your .mix file as follow, I believe you will invert the signal as you want: 1000(+1), 1500 (0), 2000(-1)

I would propose like the following:

Go to src/Firmware/ROMFS/px4fmu_common/mixers/ and find the quad_x.main.mix and change it as follow:

R: 4x -10000 -10000 -10000 0

Save the file.

Make a clear at firmware directory (terminal make clear) and build the code again.

Upload and check if it is what you were expecting.

Please, check on the bench before fly and tell us if it worked for you.

Paulo

Hello Zec,

I am back.

Today I had time to check the answer that I provided you yesterday and after a deeper analysis I found that the solution I gave you does not work without changing the code.

As changing the code is inevitable, there is a solution that is simple and may solve your problem.

I have checked in HILT and it sounds ok:

First, keep the *mix file as before (without the changes I proposed).

Go to output_limit.cpp and look for this line:

effective_output[i] = control_value * (max_output[i] - min_output[i]) / 2 + (max_output[i] + min_output[i]) / 2;

Replace the line by this:

effective_output[i] = -1.0f *control_value * (max_output[i] - min_output[i]) / 2 + (max_output[i] + min_output[i]) / 2;

This equation is responsible for converting control signal to PWM width.

I made some changes in the code to test this modification, and below are the results:

Have in mind that I had to null the pitch and roll commands to make the test, so, only thrust command is working, otherwise the system would be unstable because the commands are inverted.

In the first figure the throttle (left bottom) lever of virtual control is low and the vehicle is not armed and the result is the PWM outputs are the default disarmed value of 900.

Second figure, the vehicle is armed and the throttle lever is minimum but the PWM output is around maximum: 1980

Third figure shows the lever in the neutral position and the PWM outputs around 1500.

The forth figure show the throttle lever at maximum and the PWM output at minimum of 1000.

1 Like