rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/main/fc/core.c
View File

@ -320,7 +320,7 @@ void updateArmingStatus(void)
unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
}
if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
if (!isUpright() && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
setArmingDisabled(ARMING_DISABLED_ANGLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_ANGLE);
@ -627,7 +627,7 @@ static void updateInflightCalibrationState(void)
}
#if defined(USE_GPS) || defined(USE_MAG)
void updateMagHold(void)
static void updateMagHold(void)
{
if (fabsf(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
@ -636,7 +636,7 @@ void updateMagHold(void)
if (dif >= +180)
dif -= 360;
dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
if (STATE(SMALL_ANGLE)) {
if (isUpright()) {
rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30; // 18 deg
}
} else
@ -1130,8 +1130,8 @@ static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
startTime = micros();
}
#ifdef USE_MAG
if (sensors(SENSOR_MAG)) {
#if defined(USE_GPS) || defined(USE_MAG)
if (sensors(SENSOR_GPS) || sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif

2
src/main/fc/init.c
View File

@ -915,8 +915,6 @@ void init(void)
timerStart();
#endif
ENABLE_STATE(SMALL_ANGLE);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);

1
src/main/fc/runtime_config.h
View File

@ -114,7 +114,6 @@ typedef enum {
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))

141
src/main/flight/imu.c
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;
}

3
src/main/flight/imu.h
View File

@ -85,8 +85,7 @@ void imuSetHasNewData(uint32_t dt);
#endif
#endif
void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd);
bool imuQuaternionHeadfreeOffsetSet(void);
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v);
void imuComputeQuaternionFromRPY(quaternionProducts *qP, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw);
bool shouldInitializeGPSHeading(void);
bool isUpright(void);

4
src/main/osd/osd_elements.c
View File

