Motion, pressure and heading: the Vector S3's sensors

Pick up a Vector S3 and it knows. It knows which way is down, how fast you're turning it, roughly how high off the ground it is, and which way it's pointing.

That's four different senses, packed into three tiny chips on the back of the board. Used well, they turn the Vector into a spirit level, a step counter, a compass, or the balance system of a small robot.

Used naively, every one of them will mislead you in its own particular way. This article covers what each sensor physically measures, where it goes wrong, and how to combine them so their weaknesses cancel out.

Four senses, three chips

All three chips sit on the board's I2C bus, the same two wires that the Qwiic connector carries. If I2C is new to you, I2C and the Qwiic connector explains how several chips share it.

A quick way to check all three are alive is to ask the bus who's there. On the Vector, I2C runs on GPIO14 (SDA) and GPIO13 (SCL):

#include <Wire.h>
 
void setup() {
  Serial.begin(115200);
  delay(1000);
  Wire.begin(14, 13);            // SDA = GPIO14, SCL = GPIO13
 
  for (uint8_t addr = 1; addr < 127; addr++) {
    Wire.beginTransmission(addr);
    if (Wire.endTransmission() == 0) {
      Serial.printf("Found a device at 0x%02X\n", addr);
    }
  }
}
 
void loop() {}

You should see 0x68, 0x7C, and one of 0x76 or 0x77. To actually read the sensors, use a library written for each chip; the Arduino Library Manager is the place to look. The sketches later in this article use stand-in functions like readAccel() where your library's calls go.

The accelerometer: which way is down

An accelerometer measures acceleration along three axes, X, Y and Z, usually in g, where 1 g is the pull of Earth's gravity (9.81 m/s²).

Here's the part that surprises everyone: a board sitting still on a table reads 1 g, not zero. An accelerometer can't feel gravity pulling down on it directly. What it feels is the table pushing up, stopping it from falling. So a board lying flat reads +1 g on Z and nothing on X or Y.

That turns out to be wonderfully useful. Tip the board, and that 1 g gets shared out between the three axes. The split tells you exactly how the board is tilted.

The tilt comes out of the three readings with a little trigonometry:

float roll  = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;   // side to side
float pitch = atan2(-ay, az) * 180 / PI;                        // front to back

Now shake it. Any real movement adds its own acceleration on top of gravity, and the accelerometer can't tell the two apart. While the board is moving, the tilt it works out is wrong. That's the accelerometer's weakness: right on average, but jumpy the moment anything moves. A quick check is the total: if the three axes don't add up to about 1 g, something other than gravity is involved.

The opposite also works. In free fall, the board isn't being pushed by anything, so all three axes read zero. The ICM-42670 can spot that by itself.

The gyroscope: how fast it's turning

The same ICM-42670 chip contains a gyroscope. It doesn't measure an angle at all. It measures how fast the board is rotating, in degrees per second, around each of the three axes.

That makes it smooth and immune to shaking: a vibration that sends the accelerometer haywire barely registers as rotation. To get an angle, you add up the rotation over time. Turning at 90° per second for half a second means you've turned 45°.

The catch is in that adding up. Every gyro has a small error called bias: it reports a little rotation even when perfectly still. A bias of 1° per second sounds harmless, but add it up and after a minute your angle is 60° out. The gyro's weakness is the mirror image of the accelerometer's: smooth and right in the short term, but it drifts away over time.

Combining them: sensor fusion

So the accelerometer is right in the long run but noisy, and the gyro is smooth in the short run but drifts. Put them together and each one covers for the other. This is called sensor fusion, and the simplest version is the complementary filter:

1. Start from your last angle and add the gyro's rotation. That's smooth and quick.
2. Nudge the result a tiny bit toward what the accelerometer says. That stops the drift.

The slider sets how long the filter trusts the gyro before the accelerometer drags it back. Too short and the blended line gets jumpy like the accelerometer. Too long and it slowly drifts like the gyro. Somewhere around half a second is a good place to start. Turn on the motor vibration and notice how the blend keeps working while the raw accelerometer falls apart.

In code, it's one line:

// Stand-ins: replace readAccel() and readGyroX() with your IMU library's calls.
float angle = 0;
unsigned long last = micros();
 
void loop() {
  float ax, ay, az;              // in g
  readAccel(ax, ay, az);
  float rate = readGyroX();      // in degrees per second
 
  float dt = (micros() - last) / 1e6;
  last = micros();
 
  float accelAngle = atan2(-ay, az) * 180 / PI;
  angle = 0.98 * (angle + rate * dt) + 0.02 * accelAngle;
}

The 0.98 is the trust setting in disguise. Read 100 times a second, 0.98 works out at about half a second of trusting the gyro.

The ICM-42670 can also do some of this thinking itself. It can detect motion, taps, tilt, free fall and steps on its own, and signal the ESP32-S3 on GPIO21, the INT pin. That pin is active low with a 10 kΩ pull-up, the same arrangement as a button in Buttons, pull-ups and bounce. This lets the ESP32-S3 sleep until something actually happens.

The barometer: height from air pressure

The SPL07-003 measures air pressure. You're sitting at the bottom of a deep ocean of air, and the higher you go, the less of it is pressing down on you. Near sea level, pressure drops by about 12 pascals for every metre you climb.

Normal air pressure is around 101,325 Pa, so one metre changes it by roughly a hundredth of a percent. The barometer on the Vector is sensitive enough to resolve roughly 25 cm of height, enough to tell which floor of a building it's on.

The storm shows the barometer's weakness. It can't tell weather from height. A weather front moving through changes the air pressure by far more than climbing a few floors does, so a board that hasn't moved can appear to rise or fall tens of metres over a day.

The fix is to measure changes, not absolute height. Take a reading when you know where you are, like the ground floor, and measure everything relative to it. Changes over a few seconds, like walking up a flight of stairs, are almost all height. Changes over hours are mostly weather.

The barometer also measures temperature, since it needs that internally to correct its own readings.

The compass: which way is north

The QMC6309 is a magnetometer. It measures the magnetic field along three axes. Earth's field is weak, but it's everywhere, and its horizontal part points roughly toward magnetic north. Hold the board flat and the heading is:

float heading = atan2(my, mx) * 180 / PI;   // mx, my from the magnetometer
if (heading < 0) heading += 360;

Depending on how the chip is mounted, you may need to swap or flip an axis to match the board's front.

Three things get in the way.

Tilt. That formula only works with the board flat. Tip it, and part of Earth's field swings onto the Z axis. A tilt-compensated compass uses the accelerometer's tilt to rotate the magnetic reading back to level before working out the heading. Another reason sensors are better together.

Hard iron. Anything magnetic that moves with the board, like a speaker magnet, a steel screw or the board's own parts, adds a constant offset. Instead of the readings tracing a circle centred on zero as you turn the board, the circle is shifted to one side.

Soft iron. Nearby steel that isn't magnetised itself bends Earth's field around it, squashing that circle into an oval.

Calibration fixes both. Slowly turn the board through every direction you can, record the smallest and largest reading on each axis, and use them to recentre and rescale:

// After turning the board through every direction:
float offsetX = (maxX + minX) / 2;   // hard iron: recentre
float offsetY = (maxY + minY) / 2;
float mx = (rawX - offsetX) / (maxX - minX);   // soft iron: rescale
float my = (rawY - offsetY) / (maxY - minY);

One more subtlety: a compass points to magnetic north, which can be several degrees away from true north depending on where you live. For a robot finding its way around a room this doesn't matter. For navigation, look up the local declination and add it.

Taming noise

Every sensor is noisy to some degree. The simplest cure is averaging: take several readings and use their mean.

const int N = 10;
float history[N];
int next = 0;
 
float smooth(float reading) {
  history[next] = reading;
  next = (next + 1) % N;
  float sum = 0;
  for (int i = 0; i < N; i++) sum += history[i];
  return sum / N;
}

There's always a trade-off. Averaging more readings makes the output calmer, but slower to react. A reading averaged over the last second is always about half a second behind reality. Choose based on what you're building: a bubble level can afford to be slow, a balancing robot can't.

Things to build

A bubble level. Light one pixel on the 5×5 matrix and move it toward whichever edge is lowest, exactly like the widget above. This sketch uses the matrix setup from the Vector quick start, and assumes the pixels are numbered row by row from the top left. If yours run differently, change the pixel mapping, and if the dot rolls uphill, flip the sign in front of ax or ay: it depends on which way the IMU is mounted.

#include <Adafruit_NeoPixel.h>
 
Adafruit_NeoPixel matrix(25, 3, NEO_GRB + NEO_KHZ800);
 
void setup() {
  pinMode(47, OUTPUT);
  digitalWrite(47, LOW);          // power the matrix
  matrix.begin();
  matrix.setBrightness(40);
  // ...start your IMU library here
}
 
void loop() {
  float ax, ay, az;
  readAccel(ax, ay, az);          // stand-in for your IMU library
 
  // The ball rolls downhill, opposite to what the accelerometer reports.
  int col = constrain(round(2 - ax / 0.3), 0, 4);
  int row = constrain(round(2 + ay / 0.3), 0, 4);
 
  matrix.clear();
  matrix.setPixelColor(row * 5 + col, matrix.Color(0, 60, 80));
  matrix.show();
  delay(20);
}

A stair counter. Watch the barometer for drops of about 36 Pa, one storey's worth of stairs, over a few seconds, and combine it with the IMU's built-in step counting. Steps plus rising height means stairs.

A compass that points north. Light the matrix's outer ring of 16 pixels and pick the one that points north as you turn the board. Calibrate it first, and add tilt compensation once the basic version works.

A board that sleeps until it's picked up. Enable the IMU's wake-on-motion, put the ESP32-S3 into deep sleep, and let GPIO21 wake it when someone moves it. It lasts far longer on a battery than a board that stays awake.

All of the Vector's sensor pins and addresses are listed in the onboard hardware docs.