Category Archives: c++

Android USB Tethering & Debugging

If you want to debug an Android application that needs USB tethering – say a FPV app – then you can use the following steps.

  1. You need to first connect the device to the same wifi network as your PC.
  2. Then start the USB tethering and provide internet to whatever you need – say a Raspberry PI running wifibroadcast or my RC software.
  3. Run this in a terminal:
    adb connect 192.168.1.33:5555
    where 192.168… is the WiFi IP of your device
  4. That’s it. The device is now visible to Android Studio & QtCreator just like if it were connected by USB. I didn’t notice any slowdowns in deployment vs USB – but my app is rather small (a few MB)

 

 

si4463 FPV #2

In my last post I talked about using the si4463 chip to send video, telemetry and RC data to the quadcopter. I calculated the bandwidth and it seemed that 500kbps video with 5/7 FEC coding should be possible.

I wrote the code, linked everything together and it kind of worked. The video was stable, not many packets lost but the latency was pretty bad. It got up to ~200ms from the current 100-130ms but worse than this, the video was very choppy. I managed to narrow it down to the H264 codec in the raspberry pi: the bitrate you configure in the codec is an average bitrate, per second. Each frame can vary a lot in size as long as the average is preserved (with some allowed over and undershoot). I got I-frames of 12-16KB and P-frames of 400-500 bytes (at 30 FPS). The I-frames took way longer to send than the P-frames and this resulted in a choppy video. I tried to play around with settings – like disabling CABAC and activating CBR but nothing made the bitstream uniform enough.

The final, biggest problem was actually caused by the RF4463F30 module – and it’s the same problem I had months ago when I tried to use them: they introduce a LOT of noise in the power line, enough to cause all the I2C chips on the quad to fail.

I tried all kinds of capacitors to decouple the module and reduced the noise a lot but there seem to always be some capacitive/inductive coupled noise persisting.

In the end I just gave up and went back to the RFM22B chip which just works. For video I went back to the monitor mode/injection system but did change the modulation to CCK, 5.5Mb to hopefully reduce the possibility of interference with other 2.4Ghz RC systems. CCK is spread spectrum similar to DSSS and should be able to coexist with RC systems around.

 

So yeah, FPV through a very constrained channel with a temperamental H264 encoder and a very temperamental transceiver module is not fun…

 

GPIO ramp up/down

While testing the menu system yesterday I notice a strange problem. Sometimes pressing  a button triggers not only the GPIO associated with that line, but adjacent lines as well.

My first thought was ‘wiring problem’ as everything is crammed inside the RC with 30+ jumper wires, so I started shaking and moving wires around but it didn’t seem to change much.

Then I turned to code – I thought I might have a bug – and after close introspection the code seemed correct:

  • Set the column GPIO as output
  • Set the column GPIO high (+3.3v)
  • Read all 4 line GPIOs, see which button is pressed
  • Set the column GPIO as input to turn it off
  • Repeat

Everything seemed ok so I took out the oscilloscope and probed one of the columns.

Here’s what I got:

screenshot-from-2016-11-21-09-50-26

I probe 2 column GPIOs here and the trigger is on the green trace. The 2 problems become obvious – first of all the pin goes low too slow so by the time the next pin is high, the previous one still has ~2.3V, enough to register as high. And second – the code lops through this without any delay between pins.

On paper, the code works correctly but not so in real life where there is inductance and bandwidth limits for triggering GPIOs high/low.

 

The fix is trivial. I changed the loop like this:

  • Set the column GPIO as output
  • Set the column GPIO high
  • Wait 2 microseconds for the pin to settle
  • Read all 4 line GPIOs
  • Set the column GPIO low <— this causes the voltage to drop way quicker than just setting the gpio as input
  • Set the column GPIO as input
  • Wait 2 microseconds for the pin to settle
  • Repeat

 

Here’s the trace on the oscilloscope after the changes:

screenshot-from-2016-11-21-10-08-42

 

As you can see there is a clear, safe distance between triggering adjacent column GPIOs and (not visible in the trace) the lines are sampled right in the middle of the high interval.

This works perfectly now.

Silkopter FC

Finally, after some DHL delivery trouble the boards arrived:

pcb

After watching a few tutorials I immediately tried soldering the most difficult part – the MPU9250 chip. Turns out – if you don’t have the right flux type it’s impossible. I managed to ruin 2 boards in the process and broken 1 chip…

broken pcb

Notice the PCB changing color next to the IMU I2C pins and the ‘dry’ look. The pin header holes are also darker than the others due to the heat.

 

2 days later my new flux came: Chip Quick SMD291 ordered from farnell. This time it went much better as I was able to solder 2 chips in 5 minutes, from the first try. The trick is that tacky solder flux doesn’t evaporate from the hot air and keeps the chip floating while the solder melts (and prevents oxidation). When the solder flows, the chip kinda snaps into place and that’s it. In my first tries I used a flux pen that dried immediately and the friction between the chip and the PCB was greater than the solder surface tension – preventing the chip from snapping into place.

So this is the board almost complete, except for the ADS1115 ADC that didn’t arrive still:

final pcb

 

The last few days I spent trying to get the RF4463 chip to respond on SPI1. I expected surprises and I got them:

  • first of all, activating SPI1 on a RPI3B results in boot times of 2 minutes due to an interrupt issue with the UART driver. Solution: disable the bluetooth and use ttyAMA0 instead of ttyS0
  • next I simply couldn’t get the chip to respond at all. 2 days I spent on this until I went back to the schematic thinking the wiring is not ok. Then I realized that the chip select is connected to CS2 which is GPIO 16 but the SPI1 overlay I used uses GPIO 18 as CS. Duh! Solved by adding dtoverlay=spi1-1cs,cs0_pin=16 in /boot/config.txt
  • that’s it – it works now 🙂

 

In the last moment I added 3 WS2812B RGB LEDs thinking that I will be able to drive them with PIGPIO. I didn’t check the datasheet closely enough to see that they need PWM impulses in the order of 0.5ns whichare way below the resolution of the library.

So today after soldering the LEDs I realized that I cannot drive them. Desperate, I turned to the schematic and saw that by sheer luck I connected the LEDs to GPIO 13 – which happens to be the hardware PWM channel 1. Yaay – exactly what this library needs.

So I downloaded the lib, configured it on DMA channel 4 and PWM channel 1 and ran it and voila! It works with <1% CPU usage:

final pcb2

 

So now I have almost all the hardware working – the dual IMUs, the Baros, the RF chip and most importantly – vital for a 2016 multirotor – the LEDs.

 

I have 6 unpopulated left to give away (for free of course, shipping covered by you) if someone is interested!

boards

 

So now – back to software. The RC is coming along nicely.

Classic TX

I thought to give my Flysky TH9x TX a change to use it with silkopter so I made a new node called “CPPM Receiver”. This samples a CPPM stream on a GPIO and outputs the PWM streams that can be fed into the main node.

 

It uses the PIGPIO setAlertFunc which calls a user provided function for all level transitions on a certain gpio pin. The resolution is 5 microseconds which is waay more than needed for a standard CPPM stream. The gap between channels is 400 microseconds so way bigger than the minimum resolution of the library.

 

It started more of a proof of concept but in the end I think it’s perfectly usable.

Here’s a video with it in action, connected to a D8R-II receiver:

 

 

Definition Language

aaand it’s done!

The definition language for specifying node configuration is finished – at least in principle.

It ended up being a stand-alone library called def_lang (uninspired, I know)

Basically I can now define things like this:

namespace silk
{
  alias mass_t = ufloat;

  struct UAV_Config_Base
  {
    string name : [ui_name = "Name"];
    mass_t mass : [ui_name = "Mass (Kg)"];
    ufloat moment_of_inertia : [ui_name = "Moment Of Inertia"];
  };

  struct Multirotor_Config : public UAV_Config_Base
  {
    struct Motor
    {
      math::vec3f position;
      direction_t thrust_vector = {0.f, 0.f, 1.f};
      bool clockwise = false;
    };

    ufloat radius = 1.f; //m
    ufloat height = 1.f; //m
    ufloat motor_z_torque = 0.f; //Nm
    ufloat motor_thrust = 1.f; //N
    ufloat motor_acceleration = 10.f; //N/s
    ufloat motor_deceleration = 10.f; //N/s
    vector<Motor> motors;
  };
}

And use this definition in 2 possible ways:

    • Dynamically, through a type system:
ts::ast::Builder builder;

auto parse_result = builder.parse_file("definition_file.def");
if (parse_result != ts::success)
{
  std::cerr << parse_result.error().what();
  return;
}

//walk and display the AST
std::cout << builder.get_ast_root_node().to_string(0, true) << std::endl;

ts::Type_System ts;
ts.populate_builtin_types(); //this adds standard built-in types like float and double

auto compile_result = builder.compile(ts); //populate the typesystem from the AST
if (compile_result != ts::success)
{
  std::cerr << compile_result.error().what();
  return;
}

//now the type system is fully populated and all the types is it are instantiable
std::shared_ptr<ts::IStruct_Type> base_type = ts.find_specialized_symbol_by_path<ts::IStruct_Type>("silk::UAV_Config_Base");
std::shared_ptr<ts::IStruct_Type> type = ts.find_specialized_symbol_by_path<ts::IStruct_Type>("silk::Multirotor_Config");

assert(base_type->is_base_of(*type)); //inheritance is respected

std::shared_ptr<ts::IStruct_Value> multirotor_config_value = type->create_specialized_value(); //create a value of type Multirotor_Config
auto construct_result = multirotor_config_value->construct(); //construct it. This struct is default constructible. An alternative is to initialize it using an initializer list

//let's get (select) some members
//the full dot sintax is supported when selecting so things like motors[0].position works
std::shared_ptr<ts::IString_Value> name_value = multirotor_config_value->select_specialized<ts::IString_Value>("name");

//all setters might fail so I need to check the return value
auto result = name_value->set_value("silkopter");
TS_ASSERT(result == ts::success);

//values are serializable as well to an intermediary tree
auto serialize_result = multirotor_config_value->serialize();
TS_ASSERT(serialize_result == ts::success);

//the intermediary tree can be serialized to json or binary
std::string json = ts::serialization::to_json(serialize_result.payload(), true);

//a more complicated example:
//get the motors vector
std::shared_ptr<ts::IVector_Value> motors_vector_value = multirotor_config_value->select_specialized<ts::IVector_Value>("motors");

//create a new motor
std::shared_ptr<ts::IStruct_Value> motor_value = ts.create_value<ts::IStruct_Type>("silk::Multirotor_Config::Motor");

//change it's position
motor_value->select_specialized<ts::IVec3f_Value>("position")->set_value(ts::vec3f(0, 1, 0));

//add it to the vector
auto result = motors_vector_value->insert_value(0, motor_value);
assert(result == ts::success);

Basically  it’s a full type system implementation similar to the C++ one with type safety, templated types, aliases, values, casting, inheritance etc etc. But on top of the standard C++ features it also allows custom attributes on members and types (like ui_name for example), it supports reflection so everything can be enumerated and it’s serializable. This dynamic one will be used in the GS to generate UI for all the nodes. Like this the GS can work with new nodes without me having to do custom UI for each node I intend to add (or everytime I change the node config to add/remove a parameter, or change a type etc)

 

  • Statically through generated C++ code

From the definition above I can also generate C++ code for all the structs and work with them through getters and setters. Each type in the type system can be mapped to a native C++ type. So in this example, the above structure would be usable like this from generated code:

assert(std::is_base_of<silk::UAV_Config_Base, silk::Multirotor_Config>::value); //inheritance is respected

//create a value of type Multirotor_Config. This also default constructs it
silk::Multirotor_Config multirotor_config_value;

//all members have setters and getters generated for them
std::string name_value = multirotor_config_value.get_name();
multirotor_config.set_value("silkopter");

//values are serializable as well to an intermediary tree
auto serialize_result = silk::serialize(multirotor_config_value); //serialization methods are generated as well
TS_ASSERT(serialize_result == ts::success);

//the intermediary tree can be serialized to json or binary
std::string json = ts::serialization::to_json(serialize_result.payload(), true);

//a more complicated example:
//get the motors vector
std::vector<silk::Multirotor_Config::Motor> motors_vector_value = multirotor_config_value.get_motors();

//create a new motor
silk::Multirotor_Config::Motor motor_value;

//change it's position
motor_value.set_position(ts::vec3f(0, 1, 0));

//add it to the vector
motors_vector_value.insert(motors_vector_value.begin(), motor_value);

//set it back in the config
multirotor_config_value.set_motors(motors_vector_value);

The generated code is simpler to work with partly because it’s a lower level of abstraction (I use the build-in C++ typesystem and work in value space) but also because it’s familiar.

The interested thing is that the generated code keeps inside as a string the definition file contents that it was generated from. This is useful to implement the following flow:

  1. I write the definition file for the bran and all its nodes
  2. I generate the c++ code for the brain. Now all the nodes can use C++ structs for the parameters and config structs. Simple and type-safe as it’s impossible to access non-existing fields or use them with the wrong type. It’s standard, simple C++ structs
  3. When the brain connects to the GS, it sends the def file it was generated from
  4. The GS instantiates a dynamic Typesystem and parses the def file from the brain, so now it will understand and be able to (de)serialize the data it will receive from the brain – like node configs, etc

If I want to add a new node, I add in the def, regenerate the code and use the resulting config struct in the new node. The GS will automatically know how to handle the new node as it will receive the def for it next time it connects to the brain

 

Easy peasy

Node Definition

As I mentioned in my previous post I’m designing a domain specific language to define the node properties. So far it looks like this:

import "UAV_Config.def"

namespace silk
{

struct Multirotor_Config : public UAV_Config
{
    struct Motor
    {
        vec3f position = {0, 0, 0};
        bool clockwise = false;
    };

    string name;
    float mass = 1.f : [ ui_name = "Mass (Kg)", min = 0.f ];
    float height = 0.5f : [ ui_name = "Height (m)", min = 0.f ];
    float radius = 0.5f : [ ui_name = "Radius (m)", min = 0.f ];
    float motor_thrust = 1.f : [ ui_name = "Motor Thrust (N)", min = 0.f ];
    float motor_z_thrust = 1.f : [ ui_name = "Motor Z Torque (Nm)", min = 0.f ];
    float motor_acceleration = 10.f : [ ui_name = "Motor Acceleration (N/s)", min = 0.f ];
    float motor_deceleration = 10.f : [ ui_name = "Motor Deceleration (N/s)", min = 0.f ];
    vector<Motor> motors : [ ui_name = "Motors" ];
}

}

It looks a lot like C++ but allows some extra attributes on types and members, like ui_name to have a nicer name displayed in the editor, a min/max value for numeric types, the number of decimals for floating points etc.

The grammar is done with flex/bison and it will actually be a full typesystem that I will use to generate serialization/deserialization code offline and to create UIs online in the GS.

The language will support:

  • namespaces, needed to be able to generate the code in the correct namespace in C++ and to separate the types properly
  • basic types, like ints, floats but also uint8_t and friends
  • structs and classes together with inheritance.
  • templated types like vectors.
  • math types like vec2/3/4, quat and matrices
  • initializer lists for vec3/4 etc
  • custom attributes
  • imports

The compiler will take a file and output a typesystem that contains all the types defined in the file. These types will have reflection so they can be traversed. From this typesystem I can generate the C++ code for the brain – the serialization/deserialization to binary and json – and I can generate UI widgets in the GS as well.

I’m still struggling with the grammar definition but over this weekend I will get it done.

I have to admit – doing a domain specific language and a typesystem is actually very satisfying and challenging. I wish C++ would help a bit more with common things like reflection and such. I even considered moving everything to rust or go but then I’ll have another problem – QT and other library bindings…

 

Mixing Motors #3

I’ve written about this before (here & here). In the previous code I was making a few assumptions:
1. Overall motor rotation is zero – so there is the same number of clockwise and counter-clockwise motors
2. Thrust vector is always pointing downwards
3. No physical units. The old mixer worked in ratios – full rotation along X axis, 0.3 rotation along Z Axis and so on

Now I want to get rid of all these assumptions and build a mixer that given a desired torque and the motor+quad configuration – it can figure out how much throttle to apply to each motor.

This is proving more complicated than I thought and the fact that I don’t have any formal math background doesn’t help at all.

So my requirements:
– Work in physical units. I want to to send the mixer 2 quantities: a torque in Nm and a collective force in N.
– Any (valid) motor configuration. X and + frames, quad, hexa, octo, spider, Y6 etc. Including motors with random thrust vectors – like a tricopter. Non-downward thrust vectors opens the door to a nice simplification as we’ll see below.
– Unbalanced setups. In reality it often happens that motors differ and I want my mixer to account for this and compensate without forcing the PIDs to do it

