mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 06:15:14 +03:00
Initial cut on full quaternion IMU conversion (#2894)
* Initial cut on full quaternion/vector IMU conversion * More accurate quaternion integration * Refactor vector struct per @ledvinap suggection * Implement rotation matrix from axis/angle; Refactor mag declination to have orientation correspond to RPY angles * Use magnetic North vector as a reference
This commit is contained in:
Konstantin Sharlaimov
GitHub
parent
0ede6d52d6
commit
e174e5a48d
23 changed files with 801 additions and 498 deletions
|
|
@ -21,6 +21,8 @@
|
|||
|
||||
#include "axis.h"
|
||||
#include "maths.h"
|
||||
#include "vector.h"
|
||||
#include "quaternion.h"
|
||||
|
||||
// http://lolengine.net/blog/2011/12/21/better-function-approximations
|
||||
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
|
||||
|
|
@ -203,59 +205,65 @@ float scaleRangef(float x, float srcMin, float srcMax, float destMin, float dest
|
|||
return ((a / b) + destMin);
|
||||
}
|
||||
|
||||
// Normalize a vector
|
||||
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
|
||||
{
|
||||
float length;
|
||||
|
||||
length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
|
||||
if (length != 0) {
|
||||
dest->X = src->X / length;
|
||||
dest->Y = src->Y / length;
|
||||
dest->Z = src->Z / length;
|
||||
}
|
||||
}
|
||||
|
||||
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
|
||||
// Build rMat from Tait–Bryan angles (convention X1, Y2, Z3)
|
||||
void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles)
|
||||
{
|
||||
float cosx, sinx, cosy, siny, cosz, sinz;
|
||||
float coszcosx, sinzcosx, coszsinx, sinzsinx;
|
||||
|
||||
cosx = cos_approx(delta->angles.roll);
|
||||
sinx = sin_approx(delta->angles.roll);
|
||||
cosy = cos_approx(delta->angles.pitch);
|
||||
siny = sin_approx(delta->angles.pitch);
|
||||
cosz = cos_approx(delta->angles.yaw);
|
||||
sinz = sin_approx(delta->angles.yaw);
|
||||
cosx = cos_approx(angles->angles.roll);
|
||||
sinx = sin_approx(angles->angles.roll);
|
||||
cosy = cos_approx(angles->angles.pitch);
|
||||
siny = sin_approx(angles->angles.pitch);
|
||||
cosz = cos_approx(angles->angles.yaw);
|
||||
sinz = sin_approx(angles->angles.yaw);
|
||||
|
||||
coszcosx = cosz * cosx;
|
||||
sinzcosx = sinz * cosx;
|
||||
coszsinx = sinx * cosz;
|
||||
sinzsinx = sinx * sinz;
|
||||
|
||||
matrix[0][X] = cosz * cosy;
|
||||
matrix[0][Y] = -cosy * sinz;
|
||||
matrix[0][Z] = siny;
|
||||
matrix[1][X] = sinzcosx + (coszsinx * siny);
|
||||
matrix[1][Y] = coszcosx - (sinzsinx * siny);
|
||||
matrix[1][Z] = -sinx * cosy;
|
||||
matrix[2][X] = (sinzsinx) - (coszcosx * siny);
|
||||
matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
|
||||
matrix[2][Z] = cosy * cosx;
|
||||
rmat->m[0][X] = cosz * cosy;
|
||||
rmat->m[0][Y] = -cosy * sinz;
|
||||
rmat->m[0][Z] = siny;
|
||||
rmat->m[1][X] = sinzcosx + (coszsinx * siny);
|
||||
rmat->m[1][Y] = coszcosx - (sinzsinx * siny);
|
||||
rmat->m[1][Z] = -sinx * cosy;
|
||||
rmat->m[2][X] = (sinzsinx) - (coszcosx * siny);
|
||||
rmat->m[2][Y] = (coszsinx) + (sinzcosx * siny);
|
||||
rmat->m[2][Z] = cosy * cosx;
|
||||
}
|
||||
|
||||
// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
|
||||
void rotateV(struct fp_vector *v, fp_angles_t *delta)
|
||||
void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a)
|
||||
{
|
||||
struct fp_vector v_tmp = *v;
|
||||
const float sang = sin_approx(a->angle);
|
||||
const float cang = cos_approx(a->angle);
|
||||
const float C = 1.0f - cang;
|
||||
|
||||
float matrix[3][3];
|
||||
const float xC = a->axis.x * C;
|
||||
const float yC = a->axis.y * C;
|
||||
const float zC = a->axis.z * C;
|
||||
const float xxC = a->axis.x * xC;
|
||||
const float yyC = a->axis.y * yC;
|
||||
const float zzC = a->axis.z * zC;
|
||||
const float xyC = a->axis.x * yC;
|
||||
const float yzC = a->axis.y * zC;
|
||||
const float zxC = a->axis.z * xC;
|
||||
const float xs = a->axis.x * sang;
|
||||
const float ys = a->axis.y * sang;
|
||||
const float zs = a->axis.z * sang;
|
||||
|
||||
buildRotationMatrix(delta, matrix);
|
||||
rmat->m[0][X] = xxC + cang;
|
||||
rmat->m[0][Y] = xyC - zs;
|
||||
rmat->m[0][Z] = zxC + ys;
|
||||
|
||||
v->X = v_tmp.X * matrix[0][X] + v_tmp.Y * matrix[1][X] + v_tmp.Z * matrix[2][X];
|
||||
v->Y = v_tmp.X * matrix[0][Y] + v_tmp.Y * matrix[1][Y] + v_tmp.Z * matrix[2][Y];
|
||||
v->Z = v_tmp.X * matrix[0][Z] + v_tmp.Y * matrix[1][Z] + v_tmp.Z * matrix[2][Z];
|
||||
rmat->m[1][X] = zxC + ys;
|
||||
rmat->m[1][Y] = yyC + cang;
|
||||
rmat->m[1][Z] = yzC - xs;
|
||||
|
||||
rmat->m[2][X] = zxC - ys;
|
||||
rmat->m[2][Y] = yzC + xs;
|
||||
rmat->m[2][Z] = zzC + cang;
|
||||
}
|
||||
|
||||
// Quick median filter implementation
|
||||
|
|
|
|||
|
|
@ -81,26 +81,7 @@
|
|||
#define _ABS_I(x, var) _ABS_II(x, var)
|
||||
#define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
|
||||
|
||||
typedef struct stdev_s
|
||||
{
|
||||
float m_oldM, m_newM, m_oldS, m_newS;
|
||||
int m_n;
|
||||
} stdev_t;
|
||||
|
||||
// Floating point 3 vector.
|
||||
typedef struct fp_vector {
|
||||
float X;
|
||||
float Y;
|
||||
float Z;
|
||||
} t_fp_vector_def;
|
||||
|
||||
typedef union {
|
||||
float A[3];
|
||||
t_fp_vector_def V;
|
||||
} t_fp_vector;
|
||||
|
||||
// Floating point Euler angles.
|
||||
// Be carefull, could be either of degrees or radians.
|
||||
typedef struct fp_angles {
|
||||
float roll;
|
||||
float pitch;
|
||||
|
|
@ -112,6 +93,12 @@ typedef union {
|
|||
fp_angles_def angles;
|
||||
} fp_angles_t;
|
||||
|
||||
typedef struct stdev_s
|
||||
{
|
||||
float m_oldM, m_newM, m_oldS, m_newS;
|
||||
int m_n;
|
||||
} stdev_t;
|
||||
|
||||
typedef struct filterWithBufferSample_s {
|
||||
float value;
|
||||
uint32_t timestamp;
|
||||
|
|
@ -149,11 +136,6 @@ float degreesToRadians(int16_t degrees);
|
|||
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
|
||||
float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
|
||||
|
||||
void normalizeV(struct fp_vector *src, struct fp_vector *dest);
|
||||
|
||||
void rotateV(struct fp_vector *v, fp_angles_t *delta);
|
||||
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]);
|
||||
|
||||
int32_t wrap_18000(int32_t angle);
|
||||
int32_t wrap_36000(int32_t angle);
|
||||
|
||||
|
|
|
|||
199
src/main/common/quaternion.h
Normal file
199
src/main/common/quaternion.h
Normal file
|
|
@ -0,0 +1,199 @@
|
|||
/*
|
||||
* This file is part of INAV.
|
||||
*
|
||||
* INAV is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* INAV is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* Original implementation by HaukeRa
|
||||
* Refactored and adapted by DigitalEntity
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
typedef struct {
|
||||
float q0, q1, q2, q3;
|
||||
} fpQuaternion_t;
|
||||
|
||||
static inline fpQuaternion_t * quaternionInitUnit(fpQuaternion_t * result)
|
||||
{
|
||||
result->q0 = 1.0f;
|
||||
result->q1 = 0.0f;
|
||||
result->q2 = 0.0f;
|
||||
result->q3 = 0.0f;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpQuaternion_t * quaternionInitFromVector(fpQuaternion_t * result, const fpVector3_t * v)
|
||||
{
|
||||
result->q0 = 0.0f;
|
||||
result->q1 = v->x;
|
||||
result->q2 = v->y;
|
||||
result->q3 = v->z;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline void quaternionToAxisAngle(fpAxisAngle_t * result, const fpQuaternion_t * q)
|
||||
{
|
||||
fpAxisAngle_t a = {.axis = {{1.0f, 0.0f, 0.0f}}};
|
||||
|
||||
a.angle = 2.0f * acos_approx(constrainf(q->q0, -1.0f, 1.0f));
|
||||
|
||||
if (a.angle > M_PIf) {
|
||||
a.angle -= 2.0f * M_PIf;
|
||||
}
|
||||
|
||||
const float sinVal = sqrt(1.0f - q->q0 * q->q0);
|
||||
|
||||
// Axis is only valid when rotation is large enough sin(0.0057 deg) = 0.0001
|
||||
if (sinVal > 1e-4f) {
|
||||
a.axis.x = q->q1 / sinVal;
|
||||
a.axis.y = q->q2 / sinVal;
|
||||
a.axis.z = q->q3 / sinVal;
|
||||
} else {
|
||||
a.angle = 0;
|
||||
}
|
||||
|
||||
*result = a;
|
||||
}
|
||||
|
||||
static inline fpQuaternion_t * axisAngleToQuaternion(fpQuaternion_t * result, const fpAxisAngle_t * a)
|
||||
{
|
||||
fpQuaternion_t q;
|
||||
const float s = sin_approx(a->angle / 2.0f);
|
||||
|
||||
q.q0 = cos_approx(a->angle / 2.0f);
|
||||
q.q1 = -a->axis.x * s;
|
||||
q.q2 = -a->axis.y * s;
|
||||
q.q3 = -a->axis.z * s;
|
||||
|
||||
*result = q;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline float quaternionNormSqared(const fpQuaternion_t * q)
|
||||
{
|
||||
return sq(q->q0) + sq(q->q1) + sq(q->q2) + sq(q->q3);
|
||||
}
|
||||
|
||||
static inline fpQuaternion_t * quaternionMultiply(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)
|
||||
{
|
||||
fpQuaternion_t p;
|
||||
|
||||
p.q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;
|
||||
p.q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;
|
||||
p.q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;
|
||||
p.q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;
|
||||
|
||||
*result = p;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpQuaternion_t * quaternionScale(fpQuaternion_t * result, const fpQuaternion_t * a, const float b)
|
||||
{
|
||||
fpQuaternion_t p;
|
||||
|
||||
p.q0 = a->q0 * b;
|
||||
p.q1 = a->q1 * b;
|
||||
p.q2 = a->q2 * b;
|
||||
p.q3 = a->q3 * b;
|
||||
|
||||
*result = p;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpQuaternion_t * quaternionAdd(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)
|
||||
{
|
||||
fpQuaternion_t p;
|
||||
|
||||
p.q0 = a->q0 + b->q0;
|
||||
p.q1 = a->q1 + b->q1;
|
||||
p.q2 = a->q2 + b->q2;
|
||||
p.q3 = a->q3 + b->q3;
|
||||
|
||||
*result = p;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpQuaternion_t * quaternionConjugate(fpQuaternion_t * result, const fpQuaternion_t * q)
|
||||
{
|
||||
result->q0 = q->q0;
|
||||
result->q1 = -q->q1;
|
||||
result->q2 = -q->q2;
|
||||
result->q3 = -q->q3;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q)
|
||||
{
|
||||
float mod = sqrtf(quaternionNormSqared(q));
|
||||
if (mod < 1e-6f) {
|
||||
// Length is too small - re-initialize to zero rotation
|
||||
result->q0 = 1;
|
||||
result->q1 = 0;
|
||||
result->q2 = 0;
|
||||
result->q3 = 0;
|
||||
}
|
||||
else {
|
||||
result->q0 = q->q0 / mod;
|
||||
result->q1 = q->q1 / mod;
|
||||
result->q2 = q->q2 / mod;
|
||||
result->q3 = q->q3 / mod;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpVector3_t * quaternionRotateVector(fpVector3_t * result, const fpVector3_t * vect, const fpQuaternion_t * ref)
|
||||
{
|
||||
fpQuaternion_t vectQuat, refConj;
|
||||
|
||||
vectQuat.q0 = 0;
|
||||
vectQuat.q1 = vect->x;
|
||||
vectQuat.q2 = vect->y;
|
||||
vectQuat.q3 = vect->z;
|
||||
|
||||
quaternionConjugate(&refConj, ref);
|
||||
quaternionMultiply(&vectQuat, &refConj, &vectQuat);
|
||||
quaternionMultiply(&vectQuat, &vectQuat, ref);
|
||||
|
||||
result->x = vectQuat.q1;
|
||||
result->y = vectQuat.q2;
|
||||
result->z = vectQuat.q3;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpVector3_t * quaternionRotateVectorInv(fpVector3_t * result, const fpVector3_t * vect, const fpQuaternion_t * ref)
|
||||
{
|
||||
fpQuaternion_t vectQuat, refConj;
|
||||
|
||||
vectQuat.q0 = 0;
|
||||
vectQuat.q1 = vect->x;
|
||||
vectQuat.q2 = vect->y;
|
||||
vectQuat.q3 = vect->z;
|
||||
|
||||
quaternionConjugate(&refConj, ref);
|
||||
quaternionMultiply(&vectQuat, ref, &vectQuat);
|
||||
quaternionMultiply(&vectQuat, &vectQuat, &refConj);
|
||||
|
||||
result->x = vectQuat.q1;
|
||||
result->y = vectQuat.q2;
|
||||
result->z = vectQuat.q3;
|
||||
return result;
|
||||
}
|
||||
111
src/main/common/vector.h
Normal file
111
src/main/common/vector.h
Normal file
|
|
@ -0,0 +1,111 @@
|
|||
/*
|
||||
* This file is part of INAV.
|
||||
*
|
||||
* INAV is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* INAV is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/maths.h"
|
||||
|
||||
typedef union {
|
||||
float v[3];
|
||||
struct {
|
||||
float x,y,z;
|
||||
};
|
||||
} fpVector3_t;
|
||||
|
||||
typedef struct {
|
||||
float m[3][3];
|
||||
} fpMat3_t;
|
||||
|
||||
typedef struct {
|
||||
fpVector3_t axis;
|
||||
float angle;
|
||||
} fpAxisAngle_t;
|
||||
|
||||
void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles);
|
||||
void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a);
|
||||
|
||||
static inline fpVector3_t * rotationMatrixRotateVector(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat)
|
||||
{
|
||||
fpVector3_t r;
|
||||
|
||||
r.x = rmat->m[0][0] * a->x + rmat->m[1][0] * a->y + rmat->m[2][0] * a->z;
|
||||
r.y = rmat->m[0][1] * a->x + rmat->m[1][1] * a->y + rmat->m[2][1] * a->z;
|
||||
r.z = rmat->m[0][2] * a->x + rmat->m[1][2] * a->y + rmat->m[2][2] * a->z;
|
||||
|
||||
*result = r;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline float vectorNormSquared(const fpVector3_t * v)
|
||||
{
|
||||
return sq(v->x) + sq(v->y) + sq(v->z);
|
||||
}
|
||||
|
||||
static inline fpVector3_t * vectorNormalize(fpVector3_t * result, const fpVector3_t * v)
|
||||
{
|
||||
float length = sqrtf(vectorNormSquared(v));
|
||||
if (length != 0) {
|
||||
result->x = v->x / length;
|
||||
result->y = v->y / length;
|
||||
result->z = v->z / length;
|
||||
}
|
||||
else {
|
||||
result->x = 0;
|
||||
result->y = 0;
|
||||
result->z = 0;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpVector3_t * vectorCrossProduct(fpVector3_t * result, const fpVector3_t * a, const fpVector3_t * b)
|
||||
{
|
||||
fpVector3_t ab;
|
||||
|
||||
ab.x = a->y * b->z - a->z * b->y;
|
||||
ab.y = a->z * b->x - a->x * b->z;
|
||||
ab.z = a->x * b->y - a->y * b->x;
|
||||
|
||||
*result = ab;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpVector3_t * vectorAdd(fpVector3_t * result, const fpVector3_t * a, const fpVector3_t * b)
|
||||
{
|
||||
fpVector3_t ab;
|
||||
|
||||
ab.x = a->x + b->x;
|
||||
ab.y = a->y + b->y;
|
||||
ab.z = a->z + b->z;
|
||||
|
||||
*result = ab;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline fpVector3_t * vectorScale(fpVector3_t * result, const fpVector3_t * a, const float b)
|
||||
{
|
||||
fpVector3_t ab;
|
||||
|
||||
ab.x = a->x * b;
|
||||
ab.y = a->y * b;
|
||||
ab.z = a->z * b;
|
||||
|
||||
*result = ab;
|
||||
return result;
|
||||
}
|
||||
|
|
@ -26,10 +26,13 @@
|
|||
#include "blackbox/blackbox.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
#include "common/quaternion.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
|
|
@ -74,16 +77,16 @@
|
|||
#define MAX_ACC_SQ_NEARNESS 25 // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
|
||||
#define MAX_GPS_HEADING_ERROR_DEG 60 // Amount of error between GPS CoG and estimated Yaw at witch we stop trusting GPS and fallback to MAG
|
||||
|
||||
FASTRAM t_fp_vector imuMeasuredAccelBF;
|
||||
FASTRAM t_fp_vector imuMeasuredRotationBF;
|
||||
FASTRAM fpVector3_t imuMeasuredAccelBF;
|
||||
FASTRAM fpVector3_t imuMeasuredRotationBF;
|
||||
STATIC_FASTRAM float smallAngleCosZ;
|
||||
|
||||
STATIC_FASTRAM bool isAccelUpdatedAtLeastOnce;
|
||||
STATIC_FASTRAM fpVector3_t vCorrectedMagNorth; // Magnetic North vector in EF (true North rotated by declination)
|
||||
|
||||
STATIC_FASTRAM_UNIT_TESTED float q0, q1, q2, q3; // quaternion of sensor frame relative to earth frame
|
||||
STATIC_FASTRAM_UNIT_TESTED float rMat[3][3];
|
||||
|
||||
FASTRAM fpQuaternion_t orientation;
|
||||
FASTRAM attitudeEulerAngles_t attitude; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
STATIC_FASTRAM_UNIT_TESTED float rMat[3][3];
|
||||
|
||||
STATIC_FASTRAM imuRuntimeConfig_t imuRuntimeConfig;
|
||||
|
||||
|
|
@ -101,16 +104,16 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
|
|||
|
||||
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
|
||||
{
|
||||
float q1q1 = q1 * q1;
|
||||
float q2q2 = q2 * q2;
|
||||
float q3q3 = q3 * q3;
|
||||
float q1q1 = orientation.q1 * orientation.q1;
|
||||
float q2q2 = orientation.q2 * orientation.q2;
|
||||
float q3q3 = orientation.q3 * orientation.q3;
|
||||
|
||||
float q0q1 = q0 * q1;
|
||||
float q0q2 = q0 * q2;
|
||||
float q0q3 = q0 * q3;
|
||||
float q1q2 = q1 * q2;
|
||||
float q1q3 = q1 * q3;
|
||||
float q2q3 = q2 * q3;
|
||||
float q0q1 = orientation.q0 * orientation.q1;
|
||||
float q0q2 = orientation.q0 * orientation.q2;
|
||||
float q0q3 = orientation.q0 * orientation.q3;
|
||||
float q1q2 = orientation.q1 * orientation.q2;
|
||||
float q1q3 = orientation.q1 * orientation.q3;
|
||||
float q2q3 = orientation.q2 * orientation.q3;
|
||||
|
||||
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
|
||||
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
|
||||
|
|
@ -139,49 +142,46 @@ void imuInit(void)
|
|||
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
imuMeasuredAccelBF.A[axis] = 0;
|
||||
imuMeasuredAccelBF.v[axis] = 0;
|
||||
}
|
||||
|
||||
// Explicitly initialize FASTRAM statics
|
||||
isAccelUpdatedAtLeastOnce = false;
|
||||
gpsHeadingInitialized = false;
|
||||
q0 = 1.0f;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
|
||||
// Create magnetic declination matrix
|
||||
const int deg = compassConfig()->mag_declination / 100;
|
||||
const int min = compassConfig()->mag_declination % 100;
|
||||
imuSetMagneticDeclination(deg + min / 60.0f);
|
||||
|
||||
quaternionInitUnit(&orientation);
|
||||
imuComputeRotationMatrix();
|
||||
}
|
||||
|
||||
void imuTransformVectorBodyToEarth(t_fp_vector * v)
|
||||
void imuSetMagneticDeclination(float declinationDeg)
|
||||
{
|
||||
/* From body frame to earth frame */
|
||||
const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
|
||||
const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
|
||||
const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
|
||||
|
||||
v->V.X = x;
|
||||
v->V.Y = -y;
|
||||
v->V.Z = z;
|
||||
const float declinationRad = -DEGREES_TO_RADIANS(declinationDeg);
|
||||
vCorrectedMagNorth.x = cos_approx(declinationRad);
|
||||
vCorrectedMagNorth.y = sin_approx(declinationRad);
|
||||
vCorrectedMagNorth.z = 0;
|
||||
}
|
||||
|
||||
void imuTransformVectorEarthToBody(t_fp_vector * v)
|
||||
void imuTransformVectorBodyToEarth(fpVector3_t * v)
|
||||
{
|
||||
v->V.Y = -v->V.Y;
|
||||
// From body frame to earth frame
|
||||
quaternionRotateVectorInv(v, v, &orientation);
|
||||
|
||||
/* From earth frame to body frame */
|
||||
const float x = rMat[0][0] * v->V.X + rMat[1][0] * v->V.Y + rMat[2][0] * v->V.Z;
|
||||
const float y = rMat[0][1] * v->V.X + rMat[1][1] * v->V.Y + rMat[2][1] * v->V.Z;
|
||||
const float z = rMat[0][2] * v->V.X + rMat[1][2] * v->V.Y + rMat[2][2] * v->V.Z;
|
||||
|
||||
v->V.X = x;
|
||||
v->V.Y = y;
|
||||
v->V.Z = z;
|
||||
// HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)
|
||||
v->y = -v->y;
|
||||
}
|
||||
|
||||
static float invSqrt(float x)
|
||||
void imuTransformVectorEarthToBody(fpVector3_t * v)
|
||||
{
|
||||
return 1.0f / sqrtf(x);
|
||||
// HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)
|
||||
v->y = -v->y;
|
||||
|
||||
// From earth frame to body frame
|
||||
quaternionRotateVector(v, v, &orientation);
|
||||
}
|
||||
|
||||
#if defined(USE_GPS) || defined(HIL)
|
||||
|
|
@ -200,10 +200,10 @@ STATIC_UNIT_TESTED void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t
|
|||
const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
|
||||
const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
|
||||
|
||||
q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
|
||||
q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
|
||||
q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
|
||||
q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
|
||||
orientation.q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
|
||||
orientation.q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
|
||||
orientation.q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
|
||||
orientation.q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
|
||||
|
||||
imuComputeRotationMatrix();
|
||||
}
|
||||
|
|
@ -224,187 +224,189 @@ static float imuGetPGainScaleFactor(void)
|
|||
}
|
||||
}
|
||||
|
||||
static void imuResetOrientationQuaternion(const float ax, const float ay, const float az)
|
||||
static void imuResetOrientationQuaternion(const fpVector3_t * accBF)
|
||||
{
|
||||
const float accNorm = sqrtf(ax * ax + ay * ay + az * az);
|
||||
const float accNorm = sqrtf(vectorNormSquared(accBF));
|
||||
|
||||
q0 = az + accNorm;
|
||||
q1 = ay;
|
||||
q2 = -ax;
|
||||
q3 = 0.0f;
|
||||
orientation.q0 = accBF->z + accNorm;
|
||||
orientation.q1 = accBF->y;
|
||||
orientation.q2 = -accBF->x;
|
||||
orientation.q3 = 0.0f;
|
||||
|
||||
const float recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
quaternionNormalize(&orientation, &orientation);
|
||||
}
|
||||
|
||||
static void imuCheckAndResetOrientationQuaternion(const float ax, const float ay, const float az)
|
||||
static void imuCheckAndResetOrientationQuaternion(const fpVector3_t * accBF)
|
||||
{
|
||||
// Check if some calculation in IMU update yield NAN or zero quaternion
|
||||
// Reset quaternion from accelerometer - this might be incorrect, but it's better than no attitude at all
|
||||
const float check = fabs(orientation.q0) + fabs(orientation.q1) + fabs(orientation.q2) + fabs(orientation.q3);
|
||||
|
||||
const bool isNan = (isnan(q0) || isnan(q1) || isnan(q2) || isnan(q3));
|
||||
const bool isInf = (isinf(q0) || isinf(q1) || isinf(q2) || isinf(q3));
|
||||
const bool isZero = (ABS(q0) < 1e-3f && ABS(q1) < 1e-3f && ABS(q2) < 1e-3f && ABS(q3) < 1e-3f);
|
||||
if (!isnan(check) && !isinf(check)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (isNan || isZero || isInf) {
|
||||
imuResetOrientationQuaternion(ax, ay, az);
|
||||
const float normSq = quaternionNormSqared(&orientation);
|
||||
if (normSq > (1.0f - 1e-6f) && normSq < (1.0f + 1e-6f)) {
|
||||
return;
|
||||
}
|
||||
|
||||
imuResetOrientationQuaternion(accBF);
|
||||
DEBUG_TRACE("AHRS orientation quaternion error");
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
if (feature(FEATURE_BLACKBOX)) {
|
||||
blackboxLogEvent(FLIGHT_LOG_EVENT_IMU_FAILURE, NULL);
|
||||
}
|
||||
#endif
|
||||
if (feature(FEATURE_BLACKBOX)) {
|
||||
blackboxLogEvent(FLIGHT_LOG_EVENT_IMU_FAILURE, NULL);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
|
||||
bool useAcc, float ax, float ay, float az,
|
||||
bool useMag, float mx, float my, float mz,
|
||||
bool useCOG, float courseOverGround)
|
||||
static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround)
|
||||
{
|
||||
STATIC_FASTRAM float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; // integral error terms scaled by Ki
|
||||
STATIC_FASTRAM float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; // integral error terms scaled by Ki
|
||||
float ex, ey, ez;
|
||||
STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
|
||||
|
||||
fpVector3_t vRotation = *gyroBF;
|
||||
|
||||
/* Calculate general spin rate (rad/s) */
|
||||
const float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);
|
||||
const float spin_rate_sq = vectorNormSquared(&vRotation);
|
||||
|
||||
/* Step 1: Yaw correction */
|
||||
// Use measured magnetic field vector
|
||||
if (useMag || useCOG) {
|
||||
float kpMag = imuRuntimeConfig.dcm_kp_mag * imuGetPGainScaleFactor();
|
||||
const float magMagnitudeSq = mx * mx + my * my + mz * mz;
|
||||
if (magBF || useCOG) {
|
||||
static const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } };
|
||||
|
||||
if (useMag && magMagnitudeSq > 0.01f) {
|
||||
// Normalise magnetometer measurement
|
||||
const float magRecipNorm = invSqrt(magMagnitudeSq);
|
||||
mx *= magRecipNorm;
|
||||
my *= magRecipNorm;
|
||||
mz *= magRecipNorm;
|
||||
fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
|
||||
|
||||
if (magBF && vectorNormSquared(magBF) > 0.01f) {
|
||||
fpVector3_t vMag;
|
||||
|
||||
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
|
||||
// This way magnetic field will only affect heading and wont mess roll/pitch angles
|
||||
|
||||
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
|
||||
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
|
||||
const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
|
||||
const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
|
||||
const float bx = sqrtf(hx * hx + hy * hy);
|
||||
// This should yield direction to magnetic North (1; 0; 0)
|
||||
quaternionRotateVectorInv(&vMag, magBF, &orientation); // BF -> EF
|
||||
|
||||
// Ignore magnetic inclination
|
||||
vMag.z = 0.0f;
|
||||
|
||||
// Normalize to unit vector
|
||||
vectorNormalize(&vMag, &vMag);
|
||||
|
||||
// Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero
|
||||
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
|
||||
const float ez_ef = -(hy * bx);
|
||||
vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth);
|
||||
|
||||
// Rotate mag error vector back to BF and accumulate
|
||||
ex = rMat[2][0] * ez_ef;
|
||||
ey = rMat[2][1] * ez_ef;
|
||||
ez = rMat[2][2] * ez_ef;
|
||||
// Rotate error back into body frame
|
||||
quaternionRotateVector(&vErr, &vErr, &orientation);
|
||||
}
|
||||
else if (useCOG) {
|
||||
fpVector3_t vHeadingEF;
|
||||
|
||||
// Use raw heading error (from GPS or whatever else)
|
||||
while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf);
|
||||
while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf);
|
||||
|
||||
// William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23
|
||||
// (Rxx; Ryx) - measured (estimated) heading vector (EF)
|
||||
// (cos(COG), sin(COG)) - reference heading vector (EF)
|
||||
// error is cross product between reference heading and estimated heading (calculated in EF)
|
||||
const float ez_ef = - sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0];
|
||||
// (-cos(COG), sin(COG)) - reference heading vector (EF)
|
||||
|
||||
ex = rMat[2][0] * ez_ef;
|
||||
ey = rMat[2][1] * ez_ef;
|
||||
ez = rMat[2][2] * ez_ef;
|
||||
}
|
||||
else {
|
||||
ex = 0;
|
||||
ey = 0;
|
||||
ez = 0;
|
||||
// Compute heading vector in EF from scalar CoG
|
||||
fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } };
|
||||
|
||||
// Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
|
||||
quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
|
||||
vHeadingEF.z = 0.0f;
|
||||
|
||||
// Normalize to unit vector
|
||||
vectorNormalize(&vHeadingEF, &vHeadingEF);
|
||||
|
||||
// error is cross product between reference heading and estimated heading (calculated in EF)
|
||||
vectorCrossProduct(&vErr, &vCoG, &vHeadingEF);
|
||||
|
||||
// Rotate error back into body frame
|
||||
quaternionRotateVector(&vErr, &vErr, &orientation);
|
||||
}
|
||||
|
||||
// Compute and apply integral feedback if enabled
|
||||
if (imuRuntimeConfig.dcm_ki_mag > 0.0f) {
|
||||
// Stop integrating if spinning beyond the certain limit
|
||||
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
|
||||
integralMagX += imuRuntimeConfig.dcm_ki_mag * ex * dt; // integral error scaled by Ki
|
||||
integralMagY += imuRuntimeConfig.dcm_ki_mag * ey * dt;
|
||||
integralMagZ += imuRuntimeConfig.dcm_ki_mag * ez * dt;
|
||||
fpVector3_t vTmp;
|
||||
|
||||
gx += integralMagX;
|
||||
gy += integralMagY;
|
||||
gz += integralMagZ;
|
||||
// integral error scaled by Ki
|
||||
vectorScale(&vTmp, &vErr, imuRuntimeConfig.dcm_ki_mag * dt);
|
||||
vectorAdd(&vGyroDriftEstimate, &vGyroDriftEstimate, &vTmp);
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate kP gain and apply proportional feedback
|
||||
gx += kpMag * ex;
|
||||
gy += kpMag * ey;
|
||||
gz += kpMag * ez;
|
||||
vectorScale(&vErr, &vErr, imuRuntimeConfig.dcm_kp_mag * imuGetPGainScaleFactor());
|
||||
vectorAdd(&vRotation, &vRotation, &vErr);
|
||||
}
|
||||
|
||||
|
||||
/* Step 2: Roll and pitch correction - use measured acceleration vector */
|
||||
if (useAcc) {
|
||||
float kpAcc = imuRuntimeConfig.dcm_kp_acc * imuGetPGainScaleFactor();
|
||||
const float accRecipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
if (accBF) {
|
||||
static const fpVector3_t vGravity = { .v = { 0.0f, 0.0f, 1.0f } };
|
||||
fpVector3_t vEstGravity, vAcc, vErr;
|
||||
|
||||
// Just scale by 1G length - That's our vector adjustment. Rather than
|
||||
// using one-over-exact length (which needs a costly square root), we already
|
||||
// know the vector is enough "roughly unit length" and since it is only weighted
|
||||
// in by a certain amount anyway later, having that exact is meaningless. (c) MasterZap
|
||||
ax *= accRecipNorm;
|
||||
ay *= accRecipNorm;
|
||||
az *= accRecipNorm;
|
||||
// Calculate estimated gravity vector in body frame
|
||||
quaternionRotateVector(&vEstGravity, &vGravity, &orientation); // EF -> BF
|
||||
|
||||
// Error is sum of cross product between estimated direction and measured direction of gravity
|
||||
ex = (ay * rMat[2][2] - az * rMat[2][1]);
|
||||
ey = (az * rMat[2][0] - ax * rMat[2][2]);
|
||||
ez = (ax * rMat[2][1] - ay * rMat[2][0]);
|
||||
vectorNormalize(&vAcc, accBF);
|
||||
vectorCrossProduct(&vErr, &vAcc, &vEstGravity);
|
||||
|
||||
// Compute and apply integral feedback if enabled
|
||||
if (imuRuntimeConfig.dcm_ki_acc > 0.0f) {
|
||||
// Stop integrating if spinning beyond the certain limit
|
||||
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
|
||||
integralAccX += imuRuntimeConfig.dcm_ki_acc * ex * dt; // integral error scaled by Ki
|
||||
integralAccY += imuRuntimeConfig.dcm_ki_acc * ey * dt;
|
||||
integralAccZ += imuRuntimeConfig.dcm_ki_acc * ez * dt;
|
||||
fpVector3_t vTmp;
|
||||
|
||||
gx += integralAccX;
|
||||
gy += integralAccY;
|
||||
gz += integralAccZ;
|
||||
// integral error scaled by Ki
|
||||
vectorScale(&vTmp, &vErr, imuRuntimeConfig.dcm_ki_acc * dt);
|
||||
vectorAdd(&vGyroDriftEstimate, &vGyroDriftEstimate, &vTmp);
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate kP gain and apply proportional feedback
|
||||
gx += kpAcc * ex;
|
||||
gy += kpAcc * ey;
|
||||
gz += kpAcc * ez;
|
||||
vectorScale(&vErr, &vErr, imuRuntimeConfig.dcm_kp_acc * imuGetPGainScaleFactor());
|
||||
vectorAdd(&vRotation, &vRotation, &vErr);
|
||||
}
|
||||
|
||||
// Apply gyro drift correction
|
||||
vectorAdd(&vRotation, &vRotation, &vGyroDriftEstimate);
|
||||
|
||||
// Integrate rate of change of quaternion
|
||||
gx *= (0.5f * dt);
|
||||
gy *= (0.5f * dt);
|
||||
gz *= (0.5f * dt);
|
||||
fpVector3_t vTheta;
|
||||
fpQuaternion_t deltaQ;
|
||||
|
||||
const float qa = q0;
|
||||
const float qb = q1;
|
||||
const float qc = q2;
|
||||
q0 += (-qb * gx - qc * gy - q3 * gz);
|
||||
q1 += (qa * gx + qc * gz - q3 * gy);
|
||||
q2 += (qa * gy - qb * gz + q3 * gx);
|
||||
q3 += (qa * gz + qb * gy - qc * gx);
|
||||
vectorScale(&vTheta, &vRotation, 0.5f * dt);
|
||||
quaternionInitFromVector(&deltaQ, &vTheta);
|
||||
const float thetaMagnitudeSq = vectorNormSquared(&vTheta);
|
||||
|
||||
// Normalise quaternion
|
||||
const float quatRecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= quatRecipNorm;
|
||||
q1 *= quatRecipNorm;
|
||||
q2 *= quatRecipNorm;
|
||||
q3 *= quatRecipNorm;
|
||||
// Calculate quaternion delta:
|
||||
// Theta is a axis/angle rotation. Direction of a vector is axis, magnitude is angle/2.
|
||||
// Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
|
||||
// For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision -
|
||||
// then we can safely use the "low angle" approximated version without loss of accuracy.
|
||||
if (thetaMagnitudeSq < sqrtf(24.0f * 1e-6f)) {
|
||||
quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f);
|
||||
deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f;
|
||||
}
|
||||
else {
|
||||
const float thetaMagnitude = sqrtf(thetaMagnitudeSq);
|
||||
quaternionScale(&deltaQ, &deltaQ, sin_approx(thetaMagnitude) / thetaMagnitude);
|
||||
deltaQ.q0 = cos_approx(thetaMagnitude);
|
||||
}
|
||||
|
||||
// Calculate final orientation and renormalize
|
||||
quaternionMultiply(&orientation, &orientation, &deltaQ);
|
||||
quaternionNormalize(&orientation, &orientation);
|
||||
|
||||
// Check for invalid quaternion
|
||||
imuCheckAndResetOrientationQuaternion(ax, ay, az);
|
||||
imuCheckAndResetOrientationQuaternion(accBF);
|
||||
|
||||
// Pre-compute rotation matrix from quaternion
|
||||
imuComputeRotationMatrix();
|
||||
|
|
@ -415,13 +417,13 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
|
|||
/* Compute pitch/roll angles */
|
||||
attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
|
||||
attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));
|
||||
attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + mag.magneticDeclination;
|
||||
attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0]));
|
||||
|
||||
if (attitude.values.yaw < 0)
|
||||
attitude.values.yaw += 3600;
|
||||
|
||||
/* Update small angle state */
|
||||
if (rMat[2][2] > smallAngleCosZ) {
|
||||
if (calculateCosTiltAngle() > smallAngleCosZ) {
|
||||
ENABLE_STATE(SMALL_ANGLE);
|
||||
} else {
|
||||
DISABLE_STATE(SMALL_ANGLE);
|
||||
|
|
@ -500,10 +502,12 @@ static void imuCalculateEstimatedAttitude(float dT)
|
|||
}
|
||||
#endif
|
||||
|
||||
imuMahonyAHRSupdate(dT, imuMeasuredRotationBF.A[X], imuMeasuredRotationBF.A[Y], imuMeasuredRotationBF.A[Z],
|
||||
useAcc, imuMeasuredAccelBF.A[X], imuMeasuredAccelBF.A[Y], imuMeasuredAccelBF.A[Z],
|
||||
useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
|
||||
useCOG, courseOverGround);
|
||||
fpVector3_t measuredMagBF = { .v = { mag.magADC[X], mag.magADC[Y], mag.magADC[Z] } };
|
||||
|
||||
imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF,
|
||||
useAcc ? &imuMeasuredAccelBF : NULL,
|
||||
useMag ? &measuredMagBF : NULL,
|
||||
useCOG, courseOverGround);
|
||||
|
||||
imuUpdateEulerAngles();
|
||||
}
|
||||
|
|
@ -583,5 +587,5 @@ bool isImuHeadingValid(void)
|
|||
|
||||
float calculateCosTiltAngle(void)
|
||||
{
|
||||
return rMat[2][2];
|
||||
return 1.0f - 2.0f * sq(orientation.q1) - 2.0f * sq(orientation.q2);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,11 +19,13 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
#include "common/quaternion.h"
|
||||
#include "common/time.h"
|
||||
#include "config/parameter_group.h"
|
||||
|
||||
extern t_fp_vector imuMeasuredAccelBF; // cm/s/s
|
||||
extern t_fp_vector imuMeasuredRotationBF; // rad/s
|
||||
extern fpVector3_t imuMeasuredAccelBF; // cm/s/s
|
||||
extern fpVector3_t imuMeasuredRotationBF; // rad/s
|
||||
|
||||
typedef union {
|
||||
int16_t raw[XYZ_AXIS_COUNT];
|
||||
|
|
@ -35,6 +37,7 @@ typedef union {
|
|||
} values;
|
||||
} attitudeEulerAngles_t;
|
||||
|
||||
extern fpQuaternion_t orientation;
|
||||
extern attitudeEulerAngles_t attitude;
|
||||
|
||||
typedef struct imuConfig_s {
|
||||
|
|
@ -57,13 +60,14 @@ typedef struct imuRuntimeConfig_s {
|
|||
|
||||
void imuConfigure(void);
|
||||
|
||||
void imuSetMagneticDeclination(float declinationDeg);
|
||||
void imuUpdateAttitude(timeUs_t currentTimeUs);
|
||||
void imuUpdateAccelerometer(void);
|
||||
float calculateCosTiltAngle(void);
|
||||
bool isImuReady(void);
|
||||
bool isImuHeadingValid(void);
|
||||
|
||||
void imuTransformVectorBodyToEarth(t_fp_vector * v);
|
||||
void imuTransformVectorEarthToBody(t_fp_vector * v);
|
||||
void imuTransformVectorBodyToEarth(fpVector3_t * v);
|
||||
void imuTransformVectorEarthToBody(fpVector3_t * v);
|
||||
|
||||
void imuInit(void);
|
||||
|
|
|
|||
|
|
@ -638,9 +638,9 @@ float pidHeadingHold(void)
|
|||
*/
|
||||
static void pidTurnAssistant(pidState_t *pidState)
|
||||
{
|
||||
t_fp_vector targetRates;
|
||||
targetRates.V.X = 0.0f;
|
||||
targetRates.V.Y = 0.0f;
|
||||
fpVector3_t targetRates;
|
||||
targetRates.x = 0.0f;
|
||||
targetRates.y = 0.0f;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (calculateCosTiltAngle() >= 0.173648f) {
|
||||
|
|
@ -667,7 +667,7 @@ static void pidTurnAssistant(pidState_t *pidState)
|
|||
float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
|
||||
float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn;
|
||||
|
||||
targetRates.V.Z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
|
||||
targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
|
||||
}
|
||||
else {
|
||||
// Don't allow coordinated turn calculation if airplane is in hard bank or steep climb/dive
|
||||
|
|
@ -675,22 +675,22 @@ static void pidTurnAssistant(pidState_t *pidState)
|
|||
}
|
||||
}
|
||||
else {
|
||||
targetRates.V.Z = pidState[YAW].rateTarget;
|
||||
targetRates.z = pidState[YAW].rateTarget;
|
||||
}
|
||||
|
||||
// Transform calculated rate offsets into body frame and apply
|
||||
imuTransformVectorEarthToBody(&targetRates);
|
||||
|
||||
// Add in roll and pitch
|
||||
pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.V.X, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f);
|
||||
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.V.Y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
|
||||
pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f);
|
||||
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
|
||||
|
||||
// Replace YAW on quads - add it in on airplanes
|
||||
if (STATE(FIXED_WING)) {
|
||||
pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.V.Z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
|
||||
pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
|
||||
}
|
||||
else {
|
||||
pidState[YAW].rateTarget = constrainf(targetRates.V.Z, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
|
||||
pidState[YAW].rateTarget = constrainf(targetRates.z, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -175,11 +175,11 @@ void resetGCSFlags(void);
|
|||
|
||||
static bool posEstimationHasGlobalReference(void);
|
||||
static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint);
|
||||
static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos);
|
||||
void calculateInitialHoldPosition(t_fp_vector * pos);
|
||||
void calculateFarAwayTarget(t_fp_vector * farAwayPos, int32_t yaw, int32_t distance);
|
||||
static void calcualteAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
|
||||
void calculateInitialHoldPosition(fpVector3_t * pos);
|
||||
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
|
||||
|
||||
void initializeRTHSanityChecker(const t_fp_vector * pos);
|
||||
void initializeRTHSanityChecker(const fpVector3_t * pos);
|
||||
bool validateRTHSanityChecker(void);
|
||||
|
||||
/*************************************************************************************************/
|
||||
|
|
@ -722,7 +722,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(n
|
|||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
t_fp_vector targetHoldPos;
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
}
|
||||
|
|
@ -736,7 +736,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(
|
|||
|
||||
// If GCS was disabled - reset 2D pos setpoint
|
||||
if (posControl.flags.isGCSAssistedNavigationReset) {
|
||||
t_fp_vector targetHoldPos;
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
resetGCSFlags();
|
||||
|
|
@ -765,7 +765,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
|
|||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
t_fp_vector targetHoldPos;
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
}
|
||||
|
|
@ -779,7 +779,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
|
|||
|
||||
// If GCS was disabled - reset 2D pos setpoint
|
||||
if (posControl.flags.isGCSAssistedNavigationReset) {
|
||||
t_fp_vector targetHoldPos;
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -824,7 +824,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
|
||||
}
|
||||
else {
|
||||
t_fp_vector targetHoldPos;
|
||||
fpVector3_t targetHoldPos;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
// Airplane - climbout before turning around
|
||||
|
|
@ -864,10 +864,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
// If we have valid pos sensor OR we are configured to ignore GPS loss
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
const float rthAltitudeMargin = STATE(FIXED_WING) ?
|
||||
MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)) : // Airplane
|
||||
MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)); // Copters
|
||||
MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) : // Airplane
|
||||
MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters
|
||||
|
||||
if (((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
|
||||
if (((posControl.actualState.pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
|
||||
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
|
||||
if (STATE(FIXED_WING)) {
|
||||
initializeRTHSanityChecker(&posControl.actualState.pos);
|
||||
|
|
@ -885,16 +885,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
|
||||
// Climb to safe altitude and turn to correct direction
|
||||
if (STATE(FIXED_WING)) {
|
||||
t_fp_vector pos = posControl.homeWaypointAbove.pos;
|
||||
pos.V.Z += FW_RTH_CLIMB_OVERSHOOT_CM;
|
||||
fpVector3_t pos = posControl.homeWaypointAbove.pos;
|
||||
pos.z += FW_RTH_CLIMB_OVERSHOOT_CM;
|
||||
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
|
||||
}
|
||||
else {
|
||||
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
|
||||
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.
|
||||
t_fp_vector pos = posControl.homeWaypointAbove.pos;
|
||||
pos.V.Z += MR_RTH_CLIMB_OVERSHOOT_CM;
|
||||
fpVector3_t pos = posControl.homeWaypointAbove.pos;
|
||||
pos.z += MR_RTH_CLIMB_OVERSHOOT_CM;
|
||||
|
||||
if (navConfig()->general.flags.rth_tail_first) {
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
|
|
@ -1015,7 +1015,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
}
|
||||
else {
|
||||
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
|
||||
float descentVelScaling = (posControl.actualState.pos.V.Z - posControl.homePosition.pos.V.Z - navConfig()->general.land_slowdown_minalt)
|
||||
float descentVelScaling = (posControl.actualState.pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
|
||||
/ (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
|
||||
|
||||
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
|
||||
|
|
@ -1472,11 +1472,11 @@ bool checkForPositionSensorTimeout(void)
|
|||
*-----------------------------------------------------------*/
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
|
||||
{
|
||||
posControl.actualState.pos.V.X = newX;
|
||||
posControl.actualState.pos.V.Y = newY;
|
||||
posControl.actualState.pos.x = newX;
|
||||
posControl.actualState.pos.y = newY;
|
||||
|
||||
posControl.actualState.vel.V.X = newVelX;
|
||||
posControl.actualState.vel.V.Y = newVelY;
|
||||
posControl.actualState.vel.x = newVelX;
|
||||
posControl.actualState.vel.y = newVelY;
|
||||
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
|
||||
|
||||
if (estimateValid) {
|
||||
|
|
@ -1502,8 +1502,8 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
|
|||
*-----------------------------------------------------------*/
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
|
||||
{
|
||||
posControl.actualState.pos.V.Z = newAltitude;
|
||||
posControl.actualState.vel.V.Z = newVelocity;
|
||||
posControl.actualState.pos.z = newAltitude;
|
||||
posControl.actualState.vel.z = newVelocity;
|
||||
posControl.actualState.surface = surfaceDistance;
|
||||
posControl.actualState.surfaceVel = surfaceVelocity;
|
||||
|
||||
|
|
@ -1544,8 +1544,8 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
|
|||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navLatestActualPosition[Z] = constrain(posControl.actualState.pos.V.Z, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(posControl.actualState.vel.V.Z, -32678, 32767);
|
||||
navLatestActualPosition[Z] = constrain(posControl.actualState.pos.z, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(posControl.actualState.vel.z, -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -1568,18 +1568,18 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
|
|||
/*-----------------------------------------------------------
|
||||
* Calculates distance and bearing to destination point
|
||||
*-----------------------------------------------------------*/
|
||||
uint32_t calculateDistanceToDestination(const t_fp_vector * destinationPos)
|
||||
uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
|
||||
{
|
||||
const float deltaX = destinationPos->V.X - posControl.actualState.pos.V.X;
|
||||
const float deltaY = destinationPos->V.Y - posControl.actualState.pos.V.Y;
|
||||
const float deltaX = destinationPos->x - posControl.actualState.pos.x;
|
||||
const float deltaY = destinationPos->y - posControl.actualState.pos.y;
|
||||
|
||||
return sqrtf(sq(deltaX) + sq(deltaY));
|
||||
}
|
||||
|
||||
int32_t calculateBearingToDestination(const t_fp_vector * destinationPos)
|
||||
int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
|
||||
{
|
||||
const float deltaX = destinationPos->V.X - posControl.actualState.pos.V.X;
|
||||
const float deltaY = destinationPos->V.Y - posControl.actualState.pos.V.Y;
|
||||
const float deltaX = destinationPos->x - posControl.actualState.pos.x;
|
||||
const float deltaY = destinationPos->y - posControl.actualState.pos.y;
|
||||
|
||||
return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
|
||||
}
|
||||
|
|
@ -1626,33 +1626,33 @@ static void updateDesiredRTHAltitude(void)
|
|||
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
|
||||
switch (navConfig()->general.flags.rth_alt_control_mode) {
|
||||
case NAV_RTH_NO_ALT:
|
||||
posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z;
|
||||
break;
|
||||
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
|
||||
posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z + navConfig()->general.rth_altitude;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z + navConfig()->general.rth_altitude;
|
||||
break;
|
||||
case NAV_RTH_MAX_ALT:
|
||||
posControl.homeWaypointAbove.pos.V.Z = MAX(posControl.homeWaypointAbove.pos.V.Z, posControl.actualState.pos.V.Z);
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.pos.z);
|
||||
break;
|
||||
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
|
||||
posControl.homeWaypointAbove.pos.V.Z = MAX(posControl.homePosition.pos.V.Z + navConfig()->general.rth_altitude, posControl.actualState.pos.V.Z);
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.pos.z);
|
||||
break;
|
||||
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
|
||||
default:
|
||||
posControl.homeWaypointAbove.pos.V.Z = posControl.homePosition.pos.V.Z + navConfig()->general.rth_altitude;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* RTH sanity test logic
|
||||
*-----------------------------------------------------------*/
|
||||
void initializeRTHSanityChecker(const t_fp_vector * pos)
|
||||
void initializeRTHSanityChecker(const fpVector3_t * pos)
|
||||
{
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
|
||||
|
|
@ -1692,17 +1692,17 @@ bool validateRTHSanityChecker(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Reset home position to current position
|
||||
*-----------------------------------------------------------*/
|
||||
void setHomePosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask)
|
||||
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
|
||||
{
|
||||
// XY-position
|
||||
if ((useMask & NAV_POS_UPDATE_XY) != 0) {
|
||||
posControl.homePosition.pos.V.X = pos->V.X;
|
||||
posControl.homePosition.pos.V.Y = pos->V.Y;
|
||||
posControl.homePosition.pos.x = pos->x;
|
||||
posControl.homePosition.pos.y = pos->y;
|
||||
}
|
||||
|
||||
// Z-position
|
||||
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
|
||||
posControl.homePosition.pos.V.Z = pos->V.Z;
|
||||
posControl.homePosition.pos.z = pos->z;
|
||||
}
|
||||
|
||||
// Heading
|
||||
|
|
@ -1780,7 +1780,7 @@ int32_t getTotalTravelDistance(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Calculate platform-specific hold position (account for deceleration)
|
||||
*-----------------------------------------------------------*/
|
||||
void calculateInitialHoldPosition(t_fp_vector * pos)
|
||||
void calculateInitialHoldPosition(fpVector3_t * pos)
|
||||
{
|
||||
if (STATE(FIXED_WING)) { // FIXED_WING
|
||||
calculateFixedWingInitialHoldPosition(pos);
|
||||
|
|
@ -1793,19 +1793,19 @@ void calculateInitialHoldPosition(t_fp_vector * pos)
|
|||
/*-----------------------------------------------------------
|
||||
* Set active XYZ-target and desired heading
|
||||
*-----------------------------------------------------------*/
|
||||
void setDesiredPosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask)
|
||||
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
|
||||
{
|
||||
// XY-position
|
||||
if ((useMask & NAV_POS_UPDATE_XY) != 0) {
|
||||
posControl.desiredState.pos.V.X = pos->V.X;
|
||||
posControl.desiredState.pos.V.Y = pos->V.Y;
|
||||
posControl.desiredState.pos.x = pos->x;
|
||||
posControl.desiredState.pos.y = pos->y;
|
||||
}
|
||||
|
||||
// Z-position
|
||||
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
|
||||
posControl.desiredState.surface = -1; // When we directly set altitude target we must reset surface tracking
|
||||
posControl.desiredState.pos.V.Z = pos->V.Z;
|
||||
posControl.desiredState.pos.z = pos->z;
|
||||
}
|
||||
|
||||
// Heading
|
||||
|
|
@ -1821,11 +1821,11 @@ void setDesiredPosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlag
|
|||
}
|
||||
}
|
||||
|
||||
void calculateFarAwayTarget(t_fp_vector * farAwayPos, int32_t yaw, int32_t distance)
|
||||
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance)
|
||||
{
|
||||
farAwayPos->V.X = posControl.actualState.pos.V.X + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->V.Y = posControl.actualState.pos.V.Y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->V.Z = posControl.actualState.pos.V.Z;
|
||||
farAwayPos->x = posControl.actualState.pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->y = posControl.actualState.pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->z = posControl.actualState.pos.z;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -1865,7 +1865,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
|
||||
if (mode == ROC_TO_ALT_RESET) {
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z;
|
||||
posControl.desiredState.pos.z = posControl.actualState.pos.z;
|
||||
}
|
||||
else {
|
||||
if (STATE(FIXED_WING)) {
|
||||
|
|
@ -1876,15 +1876,15 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 1, timeDelta * 1000);
|
||||
|
||||
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
posControl.desiredState.pos.V.Z += desiredClimbRate * timeDelta;
|
||||
posControl.desiredState.pos.V.Z = constrainf(posControl.desiredState.pos.V.Z, // FIXME: calculate sanity limits in a smarter way
|
||||
posControl.actualState.pos.V.Z - 500,
|
||||
posControl.actualState.pos.V.Z + 500);
|
||||
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
|
||||
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, // FIXME: calculate sanity limits in a smarter way
|
||||
posControl.actualState.pos.z - 500,
|
||||
posControl.actualState.pos.z + 500);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
|
||||
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.desiredState.pos.z = posControl.actualState.pos.z + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
|
||||
}
|
||||
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
|
|
@ -2127,7 +2127,7 @@ bool saveNonVolatileWaypointList(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
static void mapWaypointToLocalPosition(t_fp_vector * localPos, const navWaypoint_t * waypoint)
|
||||
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint)
|
||||
{
|
||||
gpsLocation_t wpLLH;
|
||||
|
||||
|
|
@ -2138,7 +2138,7 @@ static void mapWaypointToLocalPosition(t_fp_vector * localPos, const navWaypoint
|
|||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, localPos, GEO_ALT_RELATIVE);
|
||||
}
|
||||
|
||||
static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos)
|
||||
static void calcualteAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
|
||||
{
|
||||
posControl.activeWaypoint.pos = *pos;
|
||||
|
||||
|
|
@ -2151,7 +2151,7 @@ static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos
|
|||
|
||||
static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
||||
{
|
||||
t_fp_vector localPos;
|
||||
fpVector3_t localPos;
|
||||
mapWaypointToLocalPosition(&localPos, waypoint);
|
||||
calcualteAndSetActiveWaypointToLocalPosition(&localPos);
|
||||
}
|
||||
|
|
@ -2299,9 +2299,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
|
||||
if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8);
|
||||
|
||||
navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767);
|
||||
navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767);
|
||||
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
|
||||
navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.x), -32678, 32767);
|
||||
navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.y), -32678, 32767);
|
||||
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.z), -32678, 32767);
|
||||
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
|
@ -2496,7 +2496,7 @@ bool navigationBlockArming(void)
|
|||
|
||||
// Don't allow arming if first waypoint is farther than configured safe distance
|
||||
if (posControl.waypointCount > 0) {
|
||||
t_fp_vector startingWaypointPos;
|
||||
fpVector3_t startingWaypointPos;
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]);
|
||||
|
||||
const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
|
||||
|
|
@ -2651,12 +2651,12 @@ void navigationInit(void)
|
|||
*-----------------------------------------------------------*/
|
||||
float getEstimatedActualVelocity(int axis)
|
||||
{
|
||||
return posControl.actualState.vel.A[axis];
|
||||
return posControl.actualState.vel.v[axis];
|
||||
}
|
||||
|
||||
float getEstimatedActualPosition(int axis)
|
||||
{
|
||||
return posControl.actualState.pos.A[axis];
|
||||
return posControl.actualState.pos.v[axis];
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
|
||||
|
|
@ -194,7 +195,7 @@ typedef struct {
|
|||
} navWaypoint_t;
|
||||
|
||||
typedef struct {
|
||||
t_fp_vector pos;
|
||||
fpVector3_t pos;
|
||||
int32_t yaw; // deg * 100
|
||||
} navWaypointPosition_t;
|
||||
|
||||
|
|
@ -300,8 +301,8 @@ typedef enum {
|
|||
} geoOriginResetMode_e;
|
||||
|
||||
void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginResetMode_e resetMode);
|
||||
void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, t_fp_vector * pos, geoAltitudeConversionMode_e altConv);
|
||||
void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const t_fp_vector * pos, gpsLocation_t * llh);
|
||||
void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, fpVector3_t * pos, geoAltitudeConversionMode_e altConv);
|
||||
void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh);
|
||||
float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
|
||||
|
||||
/* Failsafe-forced RTH mode */
|
||||
|
|
|
|||
|
|
@ -104,10 +104,10 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
|
|||
// velocity error. We use PID controller on altitude error and calculate desired pitch angle
|
||||
|
||||
// Update energies
|
||||
const float demSPE = (posControl.desiredState.pos.V.Z / 100.0f) * GRAVITY_MSS;
|
||||
const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS;
|
||||
const float demSKE = 0.0f;
|
||||
|
||||
const float estSPE = (posControl.actualState.pos.V.Z / 100.0f) * GRAVITY_MSS;
|
||||
const float estSPE = (posControl.actualState.pos.z / 100.0f) * GRAVITY_MSS;
|
||||
const float estSKE = 0.0f;
|
||||
|
||||
// speedWeight controls balance between potential and kinetic energy used for pitch controller
|
||||
|
|
@ -188,14 +188,14 @@ bool adjustFixedWingHeadingFromRCInput(void)
|
|||
/*-----------------------------------------------------------
|
||||
* XY-position controller for multicopter aircraft
|
||||
*-----------------------------------------------------------*/
|
||||
static t_fp_vector virtualDesiredPosition;
|
||||
static fpVector3_t virtualDesiredPosition;
|
||||
static pt1Filter_t fwPosControllerCorrectionFilterState;
|
||||
|
||||
void resetFixedWingPositionController(void)
|
||||
{
|
||||
virtualDesiredPosition.V.X = 0;
|
||||
virtualDesiredPosition.V.Y = 0;
|
||||
virtualDesiredPosition.V.Z = 0;
|
||||
virtualDesiredPosition.x = 0;
|
||||
virtualDesiredPosition.y = 0;
|
||||
virtualDesiredPosition.z = 0;
|
||||
|
||||
navPidReset(&posControl.pids.fw_nav);
|
||||
posControl.rcAdjustment[ROLL] = 0;
|
||||
|
|
@ -206,8 +206,8 @@ void resetFixedWingPositionController(void)
|
|||
|
||||
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
||||
{
|
||||
float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X;
|
||||
float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y;
|
||||
float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x;
|
||||
float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y;
|
||||
|
||||
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
|
||||
|
||||
|
|
@ -225,18 +225,18 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
// We are closing in on a waypoint, calculate circular loiter
|
||||
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f);
|
||||
|
||||
float loiterTargetX = posControl.desiredState.pos.V.X + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
|
||||
float loiterTargetY = posControl.desiredState.pos.V.Y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
|
||||
float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
|
||||
float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
|
||||
|
||||
// We have temporary loiter target. Recalculate distance and position error
|
||||
posErrorX = loiterTargetX - posControl.actualState.pos.V.X;
|
||||
posErrorY = loiterTargetY - posControl.actualState.pos.V.Y;
|
||||
posErrorX = loiterTargetX - posControl.actualState.pos.x;
|
||||
posErrorY = loiterTargetY - posControl.actualState.pos.y;
|
||||
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
|
||||
}
|
||||
|
||||
// Calculate virtual waypoint
|
||||
virtualDesiredPosition.V.X = posControl.actualState.pos.V.X + posErrorX * (trackingDistance / distanceToActualTarget);
|
||||
virtualDesiredPosition.V.Y = posControl.actualState.pos.V.Y + posErrorY * (trackingDistance / distanceToActualTarget);
|
||||
virtualDesiredPosition.x = posControl.actualState.pos.x + posErrorX * (trackingDistance / distanceToActualTarget);
|
||||
virtualDesiredPosition.y = posControl.actualState.pos.y + posErrorY * (trackingDistance / distanceToActualTarget);
|
||||
|
||||
// Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
|
|
@ -246,8 +246,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod;
|
||||
|
||||
// Rotate this target shift from body frame to to earth frame and apply to position target
|
||||
virtualDesiredPosition.V.X += -rcShiftY * posControl.actualState.sinYaw;
|
||||
virtualDesiredPosition.V.Y += rcShiftY * posControl.actualState.cosYaw;
|
||||
virtualDesiredPosition.x += -rcShiftY * posControl.actualState.sinYaw;
|
||||
virtualDesiredPosition.y += rcShiftY * posControl.actualState.cosYaw;
|
||||
|
||||
posControl.flags.isAdjustingPosition = true;
|
||||
}
|
||||
|
|
@ -473,7 +473,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
* TODO refactor conditions in this metod if logic is proven to be correct
|
||||
*/
|
||||
if (navStateFlags & NAV_CTL_LAND) {
|
||||
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (posControl.actualState.pos.V.Z <= navConfig()->general.land_slowdown_minalt)) ||
|
||||
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (posControl.actualState.pos.z <= navConfig()->general.land_slowdown_minalt)) ||
|
||||
((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surface <= navConfig()->general.land_slowdown_minalt)) ) {
|
||||
/*
|
||||
* Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
|
||||
|
|
@ -531,7 +531,7 @@ void applyFixedWingEmergencyLandingController(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Calculate loiter target based on current position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void calculateFixedWingInitialHoldPosition(t_fp_vector * pos)
|
||||
void calculateFixedWingInitialHoldPosition(fpVector3_t * pos)
|
||||
{
|
||||
// TODO: stub, this should account for velocity and target loiter radius
|
||||
*pos = posControl.actualState.pos;
|
||||
|
|
|
|||
|
|
@ -76,12 +76,12 @@ static FixedWingLaunchState_t launchState;
|
|||
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
|
||||
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
|
||||
{
|
||||
const float swingVelocity = (ABS(imuMeasuredRotationBF.A[Z]) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.A[Y] / imuMeasuredRotationBF.A[Z]) : 0;
|
||||
const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.A[X] > navConfig()->fw.launch_accel_thresh);
|
||||
const float swingVelocity = (ABS(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
|
||||
const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
|
||||
const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
|
||||
|
||||
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
|
||||
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.A[X] > 0);
|
||||
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
|
||||
|
||||
if (isBungeeLaunched || isSwingLaunched) {
|
||||
launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate);
|
||||
|
|
|
|||
|
|
@ -147,27 +147,27 @@ void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginRese
|
|||
}
|
||||
}
|
||||
|
||||
void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, t_fp_vector * pos, geoAltitudeConversionMode_e altConv)
|
||||
void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, fpVector3_t * pos, geoAltitudeConversionMode_e altConv)
|
||||
{
|
||||
if (origin->valid) {
|
||||
pos->V.X = (llh->lat - origin->lat) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
|
||||
pos->V.Y = (llh->lon - origin->lon) * (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * origin->scale);
|
||||
pos->x = (llh->lat - origin->lat) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
|
||||
pos->y = (llh->lon - origin->lon) * (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * origin->scale);
|
||||
|
||||
// If flag GEO_ALT_RELATIVE, than llh altitude is already relative to origin
|
||||
if (altConv == GEO_ALT_RELATIVE) {
|
||||
pos->V.Z = llh->alt;
|
||||
pos->z = llh->alt;
|
||||
} else {
|
||||
pos->V.Z = llh->alt - origin->alt;
|
||||
pos->z = llh->alt - origin->alt;
|
||||
}
|
||||
}
|
||||
else {
|
||||
pos->V.X = 0.0f;
|
||||
pos->V.Y = 0.0f;
|
||||
pos->V.Z = 0.0f;
|
||||
pos->x = 0.0f;
|
||||
pos->y = 0.0f;
|
||||
pos->z = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const t_fp_vector * pos, gpsLocation_t * llh)
|
||||
void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh)
|
||||
{
|
||||
float scaleLonDown;
|
||||
|
||||
|
|
@ -184,9 +184,9 @@ void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const t_fp_vector * p
|
|||
scaleLonDown = 1.0f;
|
||||
}
|
||||
|
||||
llh->lat += lrintf(pos->V.X / DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR);
|
||||
llh->lon += lrintf(pos->V.Y / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * scaleLonDown));
|
||||
llh->alt += lrintf(pos->V.Z);
|
||||
llh->lat += lrintf(pos->x / DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR);
|
||||
llh->lon += lrintf(pos->y / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * scaleLonDown));
|
||||
llh->alt += lrintf(pos->z);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ static bool prepareForTakeoffOnReset = false;
|
|||
// Position to velocity controller for Z axis
|
||||
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
|
||||
{
|
||||
const float altitudeError = posControl.desiredState.pos.V.Z - posControl.actualState.pos.V.Z;
|
||||
const float altitudeError = posControl.desiredState.pos.z - posControl.actualState.pos.z;
|
||||
float targetVel = altitudeError * posControl.pids.pos[Z].param.kP;
|
||||
|
||||
// hard limit desired target velocity to max_climb_rate
|
||||
|
|
@ -74,16 +74,16 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
|
|||
|
||||
// limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
|
||||
// if we are decelerating - don't limit (allow better recovery from falling)
|
||||
if (ABS(targetVel) > ABS(posControl.desiredState.vel.V.Z)) {
|
||||
if (ABS(targetVel) > ABS(posControl.desiredState.vel.z)) {
|
||||
const float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS / 5.0f);
|
||||
posControl.desiredState.vel.V.Z = constrainf(targetVel, posControl.desiredState.vel.V.Z - maxVelDifference, posControl.desiredState.vel.V.Z + maxVelDifference);
|
||||
posControl.desiredState.vel.z = constrainf(targetVel, posControl.desiredState.vel.z - maxVelDifference, posControl.desiredState.vel.z + maxVelDifference);
|
||||
}
|
||||
else {
|
||||
posControl.desiredState.vel.V.Z = targetVel;
|
||||
posControl.desiredState.vel.z = targetVel;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.V.Z), -32678, 32767);
|
||||
navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -93,7 +93,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
|||
const int16_t thrAdjustmentMin = (int16_t)motorConfig()->minthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
||||
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
||||
|
||||
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
|
||||
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, posControl.actualState.vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
|
||||
|
||||
posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, posControl.rcAdjustment[THROTTLE], NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
|
||||
posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
|
||||
|
|
@ -166,14 +166,14 @@ void resetMulticopterAltitudeController(void)
|
|||
|
||||
if (prepareForTakeoffOnReset) {
|
||||
/* If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump */
|
||||
posControl.desiredState.vel.V.Z = -navConfig()->general.max_manual_climb_rate;
|
||||
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
|
||||
posControl.desiredState.pos.z = posControl.actualState.pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.pids.vel[Z].integrator = -500.0f;
|
||||
pt1FilterReset(&altholdThrottleFilterState, -500.0f);
|
||||
prepareForTakeoffOnReset = false;
|
||||
}
|
||||
else {
|
||||
posControl.desiredState.vel.V.Z = posControl.actualState.vel.V.Z; // Gradually transition from current climb
|
||||
posControl.desiredState.vel.z = posControl.actualState.vel.z; // Gradually transition from current climb
|
||||
pt1FilterReset(&altholdThrottleFilterState, 0.0f);
|
||||
}
|
||||
}
|
||||
|
|
@ -270,8 +270,8 @@ bool adjustMulticopterPositionFromRCInput(void)
|
|||
const float neuVelY = rcVelX * posControl.actualState.sinYaw + rcVelY * posControl.actualState.cosYaw;
|
||||
|
||||
// Calculate new position target, so Pos-to-Vel P-controller would yield desired velocity
|
||||
posControl.desiredState.pos.V.X = posControl.actualState.pos.V.X + (neuVelX / posControl.pids.pos[X].param.kP);
|
||||
posControl.desiredState.pos.V.Y = posControl.actualState.pos.V.Y + (neuVelY / posControl.pids.pos[Y].param.kP);
|
||||
posControl.desiredState.pos.x = posControl.actualState.pos.x + (neuVelX / posControl.pids.pos[X].param.kP);
|
||||
posControl.desiredState.pos.y = posControl.actualState.pos.y + (neuVelY / posControl.pids.pos[Y].param.kP);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -279,7 +279,7 @@ bool adjustMulticopterPositionFromRCInput(void)
|
|||
else {
|
||||
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
t_fp_vector stopPosition;
|
||||
fpVector3_t stopPosition;
|
||||
calculateMulticopterInitialHoldPosition(&stopPosition);
|
||||
setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
|
||||
}
|
||||
|
|
@ -314,8 +314,8 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
|
|||
|
||||
static void updatePositionVelocityController_MC(void)
|
||||
{
|
||||
const float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X;
|
||||
const float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y;
|
||||
const float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x;
|
||||
const float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y;
|
||||
|
||||
// Calculate target velocity
|
||||
float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
|
||||
|
|
@ -335,12 +335,12 @@ static void updatePositionVelocityController_MC(void)
|
|||
// Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
|
||||
const float velHeadFactor = getVelocityHeadingAttenuationFactor();
|
||||
const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);
|
||||
posControl.desiredState.vel.V.X = newVelX * velHeadFactor * velExpoFactor;
|
||||
posControl.desiredState.vel.V.Y = newVelY * velHeadFactor * velExpoFactor;
|
||||
posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor;
|
||||
posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor;
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.V.X), -32678, 32767);
|
||||
navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.V.Y), -32678, 32767);
|
||||
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
|
||||
navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -348,8 +348,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
|
|||
{
|
||||
|
||||
// Calculate velocity error
|
||||
const float velErrorX = posControl.desiredState.vel.V.X - posControl.actualState.vel.V.X;
|
||||
const float velErrorY = posControl.desiredState.vel.V.Y - posControl.actualState.vel.V.Y;
|
||||
const float velErrorX = posControl.desiredState.vel.x - posControl.actualState.vel.x;
|
||||
const float velErrorY = posControl.desiredState.vel.y - posControl.actualState.vel.y;
|
||||
|
||||
// Calculate XY-acceleration limit according to velocity error limit
|
||||
float accelLimitX, accelLimitY;
|
||||
|
|
@ -376,8 +376,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
|
|||
// Apply PID with output limiting and I-term anti-windup
|
||||
// Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit
|
||||
// Thus we don't need to do anything else with calculated acceleration
|
||||
const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.V.X, posControl.actualState.vel.V.X, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
|
||||
const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.V.Y, posControl.actualState.vel.V.Y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
|
||||
const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, posControl.actualState.vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
|
||||
const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, posControl.actualState.vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
|
||||
|
||||
// Save last acceleration target
|
||||
lastAccelTargetX = newAccelX;
|
||||
|
|
@ -484,7 +484,7 @@ bool isMulticopterLandingDetected(void)
|
|||
}
|
||||
|
||||
// Average climb rate should be low enough
|
||||
bool verticalMovement = fabsf(posControl.actualState.vel.V.Z) > 25.0f;
|
||||
bool verticalMovement = fabsf(posControl.actualState.vel.z) > 25.0f;
|
||||
|
||||
// check if we are moving horizontally
|
||||
bool horizontalMovement = posControl.actualState.velXY > 100.0f;
|
||||
|
|
@ -585,13 +585,13 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
/*-----------------------------------------------------------
|
||||
* Calculate loiter target based on current position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void calculateMulticopterInitialHoldPosition(t_fp_vector * pos)
|
||||
void calculateMulticopterInitialHoldPosition(fpVector3_t * pos)
|
||||
{
|
||||
const float stoppingDistanceX = posControl.actualState.vel.V.X * posControl.posDecelerationTime;
|
||||
const float stoppingDistanceY = posControl.actualState.vel.V.Y * posControl.posDecelerationTime;
|
||||
const float stoppingDistanceX = posControl.actualState.vel.x * posControl.posDecelerationTime;
|
||||
const float stoppingDistanceY = posControl.actualState.vel.y * posControl.posDecelerationTime;
|
||||
|
||||
pos->V.X = posControl.actualState.pos.V.X + stoppingDistanceX;
|
||||
pos->V.Y = posControl.actualState.pos.V.Y + stoppingDistanceY;
|
||||
pos->x = posControl.actualState.pos.x + stoppingDistanceX;
|
||||
pos->y = posControl.actualState.pos.y + stoppingDistanceY;
|
||||
}
|
||||
|
||||
void resetMulticopterHeadingController(void)
|
||||
|
|
|
|||
|
|
@ -92,8 +92,8 @@ typedef struct {
|
|||
bool glitchDetected;
|
||||
bool glitchRecovery;
|
||||
#endif
|
||||
t_fp_vector pos; // GPS position in NEU coordinate system (cm)
|
||||
t_fp_vector vel; // GPS velocity (cms)
|
||||
fpVector3_t pos; // GPS position in NEU coordinate system (cm)
|
||||
fpVector3_t vel; // GPS velocity (cms)
|
||||
float eph;
|
||||
float epv;
|
||||
} navPositionEstimatorGPS_t;
|
||||
|
|
@ -133,8 +133,8 @@ typedef struct {
|
|||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
|
||||
// 3D position, velocity and confidence
|
||||
t_fp_vector pos;
|
||||
t_fp_vector vel;
|
||||
fpVector3_t pos;
|
||||
fpVector3_t vel;
|
||||
float eph;
|
||||
float epv;
|
||||
|
||||
|
|
@ -152,8 +152,8 @@ typedef struct {
|
|||
} navPositionEstimatorSTATE_t;
|
||||
|
||||
typedef struct {
|
||||
t_fp_vector accelNEU;
|
||||
t_fp_vector accelBias;
|
||||
fpVector3_t accelNEU;
|
||||
fpVector3_t accelBias;
|
||||
bool gravityCalibrationComplete;
|
||||
} navPosisitonEstimatorIMU_t;
|
||||
|
||||
|
|
@ -211,20 +211,20 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
|
|||
/* Inertial filter, implementation taken from PX4 implementation by Anton Babushkin <rk3dov@gmail.com> */
|
||||
static void inavFilterPredict(int axis, float dt, float acc)
|
||||
{
|
||||
posEstimator.est.pos.A[axis] += posEstimator.est.vel.A[axis] * dt + acc * dt * dt / 2.0f;
|
||||
posEstimator.est.vel.A[axis] += acc * dt;
|
||||
posEstimator.est.pos.v[axis] += posEstimator.est.vel.v[axis] * dt + acc * dt * dt / 2.0f;
|
||||
posEstimator.est.vel.v[axis] += acc * dt;
|
||||
}
|
||||
|
||||
static void inavFilterCorrectPos(int axis, float dt, float e, float w)
|
||||
{
|
||||
float ewdt = e * w * dt;
|
||||
posEstimator.est.pos.A[axis] += ewdt;
|
||||
posEstimator.est.vel.A[axis] += w * ewdt;
|
||||
posEstimator.est.pos.v[axis] += ewdt;
|
||||
posEstimator.est.vel.v[axis] += w * ewdt;
|
||||
}
|
||||
|
||||
static void inavFilterCorrectVel(int axis, float dt, float e, float w)
|
||||
{
|
||||
posEstimator.est.vel.A[axis] += e * w * dt;
|
||||
posEstimator.est.vel.v[axis] += e * w * dt;
|
||||
}
|
||||
|
||||
#define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
|
||||
|
|
@ -278,8 +278,8 @@ static timeUs_t getGPSDeltaTimeFilter(timeUs_t dTus)
|
|||
static bool detectGPSGlitch(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTime = 0;
|
||||
static t_fp_vector lastKnownGoodPosition;
|
||||
static t_fp_vector lastKnownGoodVelocity;
|
||||
static fpVector3_t lastKnownGoodPosition;
|
||||
static fpVector3_t lastKnownGoodVelocity;
|
||||
|
||||
bool isGlitching = false;
|
||||
|
||||
|
|
@ -287,16 +287,16 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
|
|||
isGlitching = false;
|
||||
}
|
||||
else {
|
||||
t_fp_vector predictedGpsPosition;
|
||||
fpVector3_t predictedGpsPosition;
|
||||
float gpsDistance;
|
||||
float dT = US2S(currentTimeUs - previousTime);
|
||||
|
||||
/* We predict new position based on previous GPS velocity and position */
|
||||
predictedGpsPosition.V.X = lastKnownGoodPosition.V.X + lastKnownGoodVelocity.V.X * dT;
|
||||
predictedGpsPosition.V.Y = lastKnownGoodPosition.V.Y + lastKnownGoodVelocity.V.Y * dT;
|
||||
predictedGpsPosition.x = lastKnownGoodPosition.x + lastKnownGoodVelocity.x * dT;
|
||||
predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
|
||||
|
||||
/* New pos is within predefined radius of predicted pos, radius is expanded exponentially */
|
||||
gpsDistance = sqrtf(sq(predictedGpsPosition.V.X - lastKnownGoodPosition.V.X) + sq(predictedGpsPosition.V.Y - lastKnownGoodPosition.V.Y));
|
||||
gpsDistance = sqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y));
|
||||
if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
|
||||
isGlitching = false;
|
||||
}
|
||||
|
|
@ -348,7 +348,7 @@ void onNewGPSData(void)
|
|||
/* Automatic magnetic declination calculation - do this once */
|
||||
static bool magDeclinationSet = false;
|
||||
if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
|
||||
mag.magneticDeclination = geoCalculateMagDeclination(&newLLH) * 10.0f; // heading is in 0.1deg units
|
||||
imuSetMagneticDeclination(geoCalculateMagDeclination(&newLLH));
|
||||
magDeclinationSet = true;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -375,19 +375,19 @@ void onNewGPSData(void)
|
|||
/* Use VELNED provided by GPS if available, calculate from coordinates otherwise */
|
||||
float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f);
|
||||
if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
|
||||
posEstimator.gps.vel.V.X = gpsSol.velNED[0];
|
||||
posEstimator.gps.vel.V.Y = gpsSol.velNED[1];
|
||||
posEstimator.gps.vel.x = gpsSol.velNED[0];
|
||||
posEstimator.gps.vel.y = gpsSol.velNED[1];
|
||||
}
|
||||
else {
|
||||
posEstimator.gps.vel.V.X = (posEstimator.gps.vel.V.X + (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lat - previousLat) / dT)) / 2.0f;
|
||||
posEstimator.gps.vel.V.Y = (posEstimator.gps.vel.V.Y + (gpsScaleLonDown * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lon - previousLon) / dT)) / 2.0f;
|
||||
posEstimator.gps.vel.x = (posEstimator.gps.vel.x + (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lat - previousLat) / dT)) / 2.0f;
|
||||
posEstimator.gps.vel.y = (posEstimator.gps.vel.y + (gpsScaleLonDown * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lon - previousLon) / dT)) / 2.0f;
|
||||
}
|
||||
|
||||
if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelD) {
|
||||
posEstimator.gps.vel.V.Z = - gpsSol.velNED[2]; // NEU
|
||||
posEstimator.gps.vel.z = - gpsSol.velNED[2]; // NEU
|
||||
}
|
||||
else {
|
||||
posEstimator.gps.vel.V.Z = (posEstimator.gps.vel.V.Z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f;
|
||||
posEstimator.gps.vel.z = (posEstimator.gps.vel.z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f;
|
||||
}
|
||||
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
|
|
@ -547,38 +547,38 @@ static void updateIMUTopic(void)
|
|||
static timeMs_t gravityCalibrationTimeout = 0;
|
||||
|
||||
if (!isImuReady()) {
|
||||
posEstimator.imu.accelNEU.V.X = 0;
|
||||
posEstimator.imu.accelNEU.V.Y = 0;
|
||||
posEstimator.imu.accelNEU.V.Z = 0;
|
||||
posEstimator.imu.accelNEU.x = 0;
|
||||
posEstimator.imu.accelNEU.y = 0;
|
||||
posEstimator.imu.accelNEU.z = 0;
|
||||
|
||||
gravityCalibrationTimeout = millis();
|
||||
posEstimator.imu.gravityCalibrationComplete = false;
|
||||
}
|
||||
else {
|
||||
t_fp_vector accelBF;
|
||||
fpVector3_t accelBF;
|
||||
|
||||
/* Read acceleration data in body frame */
|
||||
accelBF.V.X = imuMeasuredAccelBF.V.X;
|
||||
accelBF.V.Y = imuMeasuredAccelBF.V.Y;
|
||||
accelBF.V.Z = imuMeasuredAccelBF.V.Z;
|
||||
accelBF.x = imuMeasuredAccelBF.x;
|
||||
accelBF.y = imuMeasuredAccelBF.y;
|
||||
accelBF.z = imuMeasuredAccelBF.z;
|
||||
|
||||
/* Correct accelerometer bias */
|
||||
accelBF.V.X -= posEstimator.imu.accelBias.V.X;
|
||||
accelBF.V.Y -= posEstimator.imu.accelBias.V.Y;
|
||||
accelBF.V.Z -= posEstimator.imu.accelBias.V.Z;
|
||||
accelBF.x -= posEstimator.imu.accelBias.x;
|
||||
accelBF.y -= posEstimator.imu.accelBias.y;
|
||||
accelBF.z -= posEstimator.imu.accelBias.z;
|
||||
|
||||
/* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/
|
||||
imuTransformVectorBodyToEarth(&accelBF);
|
||||
|
||||
/* Read acceleration data in NEU frame from IMU */
|
||||
posEstimator.imu.accelNEU.V.X = accelBF.V.X;
|
||||
posEstimator.imu.accelNEU.V.Y = accelBF.V.Y;
|
||||
posEstimator.imu.accelNEU.V.Z = accelBF.V.Z;
|
||||
posEstimator.imu.accelNEU.x = accelBF.x;
|
||||
posEstimator.imu.accelNEU.y = accelBF.y;
|
||||
posEstimator.imu.accelNEU.z = accelBF.z;
|
||||
|
||||
/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
|
||||
if (!ARMING_FLAG(ARMED) && !posEstimator.imu.gravityCalibrationComplete) {
|
||||
// Slowly converge on calibrated gravity while level
|
||||
const float gravityOffsetError = posEstimator.imu.accelNEU.V.Z - calibratedGravityCMSS;
|
||||
const float gravityOffsetError = posEstimator.imu.accelNEU.z - calibratedGravityCMSS;
|
||||
calibratedGravityCMSS += gravityOffsetError * 0.0025f;
|
||||
|
||||
if (ABS(gravityOffsetError) < positionEstimationConfig()->gravity_calibration_tolerance) { // Error should be within 0.5% of calibrated gravity
|
||||
|
|
@ -593,19 +593,19 @@ static void updateIMUTopic(void)
|
|||
|
||||
/* If calibration is incomplete - report zero acceleration */
|
||||
if (posEstimator.imu.gravityCalibrationComplete) {
|
||||
posEstimator.imu.accelNEU.V.Z -= calibratedGravityCMSS;
|
||||
posEstimator.imu.accelNEU.z -= calibratedGravityCMSS;
|
||||
}
|
||||
else {
|
||||
posEstimator.imu.accelNEU.V.X = 0;
|
||||
posEstimator.imu.accelNEU.V.Y = 0;
|
||||
posEstimator.imu.accelNEU.V.Z = 0;
|
||||
posEstimator.imu.accelNEU.x = 0;
|
||||
posEstimator.imu.accelNEU.y = 0;
|
||||
posEstimator.imu.accelNEU.z = 0;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
/* Update blackbox values */
|
||||
navAccNEU[X] = posEstimator.imu.accelNEU.A[X];
|
||||
navAccNEU[Y] = posEstimator.imu.accelNEU.A[Y];
|
||||
navAccNEU[Z] = posEstimator.imu.accelNEU.A[Z];
|
||||
navAccNEU[X] = posEstimator.imu.accelNEU.x;
|
||||
navAccNEU[Y] = posEstimator.imu.accelNEU.y;
|
||||
navAccNEU[Z] = posEstimator.imu.accelNEU.z;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
|
@ -667,12 +667,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
/* Do some preparations to data */
|
||||
if (isBaroValid) {
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
posEstimator.state.baroGroundAlt = posEstimator.est.pos.V.Z;
|
||||
posEstimator.state.baroGroundAlt = posEstimator.est.pos.z;
|
||||
posEstimator.state.isBaroGroundValid = true;
|
||||
posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec
|
||||
}
|
||||
else {
|
||||
if (posEstimator.est.vel.V.Z > 15) {
|
||||
if (posEstimator.est.vel.z > 15) {
|
||||
if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
|
||||
posEstimator.state.isBaroGroundValid = false;
|
||||
}
|
||||
|
|
@ -703,14 +703,14 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
|
||||
/* Prediction step: Z-axis */
|
||||
if (isEstZValid) {
|
||||
inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.V.Z);
|
||||
inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.z);
|
||||
}
|
||||
|
||||
/* Prediction step: XY-axis */
|
||||
if (isEstXYValid) {
|
||||
if (navIsHeadingUsable()) {
|
||||
inavFilterPredict(X, dt, posEstimator.imu.accelNEU.V.X);
|
||||
inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.V.Y);
|
||||
inavFilterPredict(X, dt, posEstimator.imu.accelNEU.x);
|
||||
inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.y);
|
||||
}
|
||||
else {
|
||||
inavFilterPredict(X, dt, 0.0f);
|
||||
|
|
@ -720,7 +720,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
|
||||
/* Accelerometer bias correction */
|
||||
const bool updateAccBias = (positionEstimationConfig()->w_acc_bias > 0);
|
||||
t_fp_vector accelBiasCorr = { { 0, 0, 0} };
|
||||
fpVector3_t accelBiasCorr = { { 0, 0, 0} };
|
||||
|
||||
/* Correction step: Z-axis */
|
||||
if (useGpsZPos || isBaroValid) {
|
||||
|
|
@ -728,45 +728,45 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
|
||||
/* Handle Z-axis reset */
|
||||
if (!isEstZValid && useGpsZPos) {
|
||||
posEstimator.est.pos.V.Z = posEstimator.gps.pos.V.Z;
|
||||
posEstimator.est.vel.V.Z = posEstimator.gps.vel.V.Z;
|
||||
posEstimator.est.pos.z = posEstimator.gps.pos.z;
|
||||
posEstimator.est.vel.z = posEstimator.gps.vel.z;
|
||||
newEPV = posEstimator.gps.epv;
|
||||
}
|
||||
else {
|
||||
#if defined(USE_BARO)
|
||||
/* Apply BARO correction to altitude */
|
||||
if (isBaroValid) {
|
||||
const float baroResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.V.Z;
|
||||
const float baroResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
|
||||
inavFilterCorrectPos(Z, dt, baroResidual, positionEstimationConfig()->w_z_baro_p);
|
||||
newEPV = updateEPE(posEstimator.est.epv, dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
|
||||
|
||||
/* accelerometer bias correction for baro */
|
||||
if (updateAccBias && !isAirCushionEffectDetected) {
|
||||
accelBiasCorr.V.Z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p);
|
||||
accelBiasCorr.z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/* Apply GPS correction to altitude */
|
||||
if (useGpsZPos) {
|
||||
const float gpsResidualZ = posEstimator.gps.pos.V.Z - posEstimator.est.pos.V.Z;
|
||||
const float gpsResidualZ = posEstimator.gps.pos.z - posEstimator.est.pos.z;
|
||||
inavFilterCorrectPos(Z, dt, gpsResidualZ, positionEstimationConfig()->w_z_gps_p * gpsWeightScaler);
|
||||
newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, gpsResidualZ), positionEstimationConfig()->w_z_gps_p);
|
||||
|
||||
if (updateAccBias) {
|
||||
accelBiasCorr.V.Z -= gpsResidualZ * sq(positionEstimationConfig()->w_z_gps_p);
|
||||
accelBiasCorr.z -= gpsResidualZ * sq(positionEstimationConfig()->w_z_gps_p);
|
||||
}
|
||||
}
|
||||
|
||||
/* Apply GPS correction to climb rate */
|
||||
if (useGpsZVel) {
|
||||
const float gpsResidualZVel = posEstimator.gps.vel.V.Z - posEstimator.est.vel.V.Z;
|
||||
const float gpsResidualZVel = posEstimator.gps.vel.z - posEstimator.est.vel.z;
|
||||
inavFilterCorrectVel(Z, dt, gpsResidualZVel, positionEstimationConfig()->w_z_gps_v * sq(gpsWeightScaler));
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.V.Z, positionEstimationConfig()->w_z_res_v);
|
||||
inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.z, positionEstimationConfig()->w_z_res_v);
|
||||
}
|
||||
|
||||
/* Correction step: XY-axis */
|
||||
|
|
@ -774,17 +774,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
if (isGPSValid) {
|
||||
/* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */
|
||||
if (!isEstXYValid) {
|
||||
posEstimator.est.pos.V.X = posEstimator.gps.pos.V.X;
|
||||
posEstimator.est.pos.V.Y = posEstimator.gps.pos.V.Y;
|
||||
posEstimator.est.vel.V.X = posEstimator.gps.vel.V.X;
|
||||
posEstimator.est.vel.V.Y = posEstimator.gps.vel.V.Y;
|
||||
posEstimator.est.pos.x = posEstimator.gps.pos.x;
|
||||
posEstimator.est.pos.y = posEstimator.gps.pos.y;
|
||||
posEstimator.est.vel.x = posEstimator.gps.vel.x;
|
||||
posEstimator.est.vel.y = posEstimator.gps.vel.y;
|
||||
newEPH = posEstimator.gps.eph;
|
||||
}
|
||||
else {
|
||||
const float gpsResidualX = posEstimator.gps.pos.V.X - posEstimator.est.pos.V.X;
|
||||
const float gpsResidualY = posEstimator.gps.pos.V.Y - posEstimator.est.pos.V.Y;
|
||||
const float gpsResidualXVel = posEstimator.gps.vel.V.X - posEstimator.est.vel.V.X;
|
||||
const float gpsResidualYVel = posEstimator.gps.vel.V.Y - posEstimator.est.vel.V.Y;
|
||||
const float gpsResidualX = posEstimator.gps.pos.x - posEstimator.est.pos.x;
|
||||
const float gpsResidualY = posEstimator.gps.pos.y - posEstimator.est.pos.y;
|
||||
const float gpsResidualXVel = posEstimator.gps.vel.x - posEstimator.est.vel.x;
|
||||
const float gpsResidualYVel = posEstimator.gps.vel.y - posEstimator.est.vel.y;
|
||||
const float gpsResidualXYMagnitude = sqrtf(sq(gpsResidualX) + sq(gpsResidualY));
|
||||
|
||||
//const float gpsWeightScaler = scaleRangef(bellCurve(gpsResidualXYMagnitude, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
|
|
@ -804,21 +804,21 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
}
|
||||
}
|
||||
else {
|
||||
inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.V.X, positionEstimationConfig()->w_xy_res_v);
|
||||
inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.V.Y, positionEstimationConfig()->w_xy_res_v);
|
||||
inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.x, positionEstimationConfig()->w_xy_res_v);
|
||||
inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.y, positionEstimationConfig()->w_xy_res_v);
|
||||
}
|
||||
|
||||
/* Correct accelerometer bias */
|
||||
if (updateAccBias) {
|
||||
const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.V.X) + sq(accelBiasCorr.V.Y) + sq(accelBiasCorr.V.Z);
|
||||
const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);
|
||||
if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
|
||||
/* transform error vector from NEU frame to body frame */
|
||||
imuTransformVectorEarthToBody(&accelBiasCorr);
|
||||
|
||||
/* Correct accel bias */
|
||||
posEstimator.imu.accelBias.V.X += accelBiasCorr.V.X * positionEstimationConfig()->w_acc_bias * dt;
|
||||
posEstimator.imu.accelBias.V.Y += accelBiasCorr.V.Y * positionEstimationConfig()->w_acc_bias * dt;
|
||||
posEstimator.imu.accelBias.V.Z += accelBiasCorr.V.Z * positionEstimationConfig()->w_acc_bias * dt;
|
||||
posEstimator.imu.accelBias.x += accelBiasCorr.x * positionEstimationConfig()->w_acc_bias * dt;
|
||||
posEstimator.imu.accelBias.y += accelBiasCorr.y * positionEstimationConfig()->w_acc_bias * dt;
|
||||
posEstimator.imu.accelBias.z += accelBiasCorr.z * positionEstimationConfig()->w_acc_bias * dt;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -875,8 +875,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
if (resetSurfaceEstimate) {
|
||||
posEstimator.est.aglAlt = posEstimator.surface.alt;
|
||||
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.V.Z - posEstimator.surface.alt;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
|
||||
}
|
||||
else {
|
||||
posEstimator.est.aglVel = 0;
|
||||
|
|
@ -885,8 +885,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Update estimate
|
||||
posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.V.Z * dt * dt * 0.5f;
|
||||
posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.V.Z * dt;
|
||||
posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.z * dt * dt * 0.5f;
|
||||
posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * dt;
|
||||
|
||||
// Apply correction
|
||||
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
|
||||
|
|
@ -899,12 +899,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
|
||||
// Update estimate offset
|
||||
if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.V.Z - posEstimator.surface.alt;
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
|
||||
}
|
||||
}
|
||||
else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
|
||||
// Correct estimate from altitude fused from rangefinder and global altitude
|
||||
const float estAltResidual = (posEstimator.est.pos.V.Z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
|
||||
const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
|
||||
const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
|
||||
const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
|
||||
|
|
@ -914,13 +914,13 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
}
|
||||
else { // SURFACE_QUAL_LOW
|
||||
// In this case rangefinder can't be trusted - simply use global altitude
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
}
|
||||
}
|
||||
else {
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
}
|
||||
|
||||
|
|
@ -932,8 +932,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
#endif
|
||||
|
||||
#else
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
#endif
|
||||
}
|
||||
|
|
@ -953,19 +953,19 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
|
||||
/* Publish position update */
|
||||
if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
|
||||
updateActualHorizontalPositionAndVelocity(true, posEstimator.est.pos.V.X, posEstimator.est.pos.V.Y, posEstimator.est.vel.V.X, posEstimator.est.vel.V.Y);
|
||||
updateActualHorizontalPositionAndVelocity(true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
|
||||
}
|
||||
else {
|
||||
updateActualHorizontalPositionAndVelocity(false, posEstimator.est.pos.V.X, posEstimator.est.pos.V.Y, 0, 0);
|
||||
updateActualHorizontalPositionAndVelocity(false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
|
||||
}
|
||||
|
||||
/* Publish altitude update and set altitude validity */
|
||||
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED;
|
||||
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.V.Z, posEstimator.est.vel.V.Z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus);
|
||||
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus);
|
||||
}
|
||||
else {
|
||||
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.V.Z, 0, posEstimator.est.aglAlt, 0, EST_NONE);
|
||||
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE);
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
|
|
@ -1003,9 +1003,9 @@ void initializePositionEstimator(void)
|
|||
posEstimator.imu.gravityCalibrationComplete = false;
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
posEstimator.imu.accelBias.A[axis] = 0;
|
||||
posEstimator.est.pos.A[axis] = 0;
|
||||
posEstimator.est.vel.A[axis] = 0;
|
||||
posEstimator.imu.accelBias.v[axis] = 0;
|
||||
posEstimator.est.pos.v[axis] = 0;
|
||||
posEstimator.est.vel.v[axis] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -127,8 +127,8 @@ typedef struct navigationPIDControllers_s {
|
|||
} navigationPIDControllers_t;
|
||||
|
||||
typedef struct {
|
||||
t_fp_vector pos;
|
||||
t_fp_vector vel;
|
||||
fpVector3_t pos;
|
||||
fpVector3_t vel;
|
||||
int32_t yaw;
|
||||
float sinYaw;
|
||||
float cosYaw;
|
||||
|
|
@ -139,8 +139,8 @@ typedef struct {
|
|||
} navigationEstimatedState_t;
|
||||
|
||||
typedef struct {
|
||||
t_fp_vector pos;
|
||||
t_fp_vector vel;
|
||||
fpVector3_t pos;
|
||||
fpVector3_t vel;
|
||||
int32_t yaw;
|
||||
float surface;
|
||||
} navigationDesiredState_t;
|
||||
|
|
@ -251,7 +251,7 @@ typedef struct {
|
|||
|
||||
typedef struct {
|
||||
timeMs_t lastCheckTime;
|
||||
t_fp_vector initialPosition;
|
||||
fpVector3_t initialPosition;
|
||||
float minimalDistanceToHome;
|
||||
} rthSanityChecker_t;
|
||||
|
||||
|
|
@ -307,15 +307,15 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD);
|
|||
void navPInit(pController_t *p, float _kP);
|
||||
|
||||
bool isThrustFacingDownwards(void);
|
||||
uint32_t calculateDistanceToDestination(const t_fp_vector * destinationPos);
|
||||
int32_t calculateBearingToDestination(const t_fp_vector * destinationPos);
|
||||
uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
|
||||
int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
|
||||
void resetLandingDetector(void);
|
||||
bool isLandingDetected(void);
|
||||
|
||||
navigationFSMStateFlags_t navGetCurrentStateFlags(void);
|
||||
|
||||
void setHomePosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask);
|
||||
void setDesiredPosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask);
|
||||
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
|
||||
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
|
||||
void setDesiredSurfaceOffset(float surfaceOffset);
|
||||
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);
|
||||
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode);
|
||||
|
|
@ -352,7 +352,7 @@ void resetMulticopterLandingDetector(void);
|
|||
bool isMulticopterLandingDetected(void);
|
||||
bool isFixedWingLandingDetected(void);
|
||||
|
||||
void calculateMulticopterInitialHoldPosition(t_fp_vector * pos);
|
||||
void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
|
||||
|
||||
/* Fixed-wing specific functions */
|
||||
void setupFixedWingAltitudeController(void);
|
||||
|
|
@ -367,7 +367,7 @@ bool adjustFixedWingPositionFromRCInput(void);
|
|||
|
||||
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
|
||||
|
||||
void calculateFixedWingInitialHoldPosition(t_fp_vector * pos);
|
||||
void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
|
||||
|
||||
/* Fixed-wing launch controller */
|
||||
void resetFixedWingLaunchController(timeUs_t currentTimeUs);
|
||||
|
|
|
|||
|
|
@ -476,24 +476,24 @@ static void accUpdateAccumulatedMeasurements(void)
|
|||
/*
|
||||
* Calculate measured acceleration in body frame in g
|
||||
*/
|
||||
void accGetMeasuredAcceleration(t_fp_vector *measuredAcc)
|
||||
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
|
||||
{
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
if (accumulatedMeasurementCount) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
measuredAcc->A[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount;
|
||||
measuredAcc->v[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount;
|
||||
accumulatedMeasurements[axis] = 0;
|
||||
}
|
||||
accumulatedMeasurementCount = 0;
|
||||
}
|
||||
else {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
measuredAcc->A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
|
||||
measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
|
||||
}
|
||||
}
|
||||
#else
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
measuredAcc->A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
|
||||
measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
|
@ -65,7 +66,7 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
|
|||
bool accInit(uint32_t accTargetLooptime);
|
||||
bool accIsCalibrationComplete(void);
|
||||
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||
void accGetMeasuredAcceleration(t_fp_vector *measuredAcc);
|
||||
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
|
||||
void accUpdate(void);
|
||||
void accSetCalibrationValues(void);
|
||||
void accInitFilters(void);
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include <string.h>
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
|
|
@ -31,7 +32,7 @@
|
|||
#include "boardalignment.h"
|
||||
|
||||
static bool standardBoardAlignment = true; // board orientation correction
|
||||
static float boardRotation[3][3]; // matrix
|
||||
static fpMat3_t boardRotMatrix;
|
||||
|
||||
// no template required since defaults are zero
|
||||
PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0);
|
||||
|
|
@ -54,7 +55,7 @@ void initBoardAlignment(void)
|
|||
rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(boardAlignment()->pitchDeciDegrees);
|
||||
rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees );
|
||||
|
||||
buildRotationMatrix(&rotationAngles, boardRotation);
|
||||
rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -75,13 +76,12 @@ void applyBoardAlignment(int32_t *vec)
|
|||
return;
|
||||
}
|
||||
|
||||
int32_t x = vec[X];
|
||||
int32_t y = vec[Y];
|
||||
int32_t z = vec[Z];
|
||||
fpVector3_t fpVec = { .v = { vec[X], vec[Y], vec[Z] } };
|
||||
rotationMatrixRotateVector(&fpVec, &fpVec, &boardRotMatrix);
|
||||
|
||||
vec[X] = lrintf(boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z);
|
||||
vec[Y] = lrintf(boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z);
|
||||
vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
|
||||
vec[X] = lrintf(fpVec.x);
|
||||
vec[Y] = lrintf(fpVec.y);
|
||||
vec[Z] = lrintf(fpVec.z);
|
||||
}
|
||||
|
||||
void applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation)
|
||||
|
|
|
|||
|
|
@ -77,7 +77,6 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
|
|||
|
||||
#ifdef USE_MAG
|
||||
|
||||
static uint8_t magInit = 0;
|
||||
static uint8_t magUpdatedAtLeastOnce = 0;
|
||||
|
||||
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
|
||||
|
|
@ -264,15 +263,12 @@ bool compassInit(void)
|
|||
LED1_ON;
|
||||
const bool ret = mag.dev.init(&mag.dev);
|
||||
LED1_OFF;
|
||||
if (ret) {
|
||||
const int deg = compassConfig()->mag_declination / 100;
|
||||
const int min = compassConfig()->mag_declination % 100;
|
||||
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
|
||||
magInit = 1;
|
||||
} else {
|
||||
|
||||
if (!ret) {
|
||||
addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR);
|
||||
sensorsClear(SENSOR_MAG);
|
||||
}
|
||||
|
||||
if (compassConfig()->mag_align != ALIGN_DEFAULT) {
|
||||
mag.dev.magAlign = compassConfig()->mag_align;
|
||||
}
|
||||
|
|
@ -329,12 +325,6 @@ void compassUpdate(timeUs_t currentTimeUs)
|
|||
DISABLE_STATE(CALIBRATE_MAG);
|
||||
}
|
||||
|
||||
if (magInit) { // we apply offset only once mag calibration is done
|
||||
mag.magADC[X] -= compassConfig()->magZero.raw[X];
|
||||
mag.magADC[Y] -= compassConfig()->magZero.raw[Y];
|
||||
mag.magADC[Z] -= compassConfig()->magZero.raw[Z];
|
||||
}
|
||||
|
||||
if (calStartedAt != 0) {
|
||||
if ((currentTimeUs - calStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) {
|
||||
LED0_TOGGLE;
|
||||
|
|
@ -367,6 +357,11 @@ void compassUpdate(timeUs_t currentTimeUs)
|
|||
saveConfigAndNotify();
|
||||
}
|
||||
}
|
||||
else {
|
||||
mag.magADC[X] -= compassConfig()->magZero.raw[X];
|
||||
mag.magADC[Y] -= compassConfig()->magZero.raw[Y];
|
||||
mag.magADC[Z] -= compassConfig()->magZero.raw[Z];
|
||||
}
|
||||
|
||||
applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign);
|
||||
applyBoardAlignment(mag.magADC);
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@ typedef enum {
|
|||
|
||||
typedef struct mag_s {
|
||||
magDev_t dev;
|
||||
float magneticDeclination;
|
||||
int32_t magADC[XYZ_AXIS_COUNT];
|
||||
} mag_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -375,18 +375,18 @@ static void gyroUpdateAccumulatedRates(timeDelta_t gyroUpdateDeltaUs)
|
|||
/*
|
||||
* Calculate rotation rate in rad/s in body frame
|
||||
*/
|
||||
void gyroGetMeasuredRotationRate(t_fp_vector *measuredRotationRate)
|
||||
void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
|
||||
{
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
const float accumulatedRateTime = accumulatedRateTimeUs * 1e-6;
|
||||
accumulatedRateTimeUs = 0;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
measuredRotationRate->A[axis] = DEGREES_TO_RADIANS(accumulatedRates[axis] / accumulatedRateTime);
|
||||
measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(accumulatedRates[axis] / accumulatedRateTime);
|
||||
accumulatedRates[axis] = 0.0f;
|
||||
}
|
||||
#else
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
measuredRotationRate->A[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]);
|
||||
measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/vector.h"
|
||||
#include "common/time.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
|
@ -62,7 +63,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
|
|||
|
||||
bool gyroInit(void);
|
||||
void gyroInitFilters(void);
|
||||
void gyroGetMeasuredRotationRate(t_fp_vector *imuMeasuredRotationBF);
|
||||
void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF);
|
||||
void gyroUpdate(timeDelta_t gyroUpdateDeltaUs);
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||
bool gyroIsCalibrationComplete(void);
|
||||
|
|
|
|||
|
|
@ -61,8 +61,6 @@ bool sensorsAutodetect(void)
|
|||
pitotInit();
|
||||
#endif
|
||||
|
||||
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
|
||||
mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
|
||||
#ifdef USE_MAG
|
||||
compassInit();
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue