Milestone 0.1 demo program

[ebelski]

In order to reach milestone 0.1 we need to be able to demonstrate single motor control based on sensor readings from the MPU-9250. The physical setup will be a lever attached to the desk via a pivot with a motor, propeller, and sensor at the end. The motor/prop will move the lever up and down. We need an example rust demo with the following program flow

• Allow serial input from the command line to choose which gyro axis sensor reading we would like to use as feedback
• Allow serial input for the command line to set the PID gains
• Start the demo with a keystroke and wait for input as the set point
• Allow serial input for the angle (in degrees) that we would like as the set point
  • It would be nice to be able to change the setpoint during the demo but not a requirement

Internal to the program we should:

• Create a control loop where the setpoint is in degrees and the sensor feedback is from the gyroscope.
  • We will need to integrate the velocity to estimate the position of the sensor
  • I will attach a control loop diagram to this issue detailing what we will need to implement

[ebelski]

24 shows what the demo fixture will look like

[mciantyre]

We should re-review the MPU9250 driver code before this experiment, and make sure we can configure the sensor as needed. The current settings are arbitrary.

https://github.com/ebelski/rust-copter/blob/224c5096ab88c62965a70021c66d55af04bf8a7c/firmware/demos-teensy4/pwm-control/sensor.rs#L31-L36

Where do we think the control loop needs to run? If it can run slowly on our host, we should have most pieces in place to run the experiment. Otherwise, if we need a faster loop, we might need to move it on board.

[ebelski]

I think, in order to prove out what we eventually might do, that we'll want to get all of the 9 dof readings. Even though we'll only be using the gyro for this demo (at least initially), the quadcopter will likely be using the Madgwick filter or some equivalent where the Magnetometer will important for low frequency correction. By including these readings in the MPU-9250 polling it will give us some insight into what type of latency we can tolerate for the control loop (slower readings = more sluggish response of quadcopter to disturbances).

Is there the possibility of polling the sensor for the different readings at different rates? (e.g. Accel at 2000 Hz, Gyro at say 250 Hz , and Mag at 4 Hz (or whatever the max rate is)) Or are we locked into the slowest rate for all sensors?)

Depending on the answer to the above question, I think it will dictate where we should run the control loop. Another consideration will be the ability to ensure a quasi-deterministic sample rate for the control loop. If host or client side is able to keep time more accurately, we'll probably want to use that unless we have a compelling reason not to.

[ebelski]

Do we know what the latency of sending sensor measurements from the Teensy to the host over serial would be?

For prototyping purposes, it'd be easier to run the control loop host side in python but I worry about latency. We'd need to pipe the sensor readings from the client to the host, process them, and then send back motor throttle commands. We can definitely give this a shot as I think the development time is relatively low and it will give us an idea if the latency is acceptable.

We would need the following capability in a .hex script

• Pipe accel and gyro readings from the Teensy to my PC
• At the same time read motor throttle commands from my PC and apply them to the respective motor (we only need to actuate 1 motor for this demo)
• (I guess technically this could be done sequentially, where one fires off and waits for a reply before firing again)

We would need the following capability in a python script

• Consume the accel and gryo readings from the Teensy
• Run a semi-deterministic rate control loop to produce a throttle output for the motor
• Pipe the throttle output to the Teensy
• Change the set angle setpoint based on keyboard input

From what we were testing in #29 we have a few of these already done. I can take care of the control loop development in python but I'm not sure how to write the Teensy code or negotiate the communication between the two.

Where do we think the control loop needs to run? If it can run slowly on our host, we should have most pieces in place to run the experiment...

Based on the above, would it be relatively easy to write up the demo leaving a space for me to plug in the control loop code?

Also what do you think the max rate is that we can run the loop at given this architecture?

[ebelski]

Green is host side in python and orange is client side in rust

[mciantyre]

Is there the possibility of polling the sensor for the different readings at different rates?

Yea we can do that. Right now, we're just polling all sensors at the same rate (100Hz) regardless of if the sensor changed. We'll need some changes to the demo's polling loop to support this, or create a better example with this in mind.

Do we know what the latency of sending sensor measurements from the Teensy to the host over serial would be?

It's not that fast with the current settings.

• 115200 bits per second (baud rate)
• 10 bits per character (8N1: one start, one stop bit, plus 8 data bits)

means it takes 1 / 11520 seconds to send one byte of data.

Given the current serialization schema, 9DOF readings take up 42 bytes. This means it takes ~3.646ms to move 9DOF readings over the serial wire. That still has to go through your OS to reach your control loop. Of course we could tune this specifically for the experiment (variable sensor polling means fewer bytes on the serial wire, for example).

We could try switching the data flow to USB, instead of UART. I'd have to dig deeper into the implementation to see what our latency might be. But hopefully (without a driver change) we could move one 42 byte 9DOF reading every 125us (one bulk transfer every USB high speed microframe, see here). A custom USB driver would give us more confidence. There's one here, but it's only full speed at the moment.

I can take care of exposing the data to Python, regardless of how it's moving from the board to your PC!

[ebelski]

Honestly 3.646 ms or 274 Hz might be fast enough for what we're looking to do here in this demo but if we can get data faster that's always welcome! Because we're relying on the propeller/motor response time on a compressible fluid (air), there's probably some inherent speed limitation in how fast we can react to disturbances anyway. However, we'd always want the physics of the mechanical system to be limiting, not the electronics (i.e. blame the wrench monkey who designed the air frame...).

[mciantyre]

Awesome. #43 makes the move from UART to USB. We're now only using UART for debug logs and human-readable outputs. IMU data streaming, and PWM throttle commands, go through USB.

43 also adds a Python interface (pypwm_control) to stream IMU data, and control the motor throttle. Basic usage might resemble

import pypwm_control

imu, motors = pypwm_control.open("COMx")  # Your COM port

# Disable magnetometer
imu.disable_readings(pypwm_control.ImuReading.Mag)

for reading in imu.stream():
    if type(reading) is pypwm_control.ImuReading.Acc:
        # Handle accelerometer readings
    elif type(reading) is pypwm_control.ImuReading.Gyro:
        # ...

    # Do math in here...

    # Do things with motors
    motors.motor("B").set_throttle(72)  # 72% throttle

How does that look for a host-side control loop?

[ebelski]

Gucci, that's exactly what I was hoping for. Now to see how well I can tune this on the fly with no tools. I think this calls for the Ziegler-Nichols tuning method.

I'll start printing the fixturing for the motor today and plugging in some code.

[ebelski]

I'm starting to test this but I'm unable to initialize the motor on the quadcopter. I'm using the pwm-control demo on the Teensy and running the following script (which is pretty much what you pseudo coded above)

import pypwm_control

imu, motors = pypwm_control.open("COM3")  # Your COM port

# Disable magnetometer
imu.disable_readings(pypwm_control.ImuReading.Mag)

for reading in imu.stream():
    if type(reading) is pypwm_control.ImuReading.Acc:
        # Handle accelerometer readings
        print(reading)
    elif type(reading) is pypwm_control.ImuReading.Gyro:
        print(reading)

    # Do math in here...

    # Do things with motors
    motors.motor("B").set_throttle(15)  # 72% throttle

For the time being I'm just using this to make sure the motor will respond to my inputs. I have everything connected to the power supply and it gives me the startup beep but doesn't give me the connected protocol beep between the ESC and Teensy.

In #12 I recall this working pretty well. Did something change in the firmware that prevents initialization?

[ebelski]

I'm also trying to use esc-throttle.py to test the motors and not getting a response from the motor. For reference I'm connected to pin 7 or motor B.

[mciantyre]

I'm sorry, I'll admit that I haven't tested PWM in a while 😝 I can confirm that PWM output is broken in all of the rust-copter code. It's even broken in the teensy4-rs examples.

I think I found the problem in the PWM driver. Once I finish testing, I'll upstream the fix, and propose a temporary patch here. Take a break until I follow up.

[ebelski]

Ok no worries! I thought it was something I was doing. I'll wait until I hear back from you!

[ebelski]

@mciantyre fixed this with #46. Progress shall continue! ♟️

[ebelski]

The python AHRS package contains the Madgwick filter and other Attitude and Heading Reference Systems. www.pypi.org/project/AHRS/

fyi: the function updateIMU() has aliasing issues when using the previous quaternion. Use the np.copy() function to prevent this aliasing...

[ebelski]

@mciantyre, do you think we would be able to implement some of the AHRS algorithms on the Teensy? Like what Kris Winer has implemented for an Arduino?

https://github.com/kriswiner/MPU9250/blob/72038b040bef3cf072612cd8a8ee8f26e3c87158/MPU9250BasicAHRS.ino

I'd be interested in learning how to implement this if you'd be willing to show me?

Concurrently though I'm working on developing this in python for debugging purposes. So we can wait on this implementation until I iron out a few things regarding the stability of the Madgwick filter.

[mciantyre]

do you think we would be able to implement some of the AHRS algorithms on the Teensy? [...] I'd be interested in learning how to implement this if you'd be willing to show me?

Yea for sure! Happy to help you write it for the embedded system. There shouldn't be any problems running an AHRS filter on the board.

There's different ways we can take it:

• Write it all from scratch in either Rust, or C. We can use C code in Rust, and use Rust code in C. Pick whatever you want to learn.
• Use third-party libraries to help with our implementation (like nalgebra).
• Use an existing solution (kalman filter, madgwick filter) and integrate it.

[ebelski]

I ran into a few issues when I was working on the python side of this. I'm hoping to pick this back up again soon.

As an aside, the carbon fiber tubing for the air frame arrived yeserday 🎉