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

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/time.h"
#include "common/utils.h" #include "common/utils.h"
#include "programming/global_variables.h" #include "programming/global_variables.h"
#include "programming/pid.h"
#include "config/parameter_group_ids.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)); sbufWriteU32(dst, gvGet(i));
} }
break; 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 #endif
case MSP2_COMMON_MOTOR_MIXER: case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { 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 } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; 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 #endif
case MSP2_COMMON_SET_MOTOR_MIXER: case MSP2_COMMON_SET_MOTOR_MIXER:
sbufReadU8Safe(&tmp_u8, src); sbufReadU8Safe(&tmp_u8, src);

2
src/main/msp/msp_protocol_v2_inav.h
View file

@ -61,6 +61,8 @@
#define MSP2_INAV_SET_GLOBAL_FUNCTIONS 0x2025 #define MSP2_INAV_SET_GLOBAL_FUNCTIONS 0x2025
#define MSP2_INAV_LOGIC_CONDITIONS_STATUS 0x2026 #define MSP2_INAV_LOGIC_CONDITIONS_STATUS 0x2026
#define MSP2_INAV_GVAR_STATUS 0x2027 #define MSP2_INAV_GVAR_STATUS 0x2027
#define MSP2_INAV_PROGRAMMING_PID 0x2028
#define MSP2_INAV_SET_PROGRAMMING_PID 0x2029
#define MSP2_PID 0x2030 #define MSP2_PID 0x2030
#define MSP2_SET_PID 0x2031 #define MSP2_SET_PID 0x2031