72 lines
No EOL
4.5 KiB
C++
72 lines
No EOL
4.5 KiB
C++
#include "Adafruit_Simple_AHRS.h"
|
|
|
|
// Create a simple AHRS from an explicit accelerometer and magnetometer sensor.
|
|
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.
|
|
Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(const 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.
|
|
bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t* orientation) {
|
|
// Validate input and available sensors.
|
|
if (orientation == NULL || _accel == NULL || _mag == NULL) return false;
|
|
|
|
// Grab an acceleromter and magnetometer reading.
|
|
sensor_event_t accel_event;
|
|
_accel.getEvent(&accel_event);
|
|
sensor_event_t mag_event;
|
|
_mag.getEvent(&mag_event);
|
|
|
|
float const PI_F = 3.14159265F;
|
|
|
|
// roll: Rotation around the X-axis. -180 <= roll <= 180
|
|
// a positive roll angle is defined to be a clockwise rotation about the positive X-axis
|
|
//
|
|
// y
|
|
// roll = atan2(---)
|
|
// z
|
|
//
|
|
// where: y, z are returned value from accelerometer sensor
|
|
orientation->roll = (float)atan2(accel_event->acceleration.y, accel_event->acceleration.z);
|
|
|
|
// pitch: Rotation around the Y-axis. -180 <= roll <= 180
|
|
// a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis
|
|
//
|
|
// -x
|
|
// pitch = atan(-------------------------------)
|
|
// y * sin(roll) + z * cos(roll)
|
|
//
|
|
// where: x, y, z are returned value from accelerometer sensor
|
|
if (accel_event->acceleration.y * sin(orientation->roll) + accel_event->acceleration.z * cos(orientation->roll) == 0)
|
|
orientation->pitch = accel_event->acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
|
|
else
|
|
orientation->pitch = (float)atan(-accel_event->acceleration.x / (accel_event->acceleration.y * sin(orientation->roll) + \
|
|
accel_event->acceleration.z * cos(orientation->roll)));
|
|
|
|
// heading: Rotation around the Z-axis. -180 <= roll <= 180
|
|
// a positive heading angle is defined to be a clockwise rotation about the positive Z-axis
|
|
//
|
|
// z * sin(roll) - y * cos(roll)
|
|
// heading = atan2(--------------------------------------------------------------------------)
|
|
// x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll))
|
|
//
|
|
// where: x, y, z are returned value from magnetometer sensor
|
|
orientation->heading = (float)atan2(mag_event->magnetic.z * sin(orientation->roll) - mag_event->magnetic.y * cos(orientation->roll), \
|
|
mag_event->magnetic.x * cos(orientation->pitch) + \
|
|
mag_event->magnetic.y * sin(orientation->pitch) * sin(orientation->roll) + \
|
|
mag_event->magnetic.z * sin(orientation->pitch) * cos(orientation->roll));
|
|
|
|
|
|
// Convert angular data to degree
|
|
orientation->roll = orientation->roll * 180 / PI_F;
|
|
orientation->pitch = orientation->pitch * 180 / PI_F;
|
|
orientation->heading = orientation->heading * 180 / PI_F;
|
|
|
|
return true;
|
|
} |