Merge pull request #7541 from mikeller/excise_use_acc
Applied 'USE_ACC' consistently.
This commit is contained in:
commit
26c1177cdb
|
@ -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);
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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),
|
||||
|
||||
|
|
|
@ -15,6 +15,8 @@
|
|||
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
|
|
@ -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()
|
||||
{
|
||||
|
|
|
@ -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];
|
||||
|
|
|
@ -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,
|
||||
¤tPidSetpoint, &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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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]));
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -73,7 +73,7 @@
|
|||
|
||||
#define USE_EXTI
|
||||
|
||||
//#define USE_ACC
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
|
||||
#define USE_GYRO
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)) {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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= \
|
||||
|
|
|
@ -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*) {}
|
||||
}
|
||||
|
|
|
@ -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));
|
||||
|
|
|
@ -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;}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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*) {}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue