Airsim: How to convert rotor velocity to PWM and constant units

[p3jawors]

Hello, I am trying to create my own quadrotor flight controller using the low level PWM python api. I am using the default drone in the blocks environment. I am uncertain whether I am converting my controller output of rotor velocity in radians/second to pwm, one of the issues being units in the constants used, and whether I am looking at the right rotor params.

Question 1: Can you please let me know if I am looking in the right location and using the right units for...

The constants I am trying to define are:
a) drone mass[kg]
b) drone arm legnth[m]
c) lift constant k from Force = k * w**2
d) drag constant b from Torque = b * w**2 + I_M * dw

Looking in AirSim/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp I am using:
a) drone mass == realT box_mass = 1 - 0.055*4[kg]
b) drone arm length = 0.2275 [m]

Looking in AirSim/AirLib/include/vehicles/multirotor/RotorParams.hpp I am using:
c) Given F = k * w**2 and since the UIUC constants used are wrt max thrust and torque, I have taken
max_thrust = 4.179446268[Newtons?]
k = max_thrust[Newtons] / max_speed[revolutions/sec]**2

Am I correct in thinking that this is in Newtons? I am uncertain because of the units used in calculateMaxThrust(). I would expect in the calculation to use velocity (nsquared) in radians/second, in which case I would divide max_thrust by max_velocity[rad/sec]**2, but it is set as nsquared = revolutions_per_second**2.

d) Given Torque = b * w**2 + I_M * dw and omitting acceleration since it is typically small and zero at steady state, I have used a similar approach as in 1c) above, and obtain b by
max_torque = 0.055562[Nm?]
b = max_torque / max_speed[rad/sec]**2

Is this torque given in Newton*meters, or is it in imperial? Going by the reference linked in RotorParams.hpp It is mentioned that imperial units should be used with the UIUC constants, but metric units are used in the max thrust calculation.

Question 2: Since my control is set to work in radians/sec, I obtain the thrust constant by dividing max_thrust[N] / max_velocity[revolutions per second]**2 (1c above, and because the original calculation uses velocity in rev/sec). To convert my controller output to pwm I then take its output in radians/second and divide by max_speed[radians/second] to get a value between 0 and 1.

Is this the correct way of doing this conversion? I take it that the physics simulator determines the thrust output from the 0-1 pwm signal based on the maximum_thrust which uses UIUC constants at a set max_velocity. So if I use that max_velocity in my pwn conversion it should map correctly to my controller velocity output.

I have tried giving as much information as I can, but please let me know if there is anything else I can provide.

Software Versions
OS: Ubuntu 16.04 LTS
AirSim:1.2.7
Python: 3.5.2
Unreal: 4.24.1-0+++UE4+Release-4.24

[HyperShield]

Hi, I'm currently trying to figure out the same thing.

When is comes to the mass I think the box_mass is used to roughly calculate the inertia matrix. The mass however, is set to 1.

For the value k it is found as
k = rho*D^4*C_T = 1.225*0.2286^4*0.109919 = 0.00036771704516278653

if you multiply this by the max_speed in rps squared you get the max thrust
F_max = k*n^2 = 0.00036771704516278653*(6396.667/60)^2 = 4.179446267535562. The units of k will be kgm/rad^2.

For b it's a similar procedure where
b = rho*D^5*C_P/(2*pi) = 0.040164*1.225*0.2286^5/(2*Pi) = 4.888486266072161e-06
which results in a max torque of
tau_max = 4.888486266072161e-06*(6396.667/60)^2 = 0.055562.

For the second question I'm still struggling with this one. If you look in Firmware.hpp in the update function you can see that a conversion from controller output to motoroutput is done through the function getMotorOutput in Mixer.hpp. I've tried using this as a conversion to motor PWMs, but it did not work. My controller outputs thrust and torque which I would convert to angular speeds on the rotors and then to PWM. The approach I use now is close to working, but there is still an issue with the PWMs generated being slightly too large. Did you have any progress on this?

[HyperShield]

Did some more digging and I figured it out.

When it comes to your question 2, I will go through the process from PWM to applied force and torque on the quadrotor. That way if there are any missunderstandings others can correct it. There'll be a TL;DR at the end.

First from the client (MultirotorRpcLibClient) you call the function moveByMotorPWMsAsync' which sends a message to aMultirotorRpcLibServerthat interprets the message and callsmoveByMotorPWMs' on the quadrotor api (SimpleFlightApi which inherits from MultirotorApiBase). The quadrotor api then calls commandMotorPWMs on itself and in this function creates a GoalMode with value _passthrough_, and a goal (Axis4r) which stores the pwm values. It then calls the setGoalAndMode function on offboard_api (OffboardApi) which copies the GoalMode and goal internally. This is where the call trace ends and the rest is handled by update calls from the engine.

As the simulation is being updated it will eventually be the quadrotor api (SimpleFlightApi) turn to update and in its update function it will update the firmware (Firmware). Inside the firmware's update function the board (AirSimSimpleFlightBoard), offboard_api and the controller (CascadeController) will be updated. The controller has access to the goal and GoalMode which was stored inside the offboard_api and will use this to generate _control signals_. Inside the controller's update function it checks if the GoalMode is _passthrough_ and if it is it will create a passthrough controller (PassthroughController) that will generate the _controller output_. The passthrough controller just takes the pwms as inputs and copies the into the _controller output_. Then back in the firmware's update function the _controller output_ is copied over to the _motoroutput_ which is then copied into the board through the writeOutput function call.

Now for the physics. The physics for the quadrotor vehicle is handled by the quadrotor physics (MultiRotorPhysicsBody) which have 4 rotors (RotorActuator). Whenever the physics engine (FastPhysicsEngine) updates it also updates the physics of all the bodies in the simulation including the quadrotor by calling the updatePhysics function. Inside the updatePhysics function there's a function call to getNextKinematicsNoCollision which calculates the forces and moments acting on the quadrotor physics . Inside the getNextKinematicsNoCollision function the physics engine gets the applied forces on the quadrotor physics by calling the function getBodyWrench. In the getBodyWrench function the physics engine iterates over all the physics vertices (PhysicsBodyVertex) of the quadrotor physics , gets the forces and moments that physics vertex applies to the quadrotor physics and adds them all together to get the total forces and moments. The physics vertices of the quadrotor physics are actually the rotors, which is where we need to look now to tie everything together.

Whenever the simulation updates then updateKinematics of the quadrotor physics is called. Inside this function the quadrotor physics calls getActuation on the quadrotor api which will essentially copy the _motoroutput_ inside board into the rotors through the setControlSignal call. Now the pwm values are in the rotors which is part of the physics simulation. Inside the rotors the pwm signal is fed into a low pass filter to give a smooth signal. When the rotors are updated the parent class (PhysicsBodyVertex) of rotors is updated (we'll come back to this) and the setOuput funcion is called. Inside this function the rotor speed, thrust and torque are calculated from the pwm signal and put in an Output struct. Now in the PhysicsBodyVertex when it gets updated it calls setWrench which scales the thrust and torque (by a _air_density_ratio_ factor) in the Output struct and copies into a wrench struct. This resutling wrench is what is used by the physics engine to calculate the forces and moments of the quadrotor.

TL:DR
Now how does all of this help us calculate the pwm? Well, it tells us that the thrust used in the simulation is basically calculated as

thrust = pwm*max_thurst*air_density_ratio

therefore since thrust = k*w^2 the pwm is given as

pwm = k*w^2/(max_thrust*air_density_ratio)

Keep in mind that the angular speed of the rotor needs to be calculated through a mixer. The _air_density_ratio_ can be found through msr::airlib::Environment::State env = client.simGetGroundTruthEnvironment(); where

_air_density_ratio_ = env.air_density/1.225

This is my code to convert from thrust and torque to pwm

Eigen::Vector4f forces_to_pwm(const Eigen::Vector4f &f_u,const msr::airlib::Environment::State &env)
{
    float propeller_diameter = 0.2286, standard_air_density = 1.225, d = 0.2275; //d is arm_length
    float c_T = 0.109919*pow(propeller_diameter, 4)*standard_air_density;
    float c_Q = 0.040164*pow(propeller_diameter, 5)*standard_air_density/(2*M_PI);

    Eigen::Matrix<float,4,4> M;
    M <<    c_T,    c_T,   c_T,    c_T,
         -d*c_T,  d*c_T, d*c_T, -d*c_T,
          d*c_T, -d*c_T, d*c_T, -d*c_T,
            c_Q,    c_Q,  -c_Q,   -c_Q;

    Eigen::Vector4f thrust =  c_T*M.inverse()*f_u;

    float max_thrust = 4.179446268;
    float air_density_ratio = env.air_density/standard_air_density;
    Eigen::Vector4f pwm;

    for(int i = 0;i<thrust.size();++i){
        pwm[i] = std::max<float>(0.0,std::min<float>(1.0,thrust[i]/(air_density_ratio*max_thrust)));
    }
    return pwm;
}