adafruit-mirror/Adafruit_AHRS
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Add some doxygen documentation (#19)

Browse source 
* Add some initial NXP docu

gitignore the html folder
I feel like its better to commit the Doxyfile, as i had to add the src
folder as an input for it to generate anything.

* Add Simple AHRS documentation

Slightly improve existing NXP Fusion docu

* Formatting

* Minor Fixes

 * Fix mainpage
 * Document some classes correctly

* Delete Doxyfile
This commit is contained in:
ToMe25 2021-02-09 17:41:23 +01:00 committed by GitHub
parent 1031b89cf1
commit a6eba1cb7b
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 272 additions and 91 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
.gitignore vendored
View file

@ -1 +1,3 @@
serialconfig.txt
.pio
html

18
src/Adafruit_AHRS.h
View file

@ -1,3 +1,21 @@
/*!
* @file Adafruit_AHRS
*
* @mainpage Adafruit AHRS
*
* @section intro_sec Introduction
*
* This library lets you take an accelerometer, gyroscope and magnetometer
* and combine the data to create orientation data.
*
* Options are Mahony (lowest memory/computation),
* Madgwick (fair memory/computation), and NXP fusion/Kalman (highest).
*
* While in theory these can run on an Arduino UNO/Atmega328P we really
* recommend a SAMD21 or better. Having single-instruction floating point
* multiply and plenty of RAM will help a lot!
*/
#ifndef __ADAFRUIT_AHRS_H_
#define __ADAFRUIT_AHRS_H_

57
src/Adafruit_AHRS_FusionInterface.h
View file

@ -1,4 +1,8 @@
/*
/*!
* @file Adafruit_AHRS_FusionInterface.h
*
* @section license License
*
* The MIT License (MIT)
*
* Copyright (c) 2020 Ha Thach (tinyusb.org) for Adafruit Industries
@ -25,15 +29,64 @@
#ifndef ADAFRUIT_AHRS_FUSIONINTERFACE_H_
#define ADAFRUIT_AHRS_FUSIONINTERFACE_H_
// Interface for Fusion Algorithm
/*!
* @brief The common interface for the fusion algorithms.
*/
class Adafruit_AHRS_FusionInterface {
public:
/**************************************************************************/
/*!
* @brief Initializes the sensor fusion filter.
*
* @param sampleFrequency The sensor sample rate in herz(samples per second).
*/
/**************************************************************************/
virtual void begin(float sampleFrequency) = 0;
/**************************************************************************/
/*!
* @brief Updates the filter with new gyroscope, accelerometer, and
* magnetometer data.
*
* @param gx The gyroscope x axis. In DPS.
* @param gy The gyroscope y axis. In DPS.
* @param gz The gyroscope z axis. In DPS.
* @param ax The accelerometer x axis. In g.
* @param ay The accelerometer y axis. In g.
* @param az The accelerometer z axis. In g.
* @param mx The magnetometer x axis. In uT.
* @param my The magnetometer y axis. In uT.
* @param mz The magnetometer z axis. In uT.
*/
/**************************************************************************/
virtual void update(float gx, float gy, float gz, float ax, float ay,
float az, float mx, float my, float mz) = 0;
/**************************************************************************/
/*!
* @brief Gets the current roll of the sensors.
*
* @return The current sensor roll.
*/
/**************************************************************************/
virtual float getRoll() = 0;
/**************************************************************************/
/*!
* @brief Gets the current pitch of the sensors.
*
* @return The current sensor pitch.
*/
/**************************************************************************/
virtual float getPitch() = 0;
/**************************************************************************/
/*!
* @brief Gets the current yaw of the sensors.
*
* @return The current sensor yaw.
*/
/**************************************************************************/
virtual float getYaw() = 0;
virtual void getQuaternion(float *w, float *x, float *y, float *z) = 0;
};

90
src/Adafruit_AHRS_NXPFusion.cpp
View file

@ -1,34 +1,39 @@
// Copyright (c) 2014, Freescale Semiconductor, Inc.
// All rights reserved.
// vim: set ts=4:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of Freescale Semiconductor, Inc. nor the
// names of its contributors may be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// This is the file that contains the fusion routines. It is STRONGLY
// RECOMMENDED that the casual developer NOT TOUCH THIS FILE. The mathematics
// behind this file is extremely complex, and it will be very easy (almost
// inevitable) that you screw it up.
//
/*!
* @file Adafruit_AHRS_NXPFusion.cpp
*
* @section license License
*
* Copyright (c) 2014, Freescale Semiconductor, Inc.
* All rights reserved.
* vim: set ts=4:
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Freescale Semiconductor, Inc. nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
* This is the file that contains the fusion routines. It is STRONGLY
* RECOMMENDED that the casual developer NOT TOUCH THIS FILE. The mathematics
* behind this file is extremely complex, and it will be very easy (almost
* inevitable) that you screw it up.
*/
#include "Adafruit_AHRS_NXPFusion.h"
@ -111,8 +116,12 @@ void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[],
void fmatrixAeqRenormRotA(float A[][3]);
}
// initialize the 9DOF Kalman filter
void Adafruit_NXPSensorFusion::begin(float sampleRate) {
/**************************************************************************/
/*!
* @brief Initializes the 9DOF Kalman filter.
*/
/**************************************************************************/
void Adafruit_NXPSensorFusion::begin(float sampleFrequency) {
int8_t i, j;
// reset the flag denoting that a first 9DOF orientation lock has been
@ -121,7 +130,7 @@ void Adafruit_NXPSensorFusion::begin(float sampleRate) {
// compute and store useful product terms to save floating point calculations
// later
Fastdeltat = 1.0f / sampleRate;
Fastdeltat = 1.0f / sampleFrequency;
deltat = Fastdeltat;
deltatsq = deltat * deltat;
casq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
@ -186,11 +195,12 @@ void Adafruit_NXPSensorFusion::begin(float sampleRate) {
resetflag = 0;
}
// 9DOF orientation function implemented using a 12 element Kalman filter
// void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
// const MagCalibration_t *MagCal)
/**************************************************************************/
/*!
* @brief Updates the filter with new gyroscope, accelerometer, and magnetometer
* data.
*/
/**************************************************************************/
void Adafruit_NXPSensorFusion::update(float gx, float gy, float gz, float ax,
float ay, float az, float mx, float my,
float mz) {
@ -302,7 +312,7 @@ void Adafruit_NXPSensorFusion::update(float gx, float gy, float gz, float ax,
// (with magnitude 1g) NED gravity is along positive z axis
gSeGyMi[i] = RMi[i][Z];
// compute a priori acceleration (a-) (g, sensor frame) from decayed a
// compute the a priori acceleration (a-) (g, sensor frame) from decayed a
// posteriori estimate (g, sensor frame)
aSeMi[i] = FCA_9DOF_GBY_KALMAN * aSePl[i];

140
src/Adafruit_AHRS_NXPFusion.h
View file

@ -1,47 +1,81 @@
#include "Adafruit_AHRS_FusionInterface.h"
#include <Arduino.h>
// This is a modification of
// https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/blob/master/Sources/tasks.h
// by PJRC / Paul Stoffregen https://github.com/PaulStoffregen/NXPMotionSense
// Copyright (c) 2014, Freescale Semiconductor, Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of Freescale Semiconductor, Inc. nor the
// names of its contributors may be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
/*!
* @file Adafruit_AHRS_NXPFusion.h
*
* @section license License
*
* This is a modification of
* https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/blob/master/Sources/tasks.h
* by PJRC / Paul Stoffregen https://github.com/PaulStoffregen/NXPMotionSense
*
* Copyright (c) 2014, Freescale Semiconductor, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Freescale Semiconductor, Inc. nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef __Adafruit_Nxp_Fusion_h_
#define __Adafruit_Nxp_Fusion_h_
// changed class name to avoid collision
#include "Adafruit_AHRS_FusionInterface.h"
#include <Arduino.h>
/*!
* @brief Kalman/NXP Fusion algorithm.
*/
class Adafruit_NXPSensorFusion : public Adafruit_AHRS_FusionInterface {
public:
void begin(float sampleRate = 100.0f);
/*
* For roll, pitch, and yaw the accelerometer values can be either m/s^2 or g,
* but for linear acceleration to work they have to be in g.
/**************************************************************************/
/*!
* @brief Initializes the 9DOF Kalman filter.
*
* @param sampleFrequency The sensor sample rate in herz(samples per second).
*/
/**************************************************************************/
void begin(float sampleFrequency = 100.0f);
/**************************************************************************/
/*!
* @brief Updates the filter with new gyroscope, accelerometer, and
* magnetometer data. For roll, pitch, and yaw the accelerometer values can be
* either m/s^2 or g, but for linear acceleration they have to be in g.
*
* 9DOF orientation function implemented using a 12 element Kalman filter
*
* void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
* const AccelSensor_t *Accel, const MagSensor_t *Mag,
* const GyroSensor_t *Gyro, const MagCalibration_t *MagCal)
*
* @param gx The gyroscope x axis. In DPS.
* @param gy The gyroscope y axis. In DPS.
* @param gz The gyroscope z axis. In DPS.
* @param ax The accelerometer x axis. In g.
* @param ay The accelerometer y axis. In g.
* @param az The accelerometer z axis. In g.
* @param mx The magnetometer x axis. In uT.
* @param my The magnetometer y axis. In uT.
* @param mz The magnetometer z axis. In uT.
*/
/**************************************************************************/
void update(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz);
@ -56,28 +90,46 @@ public:
*z = qPl.q3;
}
/*
* Get the linear acceleration part of the acceleration value given to update
* in g.
/**************************************************************************/
/*!
* @brief Get the linear acceleration part of the acceleration value given to
* update.
*
* @param x The pointer to write the linear acceleration x axis to. In g.
* @param y The pointer to write the linear acceleration y axis to. In g.
* @param z The pointer to write the linear acceleration z axis to. In g.
*/
/**************************************************************************/
void getLinearAcceleration(float *x, float *y, float *z) const {
*x = aSePl[0];
*y = aSePl[1];
*z = aSePl[2];
}
/**
* The gravitational vector from the gyroscope values.
/**************************************************************************/
/*!
* @brief Get the gravity vector from the gyroscope values.
*
* @param x The pointer to write the gravity vector x axis to. In g.
* @param y The pointer to write the gravity vector y axis to. In g.
* @param z The pointer to write the gravity vector z axis to. In g.
*/
/**************************************************************************/
void getGravityVector(float *x, float *y, float *z) const {
*x = gSeGyMi[0];
*y = gSeGyMi[1];
*z = gSeGyMi[2];
}
/*
* The global geomagnetic vector in uT.
/**************************************************************************/
/*!
* @brief Get the geomagnetic vector in global frame.
*
* @param x The pointer to write the geomagnetic vector x axis to. In uT.
* @param y The pointer to write the geomagnetic vector y axis to. In uT.
* @param z The pointer to write the geomagnetic vector z axis to. In uT.
*/
/**************************************************************************/
void getGeomagneticVector(float *x, float *y, float *z) const {
*x = mGl[0];
*y = mGl[1];

22
src/Adafruit_Simple_AHRS.cpp
View file

@ -1,16 +1,32 @@
/*!
* @file Adafruit_Simple_AHRS.cpp
*/
#include "Adafruit_Simple_AHRS.h"
// Create a simple AHRS from an explicit accelerometer and magnetometer sensor.
/**************************************************************************/
/*!
* @brief Create a simple AHRS from a device with multiple sensors.
*/
/**************************************************************************/
Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor *accelerometer,
Adafruit_Sensor *magnetometer)
: _accel(accelerometer), _mag(magnetometer) {}
// Create a simple AHRS from a device with multiple sensors.
/**************************************************************************/
/*!
* @brief Create a simple AHRS from a device with multiple sensors.
*/
/**************************************************************************/
Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor_Set &sensors)
: _accel(sensors.getSensor(SENSOR_TYPE_ACCELEROMETER)),
_mag(sensors.getSensor(SENSOR_TYPE_MAGNETIC_FIELD)) {}
// Compute orientation based on accelerometer and magnetometer data.
/**************************************************************************/
/*!
* @brief Compute orientation based on accelerometer and magnetometer data.
*/
/**************************************************************************/
bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t *orientation) {
// Validate input and available sensors.
if (orientation == NULL || _accel == NULL || _mag == NULL)

32
src/Adafruit_Simple_AHRS.h
View file

@ -1,15 +1,45 @@
/*!
* @file Adafruit_Simple_AHRS.cpp
*/
#ifndef __ADAFRUIT_SIMPLE_AHRS_H__
#define __ADAFRUIT_SIMPLE_AHRS_H__
#include "Adafruit_Sensor_Set.h"
#include <Adafruit_Sensor.h>
// Simple sensor fusion AHRS using an accelerometer and magnetometer.
/*!
* @brief Simple sensor fusion AHRS using an accelerometer and magnetometer.
*/
class Adafruit_Simple_AHRS {
public:
/**************************************************************************/
/*!
* @brief Create a simple AHRS from a device with multiple sensors.
*
* @param accelerometer The accelerometer to use for this sensor fusion.
* @param magnetometer The magnetometer to use for this sensor fusion.
*/
/**************************************************************************/
Adafruit_Simple_AHRS(Adafruit_Sensor *accelerometer,
Adafruit_Sensor *magnetometer);
/**************************************************************************/
/*!
* @brief Create a simple AHRS from a device with multiple sensors.
*
* @param sensors A set of sensors containing the accelerometer and
* magnetometer for this sensor fusion.
*/
/**************************************************************************/
Adafruit_Simple_AHRS(Adafruit_Sensor_Set &sensors);
/**************************************************************************/
/*!
* @brief Compute orientation based on accelerometer and magnetometer data.
*
* @return Whether the orientation was computed.
*/
/**************************************************************************/
bool getOrientation(sensors_vec_t *orientation);
private: