Category Archives: c++

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.

Signal processing

While thinking about ways to implement multiple sensor support – for several GPS or gyroscopes – I realized that an UAV can be modeled as a signal processing device.

It can be split like this:

  • sources – like the MPU9250 which generates 4 signal streams: acceleration, angular velocity, magnetic field and temperature, the SRF02 sonar which generates a stream of distances
  • streams – like the acceleration from the imu, or the location stream from the GPS. Each stream has a sample rate
  • sink – like the pigpio PWM. Sinks accept streams of a certain sample rate only
  • and processors – like low pass filters (LPF) or the PIDs, or the AHRS. Processors don’t have an inherent sample rate. They adopt the rate of the input usually.

All of these are implemented as nodes with several inputs and outputs. All nodes are added to the HAL layer and both their properties and connections are configured from a json file.

The important aspect is that – like all signal processing – signal rate is very important and has to be properly matched between nodes to satisfy the nyquist frequency. For example the PWM has a max sample rate of 490Hz so any signal higher than ~240Hz will be seen as noise. Higher frequency signals have to be resampled and low-pass-filtered before fed in the PWM node.

I started implementation last week and – after many rewrites – I have a model that ‘feels right’.

Here are some drawings:

Ahrs_0

Here the MPU9250 node exposes 4 sub-nodes (not shown) – an accelerometer, a gyroscope, a magnetometer and a thermometer. Each of these outputs a stream: acceleration stream, angular velocity, magnetic field and temperature.

The frequency of these streams is determined by the sensor. Acceleration and Angular Velocity are at 1Khz while magnetic field is at 100Hz. Temperature (not shown in the pic) is at 10Hz.

The Magnetic Field stream is fed into a Resampler node that takes it from 100Hz to 1000Hz, ready to be fed into the AHRS procesor node. The reason for this is that upsampling the stream needs a low-pass flter to avoid parasitic frequencies. A sample rate of 100Hz can carry at most a 50Hz signal so when going to 1000Hz sample rate this max frequency has to be maintained. Simply cloning each sample 10 times is not enough as this adds a lot of high frequency harmonics.

So now all signals at 1Khz sample rate are fed into the AHRS which outputs a Reference Frame stream, also at 1Khz. This reference frame represents the rotation of the UAV relative to earths frame of reference.

Right now I have a complimentary filter for the AHRS node but in the ear future I will write an extended kalman filter (EKF) for it – as soon as this tutorial is complete.

Note200215_0

This diagram uses the AHRS output – the reference frame stream – plus the acceleration stream again to output 2 new streams – gravity vector and linear acceleration. These 2 streams will be further used for dead reckoning and stability pids.

Note200215_1_0

Here I have the basic rate PIDs diagram. The angular velocity stream is first resampled from 1KHz to 50Hz (the pic says LPF but it’s actually a resampler node). The reason for this is that the usable bandwidth of the quadcopter is ~20-30Hz. Anything higher than this is noise.

The stream is further passed to the Rate PID processor node which outputs a torque stream (also at 50Hz). Torque is converted to thrust using a model of the motors+propellers (configurable of course), which is fed to the motor mixer and then in the PWM sink.

The advantage of this system is that new processors can be easily added to accommodate new hardware. For example if I want to use 2 GPS sensors I will add a redundancy processor that takes 2 location streams and outputs the healthiest one. Or I can merge them together to get better accuracy – while the rest of the nodes don’t even know nor care about this.

Basically I have minimal coupling between the elements of the system which talk through very well defined (and type safe) interfaces.

The ground station is also simplified. It can enumerate ll the nodes, read their configuration as a json value, change it and push it back to the nodes. Changing a filter cutoff will become trivial.

So far I still working to match the old functionality but in 1-2 weeks I believe this system will be ready for a test.

Robust sensors

Thanks to HobbyKings horrible shipping times, I’m forced to use my broken Crius UBLOX GPS to test the GPS code. I’m not sure what is broken but the symptoms vary from garbage data, wrong crc, loosing the fix every 2 minutes and random periods of complete silence.

To be able to continue working I had to add a watchdog that reinitializes the GPS if it’s silent for more than 5 seconds. However – my GPS::init function is blocking and takes a few seconds to do it’s work as it has to wait for the ACK messages and retry a few times if needed. So I moved it on the secondary thread using silk::async.

void GPS_UBLOX::process()
{
    QLOG_TOPIC("gps_ublox::process");
    if (!m_is_setup)
    {
        if (!m_setup_future.valid() || //first time we're starting this
             m_setup_future.get_state() == boost::future_state::ready) //the previous async setup failed
        {
            //start the async setup
            m_setup_future = silk::async([this]() { setup(); });
        }
        return;
    }

And the watchdog:

//check if we need to reset
if (q::Clock::now() - m_sample.last_complete_time_point >= REINIT_WATCHGOD_TIMEOUT)
{
    m_is_setup = false;
}

So if at any moment the GPS stops responding I’m firing the setup on the second thread – to avoid blocking the main control thread and keep doing it until the GPS responds again.

I’m thinking to add this king of hot restart to all sensors and in case something happens to trigger it from the GS.