1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00

Improved detection of upright / 'SMALL_ANGLE' state.

This commit is contained in:
mikeller 2019-11-17 13:40:30 +13:00 committed by Michael Keller
parent 6694d4ebc8
commit cc8b8d3bf6
11 changed files with 108 additions and 95 deletions

View file

@ -101,6 +101,8 @@ static imuRuntimeConfig_t imuRuntimeConfig;
STATIC_UNIT_TESTED float rMat[3][3];
STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
// quaternion of sensor frame relative to earth frame
STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE;
STATIC_UNIT_TESTED quaternionProducts qP = QUATERNION_PRODUCTS_INITIALIZE;
@ -119,6 +121,20 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.small_angle = 25,
);
static 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;
}
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){
imuQuaternionComputeProducts(&q, &qP);
@ -320,6 +336,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix();
attitudeIsEstablished = true;
}
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
@ -338,14 +356,8 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf)));
}
if (attitude.values.yaw < 0)
if (attitude.values.yaw < 0) {
attitude.values.yaw += 3600;
// Update small angle state
if (rMat[2][2] > smallAngleCosZ) {
ENABLE_STATE(SMALL_ANGLE);
} else {
DISABLE_STATE(SMALL_ANGLE);
}
}
@ -428,6 +440,51 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
return ret;
}
static void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
if (initialRoll > 1800) {
initialRoll -= 3600;
}
if (initialPitch > 1800) {
initialPitch -= 3600;
}
if (initialYaw > 1800) {
initialYaw -= 3600;
}
const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 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 q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
quatProd->xx = sq(q1);
quatProd->yy = sq(q2);
quatProd->zz = sq(q3);
quatProd->xy = q1 * q2;
quatProd->xz = q1 * q3;
quatProd->yz = q2 * q3;
quatProd->wx = q0 * q1;
quatProd->wy = q0 * q2;
quatProd->wz = q0 * q3;
imuComputeRotationMatrix();
attitudeIsEstablished = true;
}
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{
static timeUs_t previousIMUUpdateTime;
@ -510,10 +567,10 @@ static int calculateThrottleAngleCorrection(void)
* small angle < 0.86 deg
* TODO: Define this small angle in config.
*/
if (rMat[2][2] <= 0.015f) {
if (getCosTiltAngle() <= 0.015f) {
return 0;
}
int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale);
int angle = lrintf(acos_approx(getCosTiltAngle()) * throttleAngleScale);
if (angle > 900)
angle = 900;
return lrintf(throttleAngleValue * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
@ -574,49 +631,6 @@ void getQuaternion(quaternion *quat)
quat->z = q.z;
}
void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
if (initialRoll > 1800) {
initialRoll -= 3600;
}
if (initialPitch > 1800) {
initialPitch -= 3600;
}
if (initialYaw > 1800) {
initialYaw -= 3600;
}
const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 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 q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
quatProd->xx = sq(q1);
quatProd->yy = sq(q2);
quatProd->zz = sq(q3);
quatProd->xy = q1 * q2;
quatProd->xz = q1 * q3;
quatProd->yz = q2 * q3;
quatProd->wx = q0 * q1;
quatProd->wy = q0 * q2;
quatProd->wz = q0 * q3;
imuComputeRotationMatrix();
}
#ifdef SIMULATOR_BUILD
void imuSetAttitudeRPY(float roll, float pitch, float yaw)
{
@ -628,6 +642,7 @@ void imuSetAttitudeRPY(float roll, float pitch, float yaw)
IMU_UNLOCK;
}
void imuSetAttitudeQuat(float w, float x, float y, float z)
{
IMU_LOCK;
@ -638,6 +653,9 @@ void imuSetAttitudeQuat(float w, float x, float y, float z)
q.z = z;
imuComputeRotationMatrix();
attitudeIsEstablished = true;
imuUpdateEulerAngles();
IMU_UNLOCK;
@ -655,20 +673,6 @@ void imuSetHasNewData(uint32_t dt)
}
#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)) {
@ -717,3 +721,8 @@ void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v)
v->Y = y;
v->Z = z;
}
bool isUpright(void)
{
return attitudeIsEstablished && getCosTiltAngle() <= smallAngleCosZ;
}