Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 06:15:14 +03:00
Code Issues Wiki Activity

Initial cut on full quaternion IMU conversion (#2894)

Browse source 
* 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 2018-03-15 00:19:53 +10:00 • committed by GitHub
parent 0ede6d52d6
commit e174e5a48d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
23 changed files with 801 additions and 498 deletions
Download patch file Download diff file Expand all files Collapse all files

82
src/main/common/maths.c
View file

@ -21,6 +21,8 @@
#include "axis.h" #include "axis.h"
#include "maths.h" #include "maths.h"
#include "vector.h"
#include "quaternion.h"
// http://lolengine.net/blog/2011/12/21/better-function-approximations // http://lolengine.net/blog/2011/12/21/better-function-approximations
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117 // 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); return ((a / b) + destMin);
} }
// Normalize a vector // Build rMat from Tait–Bryan angles (convention X1, Y2, Z3)
void normalizeV(struct fp_vector *src, struct fp_vector *dest) void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles)
{
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])
{ {
float cosx, sinx, cosy, siny, cosz, sinz; float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx; float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cos_approx(delta->angles.roll); cosx = cos_approx(angles->angles.roll);
sinx = sin_approx(delta->angles.roll); sinx = sin_approx(angles->angles.roll);
cosy = cos_approx(delta->angles.pitch); cosy = cos_approx(angles->angles.pitch);
siny = sin_approx(delta->angles.pitch); siny = sin_approx(angles->angles.pitch);
cosz = cos_approx(delta->angles.yaw); cosz = cos_approx(angles->angles.yaw);
sinz = sin_approx(delta->angles.yaw); sinz = sin_approx(angles->angles.yaw);
coszcosx = cosz * cosx; coszcosx = cosz * cosx;
sinzcosx = sinz * cosx; sinzcosx = sinz * cosx;
coszsinx = sinx * cosz; coszsinx = sinx * cosz;
sinzsinx = sinx * sinz; sinzsinx = sinx * sinz;
matrix[0][X] = cosz * cosy; rmat->m[0][X] = cosz * cosy;
matrix[0][Y] = -cosy * sinz; rmat->m[0][Y] = -cosy * sinz;
matrix[0][Z] = siny; rmat->m[0][Z] = siny;
matrix[1][X] = sinzcosx + (coszsinx * siny); rmat->m[1][X] = sinzcosx + (coszsinx * siny);
matrix[1][Y] = coszcosx - (sinzsinx * siny); rmat->m[1][Y] = coszcosx - (sinzsinx * siny);
matrix[1][Z] = -sinx * cosy; rmat->m[1][Z] = -sinx * cosy;
matrix[2][X] = (sinzsinx) - (coszcosx * siny); rmat->m[2][X] = (sinzsinx) - (coszcosx * siny);
matrix[2][Y] = (coszsinx) + (sinzcosx * siny); rmat->m[2][Y] = (coszsinx) + (sinzcosx * siny);
matrix[2][Z] = cosy * cosx; rmat->m[2][Z] = cosy * cosx;
} }
// Rotate a vector *v by the euler angles defined by the 3-vector *delta. void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a)
void rotateV(struct fp_vector *v, fp_angles_t *delta)
{ {
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]; rmat->m[1][X] = zxC + ys;
v->Y = v_tmp.X * matrix[0][Y] + v_tmp.Y * matrix[1][Y] + v_tmp.Z * matrix[2][Y]; rmat->m[1][Y] = yyC + cang;
v->Z = v_tmp.X * matrix[0][Z] + v_tmp.Y * matrix[1][Z] + v_tmp.Z * matrix[2][Z]; 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 // Quick median filter implementation

30
src/main/common/maths.h
View file

@ -81,26 +81,7 @@
#define _ABS_I(x, var) _ABS_II(x, var) #define _ABS_I(x, var) _ABS_II(x, var)
#define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__)) #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. // Floating point Euler angles.
// Be carefull, could be either of degrees or radians.
typedef struct fp_angles { typedef struct fp_angles {
float roll; float roll;
float pitch; float pitch;
@ -112,6 +93,12 @@ typedef union {
fp_angles_def angles; fp_angles_def angles;
} fp_angles_t; } 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 { typedef struct filterWithBufferSample_s {
float value; float value;
uint32_t timestamp; uint32_t timestamp;
@ -149,11 +136,6 @@ float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax); int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
float scaleRangef(float x, float srcMin, float srcMax, float destMin, float 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_18000(int32_t angle);
int32_t wrap_36000(int32_t angle); int32_t wrap_36000(int32_t angle);

199
src/main/common/quaternion.h Normal file
View 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
View 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;
}

312
src/main/flight/imu.c
View file

@ -26,10 +26,13 @@
#include "blackbox/blackbox.h" #include "blackbox/blackbox.h"
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/filter.h" #include "common/filter.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
#include "common/quaternion.h"
#include "config/feature.h" #include "config/feature.h"
#include "config/parameter_group.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_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 #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 fpVector3_t imuMeasuredAccelBF;
FASTRAM t_fp_vector imuMeasuredRotationBF; FASTRAM fpVector3_t imuMeasuredRotationBF;
STATIC_FASTRAM float smallAngleCosZ; STATIC_FASTRAM float smallAngleCosZ;
STATIC_FASTRAM bool isAccelUpdatedAtLeastOnce; 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 FASTRAM fpQuaternion_t orientation;
STATIC_FASTRAM_UNIT_TESTED float rMat[3][3];
FASTRAM attitudeEulerAngles_t attitude; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 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; STATIC_FASTRAM imuRuntimeConfig_t imuRuntimeConfig;
@ -101,16 +104,16 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
{ {
float q1q1 = q1 * q1; float q1q1 = orientation.q1 * orientation.q1;
float q2q2 = q2 * q2; float q2q2 = orientation.q2 * orientation.q2;
float q3q3 = q3 * q3; float q3q3 = orientation.q3 * orientation.q3;
float q0q1 = q0 * q1; float q0q1 = orientation.q0 * orientation.q1;
float q0q2 = q0 * q2; float q0q2 = orientation.q0 * orientation.q2;
float q0q3 = q0 * q3; float q0q3 = orientation.q0 * orientation.q3;
float q1q2 = q1 * q2; float q1q2 = orientation.q1 * orientation.q2;
float q1q3 = q1 * q3; float q1q3 = orientation.q1 * orientation.q3;
float q2q3 = q2 * q3; float q2q3 = orientation.q2 * orientation.q3;
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3; rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 + -q0q3); rMat[0][1] = 2.0f * (q1q2 + -q0q3);
@ -139,49 +142,46 @@ void imuInit(void)
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle)); smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuMeasuredAccelBF.A[axis] = 0; imuMeasuredAccelBF.v[axis] = 0;
} }
// Explicitly initialize FASTRAM statics // Explicitly initialize FASTRAM statics
isAccelUpdatedAtLeastOnce = false; isAccelUpdatedAtLeastOnce = false;
gpsHeadingInitialized = 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(); imuComputeRotationMatrix();
} }
void imuTransformVectorBodyToEarth(t_fp_vector * v) void imuSetMagneticDeclination(float declinationDeg)
{ {
/* From body frame to earth frame */ const float declinationRad = -DEGREES_TO_RADIANS(declinationDeg);
const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z; vCorrectedMagNorth.x = cos_approx(declinationRad);
const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z; vCorrectedMagNorth.y = sin_approx(declinationRad);
const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z; vCorrectedMagNorth.z = 0;
v->V.X = x;
v->V.Y = -y;
v->V.Z = z;
} }
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 */ // HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)
const float x = rMat[0][0] * v->V.X + rMat[1][0] * v->V.Y + rMat[2][0] * v->V.Z; v->y = -v->y;
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;
} }
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) #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 cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
const float sinYaw = sin_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; orientation.q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; orientation.q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; orientation.q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; orientation.q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
imuComputeRotationMatrix(); imuComputeRotationMatrix();
} }
@ -224,34 +224,35 @@ 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; orientation.q0 = accBF->z + accNorm;
q1 = ay; orientation.q1 = accBF->y;
q2 = -ax; orientation.q2 = -accBF->x;
q3 = 0.0f; orientation.q3 = 0.0f;
const float recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); quaternionNormalize(&orientation, &orientation);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
} }
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 // 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 // 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)); if (!isnan(check) && !isinf(check)) {
const bool isInf = (isinf(q0) || isinf(q1) || isinf(q2) || isinf(q3)); return;
const bool isZero = (ABS(q0) < 1e-3f && ABS(q1) < 1e-3f && ABS(q2) < 1e-3f && ABS(q3) < 1e-3f); }
if (isNan || isZero || isInf) { const float normSq = quaternionNormSqared(&orientation);
imuResetOrientationQuaternion(ax, ay, az); if (normSq > (1.0f - 1e-6f) && normSq < (1.0f + 1e-6f)) {
return;
}
imuResetOrientationQuaternion(accBF);
DEBUG_TRACE("AHRS orientation quaternion error");
#ifdef USE_BLACKBOX #ifdef USE_BLACKBOX
if (feature(FEATURE_BLACKBOX)) { if (feature(FEATURE_BLACKBOX)) {
@ -259,152 +260,153 @@ static void imuCheckAndResetOrientationQuaternion(const float ax, const float ay
} }
#endif #endif
} }
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround)
bool useAcc, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
bool useCOG, float courseOverGround)
{ {
STATIC_FASTRAM float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; // integral error terms scaled by Ki STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
STATIC_FASTRAM float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; // integral error terms scaled by Ki
float ex, ey, ez; fpVector3_t vRotation = *gyroBF;
/* Calculate general spin rate (rad/s) */ /* 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 */ /* Step 1: Yaw correction */
// Use measured magnetic field vector // Use measured magnetic field vector
if (useMag || useCOG) { if (magBF || useCOG) {
float kpMag = imuRuntimeConfig.dcm_kp_mag * imuGetPGainScaleFactor(); static const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } };
const float magMagnitudeSq = mx * mx + my * my + mz * mz;
if (useMag && magMagnitudeSq > 0.01f) { fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
// Normalise magnetometer measurement
const float magRecipNorm = invSqrt(magMagnitudeSq); if (magBF && vectorNormSquared(magBF) > 0.01f) {
mx *= magRecipNorm; fpVector3_t vMag;
my *= magRecipNorm;
mz *= magRecipNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). // 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 // 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) // (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) // This should yield direction to magnetic North (1; 0; 0)
const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz; quaternionRotateVectorInv(&vMag, magBF, &orientation); // BF -> EF
const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
const float bx = sqrtf(hx * hx + hy * hy);
// 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) // 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 // Rotate error back into body frame
ex = rMat[2][0] * ez_ef; quaternionRotateVector(&vErr, &vErr, &orientation);
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
} }
else if (useCOG) { else if (useCOG) {
fpVector3_t vHeadingEF;
// Use raw heading error (from GPS or whatever else) // 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);
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 // William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23
// (Rxx; Ryx) - measured (estimated) heading vector (EF) // (Rxx; Ryx) - measured (estimated) heading vector (EF)
// (cos(COG), sin(COG)) - reference 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];
ex = rMat[2][0] * ez_ef; // Compute heading vector in EF from scalar CoG
ey = rMat[2][1] * ez_ef; fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } };
ez = rMat[2][2] * ez_ef;
} // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
else { quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
ex = 0; vHeadingEF.z = 0.0f;
ey = 0;
ez = 0; // 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 // Compute and apply integral feedback if enabled
if (imuRuntimeConfig.dcm_ki_mag > 0.0f) { if (imuRuntimeConfig.dcm_ki_mag > 0.0f) {
// Stop integrating if spinning beyond the certain limit // Stop integrating if spinning beyond the certain limit
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) { if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
integralMagX += imuRuntimeConfig.dcm_ki_mag * ex * dt; // integral error scaled by Ki fpVector3_t vTmp;
integralMagY += imuRuntimeConfig.dcm_ki_mag * ey * dt;
integralMagZ += imuRuntimeConfig.dcm_ki_mag * ez * dt;
gx += integralMagX; // integral error scaled by Ki
gy += integralMagY; vectorScale(&vTmp, &vErr, imuRuntimeConfig.dcm_ki_mag * dt);
gz += integralMagZ; vectorAdd(&vGyroDriftEstimate, &vGyroDriftEstimate, &vTmp);
} }
} }
// Calculate kP gain and apply proportional feedback // Calculate kP gain and apply proportional feedback
gx += kpMag * ex; vectorScale(&vErr, &vErr, imuRuntimeConfig.dcm_kp_mag * imuGetPGainScaleFactor());
gy += kpMag * ey; vectorAdd(&vRotation, &vRotation, &vErr);
gz += kpMag * ez;
} }
/* Step 2: Roll and pitch correction - use measured acceleration vector */ /* Step 2: Roll and pitch correction - use measured acceleration vector */
if (useAcc) { if (accBF) {
float kpAcc = imuRuntimeConfig.dcm_kp_acc * imuGetPGainScaleFactor(); static const fpVector3_t vGravity = { .v = { 0.0f, 0.0f, 1.0f } };
const float accRecipNorm = invSqrt(ax * ax + ay * ay + az * az); fpVector3_t vEstGravity, vAcc, vErr;
// Just scale by 1G length - That's our vector adjustment. Rather than // Calculate estimated gravity vector in body frame
// using one-over-exact length (which needs a costly square root), we already quaternionRotateVector(&vEstGravity, &vGravity, &orientation); // EF -> BF
// 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;
// Error is sum of cross product between estimated direction and measured direction of gravity // Error is sum of cross product between estimated direction and measured direction of gravity
ex = (ay * rMat[2][2] - az * rMat[2][1]); vectorNormalize(&vAcc, accBF);
ey = (az * rMat[2][0] - ax * rMat[2][2]); vectorCrossProduct(&vErr, &vAcc, &vEstGravity);
ez = (ax * rMat[2][1] - ay * rMat[2][0]);
// Compute and apply integral feedback if enabled // Compute and apply integral feedback if enabled
if (imuRuntimeConfig.dcm_ki_acc > 0.0f) { if (imuRuntimeConfig.dcm_ki_acc > 0.0f) {
// Stop integrating if spinning beyond the certain limit // Stop integrating if spinning beyond the certain limit
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) { if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
integralAccX += imuRuntimeConfig.dcm_ki_acc * ex * dt; // integral error scaled by Ki fpVector3_t vTmp;
integralAccY += imuRuntimeConfig.dcm_ki_acc * ey * dt;
integralAccZ += imuRuntimeConfig.dcm_ki_acc * ez * dt;
gx += integralAccX; // integral error scaled by Ki
gy += integralAccY; vectorScale(&vTmp, &vErr, imuRuntimeConfig.dcm_ki_acc * dt);
gz += integralAccZ; vectorAdd(&vGyroDriftEstimate, &vGyroDriftEstimate, &vTmp);
} }
} }
// Calculate kP gain and apply proportional feedback // Calculate kP gain and apply proportional feedback
gx += kpAcc * ex; vectorScale(&vErr, &vErr, imuRuntimeConfig.dcm_kp_acc * imuGetPGainScaleFactor());
gy += kpAcc * ey; vectorAdd(&vRotation, &vRotation, &vErr);
gz += kpAcc * ez;
} }
// Apply gyro drift correction
vectorAdd(&vRotation, &vRotation, &vGyroDriftEstimate);
// Integrate rate of change of quaternion // Integrate rate of change of quaternion
gx *= (0.5f * dt); fpVector3_t vTheta;
gy *= (0.5f * dt); fpQuaternion_t deltaQ;
gz *= (0.5f * dt);
const float qa = q0; vectorScale(&vTheta, &vRotation, 0.5f * dt);
const float qb = q1; quaternionInitFromVector(&deltaQ, &vTheta);
const float qc = q2; const float thetaMagnitudeSq = vectorNormSquared(&vTheta);
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);
// Normalise quaternion // Calculate quaternion delta:
const float quatRecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); // Theta is a axis/angle rotation. Direction of a vector is axis, magnitude is angle/2.
q0 *= quatRecipNorm; // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
q1 *= quatRecipNorm; // 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 -
q2 *= quatRecipNorm; // then we can safely use the "low angle" approximated version without loss of accuracy.
q3 *= quatRecipNorm; 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 // Check for invalid quaternion
imuCheckAndResetOrientationQuaternion(ax, ay, az); imuCheckAndResetOrientationQuaternion(accBF);
// Pre-compute rotation matrix from quaternion // Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix(); imuComputeRotationMatrix();
@ -415,13 +417,13 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
/* Compute pitch/roll angles */ /* Compute pitch/roll angles */
attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2])); 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.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) if (attitude.values.yaw < 0)
attitude.values.yaw += 3600; attitude.values.yaw += 3600;
/* Update small angle state */ /* Update small angle state */
if (rMat[2][2] > smallAngleCosZ) { if (calculateCosTiltAngle() > smallAngleCosZ) {
ENABLE_STATE(SMALL_ANGLE); ENABLE_STATE(SMALL_ANGLE);
} else { } else {
DISABLE_STATE(SMALL_ANGLE); DISABLE_STATE(SMALL_ANGLE);
@ -500,9 +502,11 @@ static void imuCalculateEstimatedAttitude(float dT)
} }
#endif #endif
imuMahonyAHRSupdate(dT, imuMeasuredRotationBF.A[X], imuMeasuredRotationBF.A[Y], imuMeasuredRotationBF.A[Z], fpVector3_t measuredMagBF = { .v = { mag.magADC[X], mag.magADC[Y], mag.magADC[Z] } };
useAcc, imuMeasuredAccelBF.A[X], imuMeasuredAccelBF.A[Y], imuMeasuredAccelBF.A[Z],
useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z], imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF,
useAcc ? &imuMeasuredAccelBF : NULL,
useMag ? &measuredMagBF : NULL,
useCOG, courseOverGround); useCOG, courseOverGround);
imuUpdateEulerAngles(); imuUpdateEulerAngles();
@ -583,5 +587,5 @@ bool isImuHeadingValid(void)
float calculateCosTiltAngle(void) float calculateCosTiltAngle(void)
{ {
return rMat[2][2]; return 1.0f - 2.0f * sq(orientation.q1) - 2.0f * sq(orientation.q2);
} }

12
src/main/flight/imu.h
View file

@ -19,11 +19,13 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
#include "common/quaternion.h"
#include "common/time.h" #include "common/time.h"
#include "config/parameter_group.h" #include "config/parameter_group.h"
extern t_fp_vector imuMeasuredAccelBF; // cm/s/s extern fpVector3_t imuMeasuredAccelBF; // cm/s/s
extern t_fp_vector imuMeasuredRotationBF; // rad/s extern fpVector3_t imuMeasuredRotationBF; // rad/s
typedef union { typedef union {
int16_t raw[XYZ_AXIS_COUNT]; int16_t raw[XYZ_AXIS_COUNT];
@ -35,6 +37,7 @@ typedef union {
} values; } values;
} attitudeEulerAngles_t; } attitudeEulerAngles_t;
extern fpQuaternion_t orientation;
extern attitudeEulerAngles_t attitude; extern attitudeEulerAngles_t attitude;
typedef struct imuConfig_s { typedef struct imuConfig_s {
@ -57,13 +60,14 @@ typedef struct imuRuntimeConfig_s {
void imuConfigure(void); void imuConfigure(void);
void imuSetMagneticDeclination(float declinationDeg);
void imuUpdateAttitude(timeUs_t currentTimeUs); void imuUpdateAttitude(timeUs_t currentTimeUs);
void imuUpdateAccelerometer(void); void imuUpdateAccelerometer(void);
float calculateCosTiltAngle(void); float calculateCosTiltAngle(void);
bool isImuReady(void); bool isImuReady(void);
bool isImuHeadingValid(void); bool isImuHeadingValid(void);
void imuTransformVectorBodyToEarth(t_fp_vector * v); void imuTransformVectorBodyToEarth(fpVector3_t * v);
void imuTransformVectorEarthToBody(t_fp_vector * v); void imuTransformVectorEarthToBody(fpVector3_t * v);
void imuInit(void); void imuInit(void);

18
src/main/flight/pid.c
View file

@ -638,9 +638,9 @@ float pidHeadingHold(void)
*/ */
static void pidTurnAssistant(pidState_t *pidState) static void pidTurnAssistant(pidState_t *pidState)
{ {
t_fp_vector targetRates; fpVector3_t targetRates;
targetRates.V.X = 0.0f; targetRates.x = 0.0f;
targetRates.V.Y = 0.0f; targetRates.y = 0.0f;
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
if (calculateCosTiltAngle() >= 0.173648f) { if (calculateCosTiltAngle() >= 0.173648f) {
@ -667,7 +667,7 @@ static void pidTurnAssistant(pidState_t *pidState)
float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn; float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn;
targetRates.V.Z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
} }
else { else {
// Don't allow coordinated turn calculation if airplane is in hard bank or steep climb/dive // 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 { else {
targetRates.V.Z = pidState[YAW].rateTarget; targetRates.z = pidState[YAW].rateTarget;
} }
// Transform calculated rate offsets into body frame and apply // Transform calculated rate offsets into body frame and apply
imuTransformVectorEarthToBody(&targetRates); imuTransformVectorEarthToBody(&targetRates);
// Add in roll and pitch // 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[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.V.Y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 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 // Replace YAW on quads - add it in on airplanes
if (STATE(FIXED_WING)) { 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 { 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 #endif

132
src/main/navigation/navigation.c
View file

@ -175,11 +175,11 @@ void resetGCSFlags(void);
static bool posEstimationHasGlobalReference(void); static bool posEstimationHasGlobalReference(void);
static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint); static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint);
static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos); static void calcualteAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
void calculateInitialHoldPosition(t_fp_vector * pos); void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayTarget(t_fp_vector * farAwayPos, int32_t yaw, int32_t distance); 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); 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)) { 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); calculateInitialHoldPosition(&targetHoldPos);
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); 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 GCS was disabled - reset 2D pos setpoint
if (posControl.flags.isGCSAssistedNavigationReset) { if (posControl.flags.isGCSAssistedNavigationReset) {
t_fp_vector targetHoldPos; fpVector3_t targetHoldPos;
calculateInitialHoldPosition(&targetHoldPos); calculateInitialHoldPosition(&targetHoldPos);
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
resetGCSFlags(); 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)) { 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); calculateInitialHoldPosition(&targetHoldPos);
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); 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 GCS was disabled - reset 2D pos setpoint
if (posControl.flags.isGCSAssistedNavigationReset) { if (posControl.flags.isGCSAssistedNavigationReset) {
t_fp_vector targetHoldPos; fpVector3_t targetHoldPos;
calculateInitialHoldPosition(&targetHoldPos); calculateInitialHoldPosition(&targetHoldPos);
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); 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 return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
} }
else { else {
t_fp_vector targetHoldPos; fpVector3_t targetHoldPos;
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
// Airplane - climbout before turning around // 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 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) { if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
const float rthAltitudeMargin = STATE(FIXED_WING) ? 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(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.V.Z - posControl.homePosition.pos.V.Z)); // Copters 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 // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
initializeRTHSanityChecker(&posControl.actualState.pos); 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 // Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
t_fp_vector pos = posControl.homeWaypointAbove.pos; fpVector3_t pos = posControl.homeWaypointAbove.pos;
pos.V.Z += FW_RTH_CLIMB_OVERSHOOT_CM; pos.z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z); setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
} }
else { else {
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach // 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. // it in a reasonable time. Immediately after we finish this phase - target the original altitude.
t_fp_vector pos = posControl.homeWaypointAbove.pos; fpVector3_t pos = posControl.homeWaypointAbove.pos;
pos.V.Z += MR_RTH_CLIMB_OVERSHOOT_CM; pos.z += MR_RTH_CLIMB_OVERSHOOT_CM;
if (navConfig()->general.flags.rth_tail_first) { if (navConfig()->general.flags.rth_tail_first) {
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_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 { else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. // 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 / (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); 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) void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
{ {
posControl.actualState.pos.V.X = newX; posControl.actualState.pos.x = newX;
posControl.actualState.pos.V.Y = newY; posControl.actualState.pos.y = newY;
posControl.actualState.vel.V.X = newVelX; posControl.actualState.vel.x = newVelX;
posControl.actualState.vel.V.Y = newVelY; posControl.actualState.vel.y = newVelY;
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY)); posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
if (estimateValid) { 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) void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
{ {
posControl.actualState.pos.V.Z = newAltitude; posControl.actualState.pos.z = newAltitude;
posControl.actualState.vel.V.Z = newVelocity; posControl.actualState.vel.z = newVelocity;
posControl.actualState.surface = surfaceDistance; posControl.actualState.surface = surfaceDistance;
posControl.actualState.surfaceVel = surfaceVelocity; posControl.actualState.surfaceVel = surfaceVelocity;
@ -1544,8 +1544,8 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
} }
#if defined(NAV_BLACKBOX) #if defined(NAV_BLACKBOX)
navLatestActualPosition[Z] = constrain(posControl.actualState.pos.V.Z, -32678, 32767); navLatestActualPosition[Z] = constrain(posControl.actualState.pos.z, -32678, 32767);
navActualVelocity[Z] = constrain(posControl.actualState.vel.V.Z, -32678, 32767); navActualVelocity[Z] = constrain(posControl.actualState.vel.z, -32678, 32767);
#endif #endif
} }
@ -1568,18 +1568,18 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Calculates distance and bearing to destination point * 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 deltaX = destinationPos->x - posControl.actualState.pos.x;
const float deltaY = destinationPos->V.Y - posControl.actualState.pos.V.Y; const float deltaY = destinationPos->y - posControl.actualState.pos.y;
return sqrtf(sq(deltaX) + sq(deltaY)); 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 deltaX = destinationPos->x - posControl.actualState.pos.x;
const float deltaY = destinationPos->V.Y - posControl.actualState.pos.V.Y; const float deltaY = destinationPos->y - posControl.actualState.pos.y;
return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX))); 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)) { if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
switch (navConfig()->general.flags.rth_alt_control_mode) { switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT: case NAV_RTH_NO_ALT:
posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z; posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z;
break; break;
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin 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; break;
case NAV_RTH_MAX_ALT: 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; break;
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home 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; break;
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default: 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; break;
} }
} }
} }
else { else {
posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z; posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z;
} }
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
* RTH sanity test logic * RTH sanity test logic
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void initializeRTHSanityChecker(const t_fp_vector * pos) void initializeRTHSanityChecker(const fpVector3_t * pos)
{ {
const timeMs_t currentTimeMs = millis(); const timeMs_t currentTimeMs = millis();
@ -1692,17 +1692,17 @@ bool validateRTHSanityChecker(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Reset home position to current position * 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 // XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) { if ((useMask & NAV_POS_UPDATE_XY) != 0) {
posControl.homePosition.pos.V.X = pos->V.X; posControl.homePosition.pos.x = pos->x;
posControl.homePosition.pos.V.Y = pos->V.Y; posControl.homePosition.pos.y = pos->y;
} }
// Z-position // Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) { if ((useMask & NAV_POS_UPDATE_Z) != 0) {
posControl.homePosition.pos.V.Z = pos->V.Z; posControl.homePosition.pos.z = pos->z;
} }
// Heading // Heading
@ -1780,7 +1780,7 @@ int32_t getTotalTravelDistance(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Calculate platform-specific hold position (account for deceleration) * 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 if (STATE(FIXED_WING)) { // FIXED_WING
calculateFixedWingInitialHoldPosition(pos); calculateFixedWingInitialHoldPosition(pos);
@ -1793,19 +1793,19 @@ void calculateInitialHoldPosition(t_fp_vector * pos)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Set active XYZ-target and desired heading * 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 // XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) { if ((useMask & NAV_POS_UPDATE_XY) != 0) {
posControl.desiredState.pos.V.X = pos->V.X; posControl.desiredState.pos.x = pos->x;
posControl.desiredState.pos.V.Y = pos->V.Y; posControl.desiredState.pos.y = pos->y;
} }
// Z-position // Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) { if ((useMask & NAV_POS_UPDATE_Z) != 0) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller 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.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 // 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->x = posControl.actualState.pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
farAwayPos->V.Y = posControl.actualState.pos.V.Y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw)); farAwayPos->y = posControl.actualState.pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
farAwayPos->V.Z = posControl.actualState.pos.V.Z; farAwayPos->z = posControl.actualState.pos.z;
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -1865,7 +1865,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
if (mode == ROC_TO_ALT_RESET) { if (mode == ROC_TO_ALT_RESET) {
lastUpdateTimeUs = currentTimeUs; lastUpdateTimeUs = currentTimeUs;
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z; posControl.desiredState.pos.z = posControl.actualState.pos.z;
} }
else { else {
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
@ -1876,15 +1876,15 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 1, timeDelta * 1000); DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 1, timeDelta * 1000);
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) { if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
posControl.desiredState.pos.V.Z += desiredClimbRate * timeDelta; posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
posControl.desiredState.pos.V.Z = constrainf(posControl.desiredState.pos.V.Z, // FIXME: calculate sanity limits in a smarter way posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, // FIXME: calculate sanity limits in a smarter way
posControl.actualState.pos.V.Z - 500, posControl.actualState.pos.z - 500,
posControl.actualState.pos.V.Z + 500); posControl.actualState.pos.z + 500);
} }
} }
else { else {
// Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD // 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; lastUpdateTimeUs = currentTimeUs;
@ -2127,7 +2127,7 @@ bool saveNonVolatileWaypointList(void)
} }
#endif #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; gpsLocation_t wpLLH;
@ -2138,7 +2138,7 @@ static void mapWaypointToLocalPosition(t_fp_vector * localPos, const navWaypoint
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, localPos, GEO_ALT_RELATIVE); 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; posControl.activeWaypoint.pos = *pos;
@ -2151,7 +2151,7 @@ static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos
static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint) static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint)
{ {
t_fp_vector localPos; fpVector3_t localPos;
mapWaypointToLocalPosition(&localPos, waypoint); mapWaypointToLocalPosition(&localPos, waypoint);
calcualteAndSetActiveWaypointToLocalPosition(&localPos); calcualteAndSetActiveWaypointToLocalPosition(&localPos);
} }
@ -2299,9 +2299,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7); if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8); if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8);
navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767); navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.x), -32678, 32767);
navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767); navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.y), -32678, 32767);
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767); navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.z), -32678, 32767);
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767); navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
#endif #endif
} }
@ -2496,7 +2496,7 @@ bool navigationBlockArming(void)
// Don't allow arming if first waypoint is farther than configured safe distance // Don't allow arming if first waypoint is farther than configured safe distance
if (posControl.waypointCount > 0) { if (posControl.waypointCount > 0) {
t_fp_vector startingWaypointPos; fpVector3_t startingWaypointPos;
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]); mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]);
const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance; const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
@ -2651,12 +2651,12 @@ void navigationInit(void)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
float getEstimatedActualVelocity(int axis) float getEstimatedActualVelocity(int axis)
{ {
return posControl.actualState.vel.A[axis]; return posControl.actualState.vel.v[axis];
} }
float getEstimatedActualPosition(int axis) float getEstimatedActualPosition(int axis)
{ {
return posControl.actualState.pos.A[axis]; return posControl.actualState.pos.v[axis];
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------

7
src/main/navigation/navigation.h
View file

@ -18,6 +18,7 @@
#pragma once #pragma once
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
@ -194,7 +195,7 @@ typedef struct {
} navWaypoint_t; } navWaypoint_t;
typedef struct { typedef struct {
t_fp_vector pos; fpVector3_t pos;
int32_t yaw; // deg * 100 int32_t yaw; // deg * 100
} navWaypointPosition_t; } navWaypointPosition_t;
@ -300,8 +301,8 @@ typedef enum {
} geoOriginResetMode_e; } geoOriginResetMode_e;
void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginResetMode_e resetMode); 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 geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, fpVector3_t * pos, geoAltitudeConversionMode_e altConv);
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 geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
/* Failsafe-forced RTH mode */ /* Failsafe-forced RTH mode */

36
src/main/navigation/navigation_fixedwing.c
View file

@ -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 // velocity error. We use PID controller on altitude error and calculate desired pitch angle
// Update energies // 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 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; const float estSKE = 0.0f;
// speedWeight controls balance between potential and kinetic energy used for pitch controller // 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 * XY-position controller for multicopter aircraft
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
static t_fp_vector virtualDesiredPosition; static fpVector3_t virtualDesiredPosition;
static pt1Filter_t fwPosControllerCorrectionFilterState; static pt1Filter_t fwPosControllerCorrectionFilterState;
void resetFixedWingPositionController(void) void resetFixedWingPositionController(void)
{ {
virtualDesiredPosition.V.X = 0; virtualDesiredPosition.x = 0;
virtualDesiredPosition.V.Y = 0; virtualDesiredPosition.y = 0;
virtualDesiredPosition.V.Z = 0; virtualDesiredPosition.z = 0;
navPidReset(&posControl.pids.fw_nav); navPidReset(&posControl.pids.fw_nav);
posControl.rcAdjustment[ROLL] = 0; posControl.rcAdjustment[ROLL] = 0;
@ -206,8 +206,8 @@ void resetFixedWingPositionController(void)
static void calculateVirtualPositionTarget_FW(float trackingPeriod) static void calculateVirtualPositionTarget_FW(float trackingPeriod)
{ {
float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X; float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x;
float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y; float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y;
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); 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 // We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f); 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 loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.V.Y + navConfig()->fw.loiter_radius * sin_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 // We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - posControl.actualState.pos.V.X; posErrorX = loiterTargetX - posControl.actualState.pos.x;
posErrorY = loiterTargetY - posControl.actualState.pos.V.Y; posErrorY = loiterTargetY - posControl.actualState.pos.y;
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
} }
// Calculate virtual waypoint // Calculate virtual waypoint
virtualDesiredPosition.V.X = posControl.actualState.pos.V.X + posErrorX * (trackingDistance / distanceToActualTarget); virtualDesiredPosition.x = posControl.actualState.pos.x + posErrorX * (trackingDistance / distanceToActualTarget);
virtualDesiredPosition.V.Y = posControl.actualState.pos.V.Y + posErrorY * (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) // Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
if (posControl.flags.isAdjustingPosition) { 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; 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 // Rotate this target shift from body frame to to earth frame and apply to position target
virtualDesiredPosition.V.X += -rcShiftY * posControl.actualState.sinYaw; virtualDesiredPosition.x += -rcShiftY * posControl.actualState.sinYaw;
virtualDesiredPosition.V.Y += rcShiftY * posControl.actualState.cosYaw; virtualDesiredPosition.y += rcShiftY * posControl.actualState.cosYaw;
posControl.flags.isAdjustingPosition = true; 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 * TODO refactor conditions in this metod if logic is proven to be correct
*/ */
if (navStateFlags & NAV_CTL_LAND) { 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)) ) { ((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 * 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 * 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 // TODO: stub, this should account for velocity and target loiter radius
*pos = posControl.actualState.pos; *pos = posControl.actualState.pos;

6
src/main/navigation/navigation_fw_launch.c
View file

@ -76,12 +76,12 @@ static FixedWingLaunchState_t launchState;
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) 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 float swingVelocity = (ABS(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.A[X] > navConfig()->fw.launch_accel_thresh); 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 isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; 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) { if (isBungeeLaunched || isSwingLaunched) {
launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate); launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate);

24
src/main/navigation/navigation_geo.c
View file

@ -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) { if (origin->valid) {
pos->V.X = (llh->lat - origin->lat) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; pos->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->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 flag GEO_ALT_RELATIVE, than llh altitude is already relative to origin
if (altConv == GEO_ALT_RELATIVE) { if (altConv == GEO_ALT_RELATIVE) {
pos->V.Z = llh->alt; pos->z = llh->alt;
} else { } else {
pos->V.Z = llh->alt - origin->alt; pos->z = llh->alt - origin->alt;
} }
} }
else { else {
pos->V.X = 0.0f; pos->x = 0.0f;
pos->V.Y = 0.0f; pos->y = 0.0f;
pos->V.Z = 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; float scaleLonDown;
@ -184,9 +184,9 @@ void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const t_fp_vector * p
scaleLonDown = 1.0f; scaleLonDown = 1.0f;
} }
llh->lat += lrintf(pos->V.X / DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR); llh->lat += lrintf(pos->x / DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR);
llh->lon += lrintf(pos->V.Y / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * scaleLonDown)); llh->lon += lrintf(pos->y / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * scaleLonDown));
llh->alt += lrintf(pos->V.Z); llh->alt += lrintf(pos->z);
} }

56
src/main/navigation/navigation_multicopter.c
View file

@ -61,7 +61,7 @@ static bool prepareForTakeoffOnReset = false;
// Position to velocity controller for Z axis // Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) 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; float targetVel = altitudeError * posControl.pids.pos[Z].param.kP;
// hard limit desired target velocity to max_climb_rate // 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. // 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 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); 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 { else {
posControl.desiredState.vel.V.Z = targetVel; posControl.desiredState.vel.z = targetVel;
} }
#if defined(NAV_BLACKBOX) #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 #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 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; 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] = pt1FilterApply4(&altholdThrottleFilterState, posControl.rcAdjustment[THROTTLE], NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax); posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
@ -166,14 +166,14 @@ void resetMulticopterAltitudeController(void)
if (prepareForTakeoffOnReset) { if (prepareForTakeoffOnReset) {
/* If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump */ /* 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.vel.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.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; posControl.pids.vel[Z].integrator = -500.0f;
pt1FilterReset(&altholdThrottleFilterState, -500.0f); pt1FilterReset(&altholdThrottleFilterState, -500.0f);
prepareForTakeoffOnReset = false; prepareForTakeoffOnReset = false;
} }
else { 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); pt1FilterReset(&altholdThrottleFilterState, 0.0f);
} }
} }
@ -270,8 +270,8 @@ bool adjustMulticopterPositionFromRCInput(void)
const float neuVelY = rcVelX * posControl.actualState.sinYaw + rcVelY * posControl.actualState.cosYaw; 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 // 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.x = posControl.actualState.pos.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.y = posControl.actualState.pos.y + (neuVelY / posControl.pids.pos[Y].param.kP);
} }
return true; return true;
@ -279,7 +279,7 @@ bool adjustMulticopterPositionFromRCInput(void)
else { else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick // Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingPosition) { if (posControl.flags.isAdjustingPosition) {
t_fp_vector stopPosition; fpVector3_t stopPosition;
calculateMulticopterInitialHoldPosition(&stopPosition); calculateMulticopterInitialHoldPosition(&stopPosition);
setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY); setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
} }
@ -314,8 +314,8 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
static void updatePositionVelocityController_MC(void) static void updatePositionVelocityController_MC(void)
{ {
const float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X; const float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x;
const float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y; const float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y;
// Calculate target velocity // Calculate target velocity
float newVelX = posErrorX * posControl.pids.pos[X].param.kP; 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) // Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
const float velHeadFactor = getVelocityHeadingAttenuationFactor(); const float velHeadFactor = getVelocityHeadingAttenuationFactor();
const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed); const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);
posControl.desiredState.vel.V.X = newVelX * velHeadFactor * velExpoFactor; posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor;
posControl.desiredState.vel.V.Y = newVelY * velHeadFactor * velExpoFactor; posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor;
#if defined(NAV_BLACKBOX) #if defined(NAV_BLACKBOX)
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.V.X), -32678, 32767); navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.V.Y), -32678, 32767); navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
#endif #endif
} }
@ -348,8 +348,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
{ {
// Calculate velocity error // Calculate velocity error
const float velErrorX = posControl.desiredState.vel.V.X - posControl.actualState.vel.V.X; const float velErrorX = posControl.desiredState.vel.x - posControl.actualState.vel.x;
const float velErrorY = posControl.desiredState.vel.V.Y - posControl.actualState.vel.V.Y; const float velErrorY = posControl.desiredState.vel.y - posControl.actualState.vel.y;
// Calculate XY-acceleration limit according to velocity error limit // Calculate XY-acceleration limit according to velocity error limit
float accelLimitX, accelLimitY; 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 // 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 // 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 // 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 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.V.Y, posControl.actualState.vel.V.Y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 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 // Save last acceleration target
lastAccelTargetX = newAccelX; lastAccelTargetX = newAccelX;
@ -484,7 +484,7 @@ bool isMulticopterLandingDetected(void)
} }
// Average climb rate should be low enough // 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 // check if we are moving horizontally
bool horizontalMovement = posControl.actualState.velXY > 100.0f; 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 * 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 stoppingDistanceX = posControl.actualState.vel.x * posControl.posDecelerationTime;
const float stoppingDistanceY = posControl.actualState.vel.V.Y * posControl.posDecelerationTime; const float stoppingDistanceY = posControl.actualState.vel.y * posControl.posDecelerationTime;
pos->V.X = posControl.actualState.pos.V.X + stoppingDistanceX; pos->x = posControl.actualState.pos.x + stoppingDistanceX;
pos->V.Y = posControl.actualState.pos.V.Y + stoppingDistanceY; pos->y = posControl.actualState.pos.y + stoppingDistanceY;
} }
void resetMulticopterHeadingController(void) void resetMulticopterHeadingController(void)

184
src/main/navigation/navigation_pos_estimator.c
View file

@ -92,8 +92,8 @@ typedef struct {
bool glitchDetected; bool glitchDetected;
bool glitchRecovery; bool glitchRecovery;
#endif #endif
t_fp_vector pos; // GPS position in NEU coordinate system (cm) fpVector3_t pos; // GPS position in NEU coordinate system (cm)
t_fp_vector vel; // GPS velocity (cms) fpVector3_t vel; // GPS velocity (cms)
float eph; float eph;
float epv; float epv;
} navPositionEstimatorGPS_t; } navPositionEstimatorGPS_t;
@ -133,8 +133,8 @@ typedef struct {
timeUs_t lastUpdateTime; // Last update time (us) timeUs_t lastUpdateTime; // Last update time (us)
// 3D position, velocity and confidence // 3D position, velocity and confidence
t_fp_vector pos; fpVector3_t pos;
t_fp_vector vel; fpVector3_t vel;
float eph; float eph;
float epv; float epv;
@ -152,8 +152,8 @@ typedef struct {
} navPositionEstimatorSTATE_t; } navPositionEstimatorSTATE_t;
typedef struct { typedef struct {
t_fp_vector accelNEU; fpVector3_t accelNEU;
t_fp_vector accelBias; fpVector3_t accelBias;
bool gravityCalibrationComplete; bool gravityCalibrationComplete;
} navPosisitonEstimatorIMU_t; } 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> */ /* Inertial filter, implementation taken from PX4 implementation by Anton Babushkin <rk3dov@gmail.com> */
static void inavFilterPredict(int axis, float dt, float acc) 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.pos.v[axis] += posEstimator.est.vel.v[axis] * dt + acc * dt * dt / 2.0f;
posEstimator.est.vel.A[axis] += acc * dt; posEstimator.est.vel.v[axis] += acc * dt;
} }
static void inavFilterCorrectPos(int axis, float dt, float e, float w) static void inavFilterCorrectPos(int axis, float dt, float e, float w)
{ {
float ewdt = e * w * dt; float ewdt = e * w * dt;
posEstimator.est.pos.A[axis] += ewdt; posEstimator.est.pos.v[axis] += ewdt;
posEstimator.est.vel.A[axis] += w * ewdt; posEstimator.est.vel.v[axis] += w * ewdt;
} }
static void inavFilterCorrectVel(int axis, float dt, float e, float w) 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; } #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 bool detectGPSGlitch(timeUs_t currentTimeUs)
{ {
static timeUs_t previousTime = 0; static timeUs_t previousTime = 0;
static t_fp_vector lastKnownGoodPosition; static fpVector3_t lastKnownGoodPosition;
static t_fp_vector lastKnownGoodVelocity; static fpVector3_t lastKnownGoodVelocity;
bool isGlitching = false; bool isGlitching = false;
@ -287,16 +287,16 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
isGlitching = false; isGlitching = false;
} }
else { else {
t_fp_vector predictedGpsPosition; fpVector3_t predictedGpsPosition;
float gpsDistance; float gpsDistance;
float dT = US2S(currentTimeUs - previousTime); float dT = US2S(currentTimeUs - previousTime);
/* We predict new position based on previous GPS velocity and position */ /* We predict new position based on previous GPS velocity and position */
predictedGpsPosition.V.X = lastKnownGoodPosition.V.X + lastKnownGoodVelocity.V.X * dT; predictedGpsPosition.x = lastKnownGoodPosition.x + lastKnownGoodVelocity.x * dT;
predictedGpsPosition.V.Y = lastKnownGoodPosition.V.Y + lastKnownGoodVelocity.V.Y * dT; predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
/* New pos is within predefined radius of predicted pos, radius is expanded exponentially */ /* 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)) { if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
isGlitching = false; isGlitching = false;
} }
@ -348,7 +348,7 @@ void onNewGPSData(void)
/* Automatic magnetic declination calculation - do this once */ /* Automatic magnetic declination calculation - do this once */
static bool magDeclinationSet = false; static bool magDeclinationSet = false;
if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) { if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
mag.magneticDeclination = geoCalculateMagDeclination(&newLLH) * 10.0f; // heading is in 0.1deg units imuSetMagneticDeclination(geoCalculateMagDeclination(&newLLH));
magDeclinationSet = true; magDeclinationSet = true;
} }
#endif #endif
@ -375,19 +375,19 @@ void onNewGPSData(void)
/* Use VELNED provided by GPS if available, calculate from coordinates otherwise */ /* 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); 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) { if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
posEstimator.gps.vel.V.X = gpsSol.velNED[0]; posEstimator.gps.vel.x = gpsSol.velNED[0];
posEstimator.gps.vel.V.Y = gpsSol.velNED[1]; posEstimator.gps.vel.y = gpsSol.velNED[1];
} }
else { 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.x = (posEstimator.gps.vel.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.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) { 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 { 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) #if defined(NAV_GPS_GLITCH_DETECTION)
@ -547,38 +547,38 @@ static void updateIMUTopic(void)
static timeMs_t gravityCalibrationTimeout = 0; static timeMs_t gravityCalibrationTimeout = 0;
if (!isImuReady()) { if (!isImuReady()) {
posEstimator.imu.accelNEU.V.X = 0; posEstimator.imu.accelNEU.x = 0;
posEstimator.imu.accelNEU.V.Y = 0; posEstimator.imu.accelNEU.y = 0;
posEstimator.imu.accelNEU.V.Z = 0; posEstimator.imu.accelNEU.z = 0;
gravityCalibrationTimeout = millis(); gravityCalibrationTimeout = millis();
posEstimator.imu.gravityCalibrationComplete = false; posEstimator.imu.gravityCalibrationComplete = false;
} }
else { else {
t_fp_vector accelBF; fpVector3_t accelBF;
/* Read acceleration data in body frame */ /* Read acceleration data in body frame */
accelBF.V.X = imuMeasuredAccelBF.V.X; accelBF.x = imuMeasuredAccelBF.x;
accelBF.V.Y = imuMeasuredAccelBF.V.Y; accelBF.y = imuMeasuredAccelBF.y;
accelBF.V.Z = imuMeasuredAccelBF.V.Z; accelBF.z = imuMeasuredAccelBF.z;
/* Correct accelerometer bias */ /* Correct accelerometer bias */
accelBF.V.X -= posEstimator.imu.accelBias.V.X; accelBF.x -= posEstimator.imu.accelBias.x;
accelBF.V.Y -= posEstimator.imu.accelBias.V.Y; accelBF.y -= posEstimator.imu.accelBias.y;
accelBF.V.Z -= posEstimator.imu.accelBias.V.Z; accelBF.z -= posEstimator.imu.accelBias.z;
/* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/ /* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/
imuTransformVectorBodyToEarth(&accelBF); imuTransformVectorBodyToEarth(&accelBF);
/* Read acceleration data in NEU frame from IMU */ /* Read acceleration data in NEU frame from IMU */
posEstimator.imu.accelNEU.V.X = accelBF.V.X; posEstimator.imu.accelNEU.x = accelBF.x;
posEstimator.imu.accelNEU.V.Y = accelBF.V.Y; posEstimator.imu.accelNEU.y = accelBF.y;
posEstimator.imu.accelNEU.V.Z = accelBF.V.Z; posEstimator.imu.accelNEU.z = accelBF.z;
/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
if (!ARMING_FLAG(ARMED) && !posEstimator.imu.gravityCalibrationComplete) { if (!ARMING_FLAG(ARMED) && !posEstimator.imu.gravityCalibrationComplete) {
// Slowly converge on calibrated gravity while level // 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; calibratedGravityCMSS += gravityOffsetError * 0.0025f;
if (ABS(gravityOffsetError) < positionEstimationConfig()->gravity_calibration_tolerance) { // Error should be within 0.5% of calibrated gravity 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 calibration is incomplete - report zero acceleration */
if (posEstimator.imu.gravityCalibrationComplete) { if (posEstimator.imu.gravityCalibrationComplete) {
posEstimator.imu.accelNEU.V.Z -= calibratedGravityCMSS; posEstimator.imu.accelNEU.z -= calibratedGravityCMSS;
} }
else { else {
posEstimator.imu.accelNEU.V.X = 0; posEstimator.imu.accelNEU.x = 0;
posEstimator.imu.accelNEU.V.Y = 0; posEstimator.imu.accelNEU.y = 0;
posEstimator.imu.accelNEU.V.Z = 0; posEstimator.imu.accelNEU.z = 0;
} }
#if defined(NAV_BLACKBOX) #if defined(NAV_BLACKBOX)
/* Update blackbox values */ /* Update blackbox values */
navAccNEU[X] = posEstimator.imu.accelNEU.A[X]; navAccNEU[X] = posEstimator.imu.accelNEU.x;
navAccNEU[Y] = posEstimator.imu.accelNEU.A[Y]; navAccNEU[Y] = posEstimator.imu.accelNEU.y;
navAccNEU[Z] = posEstimator.imu.accelNEU.A[Z]; navAccNEU[Z] = posEstimator.imu.accelNEU.z;
#endif #endif
} }
} }
@ -667,12 +667,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
/* Do some preparations to data */ /* Do some preparations to data */
if (isBaroValid) { if (isBaroValid) {
if (!ARMING_FLAG(ARMED)) { 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.isBaroGroundValid = true;
posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec
} }
else { else {
if (posEstimator.est.vel.V.Z > 15) { if (posEstimator.est.vel.z > 15) {
if (currentTimeUs > posEstimator.state.baroGroundTimeout) { if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
posEstimator.state.isBaroGroundValid = false; posEstimator.state.isBaroGroundValid = false;
} }
@ -703,14 +703,14 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
/* Prediction step: Z-axis */ /* Prediction step: Z-axis */
if (isEstZValid) { if (isEstZValid) {
inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.V.Z); inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.z);
} }
/* Prediction step: XY-axis */ /* Prediction step: XY-axis */
if (isEstXYValid) { if (isEstXYValid) {
if (navIsHeadingUsable()) { if (navIsHeadingUsable()) {
inavFilterPredict(X, dt, posEstimator.imu.accelNEU.V.X); inavFilterPredict(X, dt, posEstimator.imu.accelNEU.x);
inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.V.Y); inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.y);
} }
else { else {
inavFilterPredict(X, dt, 0.0f); inavFilterPredict(X, dt, 0.0f);
@ -720,7 +720,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
/* Accelerometer bias correction */ /* Accelerometer bias correction */
const bool updateAccBias = (positionEstimationConfig()->w_acc_bias > 0); 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 */ /* Correction step: Z-axis */
if (useGpsZPos || isBaroValid) { if (useGpsZPos || isBaroValid) {
@ -728,45 +728,45 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
/* Handle Z-axis reset */ /* Handle Z-axis reset */
if (!isEstZValid && useGpsZPos) { if (!isEstZValid && useGpsZPos) {
posEstimator.est.pos.V.Z = posEstimator.gps.pos.V.Z; posEstimator.est.pos.z = posEstimator.gps.pos.z;
posEstimator.est.vel.V.Z = posEstimator.gps.vel.V.Z; posEstimator.est.vel.z = posEstimator.gps.vel.z;
newEPV = posEstimator.gps.epv; newEPV = posEstimator.gps.epv;
} }
else { else {
#if defined(USE_BARO) #if defined(USE_BARO)
/* Apply BARO correction to altitude */ /* Apply BARO correction to altitude */
if (isBaroValid) { 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); inavFilterCorrectPos(Z, dt, baroResidual, positionEstimationConfig()->w_z_baro_p);
newEPV = updateEPE(posEstimator.est.epv, dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); newEPV = updateEPE(posEstimator.est.epv, dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
/* accelerometer bias correction for baro */ /* accelerometer bias correction for baro */
if (updateAccBias && !isAirCushionEffectDetected) { if (updateAccBias && !isAirCushionEffectDetected) {
accelBiasCorr.V.Z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p); accelBiasCorr.z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p);
} }
} }
#endif #endif
/* Apply GPS correction to altitude */ /* Apply GPS correction to altitude */
if (useGpsZPos) { 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); 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); newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, gpsResidualZ), positionEstimationConfig()->w_z_gps_p);
if (updateAccBias) { 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 */ /* Apply GPS correction to climb rate */
if (useGpsZVel) { 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)); inavFilterCorrectVel(Z, dt, gpsResidualZVel, positionEstimationConfig()->w_z_gps_v * sq(gpsWeightScaler));
} }
} }
} }
else { 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 */ /* Correction step: XY-axis */
@ -774,17 +774,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
if (isGPSValid) { if (isGPSValid) {
/* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */ /* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */
if (!isEstXYValid) { if (!isEstXYValid) {
posEstimator.est.pos.V.X = posEstimator.gps.pos.V.X; posEstimator.est.pos.x = posEstimator.gps.pos.x;
posEstimator.est.pos.V.Y = posEstimator.gps.pos.V.Y; posEstimator.est.pos.y = posEstimator.gps.pos.y;
posEstimator.est.vel.V.X = posEstimator.gps.vel.V.X; posEstimator.est.vel.x = posEstimator.gps.vel.x;
posEstimator.est.vel.V.Y = posEstimator.gps.vel.V.Y; posEstimator.est.vel.y = posEstimator.gps.vel.y;
newEPH = posEstimator.gps.eph; newEPH = posEstimator.gps.eph;
} }
else { else {
const float gpsResidualX = posEstimator.gps.pos.V.X - posEstimator.est.pos.V.X; const float gpsResidualX = posEstimator.gps.pos.x - posEstimator.est.pos.x;
const float gpsResidualY = posEstimator.gps.pos.V.Y - posEstimator.est.pos.V.Y; const float gpsResidualY = posEstimator.gps.pos.y - posEstimator.est.pos.y;
const float gpsResidualXVel = posEstimator.gps.vel.V.X - posEstimator.est.vel.V.X; const float gpsResidualXVel = posEstimator.gps.vel.x - posEstimator.est.vel.x;
const float gpsResidualYVel = posEstimator.gps.vel.V.Y - posEstimator.est.vel.V.Y; const float gpsResidualYVel = posEstimator.gps.vel.y - posEstimator.est.vel.y;
const float gpsResidualXYMagnitude = sqrtf(sq(gpsResidualX) + sq(gpsResidualY)); 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); //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 { else {
inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.V.X, 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.V.Y, positionEstimationConfig()->w_xy_res_v); inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.y, positionEstimationConfig()->w_xy_res_v);
} }
/* Correct accelerometer bias */ /* Correct accelerometer bias */
if (updateAccBias) { 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)) { if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
/* transform error vector from NEU frame to body frame */ /* transform error vector from NEU frame to body frame */
imuTransformVectorEarthToBody(&accelBiasCorr); imuTransformVectorEarthToBody(&accelBiasCorr);
/* Correct accel bias */ /* Correct accel bias */
posEstimator.imu.accelBias.V.X += accelBiasCorr.V.X * positionEstimationConfig()->w_acc_bias * dt; posEstimator.imu.accelBias.x += accelBiasCorr.x * positionEstimationConfig()->w_acc_bias * dt;
posEstimator.imu.accelBias.V.Y += accelBiasCorr.V.Y * positionEstimationConfig()->w_acc_bias * dt; posEstimator.imu.accelBias.y += accelBiasCorr.y * positionEstimationConfig()->w_acc_bias * dt;
posEstimator.imu.accelBias.V.Z += accelBiasCorr.V.Z * 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) { if (resetSurfaceEstimate) {
posEstimator.est.aglAlt = posEstimator.surface.alt; posEstimator.est.aglAlt = posEstimator.surface.alt;
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
posEstimator.est.aglVel = posEstimator.est.vel.V.Z; posEstimator.est.aglVel = posEstimator.est.vel.z;
posEstimator.est.aglOffset = posEstimator.est.pos.V.Z - posEstimator.surface.alt; posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
} }
else { else {
posEstimator.est.aglVel = 0; posEstimator.est.aglVel = 0;
@ -885,8 +885,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
} }
// Update estimate // Update estimate
posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.V.Z * dt * dt * 0.5f; 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.V.Z * dt; posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * dt;
// Apply correction // Apply correction
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
@ -899,12 +899,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
// Update estimate offset // Update estimate offset
if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) { 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) { else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
// Correct estimate from altitude fused from rangefinder and global altitude // 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 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 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); const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
@ -914,13 +914,13 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
} }
else { // SURFACE_QUAL_LOW else { // SURFACE_QUAL_LOW
// In this case rangefinder can't be trusted - simply use global altitude // 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.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
posEstimator.est.aglVel = posEstimator.est.vel.V.Z; posEstimator.est.aglVel = posEstimator.est.vel.z;
} }
} }
else { else {
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z - posEstimator.est.aglOffset; posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
posEstimator.est.aglVel = posEstimator.est.vel.V.Z; posEstimator.est.aglVel = posEstimator.est.vel.z;
posEstimator.est.aglQual = SURFACE_QUAL_LOW; posEstimator.est.aglQual = SURFACE_QUAL_LOW;
} }
@ -932,8 +932,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
#endif #endif
#else #else
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z; posEstimator.est.aglAlt = posEstimator.est.pos.z;
posEstimator.est.aglVel = posEstimator.est.vel.V.Z; posEstimator.est.aglVel = posEstimator.est.vel.z;
posEstimator.est.aglQual = SURFACE_QUAL_LOW; posEstimator.est.aglQual = SURFACE_QUAL_LOW;
#endif #endif
} }
@ -953,19 +953,19 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) { if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
/* Publish position update */ /* Publish position update */
if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) { 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 { 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 */ /* Publish altitude update and set altitude validity */
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED; 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 { 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) #if defined(NAV_BLACKBOX)
@ -1003,9 +1003,9 @@ void initializePositionEstimator(void)
posEstimator.imu.gravityCalibrationComplete = false; posEstimator.imu.gravityCalibrationComplete = false;
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
posEstimator.imu.accelBias.A[axis] = 0; posEstimator.imu.accelBias.v[axis] = 0;
posEstimator.est.pos.A[axis] = 0; posEstimator.est.pos.v[axis] = 0;
posEstimator.est.vel.A[axis] = 0; posEstimator.est.vel.v[axis] = 0;
} }
} }

22
src/main/navigation/navigation_private.h
View file

@ -127,8 +127,8 @@ typedef struct navigationPIDControllers_s {
} navigationPIDControllers_t; } navigationPIDControllers_t;
typedef struct { typedef struct {
t_fp_vector pos; fpVector3_t pos;
t_fp_vector vel; fpVector3_t vel;
int32_t yaw; int32_t yaw;
float sinYaw; float sinYaw;
float cosYaw; float cosYaw;
@ -139,8 +139,8 @@ typedef struct {
} navigationEstimatedState_t; } navigationEstimatedState_t;
typedef struct { typedef struct {
t_fp_vector pos; fpVector3_t pos;
t_fp_vector vel; fpVector3_t vel;
int32_t yaw; int32_t yaw;
float surface; float surface;
} navigationDesiredState_t; } navigationDesiredState_t;
@ -251,7 +251,7 @@ typedef struct {
typedef struct { typedef struct {
timeMs_t lastCheckTime; timeMs_t lastCheckTime;
t_fp_vector initialPosition; fpVector3_t initialPosition;
float minimalDistanceToHome; float minimalDistanceToHome;
} rthSanityChecker_t; } rthSanityChecker_t;
@ -307,15 +307,15 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD);
void navPInit(pController_t *p, float _kP); void navPInit(pController_t *p, float _kP);
bool isThrustFacingDownwards(void); bool isThrustFacingDownwards(void);
uint32_t calculateDistanceToDestination(const t_fp_vector * destinationPos); uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
int32_t calculateBearingToDestination(const t_fp_vector * destinationPos); int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
void resetLandingDetector(void); void resetLandingDetector(void);
bool isLandingDetected(void); bool isLandingDetected(void);
navigationFSMStateFlags_t navGetCurrentStateFlags(void); navigationFSMStateFlags_t navGetCurrentStateFlags(void);
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);
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);
void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode); void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode);
@ -352,7 +352,7 @@ void resetMulticopterLandingDetector(void);
bool isMulticopterLandingDetected(void); bool isMulticopterLandingDetected(void);
bool isFixedWingLandingDetected(void); bool isFixedWingLandingDetected(void);
void calculateMulticopterInitialHoldPosition(t_fp_vector * pos); void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
/* Fixed-wing specific functions */ /* Fixed-wing specific functions */
void setupFixedWingAltitudeController(void); void setupFixedWingAltitudeController(void);
@ -367,7 +367,7 @@ bool adjustFixedWingPositionFromRCInput(void);
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs); void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
void calculateFixedWingInitialHoldPosition(t_fp_vector * pos); void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
/* Fixed-wing launch controller */ /* Fixed-wing launch controller */
void resetFixedWingLaunchController(timeUs_t currentTimeUs); void resetFixedWingLaunchController(timeUs_t currentTimeUs);

8
src/main/sensors/acceleration.c
View file

@ -476,24 +476,24 @@ static void accUpdateAccumulatedMeasurements(void)
/* /*
* Calculate measured acceleration in body frame in g * Calculate measured acceleration in body frame in g
*/ */
void accGetMeasuredAcceleration(t_fp_vector *measuredAcc) void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
{ {
#ifdef USE_ASYNC_GYRO_PROCESSING #ifdef USE_ASYNC_GYRO_PROCESSING
if (accumulatedMeasurementCount) { if (accumulatedMeasurementCount) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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; accumulatedMeasurements[axis] = 0;
} }
accumulatedMeasurementCount = 0; accumulatedMeasurementCount = 0;
} }
else { else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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 #else
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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 #endif
} }

3
src/main/sensors/acceleration.h
View file

@ -19,6 +19,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
@ -65,7 +66,7 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
bool accInit(uint32_t accTargetLooptime); bool accInit(uint32_t accTargetLooptime);
bool accIsCalibrationComplete(void); bool accIsCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void accGetMeasuredAcceleration(t_fp_vector *measuredAcc); void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
void accUpdate(void); void accUpdate(void);
void accSetCalibrationValues(void); void accSetCalibrationValues(void);
void accInitFilters(void); void accInitFilters(void);

16
src/main/sensors/boardalignment.c
View file

@ -21,6 +21,7 @@
#include <string.h> #include <string.h>
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
#include "common/axis.h" #include "common/axis.h"
#include "config/parameter_group.h" #include "config/parameter_group.h"
@ -31,7 +32,7 @@
#include "boardalignment.h" #include "boardalignment.h"
static bool standardBoardAlignment = true; // board orientation correction static bool standardBoardAlignment = true; // board orientation correction
static float boardRotation[3][3]; // matrix static fpMat3_t boardRotMatrix;
// no template required since defaults are zero // no template required since defaults are zero
PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0); 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.pitch = DECIDEGREES_TO_RADIANS(boardAlignment()->pitchDeciDegrees);
rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees ); rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees );
buildRotationMatrix(&rotationAngles, boardRotation); rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles);
} }
} }
@ -75,13 +76,12 @@ void applyBoardAlignment(int32_t *vec)
return; return;
} }
int32_t x = vec[X]; fpVector3_t fpVec = { .v = { vec[X], vec[Y], vec[Z] } };
int32_t y = vec[Y]; rotationMatrixRotateVector(&fpVec, &fpVec, &boardRotMatrix);
int32_t z = vec[Z];
vec[X] = lrintf(boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z); vec[X] = lrintf(fpVec.x);
vec[Y] = lrintf(boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z); vec[Y] = lrintf(fpVec.y);
vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z); vec[Z] = lrintf(fpVec.z);
} }
void applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation) void applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation)

21
src/main/sensors/compass.c
View file

@ -77,7 +77,6 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
#ifdef USE_MAG #ifdef USE_MAG
static uint8_t magInit = 0;
static uint8_t magUpdatedAtLeastOnce = 0; static uint8_t magUpdatedAtLeastOnce = 0;
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
@ -264,15 +263,12 @@ bool compassInit(void)
LED1_ON; LED1_ON;
const bool ret = mag.dev.init(&mag.dev); const bool ret = mag.dev.init(&mag.dev);
LED1_OFF; LED1_OFF;
if (ret) {
const int deg = compassConfig()->mag_declination / 100; if (!ret) {
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 {
addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR); addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR);
sensorsClear(SENSOR_MAG); sensorsClear(SENSOR_MAG);
} }
if (compassConfig()->mag_align != ALIGN_DEFAULT) { if (compassConfig()->mag_align != ALIGN_DEFAULT) {
mag.dev.magAlign = compassConfig()->mag_align; mag.dev.magAlign = compassConfig()->mag_align;
} }
@ -329,12 +325,6 @@ void compassUpdate(timeUs_t currentTimeUs)
DISABLE_STATE(CALIBRATE_MAG); 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 (calStartedAt != 0) {
if ((currentTimeUs - calStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) { if ((currentTimeUs - calStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) {
LED0_TOGGLE; LED0_TOGGLE;
@ -367,6 +357,11 @@ void compassUpdate(timeUs_t currentTimeUs)
saveConfigAndNotify(); 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); applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign);
applyBoardAlignment(mag.magADC); applyBoardAlignment(mag.magADC);

1
src/main/sensors/compass.h
View file

@ -45,7 +45,6 @@ typedef enum {
typedef struct mag_s { typedef struct mag_s {
magDev_t dev; magDev_t dev;
float magneticDeclination;
int32_t magADC[XYZ_AXIS_COUNT]; int32_t magADC[XYZ_AXIS_COUNT];
} mag_t; } mag_t;

6
src/main/sensors/gyro.c
View file

@ -375,18 +375,18 @@ static void gyroUpdateAccumulatedRates(timeDelta_t gyroUpdateDeltaUs)
/* /*
* Calculate rotation rate in rad/s in body frame * 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 #ifdef USE_ASYNC_GYRO_PROCESSING
const float accumulatedRateTime = accumulatedRateTimeUs * 1e-6; const float accumulatedRateTime = accumulatedRateTimeUs * 1e-6;
accumulatedRateTimeUs = 0; accumulatedRateTimeUs = 0;
for (int axis = 0; axis < 3; axis++) { 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; accumulatedRates[axis] = 0.0f;
} }
#else #else
for (int axis = 0; axis < 3; axis++) { 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 #endif
} }

3
src/main/sensors/gyro.h
View file

@ -19,6 +19,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/vector.h"
#include "common/time.h" #include "common/time.h"
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
@ -62,7 +63,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
bool gyroInit(void); bool gyroInit(void);
void gyroInitFilters(void); void gyroInitFilters(void);
void gyroGetMeasuredRotationRate(t_fp_vector *imuMeasuredRotationBF); void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF);
void gyroUpdate(timeDelta_t gyroUpdateDeltaUs); void gyroUpdate(timeDelta_t gyroUpdateDeltaUs);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
bool gyroIsCalibrationComplete(void); bool gyroIsCalibrationComplete(void);

2
src/main/sensors/initialisation.c
View file

@ -61,8 +61,6 @@ bool sensorsAutodetect(void)
pitotInit(); pitotInit();
#endif #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 #ifdef USE_MAG
compassInit(); compassInit();
#endif #endif