Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Report individual 6-axis calibration orientations in MSP_STATUS_EX

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-01-16 20:34:09 +10:00
parent ac3c77ae73
commit 375a6f5d71
3 changed files with 35 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/fc/fc_msp.c
View file

@ -566,6 +566,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, getCurrentProfileIndex());
sbufWriteU16(dst, averageSystemLoadPercent);
sbufWriteU16(dst, armingFlags);
sbufWriteU8(dst, getAccelerometerCalibrationAxisFlags());
break;
case MSP_STATUS:

35
src/main/sensors/acceleration.c
View file

@ -296,17 +296,45 @@ static bool calibratedAxis[6];
static int32_t accSamples[6][3];
static int calibratedAxisCount = 0;
bool getAccelerometerCalibrationAxisStatus(int axis)
{
if (isAccelerationCalibrationComplete()) {
if (STATE(ACCELEROMETER_CALIBRATED)) {
return true; // if calibration is valid - all axis are calibrated
}
else {
return calibratedAxis[axis];
}
}
else {
return calibratedAxis[axis];
}
}
uint8_t getAccelerometerCalibrationAxisFlags(void)
{
uint8_t flags = 0;
for (int i = 0; i < 6; i++) {
if (getAccelerometerCalibrationAxisStatus(0)) {
flags |= (1 << i);
}
}
return flags;
}
int getPrimaryAxisIndex(int32_t sample[3])
{
if (ABS(sample[Z]) > ABS(sample[X]) && ABS(sample[Z]) > ABS(sample[Y])) {
// Tolerate up to atan(1 / 1.5) = 33 deg tilt (in worst case 66 deg separation between points)
if ((ABS(sample[Z]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Z]) / 1.5f) > ABS(sample[Y])) {
//Z-axis
return (sample[Z] > 0) ? 0 : 1;
}
else if (ABS(sample[X]) > ABS(sample[Y]) && ABS(sample[X]) > ABS(sample[Z])) {
else if ((ABS(sample[X]) / 1.5f) > ABS(sample[Y]) && (ABS(sample[X]) / 1.5f) > ABS(sample[Z])) {
//X-axis
return (sample[X] > 0) ? 2 : 3;
}
else if (ABS(sample[Y]) > ABS(sample[X]) && ABS(sample[Y]) > ABS(sample[Z])) {
else if ((ABS(sample[Y]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Y]) / 1.5f) > ABS(sample[Z])) {
//Y-axis
return (sample[Y] > 0) ? 4 : 5;
}
@ -334,6 +362,7 @@ void performAcclerationCalibration(void)
calibratedAxisCount = 0;
sensorCalibrationResetState(&calState);
DISABLE_STATE(ACCELEROMETER_CALIBRATED);
}
if (!calibratedAxis[axisIndex]) {

2
src/main/sensors/acceleration.h
View file

@ -62,3 +62,5 @@ void updateAccelerationReadings(void);
void setAccelerationCalibrationValues(void);
void setAccelerationFilter(void);
bool isAccelerometerHealthy(void);
bool getAccelerometerCalibrationAxisStatus(int axis);
uint8_t getAccelerometerCalibrationAxisFlags(void);