mohr/fsfw
1
0
Fork 0
forked from fsfw/fsfw
Code Pull Requests Activity

updating code from Flying Laptop

Browse Source 
This is the framework of Flying Laptop OBSW version A.13.0.
This commit is contained in:
Ulrich Mohr
2018-07-12 16:29:32 +02:00
parent 1d22a6c97e
commit 575f70ba03
395 changed files with 12807 additions and 8404 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
globalfunctions/math/MatrixOperations.h
View File

@ -4,15 +4,16 @@
#include <cmath>
#include <stdint.h>
template<typename T>
template<typename T1, typename T2=T1, typename T3=T2>
class MatrixOperations {
public:
virtual ~MatrixOperations() {
}
//do not use with result == matrix1 or matrix2 //TODO?
static void multiply(const T *matrix1, const T *matrix2, T *result,
//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++) {
@ -26,21 +27,89 @@ public:
}
}
static void transpose(const T *matrix, T *transposed, uint8_t size) {
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++) {
T temp = matrix[column + size * row];
T1 temp = matrix[column + size * row];
transposed[column + size * row] = matrix[row + size * column];
transposed[row + size * column] = temp;
}
}
}
private:
MatrixOperations();
// 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_ */

5
globalfunctions/math/QuaternionOperations.cpp
View File

@ -1,6 +1,5 @@
#include <framework/globalfunctions/math/QuaternionOperations.h>
#include "QuaternionOperations.h"
#include <framework/globalfunctions/math/VectorOperations.h>
#include "VectorOperations.h"
#include <cmath>
#include <cstring>
@ -79,7 +78,7 @@ void QuaternionOperations::fromDcm(const double dcm[][3], double* quaternion,
uint8_t maxAIndex = 0;
VectorOperations<double>::maxAbsValue(a, 4, &maxAIndex);
VectorOperations<double>::maxValue(a, 4, &maxAIndex);
if (index != 0) {
*index = maxAIndex;

50
globalfunctions/math/QuaternionOperations.h
View File

@ -9,8 +9,9 @@ public:
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 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]);
@ -28,6 +29,51 @@ public:
*/
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();
};

28
globalfunctions/math/VectorOperations.h
View File

@ -38,11 +38,11 @@ public:
}
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];
}
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;
@ -76,6 +76,26 @@ public:
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();
};