magnetometer.cpp

#include "magnetometer.h"
#include <Arduino.h>
#include <Tlv493d.h>

static Tlv493d mag;

// begin() leaves the sensor in POWERDOWNMODE - TLV493D_DEFAULTMODE in the
// library's own config header - and updateData() handles that mode by switching
// the part on, blocking in delay(getMeasurementDelay()) for 10 ms, reading, and
// switching it back off. That is half of every 20 ms control cycle spent in a
// busy wait, plus two extra register writes. Master-controlled mode starts the
// next conversion the moment the last result is read out, so updateData() drops
// to a plain readout with no delay in it; it is the mode the library switches
// into for that one measurement anyway. Conversion takes 10 ms and we read
// every 20 ms, so each readout gets a completed measurement.
//
// begin() returns nothing, so the only way to find out whether the part is
// actually on the bus is to take a reading and check the error code.
//
// Take more than one. updateData() returns TLV493D_FRAME_ERROR whenever the
// channel field of the readout is non-zero, which means the registers were read
// across a conversion boundary rather than that anything is broken - the
// library's own view of how often that happens is the frame-counter check sitting
// commented out next to it with "removed due to a lot of frame errors". A single
// one here would cost the flight its only heading source, while the same part
// gets nine consecutive failures in the air before sensors.cpp gives up on it.
// Three tries a measurement period apart costs 30 ms of boot.
static const int INIT_READ_ATTEMPTS = 3;

bool initMagnetometer() {
    mag.begin();

    // setAccessMode() reports the library's BUS_OK as 0, so a true return is
    // the error case. The macro itself is not reachable from Tlv493d.h.
    bool modeBusError = mag.setAccessMode(Tlv493d::MASTERCONTROLLEDMODE);
    if (modeBusError) {
        return false;
    }

    for (int attempt = 0; attempt < INIT_READ_ATTEMPTS; attempt++) {
        delay(mag.getMeasurementDelay());   // the first pass has no conversion yet
        if (mag.updateData() == TLV493D_NO_ERROR) {
            return true;
        }
    }
    return false;
}

// Heading is taken off the X/Y plane, assuming the sensor lies flat with +X out
// the nose. That mounting has not been checked against the real airframe, so the
// axis pair and the sign may both need swapping once someone turns the vehicle
// on a bench. Do that before any cruise flight, not as a calibration nicety:
// atan2f(y, x) increases counter-clockwise while a compass increases clockwise,
// and if the sign here is wrong then the heading error in
// flight_control_horizontal.cpp comes out inverted, which turns the yaw PD into
// positive feedback. The aircraft would not read a few degrees off, it would
// turn away from the setpoint and keep turning.
//
// This is magnetic heading - no declination is applied, because nobody has
// measured the local declination.
bool readHeading(float *heading_deg) {
    if (mag.updateData() != TLV493D_NO_ERROR) {
        return false;
    }

    // RAD_TO_DEG is a double literal; keep the whole expression single precision.
    float heading = atan2f(mag.getY(), mag.getX()) * (float)RAD_TO_DEG;
    if (heading < 0.0f) {
        heading += 360.0f;
    }

    *heading_deg = heading;
    return true;
}