Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Merge branch 'betaflight' of https://github.com/borisbstyle/betaflight into betaflight

Browse source
This commit is contained in:
mikeller 2016-02-21 18:40:53 +13:00
commit 9d7529af9f
54 changed files with 8496 additions and 542 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -326,7 +326,6 @@ COMMON_SRC = build_config.c \
$(DEVICE_STDPERIPH_SRC)
HIGHEND_SRC = \
flight/gtune.c \
flight/navigation.c \
flight/gps_conversion.c \
common/colorconversion.c \

7981
betaflight_CC3D.hex Normal file
View file

File diff suppressed because it is too large Load diff

3
out.asm Normal file
View file

@ -0,0 +1,3 @@
obj/main/betaflight_NAZE.elf: file format elf32-little

7
src/main/blackbox/blackbox.c
View file

@ -1136,7 +1136,7 @@ static bool blackboxWriteSysinfo()
blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
break;
case 4:
blackboxPrintfHeaderLine("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile.rcRate8);
blackboxPrintfHeaderLine("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
break;
case 5:
blackboxPrintfHeaderLine("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle);
@ -1206,11 +1206,6 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
blackboxWriteSignedVB(data->inflightAdjustment.newValue);
}
break;
case FLIGHT_LOG_EVENT_GTUNE_RESULT:
blackboxWrite(data->gtuneCycleResult.gtuneAxis);
blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG);
blackboxWriteS16(data->gtuneCycleResult.gtuneNewP);
break;
case FLIGHT_LOG_EVENT_LOGGING_RESUME:
blackboxWriteUnsignedVB(data->loggingResume.logIteration);
blackboxWriteUnsignedVB(data->loggingResume.currentTime);

1
src/main/blackbox/blackbox_fielddefs.h
View file

@ -105,7 +105,6 @@ typedef enum FlightLogEvent {
FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
FLIGHT_LOG_EVENT_GTUNE_RESULT = 20,
FLIGHT_LOG_EVENT_LOG_END = 255
} FlightLogEvent;

13
src/main/common/maths.c
View file

@ -337,3 +337,16 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
dest[i] = array1[i] - array2[i];
}
}
int16_t qPercent(q_number_t q) {
return 100 * q.num / q.den;
}
int16_t qMultiply(q_number_t q, int16_t input) {
return input * q.num / q.den;
}
void qConstruct(q_number_t *qNumber, int16_t num, int16_t den, int qType) {
qNumber->num = (1 << qType) / den;
qNumber->den = (1 << qType) / num;
}

20
src/main/common/maths.h
View file

