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

Merge pull request #7506 from etracer65/gyro_align_msp_compatibility 

Update MSP gyro alignment support and preserve backwards compatibility
This commit is contained in:
Michael Keller 2019-02-03 10:42:26 +13:00 committed by GitHub
parent 67043e4ffc d66533fb5a
commit d833a51317
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 69 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
src/main/msp/msp.c
View File

@ -101,6 +101,7 @@
#include "pg/beeper.h" #include "pg/beeper.h"
#include "pg/board.h" #include "pg/board.h"
#include "pg/gyrodev.h"
#include "pg/pg.h" #include "pg/pg.h"
#include "pg/pg_ids.h" #include "pg/pg_ids.h"
#include "pg/rx.h" #include "pg/rx.h"
@ -1279,11 +1280,41 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
break; break;
case MSP_SENSOR_ALIGNMENT: case MSP_SENSOR_ALIGNMENT: {
sbufWriteU8(dst, 0); // gyro_align uint8_t gyroAlignment;
sbufWriteU8(dst, 0); // acc_align #ifdef USE_MULTI_GYRO
switch (gyroConfig()->gyro_to_use) {
case GYRO_CONFIG_USE_GYRO_2:
gyroAlignment = gyroDeviceConfig(1)->align;
break;
case GYRO_CONFIG_USE_GYRO_BOTH:
// for dual-gyro in "BOTH" mode we only read/write gyro 0
default:
gyroAlignment = gyroDeviceConfig(0)->align;
break;
}
#else
gyroAlignment = gyroDeviceConfig(0)->align;
#endif
sbufWriteU8(dst, gyroAlignment);
sbufWriteU8(dst, gyroAlignment); // Starting with 4.0 gyro and acc alignment are the same
sbufWriteU8(dst, compassConfig()->mag_align); sbufWriteU8(dst, compassConfig()->mag_align);
// API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
#ifdef USE_MULTI_GYRO
sbufWriteU8(dst, 1); // USE_MULTI_GYRO
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
sbufWriteU8(dst, gyroDeviceConfig(0)->align);
sbufWriteU8(dst, gyroDeviceConfig(1)->align);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1);
sbufWriteU8(dst, gyroDeviceConfig(0)->align);
sbufWriteU8(dst, ALIGN_DEFAULT);
#endif
break; break;
}
case MSP_ADVANCED_CONFIG: case MSP_ADVANCED_CONFIG:
sbufWriteU8(dst, gyroConfig()->gyro_sync_denom); sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
@ -1837,11 +1868,43 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_RESET_CURR_PID: case MSP_SET_RESET_CURR_PID:
resetPidProfile(currentPidProfile); resetPidProfile(currentPidProfile);
break; break;
case MSP_SET_SENSOR_ALIGNMENT: case MSP_SET_SENSOR_ALIGNMENT: {
sbufReadU8(src); // gyro_align // maintain backwards compatibility for API < 1.41
sbufReadU8(src); // acc_align const uint8_t gyroAlignment = sbufReadU8(src);
sbufReadU8(src); // discard deprecated acc_align
compassConfigMutable()->mag_align = sbufReadU8(src); compassConfigMutable()->mag_align = sbufReadU8(src);
if (sbufBytesRemaining(src) >= 3) {
// API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
#ifdef USE_MULTI_GYRO
gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
gyroDeviceConfigMutable(0)->align = sbufReadU8(src);
gyroDeviceConfigMutable(1)->align = sbufReadU8(src);
#else
sbufReadU8(src); // unused gyro_to_use
gyroDeviceConfigMutable(0)->align = sbufReadU8(src);
sbufReadU8(src); // unused gyro_2_sensor_align
#endif
} else {
// maintain backwards compatibility for API < 1.41
#ifdef USE_MULTI_GYRO
switch (gyroConfig()->gyro_to_use) {
case GYRO_CONFIG_USE_GYRO_2:
gyroDeviceConfigMutable(1)->align = gyroAlignment;
break;
case GYRO_CONFIG_USE_GYRO_BOTH:
// For dual-gyro in "BOTH" mode we'll only update gyro 0
default:
gyroDeviceConfigMutable(0)->align = gyroAlignment;
break;
}
#else
gyroDeviceConfigMutable(0)->align = gyroAlignment;
#endif
}
break; break;
}
case MSP_SET_ADVANCED_CONFIG: case MSP_SET_ADVANCED_CONFIG:
gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src); gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);