release 0.0.1 of fsfw added as a core
This commit is contained in:
115
fsfw/globalfunctions/math/MatrixOperations.h
Normal file
115
fsfw/globalfunctions/math/MatrixOperations.h
Normal file
@ -0,0 +1,115 @@
|
||||
#ifndef MATRIXOPERATIONS_H_
|
||||
#define MATRIXOPERATIONS_H_
|
||||
|
||||
#include <cmath>
|
||||
#include <stdint.h>
|
||||
|
||||
template<typename T1, typename T2=T1, typename T3=T2>
|
||||
class MatrixOperations {
|
||||
public:
|
||||
//do not use with result == matrix1 or matrix2
|
||||
static void multiply(const T1 *matrix1, const T2 *matrix2, T3 *result,
|
||||
uint8_t rows1, uint8_t columns1, uint8_t columns2) {
|
||||
if ((matrix1 == (T1*)result) || (matrix2 == (T2*)result)){
|
||||
//SHOULDDO find an implementation that is tolerant to this
|
||||
return;
|
||||
}
|
||||
for (uint8_t resultColumn = 0; resultColumn < columns2;
|
||||
resultColumn++) {
|
||||
for (uint8_t resultRow = 0; resultRow < rows1; resultRow++) {
|
||||
result[resultColumn + columns2 * resultRow] = 0;
|
||||
for (uint8_t i = 0; i < columns1; i++) {
|
||||
result[resultColumn + columns2 * resultRow] += matrix1[i
|
||||
+ resultRow * columns1]
|
||||
* matrix2[resultColumn + i * columns2];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void transpose(const T1 *matrix, T2 *transposed, uint8_t size) {
|
||||
uint8_t row, column;
|
||||
transposed[0] = matrix[0];
|
||||
for (column = 1; column < size; column++) {
|
||||
transposed[column + size * column] = matrix[column + size * column];
|
||||
for (row = 0; row < column; row++) {
|
||||
T1 temp = matrix[column + size * row];
|
||||
transposed[column + size * row] = matrix[row + size * column];
|
||||
transposed[row + size * column] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Overload transpose to support non symmetrical matrices
|
||||
//do not use with transposed == matrix && columns != rows
|
||||
static void transpose(const T1 *matrix, T2 *transposed, uint8_t rows, uint8_t columns) {
|
||||
uint8_t row, column;
|
||||
transposed[0] = matrix[0];
|
||||
if (matrix == transposed && columns == rows)
|
||||
{
|
||||
transpose(matrix, transposed, rows);
|
||||
}
|
||||
else if (matrix == transposed && columns != rows)
|
||||
{
|
||||
// not permitted
|
||||
return;
|
||||
}
|
||||
for (column = 0; column < columns; column++) {
|
||||
for (row = 0; row < rows; row++) {
|
||||
transposed[row + column * rows] = matrix[column + row * columns];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void add(const T1 *matrix1, const T2 *matrix2, T3 *result,
|
||||
uint8_t rows, uint8_t columns)
|
||||
{
|
||||
for (uint8_t resultColumn = 0; resultColumn < columns; resultColumn++)
|
||||
{
|
||||
for (uint8_t resultRow = 0; resultRow < rows; resultRow++)
|
||||
{
|
||||
result[resultColumn + columns * resultRow] = matrix1[resultColumn + columns * resultRow]+
|
||||
matrix2[resultColumn + columns * resultRow];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void subtract(const T1 *matrix1, const T2 *matrix2, T3 *result,
|
||||
uint8_t rows, uint8_t columns)
|
||||
{
|
||||
for (uint8_t resultColumn = 0; resultColumn < columns; resultColumn++)
|
||||
{
|
||||
for (uint8_t resultRow = 0; resultRow < rows; resultRow++)
|
||||
{
|
||||
result[resultColumn + columns * resultRow] = matrix1[resultColumn + columns * resultRow]-
|
||||
matrix2[resultColumn + columns * resultRow];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void addScalar(const T1 *matrix1, const T2 scalar, T3 *result,
|
||||
uint8_t rows, uint8_t columns)
|
||||
{
|
||||
for (uint8_t resultColumn = 0; resultColumn < columns; resultColumn++)
|
||||
{
|
||||
for (uint8_t resultRow = 0; resultRow < rows; resultRow++)
|
||||
{
|
||||
result[resultColumn + columns * resultRow] = matrix1[resultColumn + columns * resultRow]+scalar;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void multiplyScalar(const T1 *matrix1, const T2 scalar, T3 *result,
|
||||
uint8_t rows, uint8_t columns)
|
||||
{
|
||||
for (uint8_t resultColumn = 0; resultColumn < columns; resultColumn++)
|
||||
{
|
||||
for (uint8_t resultRow = 0; resultRow < rows; resultRow++)
|
||||
{
|
||||
result[resultColumn + columns * resultRow] = matrix1[resultColumn + columns * resultRow]*scalar;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* MATRIXOPERATIONS_H_ */
|
||||
156
fsfw/globalfunctions/math/QuaternionOperations.cpp
Normal file
156
fsfw/globalfunctions/math/QuaternionOperations.cpp
Normal file
@ -0,0 +1,156 @@
|
||||
#include "QuaternionOperations.h"
|
||||
#include "VectorOperations.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <stdint.h>
|
||||
|
||||
QuaternionOperations::~QuaternionOperations() {
|
||||
}
|
||||
|
||||
void QuaternionOperations::multiply(const double* q1, const double* q2,
|
||||
double* q) {
|
||||
double out[4];
|
||||
|
||||
out[0] = q1[3] * q2[0] + q1[2] * q2[1] - q1[1] * q2[2] + q1[0] * q2[3];
|
||||
out[1] = -q1[2] * q2[0] + q1[3] * q2[1] + q1[0] * q2[2] + q1[1] * q2[3];
|
||||
out[2] = q1[1] * q2[0] - q1[0] * q2[1] + q1[3] * q2[2] + q1[2] * q2[3];
|
||||
out[3] = -q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] + q1[3] * q2[3];
|
||||
|
||||
memcpy(q, out, 4 * sizeof(*q));
|
||||
}
|
||||
|
||||
void QuaternionOperations::toDcm(const double* quaternion, double dcm[][3]) {
|
||||
dcm[0][0] = 2
|
||||
* (quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3])
|
||||
- 1;
|
||||
dcm[0][1] = 2
|
||||
* (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
|
||||
dcm[0][2] = 2
|
||||
* (quaternion[0] * quaternion[2] - quaternion[1] * quaternion[3]);
|
||||
|
||||
dcm[1][0] = 2
|
||||
* (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]);
|
||||
dcm[1][1] = 2
|
||||
* (quaternion[1] * quaternion[1] + quaternion[3] * quaternion[3])
|
||||
- 1;
|
||||
dcm[1][2] = 2
|
||||
* (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]);
|
||||
|
||||
dcm[2][0] = 2
|
||||
* (quaternion[0] * quaternion[2] + quaternion[1] * quaternion[3]);
|
||||
dcm[2][1] = 2
|
||||
* (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
|
||||
dcm[2][2] = 2
|
||||
* (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3])
|
||||
- 1;
|
||||
}
|
||||
|
||||
void QuaternionOperations::inverse(const double* quaternion,
|
||||
double* inverseQuaternion) {
|
||||
memcpy(inverseQuaternion, quaternion, 4 * sizeof(*quaternion));
|
||||
VectorOperations<double>::mulScalar(inverseQuaternion, -1,
|
||||
inverseQuaternion, 3);
|
||||
}
|
||||
|
||||
QuaternionOperations::QuaternionOperations() {
|
||||
|
||||
}
|
||||
|
||||
void QuaternionOperations::normalize(const double* quaternion,
|
||||
double* unitQuaternion) {
|
||||
VectorOperations<double>::normalize(quaternion, unitQuaternion, 4);
|
||||
}
|
||||
|
||||
float QuaternionOperations::norm(const double* quaternion) {
|
||||
return VectorOperations<double>::norm(quaternion, 4);
|
||||
}
|
||||
|
||||
void QuaternionOperations::fromDcm(const double dcm[][3], double* quaternion,
|
||||
uint8_t *index) {
|
||||
|
||||
double a[4];
|
||||
|
||||
a[0] = 1 + dcm[0][0] - dcm[1][1] - dcm[2][2];
|
||||
a[1] = 1 - dcm[0][0] + dcm[1][1] - dcm[2][2];
|
||||
a[2] = 1 - dcm[0][0] - dcm[1][1] + dcm[2][2];
|
||||
a[3] = 1 + dcm[0][0] + dcm[1][1] + dcm[2][2];
|
||||
|
||||
uint8_t maxAIndex = 0;
|
||||
|
||||
VectorOperations<double>::maxValue(a, 4, &maxAIndex);
|
||||
|
||||
if (index != 0) {
|
||||
*index = maxAIndex;
|
||||
}
|
||||
|
||||
switch (maxAIndex) {
|
||||
case 0:
|
||||
quaternion[0] = 0.5 * sqrt(a[0]);
|
||||
quaternion[1] = (dcm[0][1] + dcm[1][0]) / (2 * sqrt(a[0]));
|
||||
quaternion[2] = (dcm[0][2] + dcm[2][0]) / (2 * sqrt(a[0]));
|
||||
quaternion[3] = (dcm[1][2] - dcm[2][1]) / (2 * sqrt(a[0]));
|
||||
break;
|
||||
case 1:
|
||||
quaternion[0] = (dcm[0][1] + dcm[1][0]) / (2 * sqrt(a[1]));
|
||||
quaternion[1] = 0.5 * sqrt(a[1]);
|
||||
quaternion[2] = (dcm[1][2] + dcm[2][1]) / (2 * sqrt(a[1]));
|
||||
quaternion[3] = (dcm[2][0] - dcm[0][2]) / (2 * sqrt(a[1]));
|
||||
break;
|
||||
case 2:
|
||||
quaternion[0] = (dcm[0][2] + dcm[2][0]) / (2 * sqrt(a[2]));
|
||||
quaternion[1] = (dcm[1][2] + dcm[2][1]) / (2 * sqrt(a[2]));
|
||||
quaternion[2] = 0.5 * sqrt(a[2]);
|
||||
quaternion[3] = (dcm[0][1] - dcm[1][0]) / (2 * sqrt(a[2]));
|
||||
break;
|
||||
case 3:
|
||||
quaternion[0] = (dcm[1][2] - dcm[2][1]) / (2 * sqrt(a[3]));
|
||||
quaternion[1] = (dcm[2][0] - dcm[0][2]) / (2 * sqrt(a[3]));
|
||||
quaternion[2] = (dcm[0][1] - dcm[1][0]) / (2 * sqrt(a[3]));
|
||||
quaternion[3] = 0.5 * sqrt(a[3]);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void QuaternionOperations::toDcm(const double* quaternion, float dcm[][3]) {
|
||||
dcm[0][0] = 2
|
||||
* (quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3])
|
||||
- 1;
|
||||
dcm[0][1] = 2
|
||||
* (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
|
||||
dcm[0][2] = 2
|
||||
* (quaternion[0] * quaternion[2] - quaternion[1] * quaternion[3]);
|
||||
|
||||
dcm[1][0] = 2
|
||||
* (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]);
|
||||
dcm[1][1] = 2
|
||||
* (quaternion[1] * quaternion[1] + quaternion[3] * quaternion[3])
|
||||
- 1;
|
||||
dcm[1][2] = 2
|
||||
* (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]);
|
||||
|
||||
dcm[2][0] = 2
|
||||
* (quaternion[0] * quaternion[2] + quaternion[1] * quaternion[3]);
|
||||
dcm[2][1] = 2
|
||||
* (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
|
||||
dcm[2][2] = 2
|
||||
* (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3])
|
||||
- 1;
|
||||
}
|
||||
|
||||
void QuaternionOperations::normalize(double* quaternion) {
|
||||
normalize(quaternion, quaternion);
|
||||
}
|
||||
|
||||
double QuaternionOperations::getAngle(const double* quaternion, bool abs) {
|
||||
if (quaternion[3] >= 0) {
|
||||
return 2 * acos(quaternion[3]);
|
||||
} else {
|
||||
if (abs) {
|
||||
return 2 * acos(-quaternion[3]);
|
||||
} else {
|
||||
return -2 * acos(-quaternion[3]);
|
||||
}
|
||||
}
|
||||
}
|
||||
81
fsfw/globalfunctions/math/QuaternionOperations.h
Normal file
81
fsfw/globalfunctions/math/QuaternionOperations.h
Normal file
@ -0,0 +1,81 @@
|
||||
#ifndef QUATERNIONOPERATIONS_H_
|
||||
#define QUATERNIONOPERATIONS_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
class QuaternionOperations {
|
||||
public:
|
||||
virtual ~QuaternionOperations();
|
||||
|
||||
static void multiply(const double *q1, const double *q2, double *q);
|
||||
|
||||
static void fromDcm(const double dcm[][3], double *quaternion,
|
||||
uint8_t *index = 0);
|
||||
|
||||
static void toDcm(const double *quaternion, double dcm[][3]);
|
||||
|
||||
static void toDcm(const double *quaternion, float dcm[][3]);
|
||||
|
||||
static float norm(const double *quaternion);
|
||||
|
||||
static void normalize(double *quaternion);
|
||||
|
||||
static void normalize(const double *quaternion, double *unitQuaternion);
|
||||
|
||||
static void inverse(const double *quaternion, double *inverseQuaternion);
|
||||
|
||||
/**
|
||||
* returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
|
||||
*/
|
||||
static double getAngle(const double *quaternion, bool abs = false);
|
||||
|
||||
//multiplies 3d vector with dcm derived from quaternion
|
||||
template<typename T>
|
||||
static void multiplyVector(const double *quaternion, const T *vector,
|
||||
T * result) {
|
||||
result[0] =
|
||||
(2.
|
||||
* (quaternion[0] * quaternion[0]
|
||||
+ quaternion[3] * quaternion[3]) - 1.)
|
||||
* vector[0]
|
||||
+ 2.
|
||||
* (quaternion[0] * quaternion[1]
|
||||
+ quaternion[2] * quaternion[3])
|
||||
* vector[1]
|
||||
+ 2.
|
||||
* (quaternion[0] * quaternion[2]
|
||||
- quaternion[1] * quaternion[3])
|
||||
* vector[2];
|
||||
|
||||
result[1] =
|
||||
2.
|
||||
* (quaternion[0] * quaternion[1]
|
||||
- quaternion[2] * quaternion[3]) * vector[0]
|
||||
+ (2.
|
||||
* (quaternion[1] * quaternion[1]
|
||||
+ quaternion[3] * quaternion[3]) - 1.)
|
||||
* vector[1]
|
||||
+ 2.
|
||||
* (quaternion[1] * quaternion[2]
|
||||
+ quaternion[0] * quaternion[3])
|
||||
* vector[2];
|
||||
|
||||
result[2] =
|
||||
2.
|
||||
* (quaternion[0] * quaternion[2]
|
||||
+ quaternion[1] * quaternion[3]) * vector[0]
|
||||
+ 2.
|
||||
* (quaternion[1] * quaternion[2]
|
||||
- quaternion[0] * quaternion[3])
|
||||
* vector[1]
|
||||
+ (2.
|
||||
* (quaternion[2] * quaternion[2]
|
||||
+ quaternion[3] * quaternion[3]) - 1.)
|
||||
* vector[2];
|
||||
}
|
||||
|
||||
private:
|
||||
QuaternionOperations();
|
||||
};
|
||||
|
||||
#endif /* QUATERNIONOPERATIONS_H_ */
|
||||
103
fsfw/globalfunctions/math/VectorOperations.h
Normal file
103
fsfw/globalfunctions/math/VectorOperations.h
Normal file
@ -0,0 +1,103 @@
|
||||
#ifndef VECTOROPERATIONS_
|
||||
#define VECTOROPERATIONS_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <cmath>
|
||||
|
||||
template<typename T>
|
||||
class VectorOperations {
|
||||
public:
|
||||
virtual ~VectorOperations() {
|
||||
}
|
||||
|
||||
static void cross(const T left[], const T right[], T out[]) {
|
||||
T temp[3] = { 0, 0, 0 };
|
||||
temp[0] = left[1] * right[2] - left[2] * right[1];
|
||||
temp[1] = left[2] * right[0] - left[0] * right[2];
|
||||
temp[2] = left[0] * right[1] - left[1] * right[0];
|
||||
out[0] = temp[0];
|
||||
out[1] = temp[1];
|
||||
out[2] = temp[2];
|
||||
}
|
||||
|
||||
static T dot(const T a[], const T b[]) {
|
||||
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
|
||||
}
|
||||
|
||||
static void mulScalar(const T vector[], T scalar, T out[], uint8_t size) {
|
||||
for (; size > 0; size--) {
|
||||
out[size - 1] = vector[size - 1] * scalar;
|
||||
}
|
||||
}
|
||||
|
||||
static void add(const T vector1[], const T vector2[], T sum[],
|
||||
uint8_t size = 3) {
|
||||
for (; size > 0; size--) {
|
||||
sum[size - 1] = vector1[size - 1] + vector2[size - 1];
|
||||
}
|
||||
}
|
||||
|
||||
static void subtract(const T vector1[], const T vector2[], T sum[],
|
||||
uint8_t size = 3) {
|
||||
for (; size > 0; size--) {
|
||||
sum[size - 1] = vector1[size - 1] - vector2[size - 1];
|
||||
}
|
||||
}
|
||||
|
||||
static T norm(const T *vector, uint8_t size) {
|
||||
T result = 0;
|
||||
for (; size > 0; size--) {
|
||||
result += vector[size - 1] * vector[size - 1];
|
||||
}
|
||||
result = sqrt(result);
|
||||
return result;
|
||||
}
|
||||
|
||||
static void normalize(const T *vector, T *normalizedVector, uint8_t size) {
|
||||
mulScalar(vector, 1 / norm(vector, size), normalizedVector, size);
|
||||
}
|
||||
|
||||
static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = 0) {
|
||||
|
||||
T max = -1;
|
||||
|
||||
for (; size > 0; size--) {
|
||||
T abs = vector[size - 1];
|
||||
if (abs < 0) {
|
||||
abs = -abs;
|
||||
}
|
||||
if (abs > max) {
|
||||
max = abs;
|
||||
if (index != 0) {
|
||||
*index = size - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return max;
|
||||
}
|
||||
|
||||
static T maxValue(const T *vector, uint8_t size, uint8_t *index = 0) {
|
||||
|
||||
T max = -1;
|
||||
|
||||
for (; size > 0; size--) {
|
||||
if (vector[size - 1] > max) {
|
||||
max = vector[size - 1];
|
||||
if (index != 0) {
|
||||
*index = size - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return max;
|
||||
}
|
||||
|
||||
|
||||
static void copy(const T *in, T *out, uint8_t size) {
|
||||
mulScalar(in, 1, out, size);
|
||||
}
|
||||
|
||||
private:
|
||||
VectorOperations();
|
||||
};
|
||||
|
||||
#endif /* VECTOROPERATIONS_ */
|
||||
Reference in New Issue
Block a user