HEADFREE true 3D (second edition)...
reduced memory footprint ... rebased squashed cleanup
This commit is contained in:
Adrian Miriuta
parent
36a6cfc2b1
commit
7146c40ca8
|
|
@ -117,12 +117,12 @@ MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float
|
|||
*/
|
||||
MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
|
||||
{
|
||||
float cosPhi_2 = cosf(roll / 2);
|
||||
float sinPhi_2 = sinf(roll / 2);
|
||||
float cosTheta_2 = cosf(pitch / 2);
|
||||
float sinTheta_2 = sinf(pitch / 2);
|
||||
float cosPsi_2 = cosf(yaw / 2);
|
||||
float sinPsi_2 = sinf(yaw / 2);
|
||||
float cosPhi_2 = cos_approx(roll / 2);
|
||||
float sinPhi_2 = sin_approx(roll / 2);
|
||||
float cosTheta_2 = cos_approx(pitch / 2);
|
||||
float sinTheta_2 = sin_approx(pitch / 2);
|
||||
float cosPsi_2 = cos_approx(yaw / 2);
|
||||
float sinPsi_2 = sin_approx(yaw / 2);
|
||||
quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
|
||||
|
|
@ -188,12 +188,12 @@ MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quate
|
|||
*/
|
||||
MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
|
||||
{
|
||||
float cosPhi = cosf(roll);
|
||||
float sinPhi = sinf(roll);
|
||||
float cosThe = cosf(pitch);
|
||||
float sinThe = sinf(pitch);
|
||||
float cosPsi = cosf(yaw);
|
||||
float sinPsi = sinf(yaw);
|
||||
float cosPhi = cos_approx(roll);
|
||||
float sinPhi = sin_approx(roll);
|
||||
float cosThe = cos_approx(pitch);
|
||||
float sinThe = sin_approx(pitch);
|
||||
float cosPsi = cos_approx(yaw);
|
||||
float sinPsi = sin_approx(yaw);
|
||||
|
||||
dcm[0][0] = cosThe * cosPsi;
|
||||
dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
|
||||
|
|
|
|||
|
|
@ -108,8 +108,8 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
|
|||
{
|
||||
// setup variables
|
||||
const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
|
||||
const float sn = sinf(omega);
|
||||
const float cs = cosf(omega);
|
||||
const float sn = sin_approx(omega);
|
||||
const float cs = cos_approx(omega);
|
||||
const float alpha = sn / (2.0f * Q);
|
||||
|
||||
float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0;
|
||||
|
|
|
|||
|
|
@ -106,8 +106,6 @@ enum {
|
|||
int16_t magHold;
|
||||
#endif
|
||||
|
||||
int16_t headFreeModeHold;
|
||||
|
||||
static bool reverseMotors = false;
|
||||
static bool flipOverAfterCrashMode = false;
|
||||
|
||||
|
|
@ -281,10 +279,11 @@ void tryArm(void)
|
|||
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
|
||||
|
||||
if (isModeActivationConditionPresent(BOXPREARM)) {
|
||||
ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
|
||||
}
|
||||
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
|
||||
imuQuaternionHeadfreeOffsetSet();
|
||||
|
||||
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
|
||||
|
||||
|
|
@ -524,7 +523,9 @@ void processRx(timeUs_t currentTimeUs)
|
|||
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
|
||||
}
|
||||
if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
|
||||
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
|
||||
if (imuQuaternionHeadfreeOffsetSet()){
|
||||
beeper(BEEPER_RX_SET);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -25,7 +25,6 @@ extern int16_t magHold;
|
|||
#endif
|
||||
|
||||
extern bool isRXDataNew;
|
||||
extern int16_t headFreeModeHold;
|
||||
|
||||
typedef struct throttleCorrectionConfig_s {
|
||||
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
|
||||
|
|
|
|||
|
|
@ -328,12 +328,21 @@ void updateRcCommands(void)
|
|||
}
|
||||
|
||||
if (FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
|
||||
const float cosDiff = cos_approx(radDiff);
|
||||
const float sinDiff = sin_approx(radDiff);
|
||||
const float rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
|
||||
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
|
||||
rcCommand[PITCH] = rcCommand_PITCH;
|
||||
static t_fp_vector_def rcCommandBuff;
|
||||
|
||||
rcCommandBuff.X = rcCommand[ROLL];
|
||||
rcCommandBuff.Y = rcCommand[PITCH];
|
||||
if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)))) {
|
||||
rcCommandBuff.Z = rcCommand[YAW];
|
||||
} else {
|
||||
rcCommandBuff.Z = 0;
|
||||
}
|
||||
imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
|
||||
rcCommand[ROLL] = rcCommandBuff.X;
|
||||
rcCommand[PITCH] = rcCommandBuff.Y;
|
||||
if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)))) {
|
||||
rcCommand[YAW] = rcCommandBuff.Z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -27,7 +27,6 @@
|
|||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
|
@ -87,14 +86,19 @@ static float throttleAngleScale;
|
|||
static float fc_acc;
|
||||
static float smallAngleCosZ = 0;
|
||||
|
||||
static float magneticDeclination = 0.0f; // calculated at startup from config
|
||||
|
||||
static imuRuntimeConfig_t imuRuntimeConfig;
|
||||
|
||||
STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame
|
||||
STATIC_UNIT_TESTED float rMat[3][3];
|
||||
|
||||
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
// quaternion of sensor frame relative to earth frame
|
||||
STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE;
|
||||
STATIC_UNIT_TESTED quaternionProducts qP = QUATERNION_PRODUCTS_INITIALIZE;
|
||||
// headfree quaternions
|
||||
quaternion headfree = QUATERNION_INITIALIZE;
|
||||
quaternion offset = QUATERNION_INITIALIZE;
|
||||
|
||||
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
attitudeEulerAngles_t attitude = EULER_INITIALIZE;
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
|
||||
|
||||
|
|
@ -106,34 +110,24 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
|
|||
.acc_unarmedcal = 1
|
||||
);
|
||||
|
||||
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
|
||||
{
|
||||
float q1q1 = sq(q1);
|
||||
float q2q2 = sq(q2);
|
||||
float q3q3 = sq(q3);
|
||||
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){
|
||||
imuQuaternionComputeProducts(&q, &qP);
|
||||
|
||||
float q0q1 = q0 * q1;
|
||||
float q0q2 = q0 * q2;
|
||||
float q0q3 = q0 * q3;
|
||||
float q1q2 = q1 * q2;
|
||||
float q1q3 = q1 * q3;
|
||||
float q2q3 = q2 * q3;
|
||||
rMat[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz;
|
||||
rMat[0][1] = 2.0f * (qP.xy + -qP.wz);
|
||||
rMat[0][2] = 2.0f * (qP.xz - -qP.wy);
|
||||
|
||||
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
|
||||
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
|
||||
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
|
||||
rMat[1][0] = 2.0f * (qP.xy - -qP.wz);
|
||||
rMat[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz;
|
||||
rMat[1][2] = 2.0f * (qP.yz + -qP.wx);
|
||||
|
||||
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
|
||||
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
|
||||
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
|
||||
|
||||
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
|
||||
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
|
||||
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
|
||||
rMat[2][0] = 2.0f * (qP.xz + -qP.wy);
|
||||
rMat[2][1] = 2.0f * (qP.yz - -qP.wx);
|
||||
rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy;
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) && !defined(SET_IMU_FROM_EULER)
|
||||
rMat[1][0] = -2.0f * (q1q2 - -q0q3);
|
||||
rMat[2][0] = -2.0f * (q1q3 + -q0q2);
|
||||
rMat[1][0] = -2.0f * (qP.xy - -qP.wz);
|
||||
rMat[2][0] = -2.0f * (qP.xz + -qP.wy);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -345,31 +339,42 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
|
|||
gy *= (0.5f * dt);
|
||||
gz *= (0.5f * dt);
|
||||
|
||||
const float qa = q0;
|
||||
const float qb = q1;
|
||||
const float qc = q2;
|
||||
q0 += (-qb * gx - qc * gy - q3 * gz);
|
||||
q1 += (qa * gx + qc * gz - q3 * gy);
|
||||
q2 += (qa * gy - qb * gz + q3 * gx);
|
||||
q3 += (qa * gz + qb * gy - qc * gx);
|
||||
quaternion buffer;
|
||||
buffer.w = q.w;
|
||||
buffer.x = q.x;
|
||||
buffer.y = q.y;
|
||||
buffer.z = q.z;
|
||||
|
||||
q.w += (-buffer.x * gx - buffer.y * gy - buffer.z * gz);
|
||||
q.x += (+buffer.w * gx + buffer.y * gz - buffer.z * gy);
|
||||
q.y += (+buffer.w * gy - buffer.x * gz + buffer.z * gx);
|
||||
q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx);
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z));
|
||||
q.w *= recipNorm;
|
||||
q.x *= recipNorm;
|
||||
q.y *= recipNorm;
|
||||
q.z *= recipNorm;
|
||||
|
||||
// Pre-compute rotation matrix from quaternion
|
||||
imuComputeRotationMatrix();
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
|
||||
{
|
||||
/* Compute pitch/roll angles */
|
||||
attitude.values.roll = lrintf(atan2f(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
|
||||
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acosf(-rMat[2][0])) * (1800.0f / M_PIf));
|
||||
attitude.values.yaw = lrintf((-atan2f(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + magneticDeclination));
|
||||
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void){
|
||||
quaternionProducts buffer;
|
||||
|
||||
if (FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
imuQuaternionComputeProducts(&headfree, &buffer);
|
||||
|
||||
attitude.values.roll = lrintf(atan2_approx((+2.0f * (buffer.wx + buffer.yz)), (+1.0f - 2.0f * (buffer.xx + buffer.yy))) * (1800.0f / M_PIf));
|
||||
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(+2.0f * (buffer.wy - buffer.xz))) * (1800.0f / M_PIf));
|
||||
attitude.values.yaw = lrintf((-atan2_approx((+2.0f * (buffer.wz + buffer.xy)), (+1.0f - 2.0f * (buffer.yy + buffer.zz))) * (1800.0f / M_PIf)));
|
||||
} else {
|
||||
attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
|
||||
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf));
|
||||
attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf)));
|
||||
}
|
||||
|
||||
if (attitude.values.yaw < 0)
|
||||
attitude.values.yaw += 3600;
|
||||
|
|
@ -485,7 +490,7 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
|
|||
if (rMat[2][2] <= 0.015f) {
|
||||
return 0;
|
||||
}
|
||||
int angle = lrintf(acosf(rMat[2][2]) * throttleAngleScale);
|
||||
int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale);
|
||||
if (angle > 900)
|
||||
angle = 900;
|
||||
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
|
||||
|
|
@ -506,10 +511,10 @@ void imuSetAttitudeQuat(float w, float x, float y, float z)
|
|||
{
|
||||
IMU_LOCK;
|
||||
|
||||
q0 = w;
|
||||
q1 = x;
|
||||
q2 = y;
|
||||
q3 = z;
|
||||
q.w = w;
|
||||
q.x = x;
|
||||
q.y = y;
|
||||
q.z = z;
|
||||
|
||||
imuComputeRotationMatrix();
|
||||
imuUpdateEulerAngles();
|
||||
|
|
@ -528,3 +533,62 @@ void imuSetHasNewData(uint32_t dt)
|
|||
IMU_UNLOCK;
|
||||
}
|
||||
#endif
|
||||
|
||||
void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd) {
|
||||
quatProd->ww = quat->w * quat->w;
|
||||
quatProd->wx = quat->w * quat->x;
|
||||
quatProd->wy = quat->w * quat->y;
|
||||
quatProd->wz = quat->w * quat->z;
|
||||
quatProd->xx = quat->x * quat->x;
|
||||
quatProd->xy = quat->x * quat->y;
|
||||
quatProd->xz = quat->x * quat->z;
|
||||
quatProd->yy = quat->y * quat->y;
|
||||
quatProd->yz = quat->y * quat->z;
|
||||
quatProd->zz = quat->z * quat->z;
|
||||
}
|
||||
|
||||
bool imuQuaternionHeadfreeOffsetSet(void) {
|
||||
if ((ABS(attitude.values.roll) < 450) && (ABS(attitude.values.pitch) < 450)) {
|
||||
const float yaw = -atan2_approx((+2.0f * (qP.wz + qP.xy)), (+1.0f - 2.0f * (qP.yy + qP.zz)));
|
||||
|
||||
offset.w = cos_approx(yaw/2);
|
||||
offset.x = 0;
|
||||
offset.y = 0;
|
||||
offset.z = sin_approx(yaw/2);
|
||||
|
||||
return(true);
|
||||
} else {
|
||||
return(false);
|
||||
}
|
||||
}
|
||||
|
||||
void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result) {
|
||||
const float A = (q1->w + q1->x) * (q2->w + q2->x);
|
||||
const float B = (q1->z - q1->y) * (q2->y - q2->z);
|
||||
const float C = (q1->w - q1->x) * (q2->y + q2->z);
|
||||
const float D = (q1->y + q1->z) * (q2->w - q2->x);
|
||||
const float E = (q1->x + q1->z) * (q2->x + q2->y);
|
||||
const float F = (q1->x - q1->z) * (q2->x - q2->y);
|
||||
const float G = (q1->w + q1->y) * (q2->w - q2->z);
|
||||
const float H = (q1->w - q1->y) * (q2->w + q2->z);
|
||||
|
||||
result->w = B + (- E - F + G + H) / 2.0f;
|
||||
result->x = A - (+ E + F + G + H) / 2.0f;
|
||||
result->y = C + (+ E - F + G - H) / 2.0f;
|
||||
result->z = D + (+ E - F - G + H) / 2.0f;
|
||||
}
|
||||
|
||||
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v) {
|
||||
quaternionProducts buffer;
|
||||
|
||||
imuQuaternionMultiplication(&offset, &q, &headfree);
|
||||
imuQuaternionComputeProducts(&headfree, &buffer);
|
||||
|
||||
const float x = (buffer.ww + buffer.xx - buffer.yy - buffer.zz) * v->X + 2.0f * (buffer.xy + buffer.wz) * v->Y + 2.0f * (buffer.xz - buffer.wy) * v->Z;
|
||||
const float y = 2.0f * (buffer.xy - buffer.wz) * v->X + (buffer.ww - buffer.xx + buffer.yy - buffer.zz) * v->Y + 2.0f * (buffer.yz + buffer.wx) * v->Z;
|
||||
const float z = 2.0f * (buffer.xz + buffer.wy) * v->X + 2.0f * (buffer.yz - buffer.wx) * v->Y + (buffer.ww - buffer.xx - buffer.yy + buffer.zz) * v->Z;
|
||||
|
||||
v->X = x;
|
||||
v->Y = y;
|
||||
v->Z = z;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/time.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "config/parameter_group.h"
|
||||
|
||||
// Exported symbols
|
||||
|
|
@ -28,6 +28,15 @@ extern int accSumCount;
|
|||
extern float accVelScale;
|
||||
extern int32_t accSum[XYZ_AXIS_COUNT];
|
||||
|
||||
typedef struct {
|
||||
float w,x,y,z;
|
||||
} quaternion;
|
||||
#define QUATERNION_INITIALIZE {.w=1, .x=0, .y=0,.z=0}
|
||||
|
||||
typedef struct {
|
||||
float ww,wx,wy,wz,xx,xy,xz,yy,yz,zz;
|
||||
} quaternionProducts;
|
||||
#define QUATERNION_PRODUCTS_INITIALIZE {.ww=1, .wx=0, .wy=0, .wz=0, .xx=0, .xy=0, .xz=0, .yy=0, .yz=0, .zz=0}
|
||||
|
||||
typedef union {
|
||||
int16_t raw[XYZ_AXIS_COUNT];
|
||||
|
|
@ -38,6 +47,7 @@ typedef union {
|
|||
int16_t yaw;
|
||||
} values;
|
||||
} attitudeEulerAngles_t;
|
||||
#define EULER_INITIALIZE { { 0, 0, 0 } }
|
||||
|
||||
extern attitudeEulerAngles_t attitude;
|
||||
|
||||
|
|
@ -80,3 +90,7 @@ void imuSetAttitudeQuat(float w, float x, float y, float z);
|
|||
void imuSetHasNewData(uint32_t dt);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd);
|
||||
bool imuQuaternionHeadfreeOffsetSet(void);
|
||||
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v);
|
||||
|
|
|
|||
|
|
@ -89,7 +89,7 @@ static float hanningWindow[FFT_WINDOW_SIZE];
|
|||
void initHanning()
|
||||
{
|
||||
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
|
||||
hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
|
||||
hanningWindow[i] = (0.5 - 0.5 * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -96,7 +96,8 @@ cms_unittest_SRC := \
|
|||
|
||||
|
||||
common_filter_unittest_SRC := \
|
||||
$(USER_DIR)/common/filter.c
|
||||
$(USER_DIR)/common/filter.c \
|
||||
$(USER_DIR)/common/maths.c
|
||||
|
||||
|
||||
encoding_unittest_SRC := \
|
||||
|
|
|
|||
|
|
@ -404,4 +404,5 @@ extern "C" {
|
|||
void changePidProfile(uint8_t) {}
|
||||
void dashboardEnablePageCycling(void) {}
|
||||
void dashboardDisablePageCycling(void) {}
|
||||
bool imuQuaternionHeadfreeOffsetSet(void) { return true; }
|
||||
}
|
||||
|
|
|
|||
|
|
@ -56,7 +56,7 @@ extern "C" {
|
|||
void imuComputeRotationMatrix(void);
|
||||
void imuUpdateEulerAngles(void);
|
||||
|
||||
extern float q0, q1, q2, q3;
|
||||
extern quaternion q;
|
||||
extern float rMat[3][3];
|
||||
|
||||
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
|
||||
|
|
@ -79,10 +79,10 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
|
|||
#define TOL 1e-6
|
||||
|
||||
// No rotation
|
||||
q0 = 1.0f;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
q.w = 1.0f;
|
||||
q.x = 0.0f;
|
||||
q.y = 0.0f;
|
||||
q.z = 0.0f;
|
||||
|
||||
imuComputeRotationMatrix();
|
||||
|
||||
|
|
@ -97,10 +97,10 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
|
|||
EXPECT_FLOAT_EQ(1.0f, rMat[2][2]);
|
||||
|
||||
// 90 degrees around Z axis
|
||||
q0 = sqrt2over2;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = sqrt2over2;
|
||||
q.w = sqrt2over2;
|
||||
q.x = 0.0f;
|
||||
q.y = 0.0f;
|
||||
q.z = sqrt2over2;
|
||||
|
||||
imuComputeRotationMatrix();
|
||||
|
||||
|
|
@ -115,10 +115,10 @@ TEST(FlightImuTest, TestCalculateRotationMatrix)
|
|||
EXPECT_NEAR(1.0f, rMat[2][2], TOL);
|
||||
|
||||
// 60 degrees around X axis
|
||||
q0 = 0.866f;
|
||||
q1 = 0.5f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
q.w = 0.866f;
|
||||
q.x = 0.5f;
|
||||
q.y = 0.0f;
|
||||
q.z = 0.0f;
|
||||
|
||||
imuComputeRotationMatrix();
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue