eltonlaw sundries

Building a Quadcopter: Part 2

2024-03-18 1:36AM

The flight computer is a STM32G070RB, it has relatively low power consumption and enough communication interfaces to connect an IMU, GPS and 4 ESCs so I think it should work fine for V1. I'm going to use the ST provided HAL anyways and all the chips share roughly the same abstractions so upsizing it shouldn't be too much work in the future. I'm developing with a NUCLEO board which has built-in power management and exposes the I/O pins which would have been some necessary work otherwise, building a breakout board. There's also a USB STLINK component on the board which makes flashing dead simple. When I set up a project through STM32CubeIDE and configured the target it was able to build and flash a hello world right out of the box.

Logging

HAL_UART_Transmit(logger.huart, (uint8_t *)buf, strlen(buf), HAL_MAX_DELAY) sends a char array over the UART bus so I just added some string manipulation and wrapped that in some macros.

void log_write(log_level_t level, const char *file, int line, const char *fmt, ...);
#define log_debug(...) log_write(LOG_LEVEL_DEBUG, __FILE__, __LINE__, __VA_ARGS__)
#define log_info(...) log_write(LOG_LEVEL_INFO, __FILE__, __LINE__, __VA_ARGS__)
#define log_warn(...) log_write(LOG_LEVEL_WARN, __FILE__, __LINE__, __VA_ARGS__)
#define log_error(...) log_write(LOG_LEVEL_ERROR, __FILE__, __LINE__, __VA_ARGS__)
#define log_fatal(...) log_write(LOG_LEVEL_FATAL, __FILE__, __LINE__, __VA_ARGS__)

And logging it just works like every other logger

log_info("Logging initialized %d\n", 123);

and so connecting to the UART bus's device driver gives the log

In [1]: import serial
   ...: ser = serial.Serial(
   ...:     port="/dev/ttyACM0",
   ...:     baudrate=115200,
   ...:     bytesize=serial.EIGHTBITS,
   ...:     stopbits=serial.STOPBITS_ONE
   ...: )
   ...: while True:
   ...:     line = ser.readline()
   ...:     if line != b"":
   ...:         print(line)

[../Core/Src/main.c:115] INFO - Logging initialized 123\n'

Getting raw measurements of acceleration and rotational motion

The IMU I have hooked up is the MPU6050, which is old and not manufactured anymore. Will upgrade to the ICM20948 in the future but for now this works, the board seems pretty ubiquitous, so setting it up wasn't too difficult. Initialization covers a few things:

  1. Configure I2C peripheral 1 on the flight computer for 400KHz. I noticed there would be issues doing I2C operations every few flashes, regardless of code changes and found out that the static void MX_I2C1_Init(void) generated by the STM32CubeIDE for initializing the peripheral needed some delay after running. I'm not sure why this happens since it seems like the calls to HAL_I2C_Init(...), HAL_I2CEx_ConfigAnalaogFilter(...) and HAL_I2CEx_ConfigDigitalFilter(...) all check for HAL_OK before returning. I expected it to be ready to use right away. Initialzing the MPU6050 so it retries up to 5 times with delay before panicing resolved the issue. The behaviour I'm most often now is it'll fail once to read from the bus before succeeding on the second loop. Planning to revisit this to see if there's some I2C configuration error I've done.
  2. Read the WHO_AM_I register (0x75) and check that 0x68 is being returned, this is a nice simple check for verifying physical connections and message formatting are correct.
  3. PWR_MGMT_1 register (0x6C) bit 6 needs to be cleared to disable sleep mode, by default its set.
  4. The accelerometer precision is set via ACCEL_CONFIG (0x1C) bits [4:3]. The MPU6050 is a MEMS sensor that measures differences in capacitance of electrodes on a microscopic spring (capacitive sensing), one for each direction XYZ. From the outside that corresponds to a voltage difference which can be interpreted differently depending on how precise the measurement needs to be. The full voltage range is split into 16 bits and you could bucket that between +- 2/4/8/16 G's of force.
  5. Similar to the accelerometer, gyroscope precision is set via GYRO_CONFIG (0x1B) bits [4:3] and has a similar set of 3 sensors that exploits the Coriolis effect for XYZ. The full voltage range is split again into 16 bits and depending on precision you can choose between +- 250/500/1000/2000 degrees per second.
  6. Offset calibration: The sensors aren't perfect for whatever reason (components, manufacturing etc.) and when perfectly still the accelerometer and gyroscope should output (0G, 0G, 1G) and (0 DPS, 0 DPS, 0 DPS) respectively. I'm making it take 700 readings (at rest) and averaging that to calculate a number to subtract when in use. NOTE: The Z access for the accelerometer should be 1G for Earth's gravity at ground level, might be worth it one day to be smarter about that based on elevation, but going to just use that for now.

Raw values are stored in readable registers on the device, it'll deposit the sensor data there and it can be read at any point. There are 14 8-registers all in a sequence, starting at ACCEL_XOUT_H (0x3B). It allows for a burst read to cut the I2C communication protocol overhead, requesting X reads at some address reads and continuously increments that address X times before sending the stop condition. In this case, 13 less start and stop conditions need to be sent.

I want to know how the drone is angled so the angular momentum needs to be integrated to get the total change in angle over time. I set up a timer that counts milliseconds and resets to zero right after each read, that's the delta T used.

Ending state

  1. Initialize peripherals: I2C, timer, UART
  2. Initialize UART logger
  3. Configure MPU6050 and calculate offsets
  4. Super loop
    1. Read accel and rotational motion in 3D (Ax,Ay,Az and Gx,Gy,Gz)
    2. Calculate change in time (T) since last reading
    3. Calculate integral of rotational motion to get change in theta (Tx, Ty, Tz)
    4. Log all data UART
b'[../Core/Src/main.c:115] INFO - Logging initialized\n'
b'[/home/d0nkrs/dev/robotics/stlib/Src/mpu6050.c:143] INFO - Attempt (0) to initialize MPU6050\n'
b'[/home/d0nkrs/dev/robotics/stlib/Src/mpu6050.c:59] ERROR - MPU6050: WHO_AM_I register gave unexpected value: 0\n'
b'[/home/d0nkrs/dev/robotics/stlib/Src/mpu6050.c:143] INFO - Attempt (1) to initialize MPU6050\n'
b'[../Core/Src/main.c:122] INFO - Initialized MPU6050 accel_scaler=16384.000000, gyro_scaler=131.000000, accel_offset=(x:0.467172, y:0.185669, z:-0.077263), gyro_offset=(x:2.657002, y:-1.384879, z:-2.594382)\n'
b'[../Core/Src/main.c:138] INFO - Initialization complete, starting main loop\n'
b'[../Core/Src/main.c:148] INFO - T=2.000000 Ax=0.016321 Ay=0.004909G Az=1.004316G TEMP=26.412353 Gx=0.713538 Gy=1.092630 Gz=-0.964256 Tx=0.001427 Ty=0.002185 Tz=-0.001929\n'
b'[../Core/Src/main.c:148] INFO - T=28.000000 Ax=0.004019 Ay=-0.006226G Az=0.999138G TEMP=26.412353 Gx=0.007120 Gy=-0.004434 Gz=-0.138442 Tx=0.001626 Ty=0.002061 Tz=-0.005805\n'
b'[../Core/Src/main.c:148] INFO - T=25.000000 Ax=0.005484 Ay=-0.003540G Az=1.000115G TEMP=26.459412 Gx=-0.046315 Gy=-0.172373 Gz=0.029497 Tx=0.000469 Ty=-0.002248 Tz=-0.005067\n'
b'[../Core/Src/main.c:148] INFO - T=26.000000 Ax=-0.006234 Ay=0.001831G Az=0.992302G TEMP=26.459412 Gx=-0.076849 Gy=0.110070 Gz=-0.046839 Tx=-0.001530 Ty=0.000614 Tz=-0.006285\n'
...
b'[../Core/Src/main.c:148] INFO - T=25.000000 Ax=0.005334 Ay=-0.005101G Az=0.997236G TEMP=26.647647 Gx=0.049416 Gy=0.001027 Gz=-0.040592 Tx=1.096581 Ty=-1.830758 Tz=-1.740557\n'
b'[../Core/Src/main.c:148] INFO - T=25.000000 Ax=0.000207 Ay=0.001247G Az=0.992109G TEMP=26.600588 Gx=-0.042187 Gy=-0.014241 Gz=0.142614 Tx=1.095527 Ty=-1.831114 Tz=-1.736991\n'
b'[../Core/Src/main.c:148] INFO - T=24.000000 Ax=-0.004675 Ay=0.001491G Az=0.995771G TEMP=26.553529 Gx=0.049416 Gy=0.062095 Gz=-0.078760 Tx=1.096713 Ty=-1.829624 Tz=-1.738882\n'
b'[../Core/Src/main.c:148] INFO - T=25.000000 Ax=0.005090 Ay=0.002712G Az=0.992841G TEMP=26.600588 Gx=0.148653 Gy=0.046828 Gz=-0.048226 Tx=1.100429 Ty=-1.828453 Tz=-1.740087\n'
b'[../Core/Src/main.c:148] INFO - T=25.000000 Ax=-0.000281 Ay=0.004421G Az=1.005048G TEMP=26.553529 Gx=-0.103255 Gy=-0.105844 Gz=-0.002424 Tx=1.097848 Ty=-1.831099 Tz=-1.740148\n'
b'[../Core/Src/main.c:148] INFO - T=25.000000 Ax=0.007043 Ay=-0.000706G Az=1.002607G TEMP=26.647647 Gx=0.133386 Gy=-0.067676 Gz=-0.185631 Tx=1.101182 Ty=-1.832791 Tz=-1.744789\n'
b'[../Core/Src/main.c:148] INFO - T=25.000000 Ax=0.000940 Ay=0.001735G Az=1.005048G TEMP=26.553529 Gx=-0.042187 Gy=-0.052409 Gz=0.073911 Tx=1.100128 Ty=-1.834101 Tz=-1.742941\n'
b'[../Core/Src/main.c:148] INFO - T=25.000000 Ax=0.000452 Ay=0.006374G Az=0.996747G TEMP=26.741765 Gx=0.018882 Gy=-0.166912 Gz=0.012843 Tx=1.100600 Ty=-1.838274 Tz=-1.742620\n'
...