mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 01:05:21 +03:00
Drop MSP_PID and MSP_SET_PID frames
This commit is contained in:
Pawel Spychalski (DzikuVx)
parent
8c22c97a85
commit
2953b1bbe9
3 changed files with 0 additions and 225 deletions
|
|
@ -700,14 +700,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_PID:
|
|
||||||
for (int i = 0; i < PID_ITEM_COUNT; i++) {
|
|
||||||
sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
|
|
||||||
sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
|
|
||||||
sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSP2_PID:
|
case MSP2_PID:
|
||||||
for (int i = 0; i < PID_ITEM_COUNT; i++) {
|
for (int i = 0; i < PID_ITEM_COUNT; i++) {
|
||||||
sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
|
sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
|
||||||
|
|
@ -1660,19 +1652,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
||||||
return MSP_RESULT_ERROR;
|
return MSP_RESULT_ERROR;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_SET_PID:
|
|
||||||
if (dataSize >= PID_ITEM_COUNT * 3) {
|
|
||||||
for (int i = 0; i < PID_ITEM_COUNT; i++) {
|
|
||||||
pidBankMutable()->pid[i].P = sbufReadU8(src);
|
|
||||||
pidBankMutable()->pid[i].I = sbufReadU8(src);
|
|
||||||
pidBankMutable()->pid[i].D = sbufReadU8(src);
|
|
||||||
}
|
|
||||||
schedulePidGainsUpdate();
|
|
||||||
navigationUsePIDs();
|
|
||||||
} else
|
|
||||||
return MSP_RESULT_ERROR;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSP2_SET_PID:
|
case MSP2_SET_PID:
|
||||||
if (dataSize >= PID_ITEM_COUNT * 4) {
|
if (dataSize >= PID_ITEM_COUNT * 4) {
|
||||||
for (int i = 0; i < PID_ITEM_COUNT; i++) {
|
for (int i = 0; i < PID_ITEM_COUNT; i++) {
|
||||||
|
|
|
||||||
|
|
@ -243,7 +243,6 @@
|
||||||
#define MSP_ALTITUDE 109 //out message altitude, variometer
|
#define MSP_ALTITUDE 109 //out message altitude, variometer
|
||||||
#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
|
#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
|
||||||
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
|
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
|
||||||
#define MSP_PID 112 //out message P I D coeff (9 are used currently)
|
|
||||||
#define MSP_ACTIVEBOXES 113 //out message Active box flags (full width, more than 32 bits)
|
#define MSP_ACTIVEBOXES 113 //out message Active box flags (full width, more than 32 bits)
|
||||||
#define MSP_MISC 114 //out message powermeter trig
|
#define MSP_MISC 114 //out message powermeter trig
|
||||||
#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
|
#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
|
||||||
|
|
@ -261,7 +260,6 @@
|
||||||
|
|
||||||
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
|
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
|
||||||
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
|
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
|
||||||
#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
|
|
||||||
#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
|
#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
|
||||||
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
|
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
|
||||||
#define MSP_ACC_CALIBRATION 205 //in message no param
|
#define MSP_ACC_CALIBRATION 205 //in message no param
|
||||||
|
|
|
||||||
|
|
@ -231,208 +231,6 @@ TEST_F(SerialMspUnitTest, TestMspProcessReceivedCommand)
|
||||||
EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[3]);
|
EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[3]);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialMspUnitTest, Test_PIDValuesInt)
|
|
||||||
{
|
|
||||||
// check the buffer is big enough for the data to read in
|
|
||||||
EXPECT_LE(sizeof(mspHeader_t) + 3 * PID_ITEM_COUNT + 1, MSP_PORT_INBUF_SIZE); // +1 for checksum
|
|
||||||
// set up some test data
|
|
||||||
const int P8_ROLL = 40;
|
|
||||||
const int I8_ROLL = 30;
|
|
||||||
const int D8_ROLL = 23;
|
|
||||||
const int P8_PITCH = 41;
|
|
||||||
const int I8_PITCH = 31;
|
|
||||||
const int D8_PITCH = 24;
|
|
||||||
const int P8_YAW = 85;
|
|
||||||
const int I8_YAW = 45;
|
|
||||||
const int D8_YAW = 1;
|
|
||||||
const int P8_PIDALT = 50;
|
|
||||||
const int I8_PIDALT = 2;
|
|
||||||
const int D8_PIDALT = 3;
|
|
||||||
const int P8_PIDPOS = 15; // POSHOLD_P * 100;
|
|
||||||
const int I8_PIDPOS = 4; // POSHOLD_I * 100;
|
|
||||||
const int D8_PIDPOS = 5;
|
|
||||||
const int P8_PIDPOSR = 34; // POSHOLD_RATE_P * 10;
|
|
||||||
const int I8_PIDPOSR = 14; // POSHOLD_RATE_I * 100;
|
|
||||||
const int D8_PIDPOSR = 53; // POSHOLD_RATE_D * 1000;
|
|
||||||
const int P8_PIDNAVR = 25; // NAV_P * 10;
|
|
||||||
const int I8_PIDNAVR = 33; // NAV_I * 100;
|
|
||||||
const int D8_PIDNAVR = 83; // NAV_D * 1000;
|
|
||||||
const int P8_PIDLEVEL = 90;
|
|
||||||
const int I8_PIDLEVEL = 10;
|
|
||||||
const int D8_PIDLEVEL = 100;
|
|
||||||
const int P8_PIDMAG = 40;
|
|
||||||
const int P8_PIDVEL = 120;
|
|
||||||
const int I8_PIDVEL = 45;
|
|
||||||
const int D8_PIDVEL = 7;
|
|
||||||
|
|
||||||
currentProfile = &profile;
|
|
||||||
currentProfile->pidProfile.pidController = PID_CONTROLLER_MWREWRITE;
|
|
||||||
currentProfile->pidProfile.P8[PIDROLL] = P8_ROLL;
|
|
||||||
currentProfile->pidProfile.I8[PIDROLL] = I8_ROLL;
|
|
||||||
currentProfile->pidProfile.D8[PIDROLL] = D8_ROLL;
|
|
||||||
currentProfile->pidProfile.P8[PIDPITCH] = P8_PITCH;
|
|
||||||
currentProfile->pidProfile.I8[PIDPITCH] = I8_PITCH;
|
|
||||||
currentProfile->pidProfile.D8[PIDPITCH] = D8_PITCH;
|
|
||||||
currentProfile->pidProfile.P8[PIDYAW] = P8_YAW;
|
|
||||||
currentProfile->pidProfile.I8[PIDYAW] = I8_YAW;
|
|
||||||
currentProfile->pidProfile.D8[PIDYAW] = D8_YAW;
|
|
||||||
currentProfile->pidProfile.P8[PIDALT] = P8_PIDALT;
|
|
||||||
currentProfile->pidProfile.I8[PIDALT] = I8_PIDALT;
|
|
||||||
currentProfile->pidProfile.D8[PIDALT] = D8_PIDALT;
|
|
||||||
currentProfile->pidProfile.P8[PIDPOS] = P8_PIDPOS;
|
|
||||||
currentProfile->pidProfile.I8[PIDPOS] = I8_PIDPOS;
|
|
||||||
currentProfile->pidProfile.D8[PIDPOS] = D8_PIDPOS;
|
|
||||||
currentProfile->pidProfile.P8[PIDPOSR] = P8_PIDPOSR;
|
|
||||||
currentProfile->pidProfile.I8[PIDPOSR] = I8_PIDPOSR;
|
|
||||||
currentProfile->pidProfile.D8[PIDPOSR] = D8_PIDPOSR;
|
|
||||||
currentProfile->pidProfile.P8[PIDNAVR] = P8_PIDNAVR;
|
|
||||||
currentProfile->pidProfile.I8[PIDNAVR] = I8_PIDNAVR;
|
|
||||||
currentProfile->pidProfile.D8[PIDNAVR] = D8_PIDNAVR;
|
|
||||||
currentProfile->pidProfile.P8[PIDLEVEL] = P8_PIDLEVEL;
|
|
||||||
currentProfile->pidProfile.I8[PIDLEVEL] = I8_PIDLEVEL;
|
|
||||||
currentProfile->pidProfile.D8[PIDLEVEL] = D8_PIDLEVEL;
|
|
||||||
currentProfile->pidProfile.P8[PIDMAG] = P8_PIDMAG;
|
|
||||||
currentProfile->pidProfile.P8[PIDVEL] = P8_PIDVEL;
|
|
||||||
currentProfile->pidProfile.I8[PIDVEL] = I8_PIDVEL;
|
|
||||||
currentProfile->pidProfile.D8[PIDVEL] = D8_PIDVEL;
|
|
||||||
|
|
||||||
// use the MSP to write out the PID values
|
|
||||||
serialWritePos = 0;
|
|
||||||
serialReadPos = 0;
|
|
||||||
currentPort->cmdMSP = MSP_PID;
|
|
||||||
mspProcessReceivedCommand();
|
|
||||||
EXPECT_EQ(3 * PID_ITEM_COUNT, serialBuffer.mspResponse.header.size);
|
|
||||||
// check few values, just to make sure they have been written correctly
|
|
||||||
EXPECT_EQ(P8_YAW, serialBuffer.mspResponse.payload[6]);
|
|
||||||
EXPECT_EQ(I8_YAW, serialBuffer.mspResponse.payload[7]);
|
|
||||||
EXPECT_EQ(D8_YAW, serialBuffer.mspResponse.payload[8]);
|
|
||||||
|
|
||||||
// reset test values to zero, so we can check if they get read properly
|
|
||||||
memset(¤tProfile->pidProfile, 0, sizeof(currentProfile->pidProfile));
|
|
||||||
|
|
||||||
// now use the MSP to read back the PID values and check they are the same as written
|
|
||||||
// spoof a change from the written MSP_PID to the readable MSP_SET_PID
|
|
||||||
currentPort->cmdMSP = MSP_SET_PID;
|
|
||||||
serialBuffer.mspResponse.header.direction = '<';
|
|
||||||
serialBuffer.mspResponse.header.type = currentPort->cmdMSP;
|
|
||||||
// force the checksum
|
|
||||||
serialBuffer.mspResponse.payload[3 * PID_ITEM_COUNT] ^= MSP_PID;
|
|
||||||
serialBuffer.mspResponse.payload[3 * PID_ITEM_COUNT] ^= MSP_SET_PID;
|
|
||||||
// copy the command data into the current port inBuf so it can be processed
|
|
||||||
memcpy(currentPort->inBuf, serialBuffer.buf, MSP_PORT_INBUF_SIZE);
|
|
||||||
|
|
||||||
// set the offset into the payload
|
|
||||||
currentPort->indRX = offsetof(struct mspResonse_s, payload);
|
|
||||||
mspProcessReceivedCommand();
|
|
||||||
|
|
||||||
// check the values are as expected
|
|
||||||
EXPECT_EQ(P8_ROLL, currentProfile->pidProfile.P8[PIDROLL]);
|
|
||||||
EXPECT_EQ(I8_ROLL, currentProfile->pidProfile.I8[PIDROLL]);
|
|
||||||
EXPECT_EQ(D8_ROLL, currentProfile->pidProfile.D8[PIDROLL]);
|
|
||||||
EXPECT_EQ(P8_PITCH, currentProfile->pidProfile.P8[PIDPITCH]);
|
|
||||||
EXPECT_EQ(I8_PITCH, currentProfile->pidProfile.I8[PIDPITCH]);
|
|
||||||
EXPECT_EQ(D8_PITCH, currentProfile->pidProfile.D8[PIDPITCH]);
|
|
||||||
EXPECT_EQ(P8_YAW, currentProfile->pidProfile.P8[PIDYAW]);
|
|
||||||
EXPECT_EQ(I8_YAW, currentProfile->pidProfile.I8[PIDYAW]);
|
|
||||||
EXPECT_EQ(D8_YAW, currentProfile->pidProfile.D8[PIDYAW]);
|
|
||||||
EXPECT_EQ(P8_PIDALT, currentProfile->pidProfile.P8[PIDALT]);
|
|
||||||
EXPECT_EQ(I8_PIDALT, currentProfile->pidProfile.I8[PIDALT]);
|
|
||||||
EXPECT_EQ(D8_PIDALT, currentProfile->pidProfile.D8[PIDALT]);
|
|
||||||
EXPECT_EQ(P8_PIDPOS, currentProfile->pidProfile.P8[PIDPOS]);
|
|
||||||
EXPECT_EQ(I8_PIDPOS, currentProfile->pidProfile.I8[PIDPOS]);
|
|
||||||
EXPECT_EQ(D8_PIDPOS, currentProfile->pidProfile.D8[PIDPOS]);
|
|
||||||
EXPECT_EQ(P8_PIDPOSR, currentProfile->pidProfile.P8[PIDPOSR]);
|
|
||||||
EXPECT_EQ(I8_PIDPOSR, currentProfile->pidProfile.I8[PIDPOSR]);
|
|
||||||
EXPECT_EQ(D8_PIDPOSR, currentProfile->pidProfile.D8[PIDPOSR]);
|
|
||||||
EXPECT_EQ(P8_PIDNAVR, currentProfile->pidProfile.P8[PIDNAVR]);
|
|
||||||
EXPECT_EQ(I8_PIDNAVR, currentProfile->pidProfile.I8[PIDNAVR]);
|
|
||||||
EXPECT_EQ(D8_PIDNAVR, currentProfile->pidProfile.D8[PIDNAVR]);
|
|
||||||
EXPECT_EQ(P8_PIDLEVEL, currentProfile->pidProfile.P8[PIDLEVEL]);
|
|
||||||
EXPECT_EQ(I8_PIDLEVEL, currentProfile->pidProfile.I8[PIDLEVEL]);
|
|
||||||
EXPECT_EQ(D8_PIDLEVEL, currentProfile->pidProfile.D8[PIDLEVEL]);
|
|
||||||
EXPECT_EQ(P8_PIDMAG, currentProfile->pidProfile.P8[PIDMAG]);
|
|
||||||
EXPECT_EQ(P8_PIDVEL, currentProfile->pidProfile.P8[PIDVEL]);
|
|
||||||
EXPECT_EQ(I8_PIDVEL, currentProfile->pidProfile.I8[PIDVEL]);
|
|
||||||
EXPECT_EQ(D8_PIDVEL, currentProfile->pidProfile.D8[PIDVEL]);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_F(SerialMspUnitTest, Test_PIDValuesFloat)
|
|
||||||
{
|
|
||||||
// check the buffer is big enough for the data to read in
|
|
||||||
EXPECT_LE(sizeof(mspHeader_t) + 3 * PID_ITEM_COUNT + 1, MSP_PORT_INBUF_SIZE); // +1 for checksum
|
|
||||||
|
|
||||||
// set up some test data
|
|
||||||
// use test values close to default, but make sure they are all different
|
|
||||||
const float Pf_ROLL = 1.4f;
|
|
||||||
const float If_ROLL = 0.4f;
|
|
||||||
const float Df_ROLL = 0.03f;
|
|
||||||
const float Pf_PITCH = 1.5f; // default 1.4
|
|
||||||
const float If_PITCH = 0.5f; // default 0.4
|
|
||||||
const float Df_PITCH = 0.02f; // default 0.03
|
|
||||||
const float Pf_YAW = 3.5f;
|
|
||||||
const float If_YAW = 0.6f; // default 0.4
|
|
||||||
const float Df_YAW = 0.01;
|
|
||||||
const float A_level = 5.0f;
|
|
||||||
const float H_level = 3.0f;
|
|
||||||
const uint8_t H_sensitivity = 75;
|
|
||||||
currentProfile = &profile;
|
|
||||||
currentProfile->pidProfile.pidController = PID_CONTROLLER_LUX_FLOAT;
|
|
||||||
currentProfile->pidProfile.P_f[PIDROLL] = Pf_ROLL;
|
|
||||||
currentProfile->pidProfile.I_f[PIDROLL] = If_ROLL;
|
|
||||||
currentProfile->pidProfile.D_f[PIDROLL] = Df_ROLL;
|
|
||||||
currentProfile->pidProfile.P_f[PIDPITCH] = Pf_PITCH;
|
|
||||||
currentProfile->pidProfile.I_f[PIDPITCH] = If_PITCH;
|
|
||||||
currentProfile->pidProfile.D_f[PIDPITCH] = Df_PITCH;
|
|
||||||
currentProfile->pidProfile.P_f[PIDYAW] = Pf_YAW;
|
|
||||||
currentProfile->pidProfile.I_f[PIDYAW] = If_YAW;
|
|
||||||
currentProfile->pidProfile.D_f[PIDYAW] = Df_YAW;
|
|
||||||
currentProfile->pidProfile.A_level = A_level;
|
|
||||||
currentProfile->pidProfile.H_level = H_level;
|
|
||||||
currentProfile->pidProfile.H_sensitivity = H_sensitivity;
|
|
||||||
|
|
||||||
// use the MSP to write out the PID values
|
|
||||||
serialWritePos = 0;
|
|
||||||
serialReadPos = 0;
|
|
||||||
currentPort->cmdMSP = MSP_PID;
|
|
||||||
mspProcessReceivedCommand();
|
|
||||||
EXPECT_EQ(3 * PID_ITEM_COUNT, serialBuffer.mspResponse.header.size);
|
|
||||||
|
|
||||||
// reset test values to zero, so we can check if they get read properly
|
|
||||||
memset(¤tProfile->pidProfile, 0, sizeof(currentProfile->pidProfile));
|
|
||||||
|
|
||||||
// now use the MSP to read back the PID values and check they are the same
|
|
||||||
// spoof a change from the written MSP_PID to the readable MSP_SET_PID
|
|
||||||
currentPort->cmdMSP = MSP_SET_PID;
|
|
||||||
serialBuffer.mspResponse.header.direction = '<';
|
|
||||||
serialBuffer.mspResponse.header.type = currentPort->cmdMSP;
|
|
||||||
// force the checksum
|
|
||||||
serialBuffer.mspResponse.payload[3 * PID_ITEM_COUNT] ^= MSP_PID;
|
|
||||||
serialBuffer.mspResponse.payload[3 * PID_ITEM_COUNT] ^= MSP_SET_PID;
|
|
||||||
// copy the command data into the current port inBuf so it can be processed
|
|
||||||
memcpy(currentPort->inBuf, serialBuffer.buf, MSP_PORT_INBUF_SIZE);
|
|
||||||
|
|
||||||
// need to reset the controller for values to be read back correctly
|
|
||||||
currentProfile->pidProfile.pidController = PID_CONTROLLER_LUX_FLOAT;
|
|
||||||
|
|
||||||
// set the offset into the payload
|
|
||||||
currentPort->indRX = offsetof(struct mspResonse_s, payload);
|
|
||||||
mspProcessReceivedCommand();
|
|
||||||
|
|
||||||
// check the values are as expected
|
|
||||||
EXPECT_FLOAT_EQ(Pf_ROLL, currentProfile->pidProfile.P_f[PIDROLL]);
|
|
||||||
EXPECT_FLOAT_EQ(If_ROLL, currentProfile->pidProfile.I_f[PIDROLL]);
|
|
||||||
EXPECT_FLOAT_EQ(Df_ROLL, currentProfile->pidProfile.D_f[PIDROLL]);
|
|
||||||
EXPECT_FLOAT_EQ(Pf_PITCH, currentProfile->pidProfile.P_f[PIDPITCH]);
|
|
||||||
EXPECT_FLOAT_EQ(If_PITCH, currentProfile->pidProfile.I_f[PIDPITCH]);
|
|
||||||
EXPECT_FLOAT_EQ(Df_PITCH, currentProfile->pidProfile.D_f[PIDPITCH]);
|
|
||||||
EXPECT_FLOAT_EQ(Pf_YAW, currentProfile->pidProfile.P_f[PIDYAW]);
|
|
||||||
EXPECT_FLOAT_EQ(If_YAW, currentProfile->pidProfile.I_f[PIDYAW]);
|
|
||||||
EXPECT_FLOAT_EQ(Df_YAW, currentProfile->pidProfile.D_f[PIDYAW]);
|
|
||||||
EXPECT_FLOAT_EQ(A_level, currentProfile->pidProfile.A_level);
|
|
||||||
EXPECT_FLOAT_EQ(H_level, currentProfile->pidProfile.H_level);
|
|
||||||
EXPECT_EQ(H_sensitivity, currentProfile->pidProfile.H_sensitivity);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// STUBS
|
// STUBS
|
||||||
extern "C" {
|
extern "C" {
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue