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

no overrides before sensors are calibrated

Browse source
This commit is contained in:
Roman Lut 2022-06-26 21:15:46 +03:00
parent c6fae8d562
commit 37b9bd41ab
3 changed files with 23 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -3435,7 +3435,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
LOG_D(SYSTEM, "Simulator enabled");
}
if (dataSize >= 14) {
if (dataSize >= 14 && isImuReady()) {
gpsSol.fixType = sbufReadU8(src);
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.flags.hasNewData = true;
@ -3463,6 +3463,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;// acceleration in 1G units
acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accVibeSq[X] = 0;
acc.accVibeSq[Y] = 0;
acc.accVibeSq[Z] = 0;
gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
@ -3473,21 +3476,22 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
else {
DISABLE_STATE(GPS_FIX);
}
sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_ROLL]);
sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_PITCH]);
sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_YAW]);
sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_THROTTLE]);
simulatorData.debugIndex++;
if (simulatorData.debugIndex == 8){
simulatorData.debugIndex = 0;
}
sbufWriteU8(dst, simulatorData.debugIndex);
sbufWriteU32(dst, debug[simulatorData.debugIndex]);
mspWriteSimulatorOSD(dst);
}
sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_ROLL]);
sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_PITCH]);
sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_YAW]);
sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_THROTTLE]);
simulatorData.debugIndex++;
if (simulatorData.debugIndex == 8){
simulatorData.debugIndex = 0;
}
sbufWriteU8(dst, simulatorData.debugIndex);
sbufWriteU32(dst, debug[simulatorData.debugIndex]);
mspWriteSimulatorOSD(dst);
*ret = MSP_RESULT_ACK;
break;
default:

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

@ -518,6 +518,8 @@ float accGetMeasuredMaxG(void)
void accUpdate(void)
{
if (ARMING_FLAG(SIMULATOR_MODE)) {
//output: acc.accADCf
//unused: acc.dev.ADCRaw[], acc.accClipCount, acc.accVibeSq[]
return;
}
if (!acc.dev.readFn(&acc.dev)) {

2
src/main/sensors/gyro.c
View file

@ -498,6 +498,8 @@ void FAST_CODE NOINLINE gyroFilter()
void FAST_CODE NOINLINE gyroUpdate()
{
if (ARMING_FLAG(SIMULATOR_MODE)) {
//output: gyro.gyroADCf[axis]
//unused: dev->gyroADCRaw[], dev->gyroZero[];
return;
}