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

Per @martinbudden 's formula

Browse source
This commit is contained in:
jflyper 2017-08-01 14:34:13 +09:00
parent 6e2feb5dce
commit 38b90c69fe
1 changed files with 11 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -738,14 +738,19 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
case MSP_RAW_IMU: case MSP_RAW_IMU:
{ {
// Hack scale due to choice of units for sensor data in multiwii // Hack scale due to choice of units for sensor data in multiwii
const uint8_t scale = acc.dev.acc_1G / 512;
uint8_t scale;
if (acc.dev.acc_1G > 512*4) {
scale = 8;
} else if (acc.dev.acc_1G >= 512) {
scale = 4;
} else {
scale = 1;
}
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (scale) { sbufWriteU16(dst, acc.accSmooth[i] / scale);
sbufWriteU16(dst, acc.accSmooth[i] / scale);
} else {
sbufWriteU16(dst, acc.accSmooth[i] * 2);
}
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroRateDps(i)); sbufWriteU16(dst, gyroRateDps(i));