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

MSP2 frames to support mmix command over MSP

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-01-26 11:17:31 +01:00
parent 873946d7ec
commit 8584cf50cd
2 changed files with 24 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -444,6 +444,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
#endif
case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
sbufWriteU16(dst, customMotorMixer(i)->throttle * 1000);
sbufWriteU16(dst, customMotorMixer(i)->roll * 1000);
sbufWriteU16(dst, customMotorMixer(i)->pitch * 1000);
sbufWriteU16(dst, customMotorMixer(i)->yaw * 1000);
}
break;
case MSP_MOTOR:
for (unsigned i = 0; i < 8; i++) {
sbufWriteU16(dst, i < MAX_SUPPORTED_MOTORS ? motor[i] : 0);
@ -1447,6 +1456,18 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif
break;
case MSP2_COMMON_SET_MOTOR_MIXER:
i = sbufReadU8(src);
if (i >= MAX_SUPPORTED_MOTORS) {
return MSP_RESULT_ERROR;
} else {
customMotorMixerMutable(i)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0, 1);
customMotorMixerMutable(i)->roll = constrainf(sbufReadU16(src) / 1000.0f, 0, 1);
customMotorMixerMutable(i)->pitch = constrainf(sbufReadU16(src) / 1000.0f, 0, 1);
customMotorMixerMutable(i)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0, 1);
}
break;
case MSP_SET_3D:
flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);