diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f4c493bbe0..ae717117b0 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -700,14 +700,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } 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: for (int i = 0; i < PID_ITEM_COUNT; i++) { 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; 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: if (dataSize >= PID_ITEM_COUNT * 4) { for (int i = 0; i < PID_ITEM_COUNT; i++) { diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index d4a945895d..9b45c8141c 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -243,7 +243,6 @@ #define MSP_ALTITUDE 109 //out message altitude, variometer #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_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_MISC 114 //out message powermeter trig #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_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_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 diff --git a/src/test/unit/serial_msp_unittest.cc.txt b/src/test/unit/serial_msp_unittest.cc.txt index 6817f7f0be..e13081d340 100644 --- a/src/test/unit/serial_msp_unittest.cc.txt +++ b/src/test/unit/serial_msp_unittest.cc.txt @@ -231,208 +231,6 @@ TEST_F(SerialMspUnitTest, TestMspProcessReceivedCommand) 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 extern "C" {