1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00

Drop gyro and acc alignment settings

This commit is contained in:
Pawel Spychalski (DzikuVx) 2022-05-07 21:28:18 +02:00
parent 72df0a15c5
commit 17f90314ae
8 changed files with 8 additions and 39 deletions

View file

@ -1154,8 +1154,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_SENSOR_ALIGNMENT: case MSP_SENSOR_ALIGNMENT:
sbufWriteU8(dst, gyroConfig()->gyro_align); sbufWriteU8(dst, 0); // was gyroConfig()->gyro_align
sbufWriteU8(dst, accelerometerConfig()->acc_align); sbufWriteU8(dst, 0); // was accelerometerConfig()->acc_align
#ifdef USE_MAG #ifdef USE_MAG
sbufWriteU8(dst, compassConfig()->mag_align); sbufWriteU8(dst, compassConfig()->mag_align);
#else #else
@ -2061,8 +2061,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_SENSOR_ALIGNMENT: case MSP_SET_SENSOR_ALIGNMENT:
if (dataSize == 4) { if (dataSize == 4) {
gyroConfigMutable()->gyro_align = sbufReadU8(src); sbufReadU8(src); // was gyroConfigMutable()->gyro_align
accelerometerConfigMutable()->acc_align = sbufReadU8(src); sbufReadU8(src); // was accelerometerConfigMutable()->acc_align
#ifdef USE_MAG #ifdef USE_MAG
compassConfigMutable()->mag_align = sbufReadU8(src); compassConfigMutable()->mag_align = sbufReadU8(src);
#else #else

View file

@ -216,12 +216,6 @@ groups:
description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible." description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible."
default_value: 1000 default_value: 1000
max: 9000 max: 9000
- name: align_gyro
description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP."
default_value: "DEFAULT"
field: gyro_align
type: uint8_t
table: alignment
- name: gyro_hardware_lpf - name: gyro_hardware_lpf
description: "Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first." description: "Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first."
default_value: "256HZ" default_value: "256HZ"
@ -386,12 +380,6 @@ groups:
min: 1 min: 1
max: 255 max: 255
default_value: 1 default_value: 1
- name: align_acc
description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP."
default_value: "DEFAULT"
field: acc_align
type: uint8_t
table: alignment
- name: acc_hardware - name: acc_hardware
description: "Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info" description: "Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info"
default_value: "AUTO" default_value: "AUTO"

View file

@ -84,12 +84,11 @@ static EXTENDED_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT]; static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 4); PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 5);
void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
{ {
RESET_CONFIG_2(accelerometerConfig_t, instance, RESET_CONFIG_2(accelerometerConfig_t, instance,
.acc_align = SETTING_ALIGN_ACC_DEFAULT,
.acc_hardware = SETTING_ACC_HARDWARE_DEFAULT, .acc_hardware = SETTING_ACC_HARDWARE_DEFAULT,
.acc_lpf_hz = SETTING_ACC_LPF_HZ_DEFAULT, .acc_lpf_hz = SETTING_ACC_LPF_HZ_DEFAULT,
.acc_notch_hz = SETTING_ACC_NOTCH_HZ_DEFAULT, .acc_notch_hz = SETTING_ACC_NOTCH_HZ_DEFAULT,
@ -291,11 +290,6 @@ bool accInit(uint32_t targetLooptime)
acc.extremes[axis].max = -100; acc.extremes[axis].max = -100;
} }
// At this poinrt acc.dev.accAlign was set up by the driver from the busDev record
// If configuration says different - override
if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
acc.dev.accAlign = accelerometerConfig()->acc_align;
}
return true; return true;
} }

View file

@ -67,7 +67,6 @@ typedef struct acc_s {
extern acc_t acc; extern acc_t acc;
typedef struct accelerometerConfig_s { typedef struct accelerometerConfig_s {
sensor_align_e acc_align; // acc alignment
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
flightDynamicsTrims_t accZero; // Accelerometer offset flightDynamicsTrims_t accZero; // Accelerometer offset

View file

@ -97,13 +97,12 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3); PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
.gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT, .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT,
.gyro_anti_aliasing_lpf_type = SETTING_GYRO_ANTI_ALIASING_LPF_TYPE_DEFAULT, .gyro_anti_aliasing_lpf_type = SETTING_GYRO_ANTI_ALIASING_LPF_TYPE_DEFAULT,
.gyro_align = SETTING_ALIGN_GYRO_DEFAULT,
.gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT, .gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT,
.looptime = SETTING_LOOPTIME_DEFAULT, .looptime = SETTING_LOOPTIME_DEFAULT,
#ifdef USE_DUAL_GYRO #ifdef USE_DUAL_GYRO
@ -304,13 +303,7 @@ bool gyroInit(void)
// initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs; gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs;
// At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record
// If configuration says different - override
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
gyroDev[0].gyroAlign = gyroConfig()->gyro_align;
}
gyroInitFilters(); gyroInitFilters();
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS

View file

@ -53,7 +53,6 @@ typedef struct gyro_s {
extern gyro_t gyro; extern gyro_t gyro;
typedef struct gyroConfig_s { typedef struct gyroConfig_s {
sensor_align_e gyro_align; // gyro alignment
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint16_t looptime; // imu loop time in us uint16_t looptime; // imu loop time in us
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.

View file

@ -1 +1 @@
target_stm32f405xg(BLUEJAYF4) target_stm32f405xg(BLUEJAYF4 SKIP_RELEASES)

View file

@ -27,8 +27,4 @@
// alternative defaults settings for BlueJayF4 targets // alternative defaults settings for BlueJayF4 targets
void targetConfiguration(void) void targetConfiguration(void)
{ {
if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) {
gyroConfigMutable()->gyro_align = CW180_DEG;
accelerometerConfigMutable()->acc_align = CW180_DEG;
}
} }