@ -40,6 +40,22 @@ typedef struct stdev_s
int m_n;
} stdev_t;
#define Q12(x) ((1 << 12) / x)
#define Q13(x) ((1 << 13) / x)
#define Q14(x) ((1 << 14) / x)
typedef enum {
Q12_NUMBER = 12,
Q13_NUMBER,
Q14_NUMBER
} qTypeIndex_e;
typedef struct q_number_s {
int16_t num;
int16_t den;
} q_number_t;
// Floating point 3 vector.
typedef struct fp_vector {
float X;
@ -108,3 +124,7 @@ float acos_approx(float x);
#endif
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
int16_t qPercent(q_number_t q);
int16_t qMultiply(q_number_t q, int16_t input);
void qConstruct(q_number_t *qNumber, int16_t num, int16_t den, int qType);

58
src/main/config/config.c
View file

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 122;
static const uint8_t EEPROM_CONF_VERSION = 125;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -176,9 +176,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PIDVEL] = 45;
pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->dterm_average_count = 4;
pidProfile->dterm_lpf_hz = 0; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->airModeInsaneAcrobilityFactor = 0;
pidProfile->P_f[ROLL] = 1.1f;
pidProfile->I_f[ROLL] = 0.4f;
@ -193,17 +194,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->H_level = 4.0f;
pidProfile->H_sensitivity = 75;
#ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune.
pidProfile->gtune_lolimP[YAW] = 10; // [0..200] Lower limit of YAW P during G tune.
pidProfile->gtune_hilimP[ROLL] = 100; // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis.
pidProfile->gtune_hilimP[PITCH] = 100; // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis.
pidProfile->gtune_hilimP[YAW] = 100; // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis.
pidProfile->gtune_pwr = 0; // [0..10] Strength of adjustment
pidProfile->gtune_settle_time = 450; // [200..1000] Settle time in ms
pidProfile->gtune_average_cycles = 16; // [8..128] Number of looptime cycles used for gyro average calculation
#endif
}
#ifdef GPS
@ -332,7 +322,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
void resetMixerConfig(mixerConfig_t *mixerConfig) {
mixerConfig->yaw_motor_direction = 1;
mixerConfig->airmode_saturation_limit = 50;
mixerConfig->airmode_saturation_limit = 30;
mixerConfig->yaw_jump_prevention_limit = 200;
#ifdef USE_SERVOS
mixerConfig->tri_unarmed_servo = 1;
@ -349,7 +339,8 @@ uint8_t getCurrentProfile(void)
static void setProfile(uint8_t profileIndex)
{
currentProfile = &masterConfig.profile[profileIndex];
currentControlRateProfile = &currentProfile->controlRateProfile;
currentControlRateProfileIndex = currentProfile->activeRateProfile;
currentControlRateProfile = &currentProfile->controlRateProfile[currentControlRateProfileIndex];
}
uint8_t getCurrentControlRateProfile(void)
@ -357,8 +348,15 @@ uint8_t getCurrentControlRateProfile(void)
return currentControlRateProfileIndex;
}
static void setControlRateProfile(uint8_t profileIndex) {
currentControlRateProfileIndex = profileIndex;
masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex;
currentControlRateProfile = &masterConfig.profile[getCurrentProfile()].controlRateProfile[profileIndex];
}
controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) {
return &masterConfig.profile[profileIndex].controlRateProfile;
return &masterConfig.profile[profileIndex].controlRateProfile[masterConfig.profile[profileIndex].activeRateProfile];
}
uint16_t getCurrentMinthrottle(void)
@ -401,7 +399,7 @@ static void resetConf(void)
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 1; // 188HZ
masterConfig.gyro_sync_denom = 1;
masterConfig.gyro_sync_denom = 4;
masterConfig.gyro_soft_lpf_hz = 60;
resetAccelerometerTrims(&masterConfig.accZero);
@ -444,6 +442,9 @@ static void resetConf(void)
masterConfig.rxConfig.rssi_ppm_invert = 0;
masterConfig.rxConfig.rcSmoothing = 0;
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = 4;
masterConfig.rxConfig.acroPlusFactor = 30;
masterConfig.rxConfig.acroPlusOffset = 40;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
@ -486,14 +487,15 @@ static void resetConf(void)
masterConfig.emf_avoidance = 0;
resetPidProfile(&currentProfile->pidProfile);
resetControlRateConfig(&masterConfig.profile[0].controlRateProfile);
uint8_t rI;
for (rI = 0; rI<MAX_RATEPROFILES; rI++) {
resetControlRateConfig(&masterConfig.profile[0].controlRateProfile[rI]);
}
resetRollAndPitchTrims(&masterConfig.accelerometerTrims);
masterConfig.mag_declination = 0;
masterConfig.acc_lpf_hz = 20;
masterConfig.accz_lpf_cutoff = 5.0f;
masterConfig.acc_lpf_hz = 10.0f;
masterConfig.accDeadband.xy = 40;
masterConfig.accDeadband.z = 40;
masterConfig.acc_unarmedcal = 1;
@ -720,7 +722,6 @@ void activateConfig(void)
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
#endif
currentProfile->pidProfile.pidController = constrain(currentProfile->pidProfile.pidController, 1, 2); // This should prevent UNUSED values. CF 1.11 support
pidSetController(currentProfile->pidProfile.pidController);
#ifdef GPS
@ -730,6 +731,7 @@ void activateConfig(void)
useFailsafeConfig(&masterConfig.failsafeConfig);
setAccelerationTrims(&masterConfig.accZero);
setAccelerationFilter(masterConfig.acc_lpf_hz);
mixerUseConfigs(
#ifdef USE_SERVOS
@ -745,7 +747,6 @@ void activateConfig(void)
imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
imuRuntimeConfig.acc_cut_hz = masterConfig.acc_lpf_hz;
imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal;
imuRuntimeConfig.small_angle = masterConfig.small_angle;
@ -753,7 +754,6 @@ void activateConfig(void)
&imuRuntimeConfig,
&currentProfile->pidProfile,
&masterConfig.accDeadband,
masterConfig.accz_lpf_cutoff,
masterConfig.throttle_correction_angle
);
@ -1005,6 +1005,14 @@ void changeProfile(uint8_t profileIndex)
beeperConfirmationBeeps(profileIndex + 1);
}
void changeControlRateProfile(uint8_t profileIndex) {
if (profileIndex > MAX_RATEPROFILES) {
profileIndex = MAX_RATEPROFILES - 1;
}
setControlRateProfile(profileIndex);
activateControlRateConfig();
}
void handleOneshotFeatureChangeOnRestart(void)
{
// Shutdown PWM on all motors prior to soft restart

3
src/main/config/config.h
View file

@ -18,6 +18,7 @@
#pragma once
#define MAX_PROFILE_COUNT 2
#define MAX_RATEPROFILES 3
#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
typedef enum {
@ -67,7 +68,7 @@ uint8_t getCurrentProfile(void);
void changeProfile(uint8_t profileIndex);
uint8_t getCurrentControlRateProfile(void);
void changeControlRateProfile(uint8_t profileIndex);
bool canSoftwareSerialBeUsed(void);
uint16_t getCurrentMinthrottle(void);

3
src/main/config/config_master.h
View file

@ -72,8 +72,7 @@ typedef struct master_t {
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
uint8_t acc_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
float accz_lpf_cutoff; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
float acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
accDeadband_t accDeadband;
barometerConfig_t barometerConfig;
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off

3
src/main/config/config_profile.h
View file

@ -19,5 +19,6 @@
typedef struct profile_s {
pidProfile_t pidProfile;
controlRateConfig_t controlRateProfile;
uint8_t activeRateProfile;
controlRateConfig_t controlRateProfile[MAX_RATEPROFILES];
} profile_t;

1
src/main/config/runtime_config.h
View file

@ -42,7 +42,6 @@ typedef enum {
PASSTHRU_MODE = (1 << 8),
SONAR_MODE = (1 << 9),
FAILSAFE_MODE = (1 << 10),
GTUNE_MODE = (1 << 11),
} flightModeFlags_e;
extern uint16_t flightModeFlags;

2
src/main/drivers/gyro_sync.h
View file

@ -5,7 +5,7 @@
* Author: borisb
*/
#define INTERRUPT_WAIT_TIME 12
#define INTERRUPT_WAIT_TIME 5
extern uint32_t targetLooptime;

5
src/main/drivers/pwm_mapping.c
View file

@ -784,6 +784,11 @@ if (init->useBuzzerP6) {
if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) {
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2);
}
#endif
#ifdef SPRACINGF3MINI
if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) {
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM3);
}
#endif
ppmInConfig(timerHardwarePtr);
} else if (type == MAP_TO_PWM_INPUT) {

211
src/main/flight/gtune.c
View file

@ -1,211 +0,0 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#ifdef GTUNE
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "config/config.h"
#include "blackbox/blackbox.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
extern uint16_t cycleTime;
extern uint8_t motorCount;
/*
****************************************************************************
*** G_Tune ***
****************************************************************************
G_Tune Mode
This is the multiwii implementation of ZERO-PID Algorithm
http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html
The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com)
You may use/modify this algorithm on your own risk, kindly refer to above link in any future distribution.
*/
/*
version 1.0.0: MIN & Maxis & Tuned Band
version 1.0.1:
a. error is gyro reading not rc - gyro.
b. OldError = Error no averaging.
c. No Min Maxis BOUNDRY
version 1.0.2:
a. no boundaries
b. I - Factor tune.
c. time_skip
Crashpilot: Reduced to just P tuning in a predefined range - so it is not "zero pid" anymore.
Tuning is limited to just work when stick is centered besides that YAW is tuned in non Acro as well.
See also:
http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2
http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190
Gyrosetting 2000DPS
GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s
pidProfile->gtune_lolimP[ROLL] = 10; [0..200] Lower limit of ROLL P during G tune.
pidProfile->gtune_lolimP[PITCH] = 10; [0..200] Lower limit of PITCH P during G tune.
pidProfile->gtune_lolimP[YAW] = 10; [0..200] Lower limit of YAW P during G tune.
pidProfile->gtune_hilimP[ROLL] = 100; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis.
pidProfile->gtune_hilimP[PITCH] = 100; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis.
pidProfile->gtune_hilimP[YAW] = 100; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis.
pidProfile->gtune_pwr = 0; [0..10] Strength of adjustment
pidProfile->gtune_settle_time = 450; [200..1000] Settle time in ms
pidProfile->gtune_average_cycles = 16; [8..128] Number of looptime cycles used for gyro average calculation
*/
static pidProfile_t *pidProfile;
static int16_t delay_cycles;
static int16_t time_skip[3];
static int16_t OldError[3], result_P64[3];
static int32_t AvgGyro[3];
static bool floatPID;
void updateDelayCycles(void)
{
delay_cycles = -(((int32_t)pidProfile->gtune_settle_time * 1000) / cycleTime);
}
void init_Gtune(pidProfile_t *pidProfileToTune)
{
uint8_t i;
pidProfile = pidProfileToTune;
if (pidProfile->pidController == 2) {
floatPID = true; // LuxFloat is using float values for PID settings
} else {
floatPID = false;
}
updateDelayCycles();
for (i = 0; i < 3; i++) {
if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning
pidProfile->gtune_hilimP[i] = 0; // Disable YAW tuning for everything below a quadcopter
}
if (floatPID) {
if((pidProfile->P_f[i] * 10.0f) < pidProfile->gtune_lolimP[i]) {
pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10.0f);
}
result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P.
} else {
if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) {
pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
}
result_P64[i] = (int16_t)pidProfile->P8[i] << 6; // 6 bit extra resolution for P.
}
OldError[i] = 0;
time_skip[i] = delay_cycles;
}
}
void calculate_Gtune(uint8_t axis)
{
int16_t error, diff_G, threshP;
if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) { // Block tuning on stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode
OldError[axis] = 0;
time_skip[axis] = delay_cycles; // Some settle time after stick center. default 450ms
} else {
if (!time_skip[axis]) AvgGyro[axis] = 0;
time_skip[axis]++;
if (time_skip[axis] > 0) {
if (axis == FD_YAW) {
AvgGyro[axis] += 32 * ((int16_t)gyroADC[axis] / 32); // Chop some jitter and average
} else {
AvgGyro[axis] += 128 * ((int16_t)gyroADC[axis] / 128); // Chop some jitter and average
}
}
if (time_skip[axis] == pidProfile->gtune_average_cycles) { // Looptime cycles for gyro average calculation. default 16.
AvgGyro[axis] /= time_skip[axis]; // AvgGyro[axis] has now very clean gyrodata
time_skip[axis] = 0;
if (axis == FD_YAW) {
threshP = 20;
error = -AvgGyro[axis];
} else {
threshP = 10;
error = AvgGyro[axis];
}
if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) { // Don't run when not needed or pointless to do so
diff_G = ABS(error) - ABS(OldError[axis]);
if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) {
if (diff_G > threshP) {
if (axis == FD_YAW) {
result_P64[axis] += 256 + pidProfile->gtune_pwr; // YAW ends up at low limit on float PID, give it some more to work with.
} else {
result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side.
}
} else {
if (diff_G < -threshP) {
if (axis == FD_YAW) {
result_P64[axis] -= 64 + pidProfile->gtune_pwr;
} else {
result_P64[axis] -= 32;
}
}
}
} else {
if (ABS(diff_G) > threshP && axis != FD_YAW) {
result_P64[axis] -= 32; // Don't use antiwobble for YAW
}
}
int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_gtuneCycleResult_t eventData;
eventData.gtuneAxis = axis;
eventData.gtuneGyroAVG = AvgGyro[axis];
eventData.gtuneNewP = newP; // for float PID the logged P value is still mutiplyed by 10
blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
}
#endif
if (floatPID) {
pidProfile->P_f[axis] = (float)newP / 10.0f; // new P value for float PID
} else {
pidProfile->P8[axis] = newP; // new P value
}
}
OldError[axis] = error;
}
}
}
#endif