So first step is to have a model for the motor. I have the following struct to represent the relevant motor data:

struct Motor
    {
        math::vec3f position;
        math::vec3f thrust_vector;
        bool clockwise = true;
        float max_thrust = 0; //N
        float max_rpm = 0;
        float acceleration = 0; //rpm/s
        float deceleration = 0; //rpm/s
    };

Position is in local space, relative to the center of mass of the UAV.
Thrust vector is normalized.
Max_thrust is in N and is the thrust of the motor at max_rpm.
Acceleration and deceleration are used to model motor+prop inertia.

Using the position and thrust_vector I can calculate the max torque the motor will produce:

max_torque = thrust_vector X position * max_thrust; (X is cross product)

Using max_rpm and max_thrust I can figure out how much throttle too apply to get a certain thrust using the following approximation:

//thrust increases approximately with throttle^2
auto throttle = math::sqrt<float, math::safe>(target_thrust / max_thrust);

Now the problem is – how to figure out what throttle to send to each motor to get a certain torque and total force?

Let’s consider a simple case: a perfectly balanced quadcopter with a + frame. Each motor points downwards so they generate a torque along one of the axis (and arms) but there is the yaw torque as well that needs to be taken into account. This can be modeled by simply tilting a bit the thrust vector which in turn creates a small torque on the Z axis. The tilt factor depends on the motor and prop but it’s usually a factor of the RPM or RPM^2. For now let’s consider it constant.

So each motor produces a max torque: A, B, C, D.
A and C are opposite, just like B and D:

A = -C
B = -D

What I want to find out is the factors a, b, c and d in the equation below:
1. aA + bB + cC + dD = T    where T is the target torque of the quad

As you can imagine there are an infinite number of factors that give a good answer so we need more constraints. These are:
2. I also want a total thrust of Th.
3. I want a, b, c, d to have the minimum values that result in the desired torque. So I want the optimal solution.

For a quad, equations 1 and 2 are enough to calculate the optimal factors but for a hexa, octo or any other setup it’s not enough. I don’t know how to express eq 3 mathematically but this equation should result in an unique set of factors.
So in the past days I tried to figure out how to calculate the coefficients for some very weird (but valid) motor configurations and in the end I gave up on the math approach.

So what’s left when the analytic approach is out of reach? You try brute force of course.
I wrote a small iterative algorithm that tries to find out the factors with a form of binary search. It’s very short and seems to be fast enough. It takes a motor config, a target torque and a step size and computes the factors with a precision of 0.0001.

A step size of 0.01 requires ~3-7000 iterations, 0.1 requires 3-700 and 0.5 can solve it in 50-100 iterations. Pretty good.
However I cannot prove that the algorithm will always converge (if I could prove this, then I could also solve the initial problem to begin with) and I’ve been running the algorithm all day yesterday with random valid motor configs and random target torques and so far I had no bad cases.

Here it is:

void test_mm(math::vec3f const& target, float step)
{
    struct Motor
    {
        math::vec3f torque; //precomputed maximum torque
        float factor;
    };

    //some test configuration with random data
    std::vector<Motor> motors = {{
                                     {{2, 3, 0.1}, 0},
                                     {{5, -1, -0.1}, 0},
                                     {{-1, -7, 0.1}, 0},
                                     {{-4, 2, -0.1}, 0},
                                     {{-0.1, .2, -1}, 0},
                                }};

    size_t iteration = 0;
    while (true)
    {
        //first calculate the crt torque
        math::vec3f crt;
        for (auto& m: motors)
        {
            crt += m.torque * m.factor;
        }

        //check if we're done
        if (math::equals(crt, target, 0.0001f))
        {
            QLOGI("{}: Done in {} iterations", step, iteration);
            break;
        }

        //how far are we for the target?
        //divide by motor count because I want to distribute the difference to all motors
        auto diff = (target - crt) / float(motors.size());

        //distribute the diff to all motors
        for (auto& m: motors)
        {
            //figure out how much each motor can influence the target torque
            //  by doing a dot product with the normalized torque vector
            auto t = math::normalized(m.torque);
            auto f = math::dot(t, diff) * step; //go toqards the difference in small steps so we can converge
            m.factor += f;
        }

        iteration++;
    }
}

And the test code:

q::util::Rand rnd;
while (true)
{
    math::vec3f target(rnd.get_float() * 40.f, rnd.get_float() * 40.f, rnd.get_float() * 10.f);
    test_mm(target, 0.01);
    test_mm(target, 0.1);
    test_mm(target, 0.5);
}

Now I have to test in in the simulator.

Multirotor diagram

The new GS node editor is progressing nicely and everything seems to fit. I have data visualization for most types of streams with scopes and FFTs and yesterday I added a map widget to visualize the GPS ECEF location.

On the silkopter brain side I kept having a weird feeling that something is not right, that I cannot model everything with nodes and streams. Since I tend to obsess about finding the correct paradigm to modeling a certain process, I started drawing multirotor diagrams to see if/where I hit a road block. After 6 tries I got this:

(excuse the quality, I got tired of drawing on my galaxy note 4 after the 3rd diagram and went back to paper)

20150319_000139

I’ll start from the end – the PIGPIO sink. This takes a PWM stream from the Throttle To PWM node which does exactly what the name says – converts a throttle stream into a PWM stream.

The Throttle stream comes from the motor mixer which uses data from the Multirotor Model Data node (top cloud) to convert a Torque stream and a Force stream into throttle values for each motor. This conversion can be done easily for a certain combination of motors + propellers. It doesn’t have to be very accurate as there are some PIDs before this to compensate.

The Torque stream is calculated by the AV Model node (renamed now to Rate Model) that basically combines a rate PID + a feed forward model using again the top cloudy model data. It takes as inputs an angular velocity stream from the gyro and another angular velocity target stream from the Stability Model and tries to match them. The corrections represent the Torque stream.

The Stability Model node is responsible for matching a given Reference Frame (from the AHRS node) with a target ref frame from the Pilot or Assisted Model Node. The output of the Stability Model Node goes into the Rate Model Node.

The Assisted Model Node is responsible for matching a stream of velocities from the GPS with a target velocity from the Pilot. The velocities include both horizontal and vertical (altitude) components. It’s used to model a very intuitive control scheme – move left at 1m/s, or climb at 2m/s. Its outputs are a ref frame stream for the Stability Model (which will try to match it) and an up force stream fed into the Motor Mixer Node.

The Pilot Node interprets RC inputs (not shown in the diagram) and outputs either a ref frame + up force stream when using rate/stab controls, or a velocity stream when using an assisted control scheme.

So going top-down now :

– The Pilot tells reads RC commands and outputs either low-level rotation+vertical force data or high-level velocity data

– The Assisted Model reads velocity data and tries to match those inputs by changing the orientation and altitude of the quad

– The Stability Model reads orientation data and tries to match it by rotating the quad. It uses a stab PID to do this (until I have a good FF model).

– The Rate Model reads rotation speed and tries to match it by outputting torques. It uses the quad geometry and weight + rotational inertia of the motors + props to figure out how much torque to apply to rotate with a certain speed. Small corrections are done with a rate PID

– The Motor Mixer reads torques and figures out what throttle each motor should have based on the geometry of the quad and power train.

So far this looks like it should work.

UAV Editor

The signal processing view of a quadcopter has one obvious result – a node editor.

Still WIP but here’s the first screenshot:

Screenshot from 2015-03-02 00:16:43

The left top part has some nodes that compute a Reference Frame, a Location and a Battery State from a MPU9250 imu, a MS5611 baro, a Ublox GPS and a RC5T619 ADC.

There are some processors there that do some resampling or convert streams (like an ADC_Voltmeter that converts from ADC to Voltage).

The nodes are read from the brain running on the Raspberry Pi. Each node has:

– a unique name
– a type (class_id)
– a list of inputs
– a list of outputs
– an init_params json which represents fixed configuration that is applied when the node is created. Stuff like IMU sample rate or gyro range.
– a config json that has things that can be changed at any moment – like where the inputs/outputs are connected or calibration data, or cutoff frequency for LPFs.

Each input/output has a name and a stream type that determines where it can be connected. There’s also a sample rate that has to be matched.

Next is to allow uploading of the json configs to the brain and more editing features.

The node editing is done with this library.