@ -81,6 +81,7 @@
#include "common/typeconversion.h"
#include "common/utils.h"
#include "config/config.h"
#include "config/feature.h"
#include "drivers/display.h"
@ -89,7 +90,6 @@
#include "drivers/time.h"
#include "drivers/vtx_common.h"
#include "config/config.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"
#include "fc/rc_adjustments.h"
@ -622,7 +622,7 @@ static void osdElementCrashFlipArrow(osdElementParms_t *element)
rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
}
if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !STATE(SMALL_ANGLE))) && !((imuConfig()->small_angle < 180) && STATE(SMALL_ANGLE)) && (rollAngle || pitchAngle)) {
if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) {
if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
if (pitchAngle > 0) {
if (rollAngle > 0) {

24
src/test/unit/arming_prevention_unittest.cc
View File

@ -73,6 +73,7 @@ extern "C" {
uint16_t GPS_distanceToHome = 0;
int16_t GPS_directionToHome = 0;
acc_t acc = {};
bool mockIsUpright = false;
}
uint32_t simulationFeatureFlags = 0;
@ -129,7 +130,7 @@ TEST(ArmingPreventionTest, CalibrationPowerOnGraceAngleThrottleArmSwitch)
// given
// quad is level
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
// when
updateArmingStatus();
@ -190,7 +191,7 @@ TEST(ArmingPreventionTest, ArmingGuardRadioLeftOnAndArmed)
// and
rcData[THROTTLE] = 1000;
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
// when
updateActivatedModes();
@ -268,7 +269,7 @@ TEST(ArmingPreventionTest, Prearm)
// given
rcData[THROTTLE] = 1000;
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
// when
updateActivatedModes();
@ -311,7 +312,7 @@ TEST(ArmingPreventionTest, RadioTurnedOnAtAnyTimeArmed)
// and
rcData[THROTTLE] = 1000;
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
// and
// RX has no link to radio
@ -378,7 +379,7 @@ TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadband)
// and
rcData[THROTTLE] = 1400;
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
simulationHaveRx = true;
// and
@ -447,7 +448,7 @@ TEST(ArmingPreventionTest, When3DModeDisabledThenNormalThrottleArmingConditionAp
// and
// safe throttle value for 3D mode
rcData[THROTTLE] = 1500;
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
simulationHaveRx = true;
// and
@ -545,7 +546,7 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit
// and
rcData[THROTTLE] = 1000;
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
simulationHaveRx = true;
// and
@ -641,7 +642,7 @@ TEST(ArmingPreventionTest, GPSRescueWithoutFixPreventsArm)
rcData[THROTTLE] = 1000;
rcData[AUX1] = 1000;
rcData[AUX2] = 1000;
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
// when
updateActivatedModes();
@ -755,7 +756,7 @@ TEST(ArmingPreventionTest, GPSRescueSwitchPreventsArm)
rcData[THROTTLE] = 1000;
rcData[AUX1] = 1000;
rcData[AUX2] = 1800; // Start out with rescue enabled
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
// when
updateActivatedModes();
@ -867,7 +868,7 @@ TEST(ArmingPreventionTest, ParalyzeOnAtBoot)
rcData[THROTTLE] = 1000;
rcData[AUX1] = 1000;
rcData[AUX2] = 1800; // Paralyze on at boot
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
// when
updateActivatedModes();
@ -918,7 +919,7 @@ TEST(ArmingPreventionTest, Paralyze)
rcData[AUX1] = 1000;
rcData[AUX2] = 1800; // Start out with paralyze enabled
rcData[AUX3] = 1000;
ENABLE_STATE(SMALL_ANGLE);
mockIsUpright = true;
// when
updateActivatedModes();
@ -1091,4 +1092,5 @@ extern "C" {
void pidSetItermReset(bool) {}
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
bool isFixedWing(void) { return false; }
bool isUpright(void) { return mockIsUpright; }
}

15
src/test/unit/flight_imu_unittest.cc
View File

@ -59,6 +59,7 @@ extern "C" {
extern quaternion q;
extern float rMat[3][3];
extern bool attitudeIsEstablished;
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0);
@ -164,15 +165,17 @@ TEST(FlightImuTest, TestSmallAngle)
// given
imuConfigMutable()->small_angle = 25;
imuConfigure(0, 0);
attitudeIsEstablished = true;
// and
memset(rMat, 0.0, sizeof(float) * 9);
// when
imuUpdateEulerAngles();
imuComputeRotationMatrix();
// expect
EXPECT_EQ(0, STATE(SMALL_ANGLE));
EXPECT_EQ(true, isUpright());
// given
rMat[0][0] = r1;
@ -181,19 +184,19 @@ TEST(FlightImuTest, TestSmallAngle)
rMat[2][2] = r1;
// when
imuUpdateEulerAngles();
imuComputeRotationMatrix();
// expect
EXPECT_EQ(SMALL_ANGLE, STATE(SMALL_ANGLE));
EXPECT_EQ(true, isUpright());
// given
memset(rMat, 0.0, sizeof(float) * 9);
// when
imuUpdateEulerAngles();
imuComputeRotationMatrix();
// expect
EXPECT_EQ(0, STATE(SMALL_ANGLE));
EXPECT_EQ(true, isUpright());
}
// STUBS

1
src/test/unit/link_quality_unittest.cc
View File

@ -543,4 +543,5 @@ extern "C" {
return 0.0;
}
bool isUpright(void) { return true; }
}

1
src/test/unit/osd_unittest.cc
View File

@ -1173,4 +1173,5 @@ extern "C" {
int8_t calculateThrottlePercent(void) { return 0; }
uint32_t persistentObjectRead(persistentObjectId_e) { return 0; }
void persistentObjectWrite(persistentObjectId_e, uint32_t) {}
bool isUpright(void) { return true; }
}

1
src/test/unit/vtx_unittest.cc
View File

@ -179,4 +179,5 @@ extern "C" {
void pidSetItermReset(bool) {}
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
bool isFixedWing(void) { return false; }
bool isUpright(void) { return true; }
}