21
src/main/flight/gtune.h
View file

@ -1,21 +0,0 @@
/*
* 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/>.
*/
#pragma once
void init_Gtune(pidProfile_t *pidProfileToTune);
void calculate_Gtune(uint8_t axis);

27
src/main/flight/imu.c
View file

@ -56,7 +56,6 @@
// http://gentlenav.googlecode.com/files/fastRotations.pdf
#define SPIN_RATE_LIMIT 20
int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT];
uint32_t accTimeSum = 0; // keep track for integration of acc
@ -111,14 +110,13 @@ void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig,
pidProfile_t *initialPidProfile,
accDeadband_t *initialAccDeadband,
float accz_lpf_cutoff,
uint16_t throttle_correction_angle
)
{
imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile;
accDeadband = initialAccDeadband;
fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
}
@ -377,11 +375,8 @@ static bool isMagnetometerHealthy(void)
static void imuCalculateEstimatedAttitude(void)
{
static biquad_t accLPFState[3];
static bool accStateIsSet;
static uint32_t previousIMUUpdateTime;
float rawYawError = 0;
int32_t axis;
bool useAcc = false;
bool useMag = false;
bool useYaw = false;
@ -390,20 +385,6 @@ static void imuCalculateEstimatedAttitude(void)
uint32_t deltaT = currentTime - previousIMUUpdateTime;
previousIMUUpdateTime = currentTime;
// Smooth and use only valid accelerometer readings
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz) {
if (accStateIsSet) {
accSmooth[axis] = lrintf(applyBiQuadFilter((float) accADC[axis], &accLPFState[axis]));
} else {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(imuRuntimeConfig->acc_cut_hz, &accLPFState[axis], 1000);
accStateIsSet = true;
}
} else {
accSmooth[axis] = accADC[axis];
}
}
if (imuIsAccelerometerHealthy()) {
useAcc = true;
}
@ -445,9 +426,9 @@ void imuUpdateGyroAndAttitude(void)
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
imuCalculateEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
accSmooth[X] = 0;
accSmooth[Y] = 0;
accSmooth[Z] = 0;
}
}

4
src/main/flight/imu.h
View file

@ -21,7 +21,6 @@ extern int16_t throttleAngleCorrection;
extern uint32_t accTimeSum;
extern int accSumCount;
extern float accVelScale;
extern int16_t accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT];
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
@ -74,7 +73,6 @@ void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig,
pidProfile_t *initialPidProfile,
accDeadband_t *initialAccDeadband,
float accz_lpf_cutoff,
uint16_t throttle_correction_angle
);
@ -84,7 +82,7 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
void imuUpdateGyroAndAttitude(void);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
int16_t imuCalculateHeading(t_fp_vector *vec);

56
src/main/flight/mixer.c
View file

@ -57,7 +57,6 @@
#include "config/config.h"
uint8_t motorCount;
extern float dT;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@ -749,13 +748,46 @@ STATIC_UNIT_TESTED void servoMixer(void)
#endif
void acroPlusApply(void) {
int axis;
for (axis = 0; axis < 2; axis++) {
int16_t factor;
q_number_t wowFactor;
int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100)
int16_t acroPlusStickOffset = rxConfig->acroPlusOffset * 5;
int16_t motorRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle;
if (feature(FEATURE_3D)) motorRange = (motorRange - (flight3DConfig->deadband3d_high - flight3DConfig->deadband3d_low)) / 2;
/* acro plus factor handling */
if (rxConfig->acroPlusFactor && ABS(rcCommandDeflection) > acroPlusStickOffset + 10) {
if (rcCommandDeflection > 0) {
rcCommandDeflection -= acroPlusStickOffset;
} else {
rcCommandDeflection += acroPlusStickOffset;
}
qConstruct(&wowFactor,ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500, Q12_NUMBER);
factor = qMultiply(wowFactor, (rcCommandDeflection * motorRange) / 500);
wowFactor.num = wowFactor.den - wowFactor.num;
} else {
qConstruct(&wowFactor, 1, 1, Q12_NUMBER);
factor = 0;
}
axisPID[axis] = factor + qMultiply(wowFactor, axisPID[axis]);
}
}
void mixTable(void)
{
uint32_t i;
float vbatCompensationFactor;
q_number_t vbatCompensationFactor;
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply();
}
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
@ -775,7 +807,7 @@ void mixTable(void)
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] *= vbatCompensationFactor; // Add voltage compensation
if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
@ -816,12 +848,14 @@ void mixTable(void)
if (rollPitchYawMixRange > throttleRange) {
motorLimitReached = true;
float mixReduction = (float) throttleRange / rollPitchYawMixRange;
q_number_t mixReduction;
qConstruct(&mixReduction, throttleRange, rollPitchYawMixRange, Q12_NUMBER);
//float mixReduction = (float) throttleRange / rollPitchYawMixRange;
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] = lrintf((float) rollPitchYawMix[i] * mixReduction);
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
}
// Get the maximum correction by setting offset to center. Only active below 50% of saturation levels to reduce spazzing out in crashes
if ((mixReduction > (mixerConfig->airmode_saturation_limit / 100.0f)) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if ((qPercent(mixReduction) > mixerConfig->airmode_saturation_limit) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
}
@ -927,7 +961,8 @@ void filterServos(void)
{
#ifdef USE_SERVOS
int16_t servoIdx;
static filterStatePt1_t servoFitlerState[MAX_SUPPORTED_SERVOS];
static bool servoFilterIsSet;
static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
@ -935,7 +970,12 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = filterApplyPt1(servo[servoIdx], &servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, dT);
if (!servoFilterIsSet) {
BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetLooptime);
servoFilterIsSet = true;
}
servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx]));
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}

268
src/main/flight/pid.c
View file

@ -44,10 +44,11 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/gtune.h"
#include "config/runtime_config.h"
extern uint8_t motorCount;
extern float dT;
extern bool motorLimitReached;
@ -57,32 +58,52 @@ int16_t axisPID[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
#define DELTA_TOTAL_SAMPLES 3
#define DELTA_MAX_SAMPLES 12
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3];
uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
static int32_t errorGyroI[3], errorGyroILimit[3];
static float errorGyroIf[3], errorGyroIfLimit[3];
static int32_t errorAngleI[2];
static float errorAngleIf[2];
static bool lowThrottlePidReduction;
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we using, defaultMultiWii
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
void pidResetErrorGyro(void)
void pidResetErrorAngle(void)
{
int axis;
for (axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
errorAngleIf[ROLL] = 0.0f;
errorAngleIf[PITCH] = 0.0f;
}
void pidResetErrorGyroState(uint8_t resetOption)
{
if (resetOption >= RESET_ITERM) {
int axis;
for (axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
}
}
if (resetOption == RESET_ITERM_AND_REDUCE_PID) {
lowThrottlePidReduction = true;
} else {
lowThrottlePidReduction = false;
}
}
float scaleItermToRcInput(int axis) {
void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
static float antiWindUpIncrement = 0;
@ -92,34 +113,23 @@ float scaleItermToRcInput(int axis) {
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */
iTermScaler[axis] = 0.0f;
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
} else {
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms */
if (iTermScaler[axis] < 1) {
iTermScaler[axis] = constrainf(iTermScaler[axis] + antiWindUpIncrement, 0.0f, 1.0f);
} else {
iTermScaler[axis] = 1;
if (pidProfile->pidController != PID_CONTROLLER_LUX_FLOAT) {
errorGyroI[axis] *= iTermScaler[axis];
} else {
errorGyroIf[axis] *= iTermScaler[axis];
}
}
}
return iTermScaler[axis];
}
void acroPlusApply(acroPlus_t *axisState, int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
axisState->wowFactor = 1;
axisState->factor = 0;
/* acro plus factor handling */
if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor && (!flightModeFlags)) {
axisState->wowFactor = ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f); //0-1f
axisState->factor = axisState->wowFactor * rcCommandReflection * 1000;
axisState->wowFactor = 1.0f - axisState->wowFactor;
}
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static acroPlus_t acroPlusState[3];
static biquad_t deltaBiQuadState[3];
static bool deltaStateIsSet;
@ -129,11 +139,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastErrorForDelta[3];
static float previousDelta[3][DELTA_TOTAL_SAMPLES];
static float previousDelta[3][DELTA_MAX_SAMPLES];
float delta, deltaSum;
int axis, deltaCount;
float horizonLevelStrength = 1;
float dT = (float)targetLooptime * 0.000001f;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
deltaStateIsSet = true;
@ -155,11 +167,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
uint8_t rate = 30;
// -----Get the desired angle rate depending on flight mode
if (axis == YAW || !pidProfile->airModeInsaneAcrobilityFactor || !IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
rate = controlRateConfig->rates[axis];
}
uint8_t rate = controlRateConfig->rates[axis];
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
@ -202,7 +210,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroIf[axis] *= scaleItermToRcInput(axis);
scaleItermToRcInput(axis, pidProfile);
if (antiWindupProtection || motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
@ -232,10 +240,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
} else {
// Apply moving average
deltaSum = 0;
for (deltaCount = DELTA_TOTAL_SAMPLES-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (deltaSum / DELTA_TOTAL_SAMPLES);
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (deltaSum / pidProfile->dterm_average_count);
}
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
@ -243,16 +251,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis];
}
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
if (lowThrottlePidReduction) axisPID[axis] /= 4;
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
@ -262,7 +261,145 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
}
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
int axis, deltaCount, prop = 0;
int32_t rc, error, errorAngle, delta, gyroError;
int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
static int16_t lastErrorForDelta[2];
static int32_t previousDelta[2][DELTA_MAX_SAMPLES];
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
deltaStateIsSet = true;
}
if (FLIGHT_MODE(HORIZON_MODE)) {
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
}
// PITCH & ROLL
for (axis = 0; axis < 2; axis++) {
rc = rcCommand[axis] << 1;
gyroError = gyroADC[axis] / 4;
error = rc - gyroError;
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here
if (ABS(gyroADC[axis]) > (640 * 4)) {
errorGyroI[axis] = 0;
}
// Anti windup protection
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
scaleItermToRcInput(axis, pidProfile);
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
}
ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6;
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // axis relying on ACC
// 50 degrees max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here
PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result
int16_t limit = pidProfile->D8[PIDLEVEL] * 5;
PTermACC = constrain(PTermACC, -limit, +limit);
ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9);
PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9);
}
PTerm -= ((int32_t)gyroError * dynP8[axis]) >> 6; // 32 bits is needed for calculation
//-----calculate D-term based on the configured approach (delta from measurement or deltafromError)
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = error - lastErrorForDelta[axis];
lastErrorForDelta[axis] = error;
} else { /* Delta from measurement */
delta = -(gyroError - lastErrorForDelta[axis]);
lastErrorForDelta[axis] = gyroError;
}
if (deltaStateIsSet) {
DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
} else {
// Apply moving average
DTerm = 0;
for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) DTerm += previousDelta[axis][deltaCount];
delta = (DTerm / pidProfile->dterm_average_count) * 3; // Keep same original scaling
}
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm + DTerm;
if (lowThrottlePidReduction) axisPID[axis] /= 4;
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
axisPID_D[axis] = DTerm;
#endif
}
//YAW
rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5;
#ifdef ALIENWII32
error = rc - gyroADC[FD_YAW];
#else
error = rc - (gyroADC[FD_YAW] / 4);
#endif
errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW];
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0;
PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended
// Constrain YAW by D value if not servo driven in that case servolimits apply
if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) {
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
axisPID[FD_YAW] = PTerm + ITerm;
if (lowThrottlePidReduction) axisPID[FD_YAW] /= 4;
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
axisPID_I[FD_YAW] = ITerm;
axisPID_D[FD_YAW] = 0;
#endif
}
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
@ -270,7 +407,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int axis, deltaCount;
int32_t PTerm, ITerm, DTerm, delta, deltaSum;
static int32_t lastErrorForDelta[3] = { 0, 0, 0 };
static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES];
static int32_t previousDelta[3][DELTA_MAX_SAMPLES];
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
@ -294,11 +431,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
uint8_t rate = 40;
// -----Get the desired angle rate depending on flight mode
if (axis == YAW || !pidProfile->airModeInsaneAcrobilityFactor || !IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
rate = controlRateConfig->rates[axis];
}
uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode
if (axis == FD_YAW) {
@ -348,7 +481,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis));
scaleItermToRcInput(axis, pidProfile);
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
@ -376,10 +509,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
} else {
// Apply moving average
deltaSum = 0;
for (deltaCount = DELTA_TOTAL_SAMPLES -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
for (deltaCount = pidProfile->dterm_average_count -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = deltaSum;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (deltaSum / pidProfile->dterm_average_count) * 3; // Keep same original scaling
}
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
@ -387,16 +520,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = lrintf(acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis]);
}
if (lowThrottlePidReduction) axisPID[axis] /= 4;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
@ -411,10 +536,13 @@ void pidSetController(pidControllerType_e type)
switch (type) {
default:
case PID_CONTROLLER_MWREWRITE:
pid_controller = pidRewrite;
pid_controller = pidMultiWiiRewrite;
break;
case PID_CONTROLLER_LUX_FLOAT:
pid_controller = pidLuxFloat;
break;
case PID_CONTROLLER_MW23:
pid_controller = pidMultiWii23;
}
}

29
src/main/flight/pid.h
View file

@ -37,7 +37,8 @@ typedef enum {
} pidIndex_e;
typedef enum {
PID_CONTROLLER_MWREWRITE = 1,
PID_CONTROLLER_MW23,
PID_CONTROLLER_MWREWRITE,
PID_CONTROLLER_LUX_FLOAT,
PID_COUNT
} pidControllerType_e;
@ -47,6 +48,12 @@ typedef enum {
DELTA_FROM_MEASUREMENT
} pidDeltaType_e;
typedef enum {
RESET_DISABLE = 0,
RESET_ITERM,
RESET_ITERM_AND_REDUCE_PID
} pidErrorResetOption_e;
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef struct pidProfile_s {
@ -63,28 +70,18 @@ typedef struct pidProfile_s {
float H_level;
uint8_t H_sensitivity;
uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
float dterm_lpf_hz; // Delta Filter in hz
uint8_t deltaMethod; // Alternative delta Calculation
uint8_t deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm
#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
uint8_t gtune_hilimP[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.
uint8_t gtune_pwr; // [0..10] Strength of adjustment
uint16_t gtune_settle_time; // [200..1000] Settle time in ms
uint8_t gtune_average_cycles; // [8..128] Number of looptime cycles used for gyro average calculation
#endif
} pidProfile_t;
typedef struct acroPlus_s {
float factor;
float wowFactor;
} acroPlus_t;
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
bool antiWindupProtection;
void pidSetController(pidControllerType_e type);
void pidResetErrorGyro(void);
void pidResetErrorAngle(void);
void pidResetErrorGyroState(uint8_t resetOption);

4
src/main/io/i2c_bst.c
View file

@ -322,7 +322,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD SW;", 19 },
{ BOXTELEMETRY, "TELEMETRY;", 20 },
{ BOXGTUNE, "GTUNE;", 21 },
// { BOXGTUNE, "GTUNE;", 21 },
{ BOXSONAR, "SONAR;", 22 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
@ -584,7 +584,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
@ -1689,7 +1688,6 @@ bool writeFCModeToBST(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |

11
src/main/io/rc_controls.c
View file

@ -651,7 +651,6 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
};
}
/* Is this needed ?
void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
@ -659,7 +658,7 @@ void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeProfile(position); // Is this safe to change profile "in flight" ?
changeControlRateProfile(position);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
applied = true;
}
@ -670,7 +669,7 @@ void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
beeperConfirmationBeeps(position + 1);
}
}
*/
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
@ -724,10 +723,10 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx
applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
//uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); TODO - cleanup
//uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; TODO - cleanup
uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
// applySelectAdjustment(adjustmentFunction, position);
applySelectAdjustment(adjustmentFunction, position);
}
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
}

1
src/main/io/rc_controls.h
View file

