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

MSP layer for Programming PIDs

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-12-24 21:49:50 +01:00
parent ee5bc7baef
commit 9c11ceddf1
2 changed files with 32 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -36,6 +36,7 @@
#include "common/time.h"
#include "common/utils.h"
#include "programming/global_variables.h"
#include "programming/pid.h"
#include "config/parameter_group_ids.h"
@ -553,6 +554,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU32(dst, gvGet(i));
}
break;
case MSP2_INAV_PROGRAMMING_PID:
for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
sbufWriteU8(dst, programmingPids(i)->enabled);
sbufWriteU8(dst, programmingPids(i)->setpoint.type);
sbufWriteU32(dst, programmingPids(i)->setpoint.value);
sbufWriteU8(dst, programmingPids(i)->measurement.type);
sbufWriteU32(dst, programmingPids(i)->measurement.value);
sbufWriteU16(dst, programmingPids(i)->gains.P);
sbufWriteU16(dst, programmingPids(i)->gains.I);
sbufWriteU16(dst, programmingPids(i)->gains.D);
sbufWriteU16(dst, programmingPids(i)->gains.FF);
}
break;
#endif
case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@ -1969,6 +1983,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} else
return MSP_RESULT_ERROR;
break;
case MSP2_INAV_SET_PROGRAMMING_PID:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 20) && (tmp_u8 < MAX_PROGRAMMING_PID_COUNT)) {
programmingPidsMutable(tmp_u8)->enabled = sbufReadU8(src);
programmingPidsMutable(tmp_u8)->setpoint.type = sbufReadU8(src);
programmingPidsMutable(tmp_u8)->setpoint.value = sbufReadU32(src);
programmingPidsMutable(tmp_u8)->measurement.type = sbufReadU8(src);
programmingPidsMutable(tmp_u8)->measurement.value = sbufReadU32(src);
programmingPidsMutable(tmp_u8)->gains.P = sbufReadU16(src);
programmingPidsMutable(tmp_u8)->gains.I = sbufReadU16(src);
programmingPidsMutable(tmp_u8)->gains.D = sbufReadU16(src);
programmingPidsMutable(tmp_u8)->gains.FF = sbufReadU16(src);
} else
return MSP_RESULT_ERROR;
break;
#endif
case MSP2_COMMON_SET_MOTOR_MIXER:
sbufReadU8Safe(&tmp_u8, src);