dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mathlib fixes

This commit is contained in:
Anton Babushkin 2013-12-19 14:10:25 +04:00
parent e3a5a384d7
commit ba612c3ee8
22 changed files with 513 additions and 1191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/conversion/rotation.cpp
View File

@ -41,7 +41,7 @@
#include "rotation.h"
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix)
get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix)
{
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;

2
src/lib/conversion/rotation.h
View File

@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = {
* Get the rotation matrix
*/
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix);
#endif /* ROTATION_H_ */

43
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
View File

@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
return _crosstrack_error;
}
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
const math::Vector2f &ground_speed_vector)
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
const math::Vector<2> &ground_speed_vector)
{
/* this follows the logic presented in [1] */
@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float ltrack_vel;
/* get the direction between the last (visited) and next waypoint */
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
_L1_distance = _L1_ratio * ground_speed;
/* calculate vector from A to B */
math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
/*
* check if waypoints are on top of each other. If yes,
@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
vector_AB.normalize();
/* calculate the vector from waypoint A to the aircraft */
math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
/* calculate crosstrack error (output only) */
_crosstrack_error = vector_AB % vector_A_to_airplane;
@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float alongTrackDist = vector_A_to_airplane * vector_AB;
/* estimate airplane position WRT to B */
math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
/* calculate angle of airplane position vector relative to line) */
@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
/* calculate eta to fly to waypoint A */
/* unit vector from waypoint A to current position */
math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
/*
* If the AB vector and the vector from B to airplane point in the same
@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
_nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0));
} else {
@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float eta1 = asinf(sine_eta1);
eta = eta1 + eta2;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
}
@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
_bearing_error = eta;
}
void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector2f &ground_speed_vector)
void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector<2> &ground_speed_vector)
{
/* the complete guidance logic in this section was proposed by [2] */
@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
float K_velocity = 2.0f * _L1_damping * omega;
/* update bearing to next waypoint */
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
_L1_distance = _L1_ratio * ground_speed;
/* calculate the vector from waypoint A to current position */
math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
/* store the normalized vector from waypoint A to current position */
math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
/* calculate eta angle towards the loiter center */
@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
/* angle between requested and current velocity vector */
_bearing_error = eta;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
} else {
_lateral_accel = lateral_accel_sp_circle;
_circle_mode = true;
_bearing_error = 0.0f;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
}
}
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
{
/* the complete guidance logic in this section was proposed by [2] */
@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
}
math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
{
/* this is an approximation for small angles, proposed by [2] */
math::Vector2f out;
out.setX(math::radians((target.getX() - origin.getX())));
out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
}

12
src/lib/ecl/l1/ecl_l1_pos_controller.h
View File

@ -160,8 +160,8 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
const math::Vector2f &ground_speed);
void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
const math::Vector<2> &ground_speed);
/**
@ -172,8 +172,8 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector2f &ground_speed_vector);
void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector<2> &ground_speed_vector);
/**
@ -185,7 +185,7 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
/**
@ -260,7 +260,7 @@ private:
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
* @return The vector in meters pointing from the reference position to the coordinates
*/
math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
};

6
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -29,7 +29,7 @@ using namespace math;
*
*/
void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
{
// Implement third order complementary filter for height and height rate
// estimted height rate = _integ2_state
@ -257,7 +257,7 @@ void TECS::_update_energies(void)
_SKEdot = _integ5_state * _vel_dot;
}
void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat)
{
// Calculate total energy values
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
@ -468,7 +468,7 @@ void TECS::_update_STE_rate_lim(void)
_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
}
void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
float throttle_min, float throttle_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max)
{

6
src/lib/external_lgpl/tecs/tecs.h
View File

@ -71,10 +71,10 @@ public:
// Update of the estimated height and height rate internal state
// Update of the inertial speed rate internal state
// Should be called at 50Hz or greater
void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
// Update the control loop calculations
void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
float throttle_min, float throttle_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max);
// demanded throttle in percentage
@ -338,7 +338,7 @@ private:
void _update_energies(void);
// Update Demanded Throttle
void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat);
// Detect Bad Descent
void _detect_bad_descent(void);

172
src/lib/mathlib/math/Matrix.hpp
View File

@ -42,16 +42,19 @@
#ifndef MATRIX_HPP
#define MATRIX_HPP
#include <stdio.h>
#include "../CMSIS/Include/arm_math.h"
namespace math
{
template <unsigned int N, unsigned int M>
template <unsigned int M, unsigned int N>
class Matrix;
class Quaternion;
// MxN matrix with float elements
template <unsigned int N, unsigned int M>
template <unsigned int M, unsigned int N>
class MatrixBase {
public:
/**
@ -69,10 +72,14 @@ public:
* note that this ctor will not initialize elements
*/
MatrixBase() {
//arm_mat_init_f32(&arm_mat, M, N, data);
arm_mat = {M, N, &data[0][0]};
}
MatrixBase(const float *d) {
arm_mat = {M, N, &data[0][0]};
memcpy(data, d, sizeof(data));
}
/**
* access by index
*/
@ -98,10 +105,10 @@ public:
/**
* test for equality
*/
bool operator ==(const MatrixBase<M, N> &m) {
bool operator ==(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m(i, j))
if (data[i][j] != m.data[i][j])
return false;
return true;
}
@ -109,10 +116,10 @@ public:
/**
* test for inequality
*/
bool operator !=(const MatrixBase<M, N> &m) {
bool operator !=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m(i, j))
if (data[i][j] != m.data[i][j])
return true;
return false;
}
@ -120,85 +127,97 @@ public:
/**
* set to value
*/
const MatrixBase<M, N> &operator =(const MatrixBase<M, N> &m) {
const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
memcpy(data, m.data, sizeof(data));
return *this;
return *reinterpret_cast<Matrix<M, N>*>(this);
}
/**
* negation
*/
MatrixBase<M, N> operator -(void) const {
MatrixBase<M, N> res;
Matrix<M, N> operator -(void) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
res[i][j] = -data[i][j];
res.data[i][j] = -data[i][j];
return res;
}
/**
* addition
*/
MatrixBase<M, N> operator +(const MatrixBase<M, N> &m) const {
MatrixBase<M, N> res;
Matrix<M, N> operator +(const Matrix<M, N> &m) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
res[i][j] = data[i][j] + m(i, j);
res.data[i][j] = data[i][j] + m.data[i][j];
return res;
}
MatrixBase<M, N> &operator +=(const MatrixBase<M, N> &m) {
return *this = *this + m;
Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
data[i][j] += m.data[i][j];
return *reinterpret_cast<Matrix<M, N>*>(this);
}
/**
* subtraction
*/
MatrixBase<M, N> operator -(const MatrixBase<M, N> &m) const {
MatrixBase<M, N> res;
Matrix<M, N> operator -(const Matrix<M, N> &m) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res[i][j] = data[i][j] - m(i, j);
res.data[i][j] = data[i][j] - m.data[i][j];
return res;
}
MatrixBase<M, N> &operator -=(const MatrixBase<M, N> &m) {
return *this = *this - m;
Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
data[i][j] -= m.data[i][j];
return *reinterpret_cast<Matrix<M, N>*>(this);
}
/**
* uniform scaling
*/
MatrixBase<M, N> operator *(const float num) const {
MatrixBase<M, N> res;
Matrix<M, N> operator *(const float num) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res[i][j] = data[i][j] * num;
res.data[i][j] = data[i][j] * num;
return res;
}
MatrixBase<M, N> &operator *=(const float num) {
return *this = *this * num;
Matrix<M, N> &operator *=(const float num) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
data[i][j] *= num;
return *reinterpret_cast<Matrix<M, N>*>(this);
}
MatrixBase<M, N> operator /(const float num) const {
MatrixBase<M, N> res;
Matrix<M, N> operator /(const float num) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res[i][j] = data[i][j] / num;
return res;
}
MatrixBase<M, N> &operator /=(const float num) {
return *this = *this / num;
Matrix<M, N> &operator /=(const float num) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
data[i][j] /= num;
return *this;
}
/**
* multiplication by another matrix
*/
template <unsigned int P>
Matrix<N, P> operator *(const Matrix<M, P> &m) const {
Matrix<N, P> res;
Matrix<M, P> operator *(const Matrix<N, P> &m) const {
Matrix<M, P> res;
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
return res;
}
@ -230,7 +249,7 @@ public:
data[i][i] = 1;
}
void dump(void) {
void print(void) {
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++)
printf("%.3f\t", data[i][j]);
@ -244,6 +263,12 @@ class Matrix : public MatrixBase<M, N> {
public:
using MatrixBase<M, N>::operator *;
Matrix() : MatrixBase<M, N>() {
}
Matrix(const float *d) : MatrixBase<M, N>(d) {
}
/**
* set to value
*/
@ -255,18 +280,28 @@ public:
/**
* multiplication by a vector
*/
Vector<N> operator *(const Vector<N> &v) const {
Vector<M> operator *(const Vector<N> &v) const {
Vector<M> res;
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
return res;
}
};
}
#include "Quaternion.hpp"
namespace math {
template <>
class Matrix<3, 3> : public MatrixBase<3, 3> {
public:
using MatrixBase<3, 3>::operator *;
Matrix() : MatrixBase<3, 3>() {
}
Matrix(const float *d) : MatrixBase<3, 3>(d) {
}
/**
* set to value
*/
@ -279,9 +314,70 @@ public:
* multiplication by a vector
*/
Vector<3> operator *(const Vector<3> &v) const {
return Vector<3>(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
return res;
}
/**
* create a rotation matrix from given quaternion
*/
void from_quaternion(const Quaternion &q) {
float aSq = q.data[0] * q.data[0];
float bSq = q.data[1] * q.data[1];
float cSq = q.data[2] * q.data[2];
float dSq = q.data[3] * q.data[3];
data[0][0] = aSq + bSq - cSq - dSq;
data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]);
data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]);
data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]);
data[1][1] = aSq - bSq + cSq - dSq;
data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]);
data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]);
data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]);
data[2][2] = aSq - bSq - cSq + dSq;
}
/**
* create a rotation matrix from given euler angles
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
*/
void from_euler(float roll, float pitch, float yaw) {
float cp = cosf(pitch);
float sp = sinf(pitch);
float sr = sinf(roll);
float cr = cosf(roll);
float sy = sinf(yaw);
float cy = cosf(yaw);
data[0][0] = cp * cy;
data[0][1] = (sr * sp * cy) - (cr * sy);
data[0][2] = (cr * sp * cy) + (sr * sy);
data[1][0] = cp * sy;
data[1][1] = (sr * sp * sy) + (cr * cy);
data[1][2] = (cr * sp * sy) - (sr * cy);
data[2][0] = -sp;
data[2][1] = sr * cp;
data[2][2] = cr * cp;
}
Vector<3> to_euler(void) const {
Vector<3> euler;
euler.data[1] = asinf(-data[2][0]);
if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];
} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];
} else {
euler.data[0] = atan2f(data[2][1], data[2][2]);
euler.data[2] = atan2f(data[1][0], data[0][0]);
}
}
};

46
src/lib/mathlib/math/Matrix3.cpp
View File

@ -1,46 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Will Perone <will.perone@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 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 THE
* COPYRIGHT OWNER OR CONTRIBUTORS 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 Matrix3.cpp
*
* 3x3 Matrix
*/
#include "Matrix3.hpp"
namespace math
{
}

356
src/lib/mathlib/math/Matrix3.hpp
View File

@ -1,356 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Will Perone <will.perone@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 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 THE
* COPYRIGHT OWNER OR CONTRIBUTORS 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 Matrix3.hpp
*
* 3x3 Matrix
*/
#ifndef MATRIX3_HPP
#define MATRIX3_HPP
#include "Vector3.hpp"
#include "../CMSIS/Include/arm_math.h"
namespace math
{
// 3x3 matrix with elements of type T
template <typename T>
class Matrix3 {
public:
/**
* matrix data[row][col]
*/
T data[3][3];
/**
* struct for using arm_math functions
*/
arm_matrix_instance_f32 arm_mat;
/**
* trivial ctor
* note that this ctor will not initialize elements
*/
Matrix3<T>() {
arm_mat = {3, 3, &data[0][0]};
}
/**
* setting ctor
*/
Matrix3<T>(const T d[3][3]) {
arm_mat = {3, 3, &data[0][0]};
memcpy(data, d, sizeof(data));
}
/**
* setting ctor
*/
Matrix3<T>(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) {
arm_mat = {3, 3, &data[0][0]};
data[0][0] = ax;
data[0][1] = ay;
data[0][2] = az;
data[1][0] = bx;
data[1][1] = by;
data[1][2] = bz;
data[2][0] = cx;
data[2][1] = cy;
data[2][2] = cz;
}
/**
* casting from a vector3f to a matrix is the tilde operator
*/
Matrix3<T>(const Vector3<T> &v) {
arm_mat = {3, 3, &data[0][0]};
data[0][0] = 0;
data[0][1] = -v.z;
data[0][2] = v.y;
data[1][0] = v.z;
data[1][1] = 0;
data[1][2] = -v.x;
data[2][0] = -v.y;
data[2][1] = v.x;
data[2][2] = 0;
}
/**
* access by index
*/
inline T &operator ()(unsigned int row, unsigned int col) {
return data[row][col];
}
/**
* access to elements by index
*/
inline const T &operator ()(unsigned int row, unsigned int col) const {
return data[row][col];
}
/**
* set to value
*/
const Matrix3<T> &operator =(const Matrix3<T> &m) {
memcpy(data, m.data, sizeof(data));
return *this;
}
/**
* test for equality
*/
bool operator ==(const Matrix3<T> &m) {
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
if (data[i][j] != m(i, j))
return false;
return true;
}
/**
* test for inequality
*/
bool operator !=(const Matrix3<T> &m) {
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
if (data[i][j] != m(i, j))
return true;
return false;
}
/**
* negation
*/
Matrix3<T> operator -(void) const {
Matrix3<T> res;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
res[i][j] = -data[i][j];
return res;
}
/**
* addition
*/
Matrix3<T> operator +(const Matrix3<T> &m) const {
Matrix3<T> res;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
res[i][j] = data[i][j] + m(i, j);
return res;
}
Matrix3<T> &operator +=(const Matrix3<T> &m) {
return *this = *this + m;
}
/**
* subtraction
*/
Matrix3<T> operator -(const Matrix3<T> &m) const {
Matrix3<T> res;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
res[i][j] = data[i][j] - m(i, j);
return res;
}
Matrix3<T> &operator -=(const Matrix3<T> &m) {
return *this = *this - m;
}
/**
* uniform scaling
*/
Matrix3<T> operator *(const T num) const {
Matrix3<T> res;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
res[i][j] = data[i][j] * num;
return res;
}
Matrix3<T> &operator *=(const T num) {
return *this = *this * num;
}
Matrix3<T> operator /(const T num) const {
Matrix3<T> res;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
res[i][j] = data[i][j] / num;
return res;
}
Matrix3<T> &operator /=(const T num) {
return *this = *this / num;
}
/**
* multiplication by a vector
*/
Vector3<T> operator *(const Vector3<T> &v) const {
return Vector3<T>(
data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z,
data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z,
data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z);
}
/**
* multiplication of transpose by a vector
*/
Vector3<T> mul_transpose(const Vector3<T> &v) const {
return Vector3<T>(
data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z,
data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z,
data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z);
}
/**
* multiplication by another matrix
*/
Matrix3<T> operator *(const Matrix3<T> &m) const {
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
Matrix3<T> res;
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
return res;
#else
return Matrix3<T>(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0),
data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1),
data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2),
data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0),
data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1),
data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2),
data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0),
data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1),
data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2));
#endif
}
Matrix3<T> &operator *=(const Matrix3<T> &m) {
return *this = *this * m;
}
/**
* transpose the matrix
*/
Matrix3<T> transposed(void) const {
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float
Matrix3<T> res;
arm_mat_trans_f32(&arm_mat, &res.arm_mat);
return res;
#else
return Matrix3<T>(data[0][0], data[1][0], data[2][0],
data[0][1], data[1][1], data[2][1],
data[0][2], data[1][2], data[2][2]);
#endif
}
/**
* inverse the matrix
*/
Matrix3<T> inversed(void) const {
Matrix3<T> res;
arm_mat_inverse_f32(&arm_mat, &res.arm_mat);
return res;
}
/**
* zero the matrix
*/
void zero(void) {
memset(data, 0, sizeof(data));
}
/**
* setup the identity matrix
*/
void identity(void) {
memset(data, 0, sizeof(data));
data[0][0] = 1;
data[1][1] = 1;
data[2][2] = 1;
}
/**
* check if any elements are NAN
*/
bool is_nan(void) {
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
if (isnan(data[i][j]))
return true;
return false;
}
/**
* create a rotation matrix from given euler angles
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
*/
void from_euler(T roll, T pitch, T yaw) {
T cp = (T)cosf(pitch);
T sp = (T)sinf(pitch);
T sr = (T)sinf(roll);
T cr = (T)cosf(roll);
T sy = (T)sinf(yaw);
T cy = (T)cosf(yaw);
data[0][0] = cp * cy;
data[0][1] = (sr * sp * cy) - (cr * sy);
data[0][2] = (cr * sp * cy) + (sr * sy);
data[1][0] = cp * sy;
data[1][1] = (sr * sp * sy) + (cr * cy);
data[1][2] = (cr * sp * sy) - (sr * cy);
data[2][0] = -sp;
data[2][1] = sr * cp;
data[2][2] = cr * cp;
}
// create eulers from a rotation matrix
//void to_euler(float *roll, float *pitch, float *yaw);
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
//void rotate(const Vector3<T> &g);
};
typedef Matrix3<float> Matrix3f;
}
#endif // MATRIX3_HPP

79
src/lib/mathlib/math/Quaternion.hpp
View File

@ -44,15 +44,17 @@
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
#include "Vector.hpp"
#include "Matrix.hpp"
namespace math
{
class Quaternion {
public:
float real;
Vector3<float> imag;
template <unsigned int N, unsigned int M>
class Matrix;
class Quaternion : public Vector<4> {
public:
/**
* trivial ctor
*/
@ -62,7 +64,74 @@ public:
/**
* setting ctor
*/
Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) {
Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) {
}
Quaternion(const Vector<4> &v) : Vector(v) {
}
Quaternion(const float *v) : Vector(v) {
}
/**
* access to elements by index
*/
/*
inline float &operator ()(unsigned int i) {
return *(&a + i);
}
*/
/**
* access to elements by index
*/
/*
inline const float &operator ()(unsigned int i) const {
return *(&a + i);
}
*/
/**
* addition
*/
/*
const Quaternion operator +(const Quaternion &q) const {
return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d);
}
*/
/**
* subtraction
*/
/*
const Quaternion operator -(const Quaternion &q) const {
return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d);
}
*/
Quaternion derivative(const Vector<3> &w) {
float dataQ[] = {
data[0], -data[1], -data[2], -data[3],
data[1], data[0], -data[3], data[2],
data[2], data[3], data[0], -data[1],
data[3], -data[2], data[1], data[0]
};
Matrix<4,4> Q(dataQ);
Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
return Q * v * 0.5f;
}
void from_euler(float roll, float pitch, float yaw) {
double cosPhi_2 = cos(double(roll) / 2.0);
double sinPhi_2 = sin(double(roll) / 2.0);
double cosTheta_2 = cos(double(pitch) / 2.0);
double sinTheta_2 = sin(double(pitch) / 2.0);
double cosPsi_2 = cos(double(yaw) / 2.0);
double sinPsi_2 = sin(double(yaw) / 2.0);
data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2;
data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2;
data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
}
};
}

230
src/lib/mathlib/math/Vector.hpp
View File

@ -42,38 +42,50 @@
#ifndef VECTOR_HPP
#define VECTOR_HPP
#include <stdio.h>
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
namespace math
{
template <unsigned int N>
class Vector;
template <unsigned int N>
class VectorBase {
public:
/**
* vector data
*/
float data[N];
/**
* struct for using arm_math functions, represents column vector
*/
arm_matrix_instance_f32 arm_col;
/**
* trivial ctor
*/
VectorBase<N>() {
VectorBase() {
arm_col = {N, 1, &data[0]};
}
/**
* copy ctor
*/
VectorBase(const VectorBase<N> &v) {
arm_col = {N, 1, &data[0]};
memcpy(data, v.data, sizeof(data));
}
/**
* setting ctor
*/
// VectorBase<N>(const float *d) {
// memcpy(data, d, sizeof(data));
//arm_col = {N, 1, &data[0]};
//}
/**
* setting ctor
*/
VectorBase<N>(const float d[]) : data(d) {
VectorBase(const float *d) {
arm_col = {N, 1, &data[0]};
memcpy(data, d, sizeof(data));
}
/**
@ -90,10 +102,18 @@ public:
return data[i];
}
unsigned int getRows() {
return N;
}
unsigned int getCols() {
return 1;
}
/**
* test for equality
*/
bool operator ==(const VectorBase<N> &v) {
bool operator ==(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v(i))
return false;
@ -103,7 +123,7 @@ public:
/**
* test for inequality
*/
bool operator !=(const VectorBase<N> &v) {
bool operator !=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v(i))
return true;
@ -113,98 +133,98 @@ public:
/**
* set to value
*/
const VectorBase<N> &operator =(const VectorBase<N> &v) {
const Vector<N> &operator =(const Vector<N> &v) {
memcpy(data, v.data, sizeof(data));
return *this;
return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* negation
*/
const VectorBase<N> operator -(void) const {
VectorBase<N> res;
const Vector<N> operator -(void) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res[i] = -data[i];
res.data[i] = -data[i];
return res;
}
/**
* addition
*/
const VectorBase<N> operator +(const VectorBase<N> &v) const {
VectorBase<N> res;
const Vector<N> operator +(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res[i] = data[i] + v(i);
res.data[i] = data[i] + v(i);
return res;
}
/**
* subtraction
*/
const VectorBase<N> operator -(const VectorBase<N> &v) const {
VectorBase<N> res;
const Vector<N> operator -(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res[i] = data[i] - v(i);
res.data[i] = data[i] - v(i);
return res;
}
/**
* uniform scaling
*/
const VectorBase<N> operator *(const float num) const {
VectorBase<N> temp(*this);
const Vector<N> operator *(const float num) const {
Vector<N> temp(*this);
return temp *= num;
}
/**
* uniform scaling
*/
const VectorBase<N> operator /(const float num) const {
VectorBase<N> temp(*this);
const Vector<N> operator /(const float num) const {
Vector<N> temp(*reinterpret_cast<const Vector<N>*>(this));
return temp /= num;
}
/**
* addition
*/
const VectorBase<N> &operator +=(const VectorBase<N> &v) {
const Vector<N> &operator +=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
data[i] += v(i);
return *this;
return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* subtraction
*/
const VectorBase<N> &operator -=(const VectorBase<N> &v) {
const Vector<N> &operator -=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
data[i] -= v(i);
return *this;
return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
const VectorBase<N> &operator *=(const float num) {
const Vector<N> &operator *=(const float num) {
for (unsigned int i = 0; i < N; i++)
data[i] *= num;
return *this;
return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
const VectorBase<N> &operator /=(const float num) {
const Vector<N> &operator /=(const float num) {
for (unsigned int i = 0; i < N; i++)
data[i] /= num;
return *this;
return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* dot product
*/
float operator *(const VectorBase<N> &v) const {
float res;
float operator *(const Vector<N> &v) const {
float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * v(i);
return res;
@ -221,7 +241,7 @@ public:
* gets the length of this vector
*/
float length() const {
return sqrtf(*this * *this);
return sqrtf(*this * *reinterpret_cast<const Vector<N>*>(this));
}
/**
@ -234,35 +254,113 @@ public:
/**
* returns the normalized version of this vector
*/
VectorBase<N> normalized() const {
Vector<N> normalized() const {
return *this / length();
}
void print(void) {
printf("[ ");
for (unsigned int i = 0; i < N; i++)
printf("%.3f\t", data[i]);
printf("]\n");
}
};
template <unsigned int N>
class Vector : public VectorBase<N> {
public:
using VectorBase<N>::operator *;
Vector() : VectorBase<N>() {
}
Vector(const float d[]) : VectorBase<N>(d) {
}
Vector(const Vector<N> &v) : VectorBase<N>(v) {
}
Vector(const VectorBase<N> &v) : VectorBase<N>(v) {
}
/**
* set to value
*/
const Vector<N> &operator =(const Vector<N> &v) {
this->arm_col = {N, 1, &this->data[0]};
memcpy(this->data, v.data, sizeof(this->data));
return *this;
}
};
template <>
class Vector<2> : public VectorBase<2> {
public:
Vector() : VectorBase<2>() {
}
Vector(const float x, const float y) : VectorBase() {
data[0] = x;
data[1] = y;
}
Vector(const Vector<2> &v) : VectorBase() {
data[0] = v.data[0];
data[1] = v.data[1];
}
Vector(const VectorBase<2> &v) : VectorBase() {
data[0] = v.data[0];
data[1] = v.data[1];
}
/**
* set to value
*/
const Vector<2> &operator =(const Vector<2> &v) {
data[0] = v.data[0];
data[1] = v.data[1];
return *this;
}
float cross(const Vector<2> &b) const {
return data[0]*b.data[1] - data[1]*b.data[0];
}
float operator %(const Vector<2> &v) const {
return cross(v);
}
};
template <>
class Vector<3> : public VectorBase<3> {
public:
Vector<3>() {
Vector() {
arm_col = {3, 1, &this->data[0]};
}
Vector<3>(const float x, const float y, const float z) {
Vector(const float x, const float y, const float z) {
arm_col = {3, 1, &this->data[0]};
data[0] = x;
data[1] = y;
data[2] = z;
}
Vector(const Vector<3> &v) : VectorBase<3>() {
data[0] = v.data[0];
data[1] = v.data[1];
data[2] = v.data[2];
}
/**
* setting ctor
*/
Vector(const float d[]) {
arm_col = {3, 1, &this->data[0]};
data[0] = d[0];
data[1] = d[1];
data[2] = d[2];
}
/**
@ -276,6 +374,56 @@ public:
}
};
template <>
class Vector<4> : public VectorBase<4> {
public:
Vector() : VectorBase() {}
Vector(const float x, const float y, const float z, const float t) : VectorBase() {
data[0] = x;
data[1] = y;
data[2] = z;
data[3] = t;
}
Vector(const Vector<4> &v) : VectorBase() {
data[0] = v.data[0];
data[1] = v.data[1];
data[2] = v.data[2];
data[3] = v.data[3];
}
Vector(const VectorBase<4> &v) : VectorBase() {
data[0] = v.data[0];
data[1] = v.data[1];
data[2] = v.data[2];
data[3] = v.data[3];
}
/**
* setting ctor
*/
/*
Vector(const float d[]) {
arm_col = {3, 1, &this->data[0]};
data[0] = d[0];
data[1] = d[1];
data[2] = d[2];
}
*/
/**
* set to value
*/
/*
const Vector<3> &operator =(const Vector<3> &v) {
data[0] = v.data[0];
data[1] = v.data[1];
data[2] = v.data[2];
return *this;
}
*/
};
}
#endif // VECTOR_HPP

269
src/lib/mathlib/math/Vector2.hpp
View File

@ -1,269 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Will Perone <will.perone@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 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 THE
* COPYRIGHT OWNER OR CONTRIBUTORS 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 Vector2.hpp
*
* 2D Vector
*/
#ifndef VECTOR2_HPP
#define VECTOR2_HPP
#include <math.h>
namespace math
{
template <typename T>
class Vector2 {
public:
T x, y;
/**
* trivial ctor
*/
Vector2<T>() {
}
/**
* setting ctor
*/
Vector2<T>(const T x0, const T y0): x(x0), y(y0) {
}
/**
* setter
*/
void set(const T x0, const T y0) {
x = x0;
y = y0;
}
/**
* access to elements by index
*/
T operator ()(unsigned int i) {
return *(&x + i);
}
/**
* access to elements by index
*/
const T operator ()(unsigned int i) const {
return *(&x + i);
}
/**
* test for equality
*/
bool operator ==(const Vector2<T> &v) {
return (x == v.x && y == v.y);
}
/**
* test for inequality
*/
bool operator !=(const Vector2<T> &v) {
return (x != v.x || y != v.y);
}
/**
* set to value
*/
const Vector2<T> &operator =(const Vector2<T> &v) {
x = v.x;
y = v.y;
return *this;
}
/**
* negation
*/
const Vector2<T> operator -(void) const {
return Vector2<T>(-x, -y);
}
/**
* addition
*/
const Vector2<T> operator +(const Vector2<T> &v) const {
return Vector2<T>(x + v.x, y + v.y);
}
/**
* subtraction
*/
const Vector2<T> operator -(const Vector2<T> &v) const {
return Vector2<T>(x - v.x, y - v.y);
}
/**
* uniform scaling
*/
const Vector2<T> operator *(const T num) const {
Vector2<T> temp(*this);
return temp *= num;
}
/**
* uniform scaling
*/
const Vector2<T> operator /(const T num) const {
Vector2<T> temp(*this);
return temp /= num;
}
/**
* addition
*/
const Vector2<T> &operator +=(const Vector2<T> &v) {
x += v.x;
y += v.y;
return *this;
}
/**
* subtraction
*/
const Vector2<T> &operator -=(const Vector2<T> &v) {
x -= v.x;
y -= v.y;
return *this;
}
/**
* uniform scaling
*/
const Vector2<T> &operator *=(const T num) {
x *= num;
y *= num;
return *this;
}
/**
* uniform scaling
*/
const Vector2<T> &operator /=(const T num) {
x /= num;
y /= num;
return *this;
}
/**
* dot product
*/
T operator *(const Vector2<T> &v) const {
return x * v.x + y * v.y;
}
/**
* cross product
*/
const float operator %(const Vector2<T> &v) const {
return x * v.y - y * v.x;
}
/**
* gets the length of this vector squared
*/
float length_squared() const {
return (*this * *this);
}
/**
* gets the length of this vector
*/
float length() const {
return (T)sqrt(*this * *this);
}
/**
* normalizes this vector
*/
void normalize() {
*this /= length();
}
/**
* returns the normalized version of this vector
*/
Vector2<T> normalized() const {
return *this / length();
}
/**
* reflects this vector about n
*/
void reflect(const Vector2<T> &n)
{
Vector2<T> orig(*this);
project(n);
*this = *this * 2 - orig;
}
/**
* projects this vector onto v
*/
void project(const Vector2<T> &v) {
*this = v * (*this * v) / (v * v);
}
/**
* returns this vector projected onto v
*/
Vector2<T> projected(const Vector2<T> &v) {
return v * (*this * v) / (v * v);
}
/**
* computes the angle between 2 arbitrary vectors
*/
static inline float angle(const Vector2<T> &v1, const Vector2<T> &v2) {
return acosf((v1 * v2) / (v1.length() * v2.length()));
}
/**
* computes the angle between 2 arbitrary normalized vectors
*/
static inline float angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2) {
return acosf(v1 * v2);
}
};
typedef Vector2<float> Vector2f;
}
#endif // VECTOR2_HPP

287
src/lib/mathlib/math/Vector3.hpp
View File

@ -1,287 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Will Perone <will.perone@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 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 THE
* COPYRIGHT OWNER OR CONTRIBUTORS 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 Vector3.hpp
*
* 3D Vector
*/
#ifndef VECTOR3_HPP
#define VECTOR3_HPP
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
namespace math
{
template <typename T>
class Vector3 {
public:
T x, y, z;
arm_matrix_instance_f32 arm_col;
/**
* trivial ctor
*/
Vector3<T>() {
arm_col = {3, 1, &x};
}
/**
* setting ctor
*/
Vector3<T>(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {
arm_col = {3, 1, &x};
}
/**
* setting ctor
*/
Vector3<T>(const T data[3]): x(data[0]), y(data[1]), z(data[2]) {
arm_col = {3, 1, &x};
}
/**
* setter
*/
void set(const T x0, const T y0, const T z0) {
x = x0;
y = y0;
z = z0;
}
/**
* access to elements by index
*/
T operator ()(unsigned int i) {
return *(&x + i);
}
/**
* access to elements by index
*/
const T operator ()(unsigned int i) const {
return *(&x + i);
}
/**
* test for equality
*/
bool operator ==(const Vector3<T> &v) {
return (x == v.x && y == v.y && z == v.z);
}
/**
* test for inequality
*/
bool operator !=(const Vector3<T> &v) {
return (x != v.x || y != v.y || z != v.z);
}
/**
* set to value
*/
const Vector3<T> &operator =(const Vector3<T> &v) {
x = v.x;
y = v.y;
z = v.z;
return *this;
}
/**
* negation
*/
const Vector3<T> operator -(void) const {
return Vector3<T>(-x, -y, -z);
}
/**
* addition
*/
const Vector3<T> operator +(const Vector3<T> &v) const {
return Vector3<T>(x + v.x, y + v.y, z + v.z);
}
/**
* subtraction
*/
const Vector3<T> operator -(const Vector3<T> &v) const {
return Vector3<T>(x - v.x, y - v.y, z - v.z);
}
/**
* uniform scaling
*/
const Vector3<T> operator *(const T num) const {
Vector3<T> temp(*this);
return temp *= num;
}
/**
* uniform scaling
*/
const Vector3<T> operator /(const T num) const {
Vector3<T> temp(*this);
return temp /= num;
}
/**
* addition
*/
const Vector3<T> &operator +=(const Vector3<T> &v) {
x += v.x;
y += v.y;
z += v.z;
return *this;
}
/**
* subtraction
*/
const Vector3<T> &operator -=(const Vector3<T> &v) {
x -= v.x;
y -= v.y;
z -= v.z;
return *this;
}
/**
* uniform scaling
*/
const Vector3<T> &operator *=(const T num) {
x *= num;
y *= num;
z *= num;
return *this;
}
/**
* uniform scaling
*/
const Vector3<T> &operator /=(const T num) {
x /= num;
y /= num;
z /= num;
return *this;
}
/**
* dot product
*/
T operator *(const Vector3<T> &v) const {
return x * v.x + y * v.y + z * v.z;
}
/**
* cross product
*/
const Vector3<T> operator %(const Vector3<T> &v) const {
Vector3<T> temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
return temp;
}
/**
* gets the length of this vector squared
*/
float length_squared() const {
return (*this * *this);
}
/**
* gets the length of this vector
*/
float length() const {
return (T)sqrt(*this * *this);
}
/**
* normalizes this vector
*/
void normalize() {
*this /= length();
}
/**
* returns the normalized version of this vector
*/
Vector3<T> normalized() const {
return *this / length();
}
/**
* reflects this vector about n
*/
void reflect(const Vector3<T> &n)
{
Vector3<T> orig(*this);
project(n);
*this = *this * 2 - orig;
}
/**
* projects this vector onto v
*/
void project(const Vector3<T> &v) {
*this = v * (*this * v) / (v * v);
}
/**
* returns this vector projected onto v
*/
Vector3<T> projected(const Vector3<T> &v) {
return v * (*this * v) / (v * v);
}
/**
* computes the angle between 2 arbitrary vectors
*/
static inline float angle(const Vector3<T> &v1, const Vector3<T> &v2) {
return acosf((v1 * v2) / (v1.length() * v2.length()));
}
/**
* computes the angle between 2 arbitrary normalized vectors
*/
static inline float angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2) {
return acosf(v1 * v2);
}
};
typedef Vector3<float> Vector3f;
}
#endif // VECTOR3_HPP

3
src/lib/mathlib/mathlib.h
View File

@ -42,10 +42,7 @@
#pragma once
#include "math/Vector.hpp"
#include "math/Vector2.hpp"
#include "math/Vector3.hpp"
#include "math/Matrix.hpp"
#include "math/Matrix3.hpp"
#include "math/Quaternion.hpp"
#include "math/Limits.hpp"

1
src/lib/mathlib/module.mk
View File

@ -35,7 +35,6 @@
# Math library
#
SRCS = math/test/test.cpp \
math/Matrix3.cpp \
math/Limits.cpp
#

92
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
View File

@ -54,17 +54,17 @@ static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// ekf matrices
F(9, 9),
G(9, 6),
P(9, 9),
P0(9, 9),
V(6, 6),
F(),
G(),
P(),
P0(),
V(),
// attitude measurement ekf matrices
HAtt(4, 9),
RAtt(4, 4),
HAtt(),
RAtt(),
// position measurement ekf matrices
HPos(6, 9),
RPos(6, 6),
HPos(),
RPos(),
// attitude representations
C_nb(),
q(),
@ -113,7 +113,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
using namespace math;
// initial state covariance matrix
P0 = Matrix::identity(9) * 0.01f;
P0.identity();
P0 *= 0.01f;
P = P0;
// initial state
@ -138,7 +139,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = Dcm(q);
C_nb.from_quaternion(q);
// HPos is constant
HPos(0, 3) = 1.0f;
@ -404,28 +405,28 @@ int KalmanNav::predictState(float dt)
// attitude prediction
if (_attitudeInitialized) {
Vector3 w(_sensors.gyro_rad_s);
Vector<3> w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
q = q.unit();
if (fabsf(q.length() - 1.0f) > 1e-4f) {
q.normalize();
}
// C_nb update
C_nb = Dcm(q);
C_nb.from_quaternion(q);
// euler update
EulerAngles euler(C_nb);
phi = euler.getPhi();
theta = euler.getTheta();
psi = euler.getPsi();
Vector<3> euler = C_nb.to_euler();
phi = euler.data[0];
theta = euler.data[1];
psi = euler.data[2];
// specific acceleration in nav frame
Vector3 accelB(_sensors.accelerometer_m_s2);
Vector3 accelN = C_nb * accelB;
Vector<3> accelB(_sensors.accelerometer_m_s2);
Vector<3> accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
@ -549,10 +550,10 @@ int KalmanNav::predictStateCovariance(float dt)
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous predictioon equations
// for discrte time EKF
// continuous prediction equations
// for discrete time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
return ret_ok;
}
@ -577,13 +578,14 @@ int KalmanNav::correctAtt()
// compensate roll and pitch, but not yaw
// XXX take the vectors out of the C_nb matrix to avoid singularities
math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
math::Matrix<3,3> C_rp;
C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
// mag measurement
Vector3 magBody(_sensors.magnetometer_ga);
Vector<3> magBody(_sensors.magnetometer_ga);
// transform to earth frame
Vector3 magNav = C_rp * magBody;
Vector<3> magNav = C_rp * magBody;
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
@ -592,12 +594,12 @@ int KalmanNav::correctAtt()
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
// accel measurement
Vector3 zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.norm();
zAccel = zAccel.unit();
Vector<3> zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.length();
zAccel.normalize();
// ignore accel correction when accel mag not close to g
Matrix RAttAdjust = RAtt;
Matrix<4,4> RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
@ -611,14 +613,10 @@ int KalmanNav::correctAtt()
}
// accel predicted measurement
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
// calculate residual
Vector y(4);
y(0) = yMag;
y(1) = zAccel(0) - zAccelHat(0);
y(2) = zAccel(1) - zAccelHat(1);
y(3) = zAccel(2) - zAccelHat(2);
Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
// HMag
HAtt(0, 2) = 1;
@ -632,9 +630,9 @@ int KalmanNav::correctAtt()
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
Matrix K = P * HAtt.transpose() * S.inverse();
Vector xCorrect = K * y;
Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
@ -669,7 +667,7 @@ int KalmanNav::correctAtt()
P = P - K * HAtt * P;
// fault detection
float beta = y.dot(S.inverse() * y);
float beta = y * (S.inversed() * y);
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
@ -678,7 +676,7 @@ int KalmanNav::correctAtt()
// update quaternions from euler
// angle correction
q = Quaternion(EulerAngles(phi, theta, psi));
q.from_euler(phi, theta, psi);
return ret_ok;
}
@ -688,7 +686,7 @@ int KalmanNav::correctPos()
using namespace math;
// residual
Vector y(6);
Vector<6> y;
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
@ -698,9 +696,9 @@ int KalmanNav::correctPos()
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
Matrix K = P * HPos.transpose() * S.inverse();
Vector xCorrect = K * y;
Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
Matrix<9,6> K = P * HPos.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
@ -735,7 +733,7 @@ int KalmanNav::correctPos()
P = P - K * HPos * P;
// fault detetcion
float beta = y.dot(S.inverse() * y);
float beta = y * (S.inversed() * y);
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {

20
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
View File

@ -125,17 +125,17 @@ public:
virtual void updateParams();
protected:
// kalman filter
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix G; /**< noise shaping matrix for gyro/accel */
math::Matrix P; /**< state covariance matrix */
math::Matrix P0; /**< initial state covariance matrix */
math::Matrix V; /**< gyro/ accel noise matrix */
math::Matrix HAtt; /**< attitude measurement matrix */
math::Matrix RAtt; /**< attitude measurement noise matrix */
math::Matrix HPos; /**< position measurement jacobian matrix */
math::Matrix RPos; /**< position measurement noise matrix */
math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */
math::Matrix<9,9> P; /**< state covariance matrix */
math::Matrix<9,9> P0; /**< initial state covariance matrix */
math::Matrix<6,6> V; /**< gyro/ accel noise matrix */
math::Matrix<4,9> HAtt; /**< attitude measurement matrix */
math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */
math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */
math::Matrix<6,6> RPos; /**< position measurement noise matrix */
// attitude
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */

14
src/modules/commander/accelerometer_calibration.cpp
View File

@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd)
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix board_rotation(3, 3);
math::Matrix<3,3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix board_rotation_t = board_rotation.transpose();
math::Vector3 accel_offs_vec;
accel_offs_vec.set(&accel_offs[0]);
math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix accel_T_mat(3, 3);
accel_T_mat.set(&accel_T[0][0]);
math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);

11
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -683,8 +683,9 @@ handle_message(mavlink_message_t *msg)
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Dcm C_nb(q);
math::EulerAngles euler(C_nb);
math::Matrix<3,3> C_nb;
C_nb.from_quaternion(q);
math::Vector<3> euler = C_nb.to_euler();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
@ -699,9 +700,9 @@ handle_message(mavlink_message_t *msg)
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
hil_attitude.roll = euler.getPhi();
hil_attitude.pitch = euler.getTheta();
hil_attitude.yaw = euler.getPsi();
hil_attitude.roll = euler(0);
hil_attitude.pitch = euler(1);
hil_attitude.yaw = euler(2);
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;

16
src/modules/navigator/navigator_main.cpp
View File

@ -422,11 +422,11 @@ Navigator::task_main()
mission_poll();
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
// Current waypoint
math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
// Global position
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
/* AUTONOMOUS FLIGHT */
@ -436,19 +436,19 @@ Navigator::task_main()
if (_mission_valid) {
// Next waypoint
math::Vector2f prev_wp;
math::Vector<2> prev_wp;
if (_global_triplet.previous_valid) {
prev_wp.setX(_global_triplet.previous.lat / 1e7f);
prev_wp.setY(_global_triplet.previous.lon / 1e7f);
prev_wp(0) = _global_triplet.previous.lat / 1e7f;
prev_wp(1) = _global_triplet.previous.lon / 1e7f;
} else {
/*
* No valid next waypoint, go for heading hold.
* This is automatically handled by the L1 library.
*/
prev_wp.setX(_global_triplet.current.lat / 1e7f);
prev_wp.setY(_global_triplet.current.lon / 1e7f);
prev_wp(0) = _global_triplet.current.lat / 1e7f;
prev_wp(1) = _global_triplet.current.lon / 1e7f;
}

14
src/modules/sensors/sensors.cpp
View File

@ -211,8 +211,8 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
struct {
@ -457,8 +457,8 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
_board_rotation(),
_external_mag_rotation(),
_mag_is_external(false)
{
@ -904,7 +904,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z};
vect = _board_rotation * vect;
raw.accelerometer_m_s2[0] = vect(0);
@ -930,7 +930,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z};
vect = _board_rotation * vect;
raw.gyro_rad_s[0] = vect(0);
@ -956,7 +956,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
if (_mag_is_external)
vect = _external_mag_rotation * vect;

23
src/systemcmds/tests/test_mathlib.cpp
View File

@ -75,15 +75,6 @@ int test_mathlib(int argc, char *argv[])
Matrix<3,3> mres3;
Matrix<4,4> mres4;
Matrix3f m3old;
m3old.identity();
Vector3f v3old;
v3old.x = 1.0f;
v3old.y = 2.0f;
v3old.z = 3.0f;
Vector3f vres3old;
Matrix3f mres3old;
unsigned int n = 60000;
hrt_abstime t0, t1;
@ -95,13 +86,6 @@ int test_mathlib(int argc, char *argv[])
t1 = hrt_absolute_time();
warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n);
t0 = hrt_absolute_time();
for (unsigned int j = 0; j < n; j++) {
vres3old = m3old * v3old;
}
t1 = hrt_absolute_time();
warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n);
t0 = hrt_absolute_time();
for (unsigned int j = 0; j < n; j++) {
mres3 = m3 * m3;
@ -109,13 +93,6 @@ int test_mathlib(int argc, char *argv[])
t1 = hrt_absolute_time();
warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n);
t0 = hrt_absolute_time();
for (unsigned int j = 0; j < n; j++) {
mres3old = m3old * m3old;
}
t1 = hrt_absolute_time();
warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n);
t0 = hrt_absolute_time();
for (unsigned int j = 0; j < n; j++) {
mres4 = m4 * m4;