Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

some fix & should be usable

Browse source 
use quaternion directly & we can fly in 3D mode now

rename uartPort_t to tcpPort_t

fix race on ACC, GYRO, IMU

fix gyro scale & disable SystemLoad calculate

update README.md

remove some unused

fix scale on 3D mode
This commit is contained in:
cs8425 2017-04-06 00:41:40 +08:00
parent 9053feb418
commit efbb7520a8
10 changed files with 350 additions and 70 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -49,6 +49,28 @@
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
#include <stdio.h>
#include <pthread.h>
#define printf printf
#define sprintf sprintf
static pthread_mutex_t imuUpdateLock;
#if defined(SIMULATOR_IMU_SYNC)
static uint32_t imuDeltaT = 0;
static bool imuUpdated = false;
#endif
#define IMU_LOCK pthread_mutex_unlock(&imuUpdateLock)
#define IMU_UNLOCK pthread_mutex_unlock(&imuUpdateLock)
#else
#define IMU_LOCK
#define IMU_UNLOCK
#endif
// the limit (in degrees/second) beyond which we stop integrating
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
@ -110,6 +132,11 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
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;
#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);
#endif
}
/*
@ -142,6 +169,12 @@ void imuInit(void)
accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
imuComputeRotationMatrix();
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
if (pthread_mutex_init(&imuUpdateLock, NULL) != 0) {
printf("Create imuUpdateLock error!\n");
}
#endif
}
void imuResetAccelerationSum(void)
@ -402,6 +435,12 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
UNUSED(useYaw);
UNUSED(rawYawError);
#else
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
// printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real());
deltaT = imuDeltaT;
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]),
useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
@ -416,7 +455,16 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
void imuUpdateAttitude(timeUs_t currentTimeUs)
{
if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
IMU_LOCK;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
if(imuUpdated == false){
IMU_UNLOCK;
return;
}
imuUpdated = false;
#endif
imuCalculateEstimatedAttitude(currentTimeUs);
IMU_UNLOCK;
} else {
acc.accSmooth[X] = 0;
acc.accSmooth[Y] = 0;
@ -448,12 +496,18 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
#ifdef SIMULATOR_BUILD
void imuSetAttitudeRPY(float roll, float pitch, float yaw)
{
IMU_LOCK;
attitude.values.roll = roll * 10;
attitude.values.pitch = pitch * 10;
attitude.values.yaw = yaw * 10;
IMU_UNLOCK;
}
void imuSetAttitudeQuat(float w, float x, float y, float z)
{
IMU_LOCK;
q0 = w;
q1 = x;
q2 = y;
@ -461,6 +515,19 @@ void imuSetAttitudeQuat(float w, float x, float y, float z)
imuComputeRotationMatrix();
imuUpdateEulerAngles();
IMU_UNLOCK;
}
#endif
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
void imuSetHasNewData(uint32_t dt)
{
IMU_LOCK;
imuUpdated = true;
imuDeltaT = dt;
IMU_UNLOCK;
}
#endif