@ -41,7 +41,6 @@ typedef enum {
BOXGOV,
BOXOSD,
BOXTELEMETRY,
BOXGTUNE,
BOXSONAR,
BOXSERVO1,
BOXSERVO2,

87
src/main/io/serial_cli.c
View file

@ -113,6 +113,7 @@ static void cliFeature(char *cmdline);
static void cliMotor(char *cmdline);
static void cliPlaySound(char *cmdline);
static void cliProfile(char *cmdline);
static void cliRateProfile(char *cmdline);
static void cliReboot(void);
static void cliSave(char *cmdline);
static void cliSerial(char *cmdline);
@ -279,6 +280,7 @@ const clicmd_t cmdTable[] = {
"[<index>]\r\n", cliPlaySound),
CLI_COMMAND_DEF("profile", "change profile",
"[<index>]", cliProfile),
CLI_COMMAND_DEF("rateprofile", "change rate profile", "[<index>]", cliRateProfile),
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail),
CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
@ -350,7 +352,7 @@ static const char * const lookupTableBlackboxDevice[] = {
static const char * const lookupTablePidController[] = {
"UNUSED", "MWREWRITE", "LUX"
"MW23", "MWREWRITE", "LUX"
};
static const char * const lookupTableSerialRX[] = {
@ -470,7 +472,7 @@ typedef enum {
// value section
MASTER_VALUE = (0 << VALUE_SECTION_OFFSET),
PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET),
PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET),
// value mode
MODE_DIRECT = (0 << VALUE_MODE_OFFSET),
MODE_LOOKUP = (1 << VALUE_MODE_OFFSET)
@ -514,6 +516,7 @@ const clivalue_t valueTable[] = {
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
{ "rc_smoothing", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON } },
{ "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } },
{ "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 13 } },
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
@ -561,17 +564,6 @@ const clivalue_t valueTable[] = {
{ "nav_speed_max", VAR_UINT16 | MASTER_VALUE, &masterConfig.gpsProfile.nav_speed_max, .config.minmax = { 10, 2000 } },
{ "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsProfile.nav_slew_rate, .config.minmax = { 0, 100 } },
#endif
#ifdef GTUNE
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } },
{ "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10, 200 } },
{ "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], .config.minmax = { 10, 200 } },
{ "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], .config.minmax = { 0, 200 } },
{ "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], .config.minmax = { 0, 200 } },
{ "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], .config.minmax = { 0, 200 } },
{ "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, .config.minmax = { 0, 10 } },
{ "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, .config.minmax = { 200, 1000 } },
{ "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 } },
#endif
{ "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } },
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} },
@ -626,22 +618,25 @@ const clivalue_t valueTable[] = {
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } },
{ "airmode_saturation_limit", VAR_UINT8 | MASTER_VALUE, &masterConfig.mixerConfig.airmode_saturation_limit, .config.minmax = { 0, 100 } },
{ "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH } },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
#ifdef USE_SERVOS
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
{ "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} },
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
#endif
{ "rc_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rcRate8, .config.minmax = { 0, 250 } },
{ "rc_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rcExpo8, .config.minmax = { 0, 100 } },
{ "rc_yaw_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rcYawExpo8, .config.minmax = { 0, 100 } },
{ "thr_mid", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.thrMid8, .config.minmax = { 0, 100 } },
{ "thr_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.thrExpo8, .config.minmax = { 0, 100 } },
{ "roll_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "pitch_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "yaw_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "tpa_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } },
{ "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } },
{ "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } },
{ "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } },
{ "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } },
{ "roll_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "pitch_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "acro_plus_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } },
{ "acro_plus_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } },
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } },
@ -658,10 +653,9 @@ const clivalue_t valueTable[] = {
#endif
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
{ "acc_lpf_hz", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 200 } },
{ "acc_lpf_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 400 } },
{ "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0, 100 } },
{ "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0, 100 } },
{ "accz_lpf_cutoff", VAR_FLOAT | MASTER_VALUE, &masterConfig.accz_lpf_cutoff, .config.minmax = { 1, 20 } },
{ "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } },
{ "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } },
{ "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } },
@ -676,6 +670,7 @@ const clivalue_t valueTable[] = {
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {2, 12 } },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },
@ -715,8 +710,6 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
{ "acro_plus_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } },
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } },
@ -1676,9 +1669,10 @@ static void dumpValues(uint16_t valueSection)
typedef enum {
DUMP_MASTER = (1 << 0),
DUMP_PROFILE = (1 << 1),
DUMP_RATES = (1 << 2),
} dumpFlags_e;
#define DUMP_ALL (DUMP_MASTER | DUMP_PROFILE)
#define DUMP_ALL (DUMP_MASTER | DUMP_PROFILE | DUMP_RATES)
static const char* const sectionBreak = "\r\n";
@ -1702,6 +1696,9 @@ static void cliDump(char *cmdline)
if (strcasecmp(cmdline, "profile") == 0) {
dumpMask = DUMP_PROFILE; // only
}
if (strcasecmp(cmdline, "rates") == 0) {
dumpMask = DUMP_RATES;
}
if (dumpMask & DUMP_MASTER) {
@ -1840,7 +1837,19 @@ static void cliDump(char *cmdline)
printSectionBreak();
dumpValues(PROFILE_VALUE);
dumpValues(PROFILE_RATE_VALUE);
}
if (dumpMask & DUMP_RATES) {
cliPrint("\r\n# dump rates\r\n");
cliPrint("\r\n# rateprofile\r\n");
cliRateProfile("");
printSectionBreak();
dumpValues(PROFILE_RATE_VALUE);
}
}
void cliEnter(serialPort_t *serialPort)
@ -2136,6 +2145,21 @@ static void cliProfile(char *cmdline)
}
}
static void cliRateProfile(char *cmdline) {
int i;
if (isEmpty(cmdline)) {
cliPrintf("rateprofile %d\r\n", getCurrentControlRateProfile());
return;
} else {
i = atoi(cmdline);
if (i >= 0 && i < MAX_RATEPROFILES) {
changeControlRateProfile(i);
cliRateProfile("");
}
}
}
static void cliReboot(void) {
cliPrint("\r\nRebooting");
bufWriterFlush(cliWriter);
@ -2198,6 +2222,10 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full)
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
}
if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) {
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile());
}
switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8:
value = *(uint8_t *)ptr;
@ -2247,6 +2275,9 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) {
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
}
if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) {
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile());
}
switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8:

9
src/main/io/serial_msp.c
View file

@ -167,7 +167,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD SW;", 19 },
{ BOXTELEMETRY, "TELEMETRY;", 20 },
{ BOXGTUNE, "GTUNE;", 21 },
// { BOXGTUNE, "GTUNE;", 21 },
{ BOXSONAR, "SONAR;", 22 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
@ -566,10 +566,6 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
}
#ifdef GTUNE
activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
#endif
memset(mspPorts, 0x00, sizeof(mspPorts));
mspAllocateSerialPorts(serialConfig);
}
@ -603,7 +599,6 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
@ -1283,7 +1278,7 @@ static bool processInCommand(void)
setGyroSamplingSpeed(read16());
break;
case MSP_SET_PID_CONTROLLER:
currentProfile->pidProfile.pidController = constrain(read8(), 1, 2);
currentProfile->pidProfile.pidController = read8();
pidSetController(currentProfile->pidProfile.pidController);
break;
case MSP_SET_PID:

7
src/main/main.c
View file

@ -651,12 +651,17 @@ int main(void) {
setTaskEnabled(TASK_GYROPID, true);
if(sensors(SENSOR_ACC)) {
uint32_t accTargetLooptime = 0;
setTaskEnabled(TASK_ACCEL, true);
switch(targetLooptime) {
case(500):
accTargetLooptime = 10000;
break;
case(375):
accTargetLooptime = 20000;
break;
case(250):
accTargetLooptime = 30000;
break;
default:
case(1000):
#ifdef STM32F10X

76
src/main/mw.c
View file

@ -78,7 +78,6 @@
#include "flight/imu.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/gtune.h"
#include "flight/navigation.h"
#include "config/runtime_config.h"
@ -101,7 +100,8 @@ enum {
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
#define IBATINTERVAL (6 * 3500)
#define GYRO_WATCHDOG_DELAY 100 // Watchdog delay for gyro sync
#define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync
#define JITTER_BUFFER_TIME 20 // cycleTime jitter buffer time
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
@ -116,10 +116,9 @@ int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
extern uint32_t currentTime;
extern uint8_t PIDweight[3];
extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
extern bool antiWindupProtection;
static filterStatePt1_t filteredCycleTimeState;
uint16_t filteredCycleTime;
static bool isRXDataNew;
@ -136,30 +135,6 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
saveConfigAndNotify();
}
#ifdef GTUNE
void updateGtuneState(void)
{
static bool GTuneWasUsed = false;
if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
ENABLE_FLIGHT_MODE(GTUNE_MODE);
init_Gtune(&currentProfile->pidProfile);
GTuneWasUsed = true;
}
if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {
saveConfigAndNotify();
GTuneWasUsed = false;
}
} else {
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
DISABLE_FLIGHT_MODE(GTUNE_MODE);
}
}
}
#endif
bool isCalibrating()
{
#ifdef BARO
@ -182,7 +157,7 @@ void filterRc(void){
// Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
rcInterpolationFactor = rxRefreshRate / targetLooptime + 1;
if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
@ -256,6 +231,10 @@ void annexCode(void)
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
if (axis == YAW) {
@ -292,6 +271,7 @@ void annexCode(void)
scaleRcCommandToFpvCamAngle();
}
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
@ -480,9 +460,15 @@ void processRx(void)
antiWindupProtection = false;
}
} else {
pidResetErrorGyro();
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
pidResetErrorGyroState(RESET_ITERM);
} else {
pidResetErrorGyroState(RESET_ITERM_AND_REDUCE_PID);
}
pidResetErrorAngle();
}
} else {
pidResetErrorGyroState(RESET_DISABLE);
antiWindupProtection = false;
}
@ -634,15 +620,6 @@ static bool haveProcessedAnnexCodeOnce = false;
void taskMainPidLoop(void)
{
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)targetLooptime * 0.000001f;
// Calculate average cycle time and average jitter
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 0.5f, dT);
debug[0] = cycleTime;
debug[1] = cycleTime - filteredCycleTime;
imuUpdateGyroAndAttitude();
annexCode();
@ -657,10 +634,6 @@ void taskMainPidLoop(void)
}
#endif
#ifdef GTUNE
updateGtuneState();
#endif
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
@ -734,12 +707,21 @@ void taskMainPidLoop(void)
// Function for loop trigger
void taskMainPidLoopCheck(void) {
// getTaskDeltaTime() returns delta time freezed at the moment of entering the scheduler. currentTime is freezed at the very same point.
// To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
static uint32_t previousTime;
cycleTime = micros() - previousTime;
previousTime = micros();
// Debugging parameters
debug[0] = cycleTime;
debug[1] = cycleTime - targetLooptime;
debug[2] = averageSystemLoadPercent;
while (1) {
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
if (gyroSyncCheckUpdate() || ((cycleTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
while (1) {
if (micros() >= JITTER_BUFFER_TIME + previousTime) break;
}
break;
}
}

17
src/main/rx/rx.c
View file

@ -95,11 +95,6 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define DELAY_5_HZ (1000000 / 5)
#define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent)
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
#ifdef STM32F303xC
#define MAX_RC_CHANNELS_HIGH_PERFORMANCE 10 // Maximum channels allowed during fast refresh rates for more performance
#else
#define MAX_RC_CHANNELS_HIGH_PERFORMANCE 8 // Maximum channels allowed during fast refresh rates for more performance
#endif
rxRuntimeConfig_t rxRuntimeConfig;
static rxConfig_t *rxConfig;
@ -464,16 +459,14 @@ static uint8_t getRxChannelCount(void) {
static uint8_t maxChannelsAllowed;
if (!maxChannelsAllowed) {
if (targetLooptime < 1000) {
if (MAX_RC_CHANNELS_HIGH_PERFORMANCE > rxRuntimeConfig.channelCount) {
maxChannelsAllowed = rxRuntimeConfig.channelCount;
} else {
maxChannelsAllowed = MAX_RC_CHANNELS_HIGH_PERFORMANCE;
}
} else {
uint8_t maxChannels = rxConfig->max_aux_channel + NON_AUX_CHANNEL_COUNT;
if (maxChannels > rxRuntimeConfig.channelCount) {
maxChannelsAllowed = rxRuntimeConfig.channelCount;
} else {
maxChannelsAllowed = maxChannels;
}
}
return maxChannelsAllowed;
}

3
src/main/rx/rx.h
View file

@ -121,6 +121,9 @@ typedef struct rxConfig_s {
uint16_t maxcheck; // maximum rc end
uint8_t rcSmoothing;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel;
uint8_t acroPlusFactor; // Air mode acrobility factor
uint8_t acroPlusOffset; // Air mode stick offset
uint16_t rx_min_usec;
uint16_t rx_max_usec;

5
src/main/scheduler.c
View file

@ -226,8 +226,7 @@ static cfTask_t cfTasks[TASK_COUNT] = {
uint16_t averageSystemLoadPercent = 0;
#define REALTIME_GUARD_INTERVAL_MIN 10
#define REALTIME_GUARD_INTERVAL_MAX 300
#define GUARD_INTERVAL 5
void taskSystem(void)
{
@ -248,7 +247,7 @@ void taskSystem(void)
}
}
realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX);
realtimeGuardInterval = GUARD_INTERVAL;
#if defined SCHEDULER_DEBUG
debug[2] = realtimeGuardInterval;
#endif

52
src/main/sensors/acceleration.c
View file

@ -17,14 +17,17 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#include "common/axis.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
@ -35,13 +38,12 @@
#include "sensors/acceleration.h"
int32_t accADC[XYZ_AXIS_COUNT];
int16_t accADCRaw[XYZ_AXIS_COUNT];
int32_t accSmooth[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions
sensor_align_e accAlign = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
int axis;
uint32_t accTargetLooptime;
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
@ -53,6 +55,10 @@ extern bool AccInflightCalibrationActive;
static flightDynamicsTrims_t *accelerationTrims;
static float accLpfCutHz = 0;
static biquad_t accFilterState[XYZ_AXIS_COUNT];
static bool accFilterInitialised = false;
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingA = calibrationCyclesRequired;
@ -82,6 +88,7 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{
static int32_t a[3];
int axis;
for (axis = 0; axis < 3; axis++) {
@ -90,10 +97,10 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
a[axis] = 0;
// Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accADC[axis];
a[axis] += accSmooth[axis];
// Reset global variables to prevent other code from using un-calibrated data
accADC[axis] = 0;
accSmooth[axis] = 0;
accelerationTrims->raw[axis] = 0;
}
@ -132,9 +139,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accADC[axis];
b[axis] += accSmooth[axis];
// Clear global variables for next reading
accADC[axis] = 0;
accSmooth[axis] = 0;
accelerationTrims->raw[axis] = 0;
}
// all values are measured
@ -166,20 +173,36 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
{
accADC[X] -= accelerationTrims->raw[X];
accADC[Y] -= accelerationTrims->raw[Y];
accADC[Z] -= accelerationTrims->raw[Z];
accSmooth[X] -= accelerationTrims->raw[X];
accSmooth[Y] -= accelerationTrims->raw[Y];
accSmooth[Z] -= accelerationTrims->raw[Z];
}
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
int16_t accADCRaw[XYZ_AXIS_COUNT];
int axis;
if (!acc.read(accADCRaw)) {
return;
}
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = accADCRaw[axis];
alignSensors(accADC, accADC, accAlign);
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(accLpfCutHz, &accFilterState[axis], accTargetLooptime);
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis]));
}
}
alignSensors(accSmooth, accSmooth, accAlign);
if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims);
@ -196,3 +219,8 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
{
accelerationTrims = accelerationTrimsToUse;
}
void setAccelerationFilter(float initialAccLpfCutHz)
{
accLpfCutHz = initialAccLpfCutHz;
}

4
src/main/sensors/acceleration.h
View file

@ -36,8 +36,9 @@ typedef enum {
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint16_t acc_1G;
extern uint32_t accTargetLooptime;
extern int32_t accADC[XYZ_AXIS_COUNT];
extern int32_t accSmooth[XYZ_AXIS_COUNT];
typedef struct rollAndPitchTrims_s {
int16_t roll;
@ -54,3 +55,4 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse);
void setAccelerationFilter(float initialAccLpfCutHz);

10
src/main/sensors/battery.c
View file

@ -210,11 +210,13 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
mAhDrawn = mAhdrawnRaw / (3600 * 100);
}
float calculateVbatPidCompensation(void) {
float batteryScaler = 1.0f;
q_number_t calculateVbatPidCompensation(void) {
q_number_t batteryScaler;
if (batteryConfig->vbatPidCompensation && feature(FEATURE_VBAT) && batteryCellCount > 1) {
// Up to 25% PID gain. Should be fine for 4,2to 3,3 difference
batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.25f);
uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount;
qConstruct(&batteryScaler, maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage), Q12_NUMBER);
} else {
qConstruct(&batteryScaler, 1, 1, Q12_NUMBER);
}
return batteryScaler;

3
src/main/sensors/battery.h
View file

@ -18,6 +18,7 @@
#pragma once
#include "rx/rx.h"
#include "common/maths.h"
#define VBAT_SCALE_DEFAULT 110
#define VBAT_RESDIVVAL_DEFAULT 10
@ -76,6 +77,6 @@ batteryConfig_t *batteryConfig;
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
int32_t currentMeterToCentiamps(uint16_t src);
float calculateVbatPidCompensation(void);
q_number_t calculateVbatPidCompensation(void);
uint8_t calculateBatteryPercentage(void);
uint8_t calculateBatteryCapacityRemainingPercentage(void);

2
src/main/sensors/compass.c
View file

@ -43,7 +43,6 @@ mag_t mag; // mag access functions
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
int16_t magADCRaw[XYZ_AXIS_COUNT];
int32_t magADC[XYZ_AXIS_COUNT];
sensor_align_e magAlign = 0;
#ifdef MAG
@ -65,6 +64,7 @@ void updateCompass(flightDynamicsTrims_t *magZero)
static uint32_t nextUpdateAt, tCal = 0;
static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
int16_t magADCRaw[XYZ_AXIS_COUNT];
uint32_t axis;
if ((int32_t)(currentTime - nextUpdateAt) < 0)

6
src/main/sensors/gyro.c
View file

@ -37,7 +37,6 @@
#include "sensors/gyro.h"
uint16_t calibratingG = 0;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int32_t gyroADC[XYZ_AXIS_COUNT];
int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
@ -45,7 +44,6 @@ static gyroConfig_t *gyroConfig;
static biquad_t gyroFilterState[3];
static bool gyroFilterStateIsSet;
static float gyroLpfCutFreq;
int axis;
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
@ -57,6 +55,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
}
void initGyroFilterCoefficients(void) {
int axis;
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime);
gyroFilterStateIsSet = true;
@ -133,6 +132,9 @@ static void applyGyroZero(void)
void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int axis;
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;

3
src/main/target/ALIENFLIGHTF3/target.h
View file

@ -152,9 +152,6 @@
//#define BLACKBOX
#define SERIAL_RX
//#define GPS
#define GTUNE
//#define DISPLAY
#define USE_SERVOS
#define USE_CLI

1
src/main/target/CHEBUZZF3/target.h
View file

@ -162,7 +162,6 @@
#endif
#define BLACKBOX
#define GTUNE
#define TELEMETRY
#define SERIAL_RX
#define USE_SERVOS

3
src/main/target/CJMCU/target.h
View file

@ -77,6 +77,3 @@
#define SKIP_CLI_COMMAND_HELP
#endif
//#undef USE_CLI
#define GTUNE
//#define BLACKBOX

1
src/main/target/COLIBRI_RACE/target.h
View file

@ -153,7 +153,6 @@
#define BLACKBOX
#define GPS
#define GTUNE
#define LED_STRIP
#define LED_STRIP_TIMER TIM16

1
src/main/target/EUSTM32F103RC/target.h
View file

@ -123,7 +123,6 @@
#define LED_STRIP_TIMER TIM3
#define BLACKBOX
#define GTUNE
#define TELEMETRY
#define SERIAL_RX
#define USE_SERVOS

1
src/main/target/IRCFUSIONF3/target.h
View file

@ -151,7 +151,6 @@
#define BLACKBOX
#define DISPLAY
#define GPS
#define GTUNE
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS

1
src/main/target/LUX_RACE/target.h
View file

@ -129,7 +129,6 @@
#define BLACKBOX
#define GPS
#define GTUNE
#define LED_STRIP
#define LED_STRIP_TIMER TIM16

1
src/main/target/MOTOLAB/target.h
View file

@ -122,7 +122,6 @@
#define BLACKBOX
#define SERIAL_RX
//#define GPS
#define GTUNE
#define DISPLAY
#define USE_SERVOS
#define USE_FLASHFS

2
src/main/target/NAZE/target.h
View file

@ -175,8 +175,6 @@
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER
//#define GPS
#define GTUNE
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX

1
src/main/target/NAZE32PRO/target.h
View file

@ -42,7 +42,6 @@
#define BLACKBOX
#define GPS
#define GTUNE
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS

1
src/main/target/PORT103R/target.h
View file

@ -151,7 +151,6 @@
#define BLACKBOX
#define GPS
#define GTUNE
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS

1
src/main/target/RMDO/target.h
View file

@ -143,7 +143,6 @@
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define BLACKBOX
#define GTUNE
#define TELEMETRY
#define SERIAL_RX
#define USE_SERVOS

1
src/main/target/SPARKY/target.h
View file

@ -120,7 +120,6 @@
#define BLACKBOX
#define GPS
#define GTUNE
#define DISPLAY
#define SERIAL_RX
#define TELEMETRY

1
src/main/target/SPRACINGF3/target.h
View file

@ -152,7 +152,6 @@
#define BLACKBOX
#define DISPLAY
#define GPS
#define GTUNE
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS

2
src/main/target/SPRACINGF3MINI/target.h
View file

@ -61,7 +61,7 @@
#define USE_MAG_AK8975
#define USE_MAG_HMC5883 // External
#define MAG_AK8975_ALIGN CW270_DEG_FLIP
#define MAG_AK8975_ALIGN CW90_DEG_FLIP
#define SONAR
#define BEEPER

1
src/main/target/STM32F3DISCOVERY/target.h
View file

@ -154,7 +154,6 @@
#define BLACKBOX
#define GPS
#define GTUNE
#define LED_STRIP
#define LED_STRIP_TIMER TIM16
#define TELEMETRY

2
src/main/version.h
View file

@ -17,7 +17,7 @@
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 4 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)