rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #7541 from mikeller/excise_use_acc 

Applied 'USE_ACC' consistently.
This commit is contained in:
Michael Keller 2019-02-08 19:48:00 +13:00 committed by GitHub
parent 0149101a56 35bc646692
commit 26c1177cdb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
30 changed files with 257 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/blackbox/blackbox.c
View File

@ -1016,7 +1016,9 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->axisPID_D[i] = pidData[i].D;
blackboxCurrent->axisPID_F[i] = pidData[i].F;
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
#if defined(USE_ACC)
blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]);
#endif
#ifdef USE_MAG
blackboxCurrent->magADC[i] = mag.magADC[i];
#endif
@ -1237,7 +1239,9 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt);
#if defined(USE_ACC)
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
#endif
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
@ -1331,8 +1335,10 @@ static bool blackboxWriteSysinfo(void)
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
#if defined(USE_ACC)
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
#endif
BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm);

2
src/main/cli/cli.c
View File

@ -4080,9 +4080,11 @@ static void cliStatus(char *cmdline)
cliPrint(", ");
}
cliPrintf("%s=%s", sensorTypeNames[i], sensorHardware);
#if defined(USE_ACC)
if (mask == SENSOR_ACC && acc.dev.revisionCode) {
cliPrintf(".%c", acc.dev.revisionCode);
}
#endif
}
}
cliPrintLinefeed();

2
src/main/cli/settings.c
View File

@ -630,6 +630,7 @@ const clivalue_t valueTable[] = {
{ "gyro_filter_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_FILTER_DEBUG }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_debug_axis) },
// PG_ACCELEROMETER_CONFIG
#if defined(USE_ACC)
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) },
#if defined(USE_GYRO_SPI_ICM20649)
{ "acc_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_high_fsr) },
@ -638,6 +639,7 @@ const clivalue_t valueTable[] = {
{ "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) },
{ "acc_trim_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) },
{ "acc_calibration", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = XYZ_AXIS_COUNT, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accZero.raw) },
#endif
// PG_COMPASS_CONFIG
#ifdef USE_MAG

10
src/main/fc/core.c
View File

@ -166,14 +166,6 @@ PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
.throttle_correction_angle = 800 // could be 80.0 deg with atlhold or 45.0 for fpv
);
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveConfigAndNotify();
}
static bool isCalibrating(void)
{
#ifdef USE_BARO
@ -977,7 +969,7 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
pidController(currentPidProfile, currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
#ifdef USE_RUNAWAY_TAKEOFF

1
src/main/fc/core.h
View File

@ -55,7 +55,6 @@ extern const char * const osdLaunchControlModeNames[LAUNCH_CONTROL_MODE_COUNT];
PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
union rollAndPitchTrims_u;
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
void handleInflightCalibrationStickPosition(void);
void resetArmingDisabled(void);

52
src/main/fc/rc_controls.c
View File

@ -30,42 +30,45 @@
#include "build/build_config.h"
#include "cms/cms.h"
#include "common/axis.h"
#include "common/maths.h"
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"
#include "cms/cms.h"
#include "drivers/camera_control.h"
#include "fc/config.h"
#include "fc/core.h"
#include "fc/rc_controls.h"
#include "fc/rc.h"
#include "fc/runtime_config.h"
#include "io/gps.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "io/vtx_control.h"
#include "io/dashboard.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
#include "flight/pid.h"
#include "flight/failsafe.h"
#include "io/beeper.h"
#include "io/dashboard.h"
#include "io/gps.h"
#include "io/motors.h"
#include "io/vtx_control.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "rc_controls.h"
// true if arming is done via the sticks (as opposed to a switch)
static bool isUsingSticksToArm = true;
@ -318,8 +321,13 @@ void processRcStickPositions()
break;
}
if (shouldApplyRollAndPitchTrimDelta) {
applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
#if defined(USE_ACC)
applyAccelerometerTrimsDelta(&accelerometerTrimsDelta);
#endif
saveConfigAndNotify();
repeatAfter(STICK_AUTOREPEAT_MS);
return;
}
} else {

5
src/main/fc/tasks.c
View File

@ -250,12 +250,13 @@ void fcTasksInit(void)
setTaskEnabled(TASK_GYROPID, true);
}
#if defined(USE_ACC)
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
setTaskEnabled(TASK_ATTITUDE, true);
}
#endif
#ifdef USE_RANGEFINDER
if (sensors(SENSOR_RANGEFINDER)) {
@ -393,8 +394,8 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_GYROPID] = DEFINE_TASK("PID", "GYRO", NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
#ifdef USE_ACC
[TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
#endif
[TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
#endif
[TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling
[TASK_DISPATCH] = DEFINE_TASK("DISPATCH", NULL, NULL, dispatchProcess, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH),

4
src/main/flight/gps_rescue.h
View File

@ -14,7 +14,9 @@
* You should have received a copy of the GNU General Public License
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "common/axis.h"
#include "pg/pg.h"

14
src/main/flight/imu.c
View File

@ -90,7 +90,6 @@ float accAverage[XYZ_AXIS_COUNT];
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
float accVelScale;
bool canUseGPSHeading = true;
static float throttleAngleScale;
@ -169,8 +168,6 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
void imuInit(void)
{
accVelScale = 9.80665f * acc.dev.acc_1G_rec / 10000.0f;
#ifdef USE_GPS
canUseGPSHeading = true;
#else
@ -195,6 +192,7 @@ void imuResetAccelerationSum(void)
accTimeSum = 0;
}
#if defined(USE_ACC)
static float invSqrt(float x)
{
return 1.0f / sqrtf(x);
@ -351,7 +349,6 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
}
}
#ifdef USE_ACC
static bool imuIsAccelerometerHealthy(float *accAverage)
{
float accMagnitudeSq = 0;
@ -365,7 +362,6 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
// Accept accel readings only in range 0.9g - 1.1g
return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
}
#endif
// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
// When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence.
@ -373,7 +369,7 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
// - wait for a 250ms period of low gyro activity to ensure the craft is not moving
// - use a large dcmKpGain value for 500ms to allow the attitude estimate to quickly converge
// - reset the gain back to the standard setting
float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
{
static bool lastArmState = false;
static timeUs_t gyroQuietPeriodTimeEnd = 0;
@ -483,6 +479,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
UNUSED(canUseGPSHeading);
UNUSED(courseOverGround);
UNUSED(deltaT);
UNUSED(imuCalcKpGain);
#else
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
@ -492,11 +489,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
float gyroAverage[XYZ_AXIS_COUNT];
gyroGetAccumulationAverage(gyroAverage);
#ifdef USE_ACC
if (accGetAccumulationAverage(accAverage)) {
useAcc = imuIsAccelerometerHealthy(accAverage);
}
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
@ -508,7 +503,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
#endif
}
int calculateThrottleAngleCorrection(void)
static int calculateThrottleAngleCorrection(void)
{
/*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
@ -551,6 +546,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
acc.accADC[Z] = 0;
}
}
#endif // USE_ACC
bool shouldInitializeGPSHeading()
{

1
src/main/flight/imu.h
View File

@ -28,7 +28,6 @@
// Exported symbols
extern uint32_t accTimeSum;
extern int accSumCount;
extern float accVelScale;
extern int32_t accSum[XYZ_AXIS_COUNT];
extern bool canUseGPSHeading;
extern float accAverage[XYZ_AXIS_COUNT];

144
src/main/flight/pid.c
View File

@ -725,6 +725,7 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
}
}
#if defined(USE_ACC)
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
{
@ -802,19 +803,6 @@ STATIC_UNIT_TESTED float pidLevel(int axis, const pidProfile_t *pidProfile, cons
return currentPidSetpoint;
}
static float accelerationLimit(int axis, float currentPidSetpoint)
{
static float previousSetpoint[XYZ_AXIS_COUNT];
const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
if (fabsf(currentVelocity) > maxVelocity[axis]) {
currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
}
previousSetpoint[axis] = currentPidSetpoint;
return currentPidSetpoint;
}
static timeUs_t crashDetectedAtUs;
static void handleCrashRecovery(
@ -885,49 +873,7 @@ static void detectAndSetCrashRecovery(
}
}
}
static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT])
{
// rotate v around rotation vector rotation
// rotation in radians, all elements must be small
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
int i_1 = (i + 1) % 3;
int i_2 = (i + 2) % 3;
float newV = v[i_1] + v[i_2] * rotation[i];
v[i_2] -= v[i_1] * rotation[i];
v[i_1] = newV;
}
}
STATIC_UNIT_TESTED void rotateItermAndAxisError()
{
if (itermRotation
#if defined(USE_ABSOLUTE_CONTROL)
|| acGain > 0
#endif
) {
const float gyroToAngle = dT * RAD;
float rotationRads[XYZ_AXIS_COUNT];
for (int i = FD_ROLL; i <= FD_YAW; i++) {
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0) {
rotateVector(axisError, rotationRads);
}
#endif
if (itermRotation) {
float v[XYZ_AXIS_COUNT];
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
v[i] = pidData[i].I;
}
rotateVector(v, rotationRads );
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pidData[i].I = v[i];
}
}
}
}
#endif // USE_ACC
#ifdef USE_ACRO_TRAINER
@ -1003,6 +949,62 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
}
#endif // USE_ACRO_TRAINER
static float accelerationLimit(int axis, float currentPidSetpoint)
{
static float previousSetpoint[XYZ_AXIS_COUNT];
const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
if (fabsf(currentVelocity) > maxVelocity[axis]) {
currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
}
previousSetpoint[axis] = currentPidSetpoint;
return currentPidSetpoint;
}
static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT])
{
// rotate v around rotation vector rotation
// rotation in radians, all elements must be small
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
int i_1 = (i + 1) % 3;
int i_2 = (i + 2) % 3;
float newV = v[i_1] + v[i_2] * rotation[i];
v[i_2] -= v[i_1] * rotation[i];
v[i_1] = newV;
}
}
STATIC_UNIT_TESTED void rotateItermAndAxisError()
{
if (itermRotation
#if defined(USE_ABSOLUTE_CONTROL)
|| acGain > 0
#endif
) {
const float gyroToAngle = dT * RAD;
float rotationRads[XYZ_AXIS_COUNT];
for (int i = FD_ROLL; i <= FD_YAW; i++) {
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0) {
rotateVector(axisError, rotationRads);
}
#endif
if (itermRotation) {
float v[XYZ_AXIS_COUNT];
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
v[i] = pidData[i].I;
}
rotateVector(v, rotationRads );
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pidData[i].I = v[i];
}
}
}
}
#ifdef USE_RC_SMOOTHING_FILTER
float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta)
{
@ -1133,6 +1135,7 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
ret = LAUNCH_CONTROL_MAX_RATE * stickDeflection * 2;
}
#if defined(USE_ACC)
// If ACC is enabled and a limit angle is set, then try to limit forward tilt
// to that angle and slow down the rate as the limit is approached to reduce overshoot
if ((axis == FD_PITCH) && (launchControlAngleLimit > 0) && (ret > 0)) {
@ -1147,20 +1150,34 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
}
}
}
#else
UNUSED(angleTrim);
#endif
return ret;
}
#endif
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
{
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
#if defined(USE_ACC)
static timeUs_t levelModeStartTimeUs = 0;
static bool gpsRescuePreviousState = false;
#endif
const float tpaFactor = getThrottlePIDAttenuation();
#if defined(USE_ACC)
const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims;
#else
UNUSED(pidProfile);
UNUSED(currentTimeUs);
#endif
#ifdef USE_TPA_MODE
const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? tpaFactor : 1.0f;
#else
@ -1173,6 +1190,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
const bool launchControlActive = isLaunchControlActive();
#if defined(USE_ACC)
const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
const bool levelModeActive = FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive;
@ -1187,6 +1205,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
levelModeStartTimeUs = 0;
}
gpsRescuePreviousState = gpsRescueIsActive;
#endif
// Dynamic i component,
if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
@ -1227,9 +1246,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID
#if defined(USE_ACC)
if (levelModeActive && (axis != FD_YAW)) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
#endif
#ifdef USE_ACRO_TRAINER
if ((axis != FD_YAW) && acroTrainerActive && !inCrashRecoveryMode && !launchControlActive) {
@ -1239,7 +1260,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
#ifdef USE_LAUNCH_CONTROL
if (launchControlActive) {
#if defined(USE_ACC)
currentPidSetpoint = applyLaunchControl(axis, angleTrim);
#else
currentPidSetpoint = applyLaunchControl(axis, NULL);
#endif
}
#endif
@ -1254,9 +1279,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
float errorRate = currentPidSetpoint - gyroRate; // r - y
#if defined(USE_ACC)
handleCrashRecovery(
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate);
#endif
const float iterm = pidData[axis].I;
float itermErrorRate = errorRate;
@ -1298,9 +1325,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
const float delta =
- (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidFrequency;
#if defined(USE_ACC)
if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
}
#endif
#if defined(USE_D_CUT)
if (dtermCutPercent){
dtermCutFactor = dtermCutRangeApplyFn((filter_t *) &dtermCutRange[axis], delta);

2
src/main/flight/pid.h
View File

@ -180,7 +180,7 @@ typedef struct pidConfig_s {
PG_DECLARE(pidConfig_t, pidConfig);
union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, timeUs_t currentTimeUs);
void pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs);
typedef struct pidAxisData_s {
float P;

2
src/main/io/dashboard.c
View File

@ -484,12 +484,14 @@ static void showSensorsPage(void)
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, " X Y Z");
#if defined(USE_ACC)
if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC[X]), lrintf(acc.accADC[Y]), lrintf(acc.accADC[Z]));
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
#endif
if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));

14
src/main/io/osd.c
View File

@ -1027,12 +1027,15 @@ static bool osdDrawSingleElement(uint8_t item)
#ifdef USE_LAUNCH_CONTROL
// Warn when in launch control mode
if (osdWarnGetState(OSD_WARNING_LAUNCH_CONTROL) && isLaunchControlActive()) {
#if defined(USE_ACC)
if (sensors(SENSOR_ACC)) {
char launchControlMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
const int pitchAngle = constrain((attitude.raw[FD_PITCH] - accelerometerConfig()->accelerometerTrims.raw[FD_PITCH]) / 10, -90, 90);
tfp_sprintf(launchControlMsg, "LAUNCH %d", pitchAngle);
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, launchControlMsg);
} else {
} else
#endif
{
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LAUNCH");
}
break;
@ -1375,6 +1378,7 @@ static void osdDrawElements(void)
return;
}
#if defined(USE_ACC)
osdGForce = 0.0f;
if (sensors(SENSOR_ACC)) {
// only calculate the G force if the element is visible or the stat is enabled
@ -1388,7 +1392,7 @@ static void osdDrawElements(void)
osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
osdDrawSingleElement(OSD_G_FORCE);
}
#endif
for (unsigned i = 0; i < sizeof(osdElementDisplayOrder); i++) {
osdDrawSingleElement(osdElementDisplayOrder[i]);
@ -1569,7 +1573,11 @@ void osdUpdateAlarms(void)
}
#ifdef USE_GPS
if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < 5) || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())) {
if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < 5)
#if defined(USE_GPS_RESCUE)
|| ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())
#endif
) {
SET_BLINK(OSD_GPS_SATS);
} else {
CLR_BLINK(OSD_GPS_SATS);

27
src/main/msp/msp.c
View File

@ -857,22 +857,27 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
case MSP_RAW_IMU:
{
#if defined(USE_ACC)
// Hack scale due to choice of units for sensor data in multiwii
uint8_t scale;
if (acc.dev.acc_1G > 512*4) {
if (acc.dev.acc_1G > 512 * 4) {
scale = 8;
} else if (acc.dev.acc_1G > 512*2) {
} else if (acc.dev.acc_1G > 512 * 2) {
scale = 4;
} else if (acc.dev.acc_1G >= 512) {
scale = 2;
} else {
scale = 1;
}
#endif
for (int i = 0; i < 3; i++) {
#if defined(USE_ACC)
sbufWriteU16(dst, lrintf(acc.accADC[i] / scale));
#else
sbufWriteU16(dst, 0);
#endif
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroRateDps(i));
@ -1125,11 +1130,13 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
#endif
#endif
#if defined(USE_ACC)
case MSP_ACC_TRIM:
sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
break;
break;
#endif
case MSP_MIXER_CONFIG:
sbufWriteU8(dst, mixerConfig()->mixerMode);
sbufWriteU8(dst, mixerConfig()->yaw_motors_reversed);
@ -1435,7 +1442,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
break;
case MSP_SENSOR_CONFIG:
#if defined(USE_ACC)
sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, barometerConfig()->baro_hardware);
sbufWriteU8(dst, compassConfig()->mag_hardware);
break;
@ -1674,10 +1685,13 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
}
#endif
break;
#if defined(USE_ACC)
case MSP_SET_ACC_TRIM:
accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src);
break;
#endif
case MSP_SET_ARMING_CONFIG:
armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
sbufReadU8(src); // reserved
@ -2063,9 +2077,14 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_SENSOR_CONFIG:
#if defined(USE_ACC)
accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
barometerConfigMutable()->baro_hardware = sbufReadU8(src);
compassConfigMutable()->mag_hardware = sbufReadU8(src);
break;
case MSP_RESET_CONF:

23
src/main/sensors/acceleration.c
View File

@ -25,6 +25,8 @@
#include "platform.h"
#ifdef USE_ACC
#include "build/debug.h"
#include "common/axis.h"
@ -33,9 +35,6 @@
#include "config/config_reset.h"
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/gyrodev.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_fake.h"
@ -73,7 +72,10 @@
#include "io/beeper.h"
#include "sensors/acceleration.h"
#include "pg/gyrodev.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
@ -82,6 +84,8 @@
#include "hardware_revision.h"
#endif
#include "acceleration.h"
FAST_RAM_ZERO_INIT acc_t acc; // acc access functions
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
@ -122,7 +126,6 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 1);
#ifdef USE_ACC
extern uint16_t InflightcalibratingA;
extern bool AccInflightCalibrationMeasurementDone;
extern bool AccInflightCalibrationSavetoEEProm;
@ -340,11 +343,7 @@ bool accInit(uint32_t gyroSamplingInverval)
break;
case 1000:
default:
#ifdef STM32F10X
acc.accSamplingInterval = 1000;
#else
acc.accSamplingInterval = 1000;
#endif
}
if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -536,4 +535,10 @@ void accInitFilters(void)
}
}
}
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
}
#endif

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

@ -65,7 +65,7 @@ typedef union rollAndPitchTrims_u {
rollAndPitchTrims_t_def values;
} rollAndPitchTrims_t;
#if defined(USE_ACC)
typedef struct accelerometerConfig_s {
uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
@ -75,6 +75,7 @@ typedef struct accelerometerConfig_s {
} accelerometerConfig_t;
PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
#endif
bool accInit(uint32_t gyroTargetLooptime);
bool accIsCalibrationComplete(void);
@ -85,3 +86,4 @@ bool accGetAccumulationAverage(float *accumulation);
union flightDynamicsTrims_u;
void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse);
void accInitFilters(void);
void applyAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);

2
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -532,10 +532,12 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
batteryConfigMutable()->vbatwarningcellvoltage = bstRead8() * 10; // vbatlevel when buzzer starts to alert
break;
#if defined(USE_ACC)
case BST_ACC_CALIBRATION:
if (!ARMING_FLAG(ARMED))
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
break;
#endif
case BST_MAG_CALIBRATION:
if (!ARMING_FLAG(ARMED))
ENABLE_STATE(CALIBRATE_MAG);

2
src/main/target/OMNIBUSF4/target.h
View File

@ -73,7 +73,7 @@
#define USE_EXTI
//#define USE_ACC
#define USE_ACC
#define USE_ACC_SPI_MPU6000
#define USE_GYRO

5
src/main/target/common_post.h
View File

@ -269,3 +269,8 @@
#if !defined(USE_BOARD_INFO)
#undef USE_SIGNATURE
#endif
#if !defined(USE_ACC)
#undef USE_GPS_RESCUE
#undef USE_ACRO_TRAINER
#endif

4
src/main/telemetry/frsky_hub.c
View File

@ -174,12 +174,14 @@ static void frSkyHubWriteByteInternal(const char data)
serialWrite(frSkyHubPort, data);
}
#if defined(USE_ACC)
static void sendAccel(void)
{
for (unsigned i = 0; i < 3; i++) {
frSkyHubWriteFrame(ID_ACC_X + i, ((int16_t)(acc.accADC[i] * acc.dev.acc_1G_rec) * 1000));
}
}
#endif
static void sendThrottleOrBatterySizeAsRpm(void)
{
@ -519,10 +521,12 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
cycleNum++;
#if defined(USE_ACC)
if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_ACC_X | SENSOR_ACC_Y | SENSOR_ACC_Z)) {
// Sent every 125ms
sendAccel();
}
#endif
#if defined(USE_BARO) || defined(USE_RANGEFINDER) || defined(USE_GPS)
if (sensors(SENSOR_BARO | SENSOR_RANGEFINDER) | sensors(SENSOR_GPS)) {

6
src/main/telemetry/ibus_shared.c
View File

@ -255,10 +255,12 @@ static uint16_t getMode()
return flightMode;
}
#if defined(USE_ACC)
static int16_t getACC(uint8_t index)
{
return (int16_t)((acc.accADC[index] * acc.dev.acc_1G_rec) * 1000);
}
#endif
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
static void setCombinedFrame(uint8_t* bufferPtr, const uint8_t* structure, uint8_t itemCount)
@ -388,11 +390,13 @@ static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length)
case IBUS_SENSOR_TYPE_BAT_CURR:
value.uint16 = (uint16_t)getAmperage();
break;
#if defined(USE_ACC)
case IBUS_SENSOR_TYPE_ACC_X:
case IBUS_SENSOR_TYPE_ACC_Y:
case IBUS_SENSOR_TYPE_ACC_Z:
value.int16 = getACC(sensorType - IBUS_SENSOR_TYPE_ACC_X);
break;
#endif
case IBUS_SENSOR_TYPE_ROLL:
case IBUS_SENSOR_TYPE_PITCH:
case IBUS_SENSOR_TYPE_YAW:
@ -405,9 +409,9 @@ static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length)
case IBUS_SENSOR_TYPE_CMP_HEAD:
value.uint16 = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
break;
#ifdef USE_VARIO
case IBUS_SENSOR_TYPE_VERTICAL_SPEED:
case IBUS_SENSOR_TYPE_CLIMB_RATE:
#ifdef USE_VARIO
value.int16 = (int16_t) constrain(getEstimatedVario(), SHRT_MIN, SHRT_MAX);
break;
#endif

2
src/main/telemetry/jetiexbus.c
View File

@ -377,6 +377,7 @@ int32_t getSensorValue(uint8_t sensor)
break;
#endif
#if defined(USE_ACC)
case EX_GFORCE_X:
return (int16_t)(((float)acc.accADC[0] / acc.dev.acc_1G) * 1000);
break;
@ -388,6 +389,7 @@ int32_t getSensorValue(uint8_t sensor)
case EX_GFORCE_Z:
return (int16_t)(((float)acc.accADC[2] / acc.dev.acc_1G) * 1000);
break;
#endif
default:
return -1;

13
src/main/telemetry/smartport.c
View File

@ -125,9 +125,11 @@ enum
FSSP_DATAID_CELLS = 0x0300 ,
FSSP_DATAID_CELLS_LAST = 0x030F ,
FSSP_DATAID_HEADING = 0x0840 ,
#if defined(USE_ACC)
FSSP_DATAID_ACCX = 0x0700 ,
FSSP_DATAID_ACCY = 0x0710 ,
FSSP_DATAID_ACCZ = 0x0720 ,
#endif
FSSP_DATAID_T1 = 0x0400 ,
FSSP_DATAID_T11 = 0x0401 ,
FSSP_DATAID_T2 = 0x0410 ,
@ -357,10 +359,12 @@ static void initSmartPortSensors(void)
}
}
if (telemetryIsSensorEnabled(SENSOR_HEADING)) {
ADD_SENSOR(FSSP_DATAID_HEADING);
}
#if defined(USE_ACC)
if (sensors(SENSOR_ACC)) {
if (telemetryIsSensorEnabled(SENSOR_HEADING)) {
ADD_SENSOR(FSSP_DATAID_HEADING);
}
if (telemetryIsSensorEnabled(SENSOR_ACC_X)) {
ADD_SENSOR(FSSP_DATAID_ACCX);
}
@ -371,6 +375,7 @@ static void initSmartPortSensors(void)
ADD_SENSOR(FSSP_DATAID_ACCZ);
}
}
#endif
if (sensors(SENSOR_BARO)) {
if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
@ -698,6 +703,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
*clearToSend = false;
break;
#if defined(USE_ACC)
case FSSP_DATAID_ACCX :
smartPortSendPackage(id, lrintf(100 * acc.accADC[X] * acc.dev.acc_1G_rec)); // Multiply by 100 to show as x.xx g on Taranis
*clearToSend = false;
@ -710,6 +716,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] * acc.dev.acc_1G_rec));
*clearToSend = false;
break;
#endif
case FSSP_DATAID_T1 :
// we send all the flags as decimal digits for easy reading

4
src/test/Makefile
View File

@ -327,9 +327,9 @@ pid_unittest_SRC := \
$(USER_DIR)/common/filter.c \
$(USER_DIR)/common/maths.c \
$(USER_DIR)/drivers/accgyro/gyro_sync.c \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/pid.c \
$(USER_DIR)/pg/pg.c \
$(USER_DIR)/fc/runtime_config.c
$(USER_DIR)/pg/pg.c
pid_unittest_DEFINES := \
USE_ITERM_RELAX= \

3
src/test/unit/arming_prevention_unittest.cc
View File

@ -1039,7 +1039,7 @@ extern "C" {
bool isGyroCalibrationComplete(void) { return gyroCalibDone; }
void gyroStartCalibration(bool) {}
bool isFirstArmingGyroCalibrationRunning(void) { return false; }
void pidController(const pidProfile_t *, const rollAndPitchTrims_t *, timeUs_t) {}
void pidController(const pidProfile_t *, timeUs_t) {}
void pidStabilisationState(pidStabilisationState_e) {}
void mixTable(timeUs_t , uint8_t) {};
void writeMotors(void) {};
@ -1089,4 +1089,5 @@ extern "C" {
bool isAltitudeOffset(void) { return false; }
float getCosTiltAngle(void) { return 0.0f; }
void pidSetItermReset(bool) {}
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
}

53
src/test/unit/pid_unittest.cc
View File

@ -64,6 +64,8 @@ extern "C" {
gyro_t gyro;
attitudeEulerAngles_t attitude;
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
bool unitLaunchControlActive = false;
launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
@ -80,7 +82,6 @@ extern "C" {
}
pidProfile_t *pidProfile;
rollAndPitchTrims_t rollAndPitchTrims = { { 0, 0 } };
int loopIter = 0;
@ -171,7 +172,7 @@ void resetTest(void) {
// Run pidloop for a while after reset
for (int loop = 0; loop < 20; loop++) {
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
}
}
@ -202,7 +203,7 @@ TEST(pidControllerTest, testStabilisationDisabled) {
// Run few loops to make sure there is no error building up when stabilisation disabled
for (int loop = 0; loop < 10; loop++) {
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// PID controller should not do anything, while stabilisation disabled
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
@ -223,7 +224,7 @@ TEST(pidControllerTest, testPidLoop) {
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// Loop 1 - Expecting zero since there is no error
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
@ -238,7 +239,7 @@ TEST(pidControllerTest, testPidLoop) {
// Add some rotation on ROLL to generate error
gyro.gyroADCf[FD_ROLL] = 100;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// Loop 2 - Expect PID loop reaction to ROLL error
ASSERT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
@ -253,7 +254,7 @@ TEST(pidControllerTest, testPidLoop) {
// Add some rotation on PITCH to generate error
gyro.gyroADCf[FD_PITCH] = -100;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// Loop 3 - Expect PID loop reaction to PITCH error, ROLL is still in error
ASSERT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
@ -268,7 +269,7 @@ TEST(pidControllerTest, testPidLoop) {
// Add some rotation on YAW to generate error
gyro.gyroADCf[FD_YAW] = 100;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// Loop 4 - Expect PID loop reaction to PITCH error, ROLL and PITCH are still in error
ASSERT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
@ -283,7 +284,7 @@ TEST(pidControllerTest, testPidLoop) {
// Simulate Iterm behaviour during mixer saturation
simulatedMotorMixRange = 1.2f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
ASSERT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8));
@ -295,7 +296,7 @@ TEST(pidControllerTest, testPidLoop) {
simulatedSetpointRate[FD_YAW] = 100;
for(int loop = 0; loop < 5; loop++) {
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
}
// Iterm is stalled as it is not accumulating anymore
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
@ -310,7 +311,7 @@ TEST(pidControllerTest, testPidLoop) {
// Now disable Stabilisation
pidStabilisationState(PID_STABILISATION_OFF);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// Should all be zero again
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
@ -404,7 +405,7 @@ TEST(pidControllerTest, testMixerSaturation) {
setStickPosition(FD_PITCH, -1.0f);
setStickPosition(FD_YAW, 1.0f);
simulatedMotorMixRange = 2.0f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// Expect no iterm accumulation
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
@ -420,7 +421,7 @@ TEST(pidControllerTest, testMixerSaturation) {
setStickPosition(FD_PITCH, -0.1f);
setStickPosition(FD_YAW, 0.1f);
simulatedMotorMixRange = 0.0f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
float rollTestIterm = pidData[FD_ROLL].I;
float pitchTestIterm = pidData[FD_PITCH].I;
float yawTestIterm = pidData[FD_YAW].I;
@ -433,7 +434,7 @@ TEST(pidControllerTest, testMixerSaturation) {
setStickPosition(FD_PITCH, -0.1f);
setStickPosition(FD_YAW, 0.1f);
simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1) / 100.0f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
ASSERT_LT(pidData[FD_ROLL].I, rollTestIterm);
ASSERT_GE(pidData[FD_PITCH].I, pitchTestIterm);
ASSERT_LT(pidData[FD_YAW].I, yawTestIterm);
@ -458,7 +459,7 @@ TEST(pidControllerTest, testCrashRecoveryMode) {
for (int loop =0; loop <= loopsToCrashTime; loop++) {
gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
// advance the time to avoid initialized state prevention of crash recovery
pidController(pidProfile, &rollAndPitchTrims, currentTestTime() + 2000000);
pidController(pidProfile, currentTestTime() + 2000000);
}
EXPECT_TRUE(crashRecoveryModeActive());
@ -479,7 +480,7 @@ TEST(pidControllerTest, testFeedForward) {
setStickPosition(FD_PITCH, -1.0f);
setStickPosition(FD_YAW, -1.0f);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
ASSERT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78));
ASSERT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03));
@ -490,7 +491,7 @@ TEST(pidControllerTest, testFeedForward) {
setStickPosition(FD_PITCH, -0.5f);
setStickPosition(FD_YAW, -0.5f);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
ASSERT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20));
ASSERT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26));
@ -498,7 +499,7 @@ TEST(pidControllerTest, testFeedForward) {
for (int loop =0; loop <= 15; loop++) {
gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
}
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
@ -667,7 +668,7 @@ TEST(pidControllerTest, testLaunchControl) {
// test that feedforward and D are disabled (always zero) when launch control is active
// set initial state
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
@ -686,7 +687,7 @@ TEST(pidControllerTest, testLaunchControl) {
gyro.gyroADCf[FD_PITCH] = 1000;
gyro.gyroADCf[FD_YAW] = -1000;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// validate that feedforwad is still 0
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
@ -705,12 +706,12 @@ TEST(pidControllerTest, testLaunchControl) {
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
gyro.gyroADCf[FD_ROLL] = -20;
gyro.gyroADCf[FD_PITCH] = 20;
gyro.gyroADCf[FD_YAW] = -20;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
ASSERT_NEAR(25.62, pidData[FD_ROLL].P, calculateTolerance(25.62));
ASSERT_NEAR(1.56, pidData[FD_ROLL].I, calculateTolerance(1.56));
@ -726,20 +727,20 @@ TEST(pidControllerTest, testLaunchControl) {
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
// first test that pitch I is prevented from going negative
gyro.gyroADCf[FD_ROLL] = 0;
gyro.gyroADCf[FD_PITCH] = 20;
gyro.gyroADCf[FD_YAW] = 0;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
gyro.gyroADCf[FD_ROLL] = 20;
gyro.gyroADCf[FD_PITCH] = -20;
gyro.gyroADCf[FD_YAW] = 20;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
@ -755,12 +756,12 @@ TEST(pidControllerTest, testLaunchControl) {
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
gyro.gyroADCf[FD_ROLL] = -20;
gyro.gyroADCf[FD_PITCH] = 20;
gyro.gyroADCf[FD_YAW] = -20;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
pidController(pidProfile, currentTestTime());
ASSERT_NEAR(25.62, pidData[FD_ROLL].P, calculateTolerance(25.62));
ASSERT_NEAR(1.56, pidData[FD_ROLL].I, calculateTolerance(1.56));

2
src/test/unit/rc_controls_unittest.cc
View File

@ -709,7 +709,7 @@ void gyroStartCalibration(bool isFirstArmingCalibration)
{
UNUSED(isFirstArmingCalibration);
}
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
void handleInflightCalibrationStickPosition(void) {}
bool featureIsEnabled(uint32_t) { return false;}
bool sensors(uint32_t) { return false;}

1
src/test/unit/target.h
View File

@ -20,6 +20,7 @@
#define SCHEDULER_DELAY_LIMIT 1
#define TASK_GYROPID_DESIRED_PERIOD 100
#define USE_ACC
#define USE_CMS
#define CMS_MAX_DEVICE 4
#define USE_FAKE_GYRO

3
src/test/unit/vtx_unittest.cc
View File

@ -133,7 +133,7 @@ extern "C" {
bool isGyroCalibrationComplete(void) { return gyroCalibDone; }
void gyroStartCalibration(bool) {}
bool isFirstArmingGyroCalibrationRunning(void) { return false; }
void pidController(const pidProfile_t *, const rollAndPitchTrims_t *, timeUs_t) {}
void pidController(const pidProfile_t *, timeUs_t) {}
void pidStabilisationState(pidStabilisationState_e) {}
void mixTable(timeUs_t , uint8_t) {};
void writeMotors(void) {};
@ -177,4 +177,5 @@ extern "C" {
void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
void osdSuppressStats(bool) {}
void pidSetItermReset(bool) {}
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
}