![]() |
Hummingbird Flight Software
Flight software for the Hummingbird FCU quadcopter flight controller. Designed to run on the Teensy 4.1. Developed with VSCode+PlatformIO.
|
Decide whether or not to rotate magnetometer measurements. More...
#include <compass.h>
Public Member Functions | |
| MagCompass (TwoWire *userWire=&SENSOR_I2C) | |
| The compass class is in charge of taking magnetometer measurements and computing tilt-compensated (magnetic) heading. More... | |
| ~MagCompass () | |
| MagCompass (const MagCompass &)=delete | |
| MagCompass & | operator= (const MagCompass &)=delete |
| bool | Initialize () |
| Initialize the compass. More... | |
| bool | Update () |
| Update the compass. More... | |
| float | GetHeading (Vectorf AccelMeas) |
| Compute and return tilt-compensated magnetic heading based on accelerometer data. More... | |
Static Public Member Functions | |
| static MagCompass & | GetInstance () |
Public Attributes | |
| uint32_t | prevUpdateMicros |
| Vectorf | Mag |
| Vectorf | MagRaw |
Private Attributes | |
| float | heading |
| LIS3MDL_Mag | MagSensor |
| LIS3MDL_MeasRange_t | magMeasRange |
| TwoWire * | SensorWire |
Decide whether or not to rotate magnetometer measurements.
The Magnetometer in the GPS is upside down and rotated. By default, apply rotations to align measurements with the body frame.
| MagCompass::MagCompass | ( | TwoWire * | userWire = &SENSOR_I2C | ) |
The compass class is in charge of taking magnetometer measurements and computing tilt-compensated (magnetic) heading.
Constructor for the compass class. Be sure to specify the I2C bus that the sensor is connected to.
| userWire | I2C bus the compass is connected to. Default Wire2 |
| MagCompass::~MagCompass | ( | ) |
|
delete |
| float MagCompass::GetHeading | ( | Vectorf | AccelMeas | ) |
Compute and return tilt-compensated magnetic heading based on accelerometer data.
Tilt-compensation equations can be found in: https://www.cypress.com/file/130456/download
| AccelMeas | [m/s/s] Vectorf of accelerometer measurements |
|
static |
| bool MagCompass::Initialize | ( | ) |
Initialize the compass.
Configures magnetometer sensor.
|
delete |
| bool MagCompass::Update | ( | ) |
Update the compass.
Record magnetometer data, rotate sensor data to the body frame, and apply calibration.
|
private |
| Vectorf MagCompass::Mag |
|
private |
| Vectorf MagCompass::MagRaw |
|
private |
| uint32_t MagCompass::prevUpdateMicros |
|
private |