mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
dd8a8aead5
* MAGHOLD constrained to 30 degrees * mag_hold_heading_diff_limit CLI setting added to allow custom MAG_HOLD diff limit * magHeading moved to pid.c and real interface created to inject data into it * mag hold controller with current logic moved to pid.c * 2Hz LPF filter added to magHold error to smoothen controller response * variable name changed to clearly state its purpose * LPF filter in MAG_HOLD controller moved after error limiting * yaw_control_direction passed to pidController as argument, mag_hold_heading_diff_limit moved to pidProfile * yaw_control_direction setting removed * pidMagHold translates directly to rotation rate in dps * pidMagHold refactored to the new principles and limiting * name of filter changed to reflect its purpose * HEADING_LOCK mode can be used only when MAG_HOLD is not enabled * compiler warning supressed * new MAG_HOLD defaults, RATE_LIMIT increased to 80dps, MAG_HOLD P to 60 * mag_rate_limit 90dps
579 lines
23 KiB
C++
579 lines
23 KiB
C++
/*
|
|
* This file is part of Cleanflight.
|
|
*
|
|
* Cleanflight is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Cleanflight is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
extern "C" {
|
|
#include <platform.h>
|
|
#include "build_config.h"
|
|
#include "version.h"
|
|
#include "debug.h"
|
|
#include "common/axis.h"
|
|
#include "common/color.h"
|
|
#include "common/maths.h"
|
|
|
|
#include "drivers/system.h"
|
|
#include "drivers/sensor.h"
|
|
#include "drivers/accgyro.h"
|
|
#include "drivers/compass.h"
|
|
#include "drivers/serial.h"
|
|
#include "drivers/bus_i2c.h"
|
|
#include "drivers/gpio.h"
|
|
#include "drivers/timer.h"
|
|
#include "drivers/pwm_rx.h"
|
|
#include "drivers/buf_writer.h"
|
|
|
|
#include "rx/rx.h"
|
|
|
|
#include "io/rc_controls.h"
|
|
#include "io/gps.h"
|
|
#include "io/gimbal.h"
|
|
#include "io/ledstrip.h"
|
|
#include "io/serial_msp.h"
|
|
#include "io/escservo.h"
|
|
|
|
#include "telemetry/telemetry.h"
|
|
|
|
#include "sensors/sensors.h"
|
|
#include "sensors/boardalignment.h"
|
|
#include "sensors/sensors.h"
|
|
#include "sensors/battery.h"
|
|
#include "sensors/sonar.h"
|
|
#include "sensors/acceleration.h"
|
|
#include "sensors/barometer.h"
|
|
#include "sensors/compass.h"
|
|
#include "sensors/gyro.h"
|
|
|
|
#include "flight/mixer.h"
|
|
#include "flight/pid.h"
|
|
#include "flight/navigation.h"
|
|
#include "flight/imu.h"
|
|
#include "flight/failsafe.h"
|
|
|
|
#include "config/runtime_config.h"
|
|
#include "config/config.h"
|
|
#include "config/config_profile.h"
|
|
#include "config/config_master.h"
|
|
}
|
|
|
|
#include "unittest_macros.h"
|
|
#include "gtest/gtest.h"
|
|
|
|
|
|
extern "C" {
|
|
void setCurrentPort(mspPort_t *port);
|
|
void mspProcessReceivedCommand();
|
|
extern mspPort_t *currentPort;
|
|
extern bufWriter_t *writer;
|
|
extern mspPort_t mspPorts[];
|
|
profile_t *currentProfile;
|
|
}
|
|
|
|
profile_t profile;
|
|
|
|
typedef struct mspHeader_s {
|
|
uint8_t dollar;
|
|
uint8_t m;
|
|
uint8_t direction;
|
|
uint8_t size;
|
|
uint8_t type;
|
|
} mspHeader_t;
|
|
|
|
typedef struct mspResonse_s {
|
|
mspHeader_t header;
|
|
uint8_t payload[];
|
|
} mspResponse_t;
|
|
|
|
#define SERIAL_BUFFER_SIZE 256
|
|
typedef union mspBuffer_u {
|
|
mspResponse_t mspResponse;
|
|
uint8_t buf[SERIAL_BUFFER_SIZE];
|
|
} mspBuffer_t;
|
|
|
|
static mspBuffer_t serialBuffer;
|
|
|
|
static int serialWritePos = 0;
|
|
static int serialReadPos = 0;
|
|
|
|
uint8_t buf[sizeof(bufWriter_t) + SERIAL_BUFFER_SIZE];
|
|
|
|
void serialWrite(serialPort_t *instance, uint8_t ch)
|
|
{
|
|
UNUSED(instance);
|
|
serialBuffer.buf[serialWritePos] = ch;
|
|
++serialWritePos;
|
|
}
|
|
|
|
void serialWriteBufShim(void *instance, uint8_t *data, int count)
|
|
{
|
|
for (uint8_t *p = data; count > 0; count--, p++) {
|
|
serialWrite((serialPort_t *)instance, *p);
|
|
}
|
|
}
|
|
|
|
void serialBeginWrite(serialPort_t *instance)
|
|
{
|
|
UNUSED(instance);
|
|
}
|
|
|
|
void serialEndWrite(serialPort_t *instance)
|
|
{
|
|
UNUSED(instance);
|
|
}
|
|
|
|
uint8_t serialRxBytesWaiting(serialPort_t *instance)
|
|
{
|
|
UNUSED(instance);
|
|
if (serialWritePos > serialReadPos) {
|
|
return serialWritePos - serialReadPos;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
uint8_t serialRead(serialPort_t *instance)
|
|
{
|
|
UNUSED(instance);
|
|
const uint8_t ch = serialBuffer.buf[serialReadPos];
|
|
++serialReadPos;
|
|
if (currentPort->indRX == MSP_PORT_INBUF_SIZE) {
|
|
currentPort->indRX = 0;
|
|
}
|
|
currentPort->inBuf[currentPort->indRX] = ch;
|
|
++currentPort->indRX;
|
|
return ch;
|
|
}
|
|
|
|
bool isSerialTransmitBufferEmpty(serialPort_t *instance)
|
|
{
|
|
UNUSED(instance);
|
|
return true;
|
|
}
|
|
|
|
class SerialMspUnitTest : public ::testing::Test {
|
|
protected:
|
|
virtual void SetUp() {
|
|
memset(serialBuffer.buf, 0, sizeof(serialBuffer));
|
|
setCurrentPort(&mspPorts[0]);
|
|
writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, &mspPorts[0]);
|
|
}
|
|
};
|
|
|
|
|
|
TEST_F(SerialMspUnitTest, TestMspProcessReceivedCommand)
|
|
{
|
|
// check the MSP_API_VERSION is written out correctly
|
|
serialWritePos = 0;
|
|
serialReadPos = 0;
|
|
currentPort->cmdMSP = MSP_API_VERSION;
|
|
mspProcessReceivedCommand();
|
|
|
|
EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
|
|
EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
|
|
EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
|
|
EXPECT_EQ(3, serialBuffer.mspResponse.header.size);
|
|
EXPECT_EQ(MSP_API_VERSION, serialBuffer.mspResponse.header.type);
|
|
EXPECT_EQ(MSP_PROTOCOL_VERSION, serialBuffer.mspResponse.payload[0]);
|
|
EXPECT_EQ(API_VERSION_MAJOR, serialBuffer.mspResponse.payload[1]);
|
|
EXPECT_EQ(API_VERSION_MINOR, serialBuffer.mspResponse.payload[2]);
|
|
int checksum = 3 ^ MSP_API_VERSION;
|
|
checksum ^= MSP_PROTOCOL_VERSION ^ API_VERSION_MAJOR ^ API_VERSION_MINOR;
|
|
EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[3]);// checksum
|
|
|
|
// check the MSP_FC_VARIANT is written out correctly
|
|
serialWritePos = 0;
|
|
serialReadPos = 0;
|
|
currentPort->cmdMSP = MSP_FC_VARIANT;
|
|
mspProcessReceivedCommand();
|
|
|
|
EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
|
|
EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
|
|
EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
|
|
EXPECT_EQ(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH, serialBuffer.mspResponse.header.size);
|
|
EXPECT_EQ(MSP_FC_VARIANT, serialBuffer.mspResponse.header.type);
|
|
EXPECT_EQ('C', serialBuffer.mspResponse.payload[0]);
|
|
EXPECT_EQ('L', serialBuffer.mspResponse.payload[1]);
|
|
EXPECT_EQ('F', serialBuffer.mspResponse.payload[2]);
|
|
EXPECT_EQ('L', serialBuffer.mspResponse.payload[3]);
|
|
checksum = FLIGHT_CONTROLLER_IDENTIFIER_LENGTH ^ MSP_FC_VARIANT;
|
|
checksum ^= 'C'^ 'L' ^ 'F' ^ 'L';
|
|
EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[4]);
|
|
|
|
// check the MSP_FC_VERSION is written out correctly
|
|
serialWritePos = 0;
|
|
serialReadPos = 0;
|
|
currentPort->cmdMSP = MSP_FC_VERSION;
|
|
mspProcessReceivedCommand();
|
|
EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
|
|
EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
|
|
EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
|
|
EXPECT_EQ(FLIGHT_CONTROLLER_VERSION_LENGTH, serialBuffer.mspResponse.header.size);
|
|
EXPECT_EQ(MSP_FC_VERSION, serialBuffer.mspResponse.header.type);
|
|
EXPECT_EQ(FC_VERSION_MAJOR, serialBuffer.mspResponse.payload[0]);
|
|
EXPECT_EQ(FC_VERSION_MINOR, serialBuffer.mspResponse.payload[1]);
|
|
EXPECT_EQ(FC_VERSION_PATCH_LEVEL, serialBuffer.mspResponse.payload[2]);
|
|
checksum = FLIGHT_CONTROLLER_VERSION_LENGTH ^ MSP_FC_VERSION;
|
|
checksum ^= FC_VERSION_MAJOR ^ FC_VERSION_MINOR ^ FC_VERSION_PATCH_LEVEL;
|
|
EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[3]);
|
|
|
|
// check the MSP_PID_CONTROLLER is written out correctly
|
|
serialWritePos = 0;
|
|
serialReadPos = 0;
|
|
currentProfile = &profile;
|
|
currentProfile->pidProfile.pidController = PID_CONTROLLER_MWREWRITE;
|
|
currentPort->cmdMSP = MSP_PID_CONTROLLER;
|
|
mspProcessReceivedCommand();
|
|
EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
|
|
EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
|
|
EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
|
|
EXPECT_EQ(1, serialBuffer.mspResponse.header.size);
|
|
EXPECT_EQ(MSP_PID_CONTROLLER, serialBuffer.mspResponse.header.type);
|
|
EXPECT_EQ(PID_CONTROLLER_MWREWRITE, serialBuffer.mspResponse.payload[0]);
|
|
checksum = 1 ^ MSP_PID_CONTROLLER ^ PID_CONTROLLER_MWREWRITE;
|
|
EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[1]);
|
|
}
|
|
|
|
TEST_F(SerialMspUnitTest, Test_PID_CONTROLLER)
|
|
{
|
|
// Use the MSP to write out the PID values
|
|
currentProfile = &profile;
|
|
currentProfile->pidProfile.pidController = PID_CONTROLLER_MWREWRITE;
|
|
serialWritePos = 0;
|
|
serialReadPos = 0;
|
|
currentPort->cmdMSP = MSP_PID_CONTROLLER;
|
|
mspProcessReceivedCommand();
|
|
EXPECT_EQ(PID_CONTROLLER_MWREWRITE, serialBuffer.mspResponse.payload[0]);
|
|
|
|
// set the pidController to a different value so we can check if it gets read back properly
|
|
currentProfile->pidProfile.pidController = PID_CONTROLLER_LUX_FLOAT;
|
|
|
|
// Now use the MSP to read back the picController and check if it is the same
|
|
// spoof a change from the written MSP_PID_CONTROLLER to the readable MSP_SET_PID_CONTROLLER
|
|
currentPort->cmdMSP = MSP_SET_PID_CONTROLLER;
|
|
serialBuffer.mspResponse.header.direction = '<';
|
|
serialBuffer.mspResponse.header.type = currentPort->cmdMSP;
|
|
// force the checksum
|
|
serialBuffer.mspResponse.payload[1] ^= MSP_PID_CONTROLLER;
|
|
serialBuffer.mspResponse.payload[1] ^= MSP_SET_PID_CONTROLLER;
|
|
// 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 pidController value has been read correctly
|
|
EXPECT_EQ(PID_CONTROLLER_MWREWRITE, currentProfile->pidProfile.pidController);
|
|
}
|
|
|
|
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" {
|
|
// from acceleration.c
|
|
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
|
|
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) {UNUSED(calibrationCyclesRequired);}
|
|
// from altitudehold.c
|
|
int32_t AltHold;
|
|
int32_t vario = 0; // variometer in cm/s
|
|
int32_t altitudeHoldGetEstimatedAltitude(void) {return 0;}
|
|
// from battery.c
|
|
uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered)
|
|
int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A)
|
|
int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start
|
|
// from compass.c
|
|
int32_t magADC[XYZ_AXIS_COUNT];
|
|
// from config.c
|
|
master_t masterConfig;
|
|
controlRateConfig_t *currentControlRateProfile;
|
|
void resetPidProfile(pidProfile_t *pidProfile) {UNUSED(pidProfile);}
|
|
void handleOneshotFeatureChangeOnRestart(void) {}
|
|
void readEEPROM(void) {}
|
|
void resetEEPROM(void) {}
|
|
void writeEEPROM(void) {}
|
|
bool feature(uint32_t mask) {UNUSED(mask);return false;}
|
|
void featureSet(uint32_t mask) {UNUSED(mask);}
|
|
void featureClearAll() {}
|
|
uint32_t featureMask(void) {return 0;}
|
|
// from debug.c
|
|
int16_t debug[DEBUG16_VALUE_COUNT];
|
|
// from gps.c
|
|
#define GPS_SV_MAXSATS 16
|
|
int32_t GPS_coord[2]; // LAT/LON
|
|
uint8_t GPS_numSat;
|
|
uint8_t GPS_update = 0; // it's a binary toggle to distinct a GPS position update
|
|
uint16_t GPS_altitude; // altitude in 0.1m
|
|
uint16_t GPS_speed; // speed in 0.1m/s
|
|
uint16_t GPS_ground_course = 0; // degrees * 10
|
|
uint8_t GPS_numCh; // Number of channels
|
|
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
|
|
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
|
|
uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
|
|
uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS]; // Carrier to Noise Ratio (Signal Strength)
|
|
// from gyro.c
|
|
int32_t gyroADC[XYZ_AXIS_COUNT];
|
|
// form imu.c
|
|
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
|
int16_t accSmooth[XYZ_AXIS_COUNT];
|
|
// from ledstrip.c
|
|
void reevalulateLedConfig(void) {}
|
|
// from mixer.c
|
|
int16_t motor[MAX_SUPPORTED_MOTORS];
|
|
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
|
int16_t servo[MAX_SUPPORTED_SERVOS];
|
|
void stopMotors(void) {}
|
|
void loadCustomServoMixer(void) {}
|
|
// from msp.c
|
|
void rxMspFrameReceive(uint16_t *frame, int channelCount) {UNUSED(frame);UNUSED(channelCount);}
|
|
// from mw.c
|
|
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
|
|
// from navigation.c
|
|
int32_t GPS_home[2];
|
|
int32_t GPS_hold[2];
|
|
uint16_t GPS_distanceToHome; // distance to home point in meters
|
|
int16_t GPS_directionToHome; // direction to home or hol point in degrees
|
|
navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode
|
|
void GPS_set_next_wp(int32_t *lat, int32_t *lon) {UNUSED(lat);UNUSED(lon);}
|
|
// from pid.c
|
|
void pidSetController(pidControllerType_e type) {UNUSED(type);}
|
|
// from rc_controls.c
|
|
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
|
|
void useRcControlsConfig(void *modeActivationConditions, void *escAndServoConfigToUse, void *pidProfileToUse) {
|
|
UNUSED(modeActivationConditions);UNUSED(escAndServoConfigToUse);UNUSED(pidProfileToUse);}
|
|
// from runtime_config.c
|
|
uint8_t armingFlags = 0;
|
|
uint8_t stateFlags = 0;
|
|
uint16_t flightModeFlags = 0;
|
|
uint16_t disableFlightMode(flightModeFlags_e mask) {UNUSED(mask);return 0;}
|
|
bool sensors(uint32_t mask) {UNUSED(mask);return 0;}
|
|
// from rx.c
|
|
uint16_t rssi = 0; // range: [0;1023]
|
|
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
|
rxRuntimeConfig_t rxRuntimeConfig;
|
|
// from system.c
|
|
void delay(uint32_t ms) {UNUSED(ms);}
|
|
// from system_stm32fN0x.c
|
|
void systemReset(void) {}
|
|
void systemResetToBootloader(void) {}
|
|
// from scheduler.c
|
|
uint16_t averageSystemLoadPercent = 0;
|
|
// from transponder_ir.c
|
|
void transponderUpdateData(uint8_t*) {}
|
|
}
|