new Baseflight PID
full gyro scale is used now and a new pid with float calculations was added based on PIDrewrite eeprom size was also increased from 1kB to 2kB Conflicts: src/config.c src/drivers/accgyro_l3g4200d.c src/drivers/accgyro_mpu3050.c src/drivers/accgyro_mpu6050.c src/flight_imu.c src/mw.c src/mw.h src/serial_cli.c src/serial_msp.c src/utils.c src/utils.h
This commit is contained in:
Lukas S
Dominic Clifton
parent
1df79e65fc
commit
cffdfb782c
|
|
@ -13,6 +13,16 @@ int constrain(int amt, int low, int high)
|
|||
return amt;
|
||||
}
|
||||
|
||||
float constrainf(float amt, float low, float high)
|
||||
{
|
||||
if (amt < low)
|
||||
return low;
|
||||
else if (amt > high)
|
||||
return high;
|
||||
else
|
||||
return amt;
|
||||
}
|
||||
|
||||
void devClear(stdev_t *dev)
|
||||
{
|
||||
dev->m_n = 0;
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@ typedef struct stdev_t
|
|||
} stdev_t;
|
||||
|
||||
int constrain(int amt, int low, int high);
|
||||
float constrainf(float amt, float low, float high);
|
||||
|
||||
void devClear(stdev_t *dev);
|
||||
void devPush(stdev_t *dev, float x);
|
||||
|
|
|
|||
15
src/config.c
15
src/config.c
|
|
@ -43,7 +43,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
|
|||
escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse,
|
||||
airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse);
|
||||
|
||||
#define FLASH_TO_RESERVE_FOR_CONFIG 0x400
|
||||
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
|
||||
|
||||
#ifdef STM32F303xC
|
||||
#define FLASH_PAGE_COUNT 128
|
||||
|
|
@ -97,6 +97,18 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->P8[PIDVEL] = 120;
|
||||
pidProfile->I8[PIDVEL] = 45;
|
||||
pidProfile->D8[PIDVEL] = 1;
|
||||
|
||||
pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully
|
||||
pidProfile->I_f[ROLL] = 0.3f;
|
||||
pidProfile->D_f[ROLL] = 0.06f;
|
||||
pidProfile->P_f[PITCH] = 2.5f;
|
||||
pidProfile->I_f[PITCH] = 0.3f;
|
||||
pidProfile->D_f[PITCH] = 0.06f;
|
||||
pidProfile->P_f[YAW] = 8.0f;
|
||||
pidProfile->I_f[YAW] = 0.5f;
|
||||
pidProfile->D_f[YAW] = 0.05f;
|
||||
pidProfile->A_level = 5.0f;
|
||||
pidProfile->H_level = 3.0f;
|
||||
}
|
||||
|
||||
void resetGpsProfile(gpsProfile_t *gpsProfile)
|
||||
|
|
@ -217,6 +229,7 @@ static void resetConf(void)
|
|||
resetFlight3DConfig(&masterConfig.flight3DConfig);
|
||||
masterConfig.motor_pwm_rate = 400;
|
||||
masterConfig.servo_pwm_rate = 50;
|
||||
|
||||
// gps/nav stuff
|
||||
masterConfig.gps_provider = GPS_NMEA;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
typedef struct profile_s {
|
||||
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
|
||||
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid
|
||||
|
||||
pidProfile_t pidProfile;
|
||||
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
|
|||
gyro->read = l3g4200dRead;
|
||||
|
||||
// 14.2857dps/lsb scalefactor
|
||||
gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
|
||||
gyro->scale = 1.0f / 14.2857f;
|
||||
|
||||
// default LPF is set to 32Hz
|
||||
switch (lpf) {
|
||||
|
|
@ -97,7 +97,8 @@ static void l3g4200dRead(int16_t *gyroData)
|
|||
uint8_t buf[6];
|
||||
|
||||
i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
|
||||
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
|
||||
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
|
||||
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
|
||||
|
||||
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -216,9 +216,9 @@ static void l3gd20GyroRead(int16_t *gyroData)
|
|||
|
||||
GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
|
||||
|
||||
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8;
|
||||
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8;
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8;
|
||||
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
|
||||
#if 0
|
||||
debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
|
||||
|
|
@ -227,14 +227,18 @@ static void l3gd20GyroRead(int16_t *gyroData)
|
|||
#endif
|
||||
}
|
||||
|
||||
#define L3GD20_GYRO_SCALE_FACTOR 0.00030543f // (17.5e-3) * pi/180 (17.5 mdps/bit)
|
||||
|
||||
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf)
|
||||
{
|
||||
gyro->init = l3gd20GyroInit;
|
||||
gyro->read = l3gd20GyroRead;
|
||||
|
||||
// 16.4 dps/lsb scalefactor
|
||||
gyro->scale = L3GD20_GYRO_SCALE_FACTOR;
|
||||
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
|
||||
//gyro->scale = L3GD20_GYRO_SCALE_FACTOR;
|
||||
|
||||
// 14.2857dps/lsb scalefactor
|
||||
gyro->scale = 1.0f / 14.2857f;
|
||||
|
||||
return true; // blindly assume it's present, for now.
|
||||
}
|
||||
|
|
|
|||
|
|
@ -21,6 +21,4 @@
|
|||
#define L3GD20_CS_PIN GPIO_Pin_3
|
||||
#define L3GD20_CS_GPIO_CLK RCC_AHBPeriph_GPIOE
|
||||
|
||||
#define L3GD20_GYRO_SCALE_FACTOR 0.00030543f // (17.5e-3) * pi/180 (17.5 mdps/bit)
|
||||
|
||||
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf);
|
||||
|
|
|
|||
|
|
@ -56,8 +56,9 @@ bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
|
|||
gyro->init = mpu3050Init;
|
||||
gyro->read = mpu3050Read;
|
||||
gyro->temperature = mpu3050ReadTemp;
|
||||
|
||||
// 16.4 dps/lsb scalefactor
|
||||
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
|
||||
gyro->scale = 1.0f / 16.4f;
|
||||
|
||||
// default lpf is 42Hz
|
||||
switch (lpf) {
|
||||
|
|
@ -107,9 +108,10 @@ static void mpu3050Read(int16_t *gyroData)
|
|||
uint8_t buf[6];
|
||||
|
||||
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
|
||||
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
|
||||
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
|
||||
|
||||
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
}
|
||||
|
||||
static void mpu3050ReadTemp(int16_t *tempData)
|
||||
|
|
|
|||
|
|
@ -194,7 +194,7 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
|
|||
gyro->read = mpu6050GyroRead;
|
||||
|
||||
// 16.4 dps/lsb scalefactor
|
||||
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
|
||||
gyro->scale = 1.0f / 16.4f;
|
||||
|
||||
// default lpf is 42Hz
|
||||
switch (lpf) {
|
||||
|
|
@ -279,7 +279,8 @@ static void mpu6050GyroRead(int16_t *gyroData)
|
|||
uint8_t buf[6];
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
|
||||
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
|
||||
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
|
||||
|
||||
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,6 +7,9 @@
|
|||
|
||||
#include "runtime_config.h"
|
||||
|
||||
#include "drivers/accgyro_common.h"
|
||||
#include "sensors_common.h"
|
||||
#include "sensors_gyro.h"
|
||||
#include "rc_controls.h"
|
||||
#include "flight_common.h"
|
||||
#include "gps_common.h"
|
||||
|
|
@ -18,6 +21,7 @@ int16_t axisPID[3];
|
|||
uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||
|
||||
static int32_t errorGyroI[3] = { 0, 0, 0 };
|
||||
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
|
||||
static int32_t errorAngleI[2] = { 0, 0 };
|
||||
|
||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
|
|
@ -45,8 +49,80 @@ void resetErrorGyro(void)
|
|||
errorGyroI[ROLL] = 0;
|
||||
errorGyroI[PITCH] = 0;
|
||||
errorGyroI[YAW] = 0;
|
||||
|
||||
errorGyroIf[ROLL] = 0;
|
||||
errorGyroIf[PITCH] = 0;
|
||||
errorGyroIf[YAW] = 0;
|
||||
}
|
||||
|
||||
|
||||
static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
|
||||
{
|
||||
float RateError, errorAngle, AngleRate, gyroRate;
|
||||
float ITerm,PTerm,DTerm;
|
||||
static float lastGyroRate[3];
|
||||
static float delta1[3], delta2[3];
|
||||
float delta, deltaSum;
|
||||
float dT;
|
||||
int axis;
|
||||
|
||||
dT = (float)cycleTime * 0.000001f;
|
||||
|
||||
// ----------PID controller----------
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
// -----Get the desired angle rate depending on flight mode
|
||||
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
|
||||
AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f;
|
||||
} else {
|
||||
// calculate error and limit the angle to 50 degrees max inclination
|
||||
errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - inclination.rawAngles[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
|
||||
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
||||
AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
|
||||
if (f.HORIZON_MODE) {
|
||||
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
|
||||
AngleRate += errorAngle * pidProfile->H_level;
|
||||
}
|
||||
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
|
||||
AngleRate = errorAngle * pidProfile->A_level;
|
||||
}
|
||||
}
|
||||
|
||||
gyroRate = gyroData[axis] * gyro.scale; // gyro output scaled to dps
|
||||
|
||||
// --------low-level gyro-based PID. ----------
|
||||
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
|
||||
// -----calculate scaled error.AngleRates
|
||||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||
RateError = AngleRate - gyroRate;
|
||||
|
||||
// -----calculate P component
|
||||
PTerm = RateError * pidProfile->P_f[axis];
|
||||
// -----calculate I component
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis], -250.0f, 250.0f);
|
||||
|
||||
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
||||
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
||||
ITerm = errorGyroIf[axis];
|
||||
|
||||
//-----calculate D-term
|
||||
delta = gyroRate - lastGyroRate[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastGyroRate[axis] = gyroRate;
|
||||
|
||||
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
|
||||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta *= (1.0f / dT);
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis], -300.0f, 300.0f);
|
||||
|
||||
// -----calculate total PID output
|
||||
axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
|
||||
}
|
||||
|
||||
}
|
||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
|
||||
{
|
||||
|
|
@ -73,14 +149,14 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
}
|
||||
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
|
||||
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
|
||||
error -= gyroData[axis];
|
||||
error -= gyroData[axis] / 4;
|
||||
|
||||
PTermGYRO = rcCommand[axis];
|
||||
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
|
||||
if (abs(gyroData[axis]) > 640)
|
||||
if (abs(gyroData[axis]) > (640 * 4))
|
||||
errorGyroI[axis] = 0;
|
||||
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) >> 6;
|
||||
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
|
||||
}
|
||||
if (f.HORIZON_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
|
||||
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
|
||||
|
|
@ -95,8 +171,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
}
|
||||
}
|
||||
|
||||
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
||||
delta = gyroData[axis] - lastGyro[axis];
|
||||
PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
||||
delta = (gyroData[axis] - lastGyro[axis]) / 4;
|
||||
lastGyro[axis] = gyroData[axis];
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
|
|
@ -144,7 +220,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
|
||||
// -----calculate scaled error.AngleRates
|
||||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||
RateError = AngleRateTmp - gyroData[axis];
|
||||
RateError = AngleRateTmp - (gyroData[axis] / 4);
|
||||
|
||||
// -----calculate P component
|
||||
PTerm = (RateError * pidProfile->P8[axis]) >> 7;
|
||||
|
|
@ -188,6 +264,8 @@ void setPIDController(int type)
|
|||
case 1:
|
||||
pid_controller = pidRewrite;
|
||||
break;
|
||||
case 2:
|
||||
pid_controller = pidBaseflight;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,6 +19,12 @@ typedef struct pidProfile_s {
|
|||
uint8_t P8[PID_ITEM_COUNT];
|
||||
uint8_t I8[PID_ITEM_COUNT];
|
||||
uint8_t D8[PID_ITEM_COUNT];
|
||||
|
||||
float P_f[3]; // float p i and d factors for the new baseflight pid
|
||||
float I_f[3];
|
||||
float D_f[3];
|
||||
float A_level;
|
||||
float H_level;
|
||||
} pidProfile_t;
|
||||
|
||||
enum {
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ float throttleAngleScale;
|
|||
int32_t BaroPID = 0;
|
||||
|
||||
float magneticDeclination = 0.0f; // calculated at startup from config
|
||||
|
||||
float gyroScaleRad;
|
||||
|
||||
// **************
|
||||
// gyro+acc IMU
|
||||
|
|
@ -80,6 +80,7 @@ void imuInit(void)
|
|||
smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f));
|
||||
accVelScale = 9.80665f / acc_1G / 10000.0f;
|
||||
throttleAngleScale = (1800.0f / M_PI) * (900.0f / currentProfile.throttle_correction_angle);
|
||||
gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f;
|
||||
|
||||
#ifdef MAG
|
||||
// if mag sensor is enabled, use it
|
||||
|
|
@ -278,7 +279,7 @@ static void getEstimatedAttitude(void)
|
|||
float scale;
|
||||
fp_angles_t deltaGyroAngle;
|
||||
deltaT = currentT - previousT;
|
||||
scale = deltaT * gyro.scale;
|
||||
scale = deltaT * gyroScaleRad;
|
||||
previousT = currentT;
|
||||
|
||||
// Initialization
|
||||
|
|
|
|||
2
src/mw.c
2
src/mw.c
|
|
@ -275,7 +275,7 @@ static void mwVario(void)
|
|||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
|
||||
|
|
|
|||
|
|
@ -270,7 +270,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "nav_speed_max", VAR_UINT16, ¤tProfile.gpsProfile.nav_speed_max, 10, 2000 },
|
||||
{ "nav_slew_rate", VAR_UINT8, ¤tProfile.gpsProfile.nav_slew_rate, 0, 100 },
|
||||
|
||||
{ "pid_controller", VAR_UINT8, ¤tProfile.pidController, 0, 1 },
|
||||
{ "pid_controller", VAR_UINT8, ¤tProfile.pidController, 0, 2 },
|
||||
|
||||
{ "p_pitch", VAR_UINT8, ¤tProfile.pidProfile.P8[PITCH], 0, 200 },
|
||||
{ "i_pitch", VAR_UINT8, ¤tProfile.pidProfile.I8[PITCH], 0, 200 },
|
||||
{ "d_pitch", VAR_UINT8, ¤tProfile.pidProfile.D8[PITCH], 0, 200 },
|
||||
|
|
@ -280,12 +281,28 @@ const clivalue_t valueTable[] = {
|
|||
{ "p_yaw", VAR_UINT8, ¤tProfile.pidProfile.P8[YAW], 0, 200 },
|
||||
{ "i_yaw", VAR_UINT8, ¤tProfile.pidProfile.I8[YAW], 0, 200 },
|
||||
{ "d_yaw", VAR_UINT8, ¤tProfile.pidProfile.D8[YAW], 0, 200 },
|
||||
|
||||
{ "Ppitchf", VAR_FLOAT, ¤tProfile.pidProfile.P_f[PITCH], 0, 100 },
|
||||
{ "Ipitchf", VAR_FLOAT, ¤tProfile.pidProfile.I_f[PITCH], 0, 100 },
|
||||
{ "Dpitchf", VAR_FLOAT, ¤tProfile.pidProfile.D_f[PITCH], 0, 100 },
|
||||
{ "Prollf", VAR_FLOAT, ¤tProfile.pidProfile.P_f[ROLL], 0, 100 },
|
||||
{ "Irollf", VAR_FLOAT, ¤tProfile.pidProfile.I_f[ROLL], 0, 100 },
|
||||
{ "Drollf", VAR_FLOAT, ¤tProfile.pidProfile.D_f[ROLL], 0, 100 },
|
||||
{ "Pyawf", VAR_FLOAT, ¤tProfile.pidProfile.P_f[YAW], 0, 100 },
|
||||
{ "Iyawf", VAR_FLOAT, ¤tProfile.pidProfile.I_f[YAW], 0, 100 },
|
||||
{ "Dyawf", VAR_FLOAT, ¤tProfile.pidProfile.D_f[YAW], 0, 100 },
|
||||
|
||||
{ "level_horizon", VAR_FLOAT, ¤tProfile.pidProfile.H_level, 0, 10 },
|
||||
{ "level_angle", VAR_FLOAT, ¤tProfile.pidProfile.A_level, 0, 10 },
|
||||
|
||||
{ "p_alt", VAR_UINT8, ¤tProfile.pidProfile.P8[PIDALT], 0, 200 },
|
||||
{ "i_alt", VAR_UINT8, ¤tProfile.pidProfile.I8[PIDALT], 0, 200 },
|
||||
{ "d_alt", VAR_UINT8, ¤tProfile.pidProfile.D8[PIDALT], 0, 200 },
|
||||
|
||||
{ "p_level", VAR_UINT8, ¤tProfile.pidProfile.P8[PIDLEVEL], 0, 200 },
|
||||
{ "i_level", VAR_UINT8, ¤tProfile.pidProfile.I8[PIDLEVEL], 0, 200 },
|
||||
{ "d_level", VAR_UINT8, ¤tProfile.pidProfile.D8[PIDLEVEL], 0, 200 },
|
||||
|
||||
{ "p_vel", VAR_UINT8, ¤tProfile.pidProfile.P8[PIDVEL], 0, 200 },
|
||||
{ "i_vel", VAR_UINT8, ¤tProfile.pidProfile.I8[PIDVEL], 0, 200 },
|
||||
{ "d_vel", VAR_UINT8, ¤tProfile.pidProfile.D8[PIDVEL], 0, 200 },
|
||||
|
|
|
|||
|
|
@ -1,11 +1,13 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "build_config.h"
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "drivers/system_common.h"
|
||||
|
|
@ -326,34 +328,45 @@ void mspInit(serialConfig_t *serialConfig)
|
|||
|
||||
idx = 0;
|
||||
availableBoxes[idx++] = BOXARM;
|
||||
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
availableBoxes[idx++] = BOXANGLE;
|
||||
availableBoxes[idx++] = BOXHORIZON;
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
availableBoxes[idx++] = BOXBARO;
|
||||
if (feature(FEATURE_VARIO))
|
||||
availableBoxes[idx++] = BOXVARIO;
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
|
||||
availableBoxes[idx++] = BOXMAG;
|
||||
availableBoxes[idx++] = BOXHEADFREE;
|
||||
availableBoxes[idx++] = BOXHEADADJ;
|
||||
}
|
||||
|
||||
if (feature(FEATURE_SERVO_TILT))
|
||||
availableBoxes[idx++] = BOXCAMSTAB;
|
||||
|
||||
if (feature(FEATURE_GPS)) {
|
||||
availableBoxes[idx++] = BOXGPSHOME;
|
||||
availableBoxes[idx++] = BOXGPSHOLD;
|
||||
}
|
||||
|
||||
if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE)
|
||||
availableBoxes[idx++] = BOXPASSTHRU;
|
||||
|
||||
availableBoxes[idx++] = BOXBEEPERON;
|
||||
|
||||
if (feature(FEATURE_INFLIGHT_ACC_CAL))
|
||||
availableBoxes[idx++] = BOXCALIB;
|
||||
|
||||
availableBoxes[idx++] = BOXOSD;
|
||||
|
||||
if (feature(FEATURE_TELEMETRY && masterConfig.telemetryConfig.telemetry_switch))
|
||||
availableBoxes[idx++] = BOXTELEMETRY;
|
||||
|
||||
numberBoxItems = idx;
|
||||
|
||||
openAllMSPSerialPorts(serialConfig);
|
||||
|
|
@ -389,10 +402,29 @@ static void evaluateCommand(void)
|
|||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_SET_PID:
|
||||
for (i = 0; i < PID_ITEM_COUNT; i++) {
|
||||
currentProfile.pidProfile.P8[i] = read8();
|
||||
currentProfile.pidProfile.I8[i] = read8();
|
||||
currentProfile.pidProfile.D8[i] = read8();
|
||||
if (currentProfile.pidController == 2) {
|
||||
for (i = 0; i < 3; i++) {
|
||||
currentProfile.pidProfile.P_f[i] = (float)read8() / 10.0f;
|
||||
currentProfile.pidProfile.I_f[i] = (float)read8() / 100.0f;
|
||||
currentProfile.pidProfile.D_f[i] = (float)read8() / 1000.0f;
|
||||
}
|
||||
for (i = 3; i < PID_ITEM_COUNT; i++) {
|
||||
if (i == PIDLEVEL) {
|
||||
currentProfile.pidProfile.A_level = (float)read8() / 10.0f;
|
||||
currentProfile.pidProfile.H_level = (float)read8() / 10.0f;
|
||||
read8();
|
||||
} else {
|
||||
currentProfile.pidProfile.P8[i] = read8();
|
||||
currentProfile.pidProfile.I8[i] = read8();
|
||||
currentProfile.pidProfile.D8[i] = read8();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (i = 0; i < PID_ITEM_COUNT; i++) {
|
||||
currentProfile.pidProfile.P8[i] = read8();
|
||||
currentProfile.pidProfile.I8[i] = read8();
|
||||
currentProfile.pidProfile.D8[i] = read8();
|
||||
}
|
||||
}
|
||||
headSerialReply(0);
|
||||
break;
|
||||
|
|
@ -599,10 +631,29 @@ static void evaluateCommand(void)
|
|||
break;
|
||||
case MSP_PID:
|
||||
headSerialReply(3 * PID_ITEM_COUNT);
|
||||
for (i = 0; i < PID_ITEM_COUNT; i++) {
|
||||
serialize8(currentProfile.pidProfile.P8[i]);
|
||||
serialize8(currentProfile.pidProfile.I8[i]);
|
||||
serialize8(currentProfile.pidProfile.D8[i]);
|
||||
if (currentProfile.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
|
||||
for (i = 0; i < 3; i++) {
|
||||
serialize8(constrain(lrintf(currentProfile.pidProfile.P_f[i] * 10.0f), 0, 250));
|
||||
serialize8(constrain(lrintf(currentProfile.pidProfile.I_f[i] * 100.0f), 0, 250));
|
||||
serialize8(constrain(lrintf(currentProfile.pidProfile.D_f[i] * 1000.0f), 0, 100));
|
||||
}
|
||||
for (i = 3; i < PID_ITEM_COUNT; i++) {
|
||||
if (i == PIDLEVEL) {
|
||||
serialize8(constrain(lrintf(currentProfile.pidProfile.A_level * 10.0f), 0, 250));
|
||||
serialize8(constrain(lrintf(currentProfile.pidProfile.H_level * 10.0f), 0, 250));
|
||||
serialize8(0);
|
||||
} else {
|
||||
serialize8(currentProfile.pidProfile.P8[i]);
|
||||
serialize8(currentProfile.pidProfile.I8[i]);
|
||||
serialize8(currentProfile.pidProfile.D8[i]);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (i = 0; i < PID_ITEM_COUNT; i++) {
|
||||
serialize8(currentProfile.pidProfile.P8[i]);
|
||||
serialize8(currentProfile.pidProfile.I8[i]);
|
||||
serialize8(currentProfile.pidProfile.D8[i]);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case MSP_PIDNAMES:
|
||||
|
|
|
|||
Loading…
Reference in New Issue