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:
parent
6694d4ebc8
commit
cc8b8d3bf6
11 changed files with 108 additions and 95 deletions
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue