mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-20 23:05:19 +03:00
Move calibrationCompleted flag to a 4th element in the accZero structure
Allows the flag indicating that calibration was completed to be output in the values for `acc_calibration`.
This commit is contained in:
parent
565f1f4db5
commit
365c6cb1f6
6 changed files with 10 additions and 8 deletions
|
@ -663,7 +663,9 @@ const clivalue_t valueTable[] = {
|
||||||
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
|
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
|
||||||
{ "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) },
|
{ "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_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) },
|
|
||||||
|
// 4 elements are output for the ACC calibration - The 3 axis values and the 4th representing whether calibration has been performed
|
||||||
|
{ "acc_calibration", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accZero.raw) },
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// PG_COMPASS_CONFIG
|
// PG_COMPASS_CONFIG
|
||||||
|
|
|
@ -220,7 +220,7 @@ static bool accNeedsCalibration(void)
|
||||||
if (sensors(SENSOR_ACC)) {
|
if (sensors(SENSOR_ACC)) {
|
||||||
|
|
||||||
// Check to see if the ACC has already been calibrated
|
// Check to see if the ACC has already been calibrated
|
||||||
if (accelerometerConfig()->calibrationCompleted ||
|
if (accelerometerConfig()->accZero.values.calibrationCompleted ||
|
||||||
accelerometerConfig()->accZero.values.roll != 0 ||
|
accelerometerConfig()->accZero.values.roll != 0 ||
|
||||||
accelerometerConfig()->accZero.values.pitch != 0 ||
|
accelerometerConfig()->accZero.values.pitch != 0 ||
|
||||||
accelerometerConfig()->accZero.values.yaw != 0) {
|
accelerometerConfig()->accZero.values.yaw != 0) {
|
||||||
|
|
|
@ -98,7 +98,7 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||||
|
|
||||||
static void setConfigCalibrationCompleted(void)
|
static void setConfigCalibrationCompleted(void)
|
||||||
{
|
{
|
||||||
accelerometerConfigMutable()->calibrationCompleted = true;
|
accelerometerConfigMutable()->accZero.values.calibrationCompleted = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void accResetRollAndPitchTrims(void)
|
void accResetRollAndPitchTrims(void)
|
||||||
|
@ -111,6 +111,7 @@ static void resetFlightDynamicsTrims(flightDynamicsTrims_t *accZero)
|
||||||
accZero->values.roll = 0;
|
accZero->values.roll = 0;
|
||||||
accZero->values.pitch = 0;
|
accZero->values.pitch = 0;
|
||||||
accZero->values.yaw = 0;
|
accZero->values.yaw = 0;
|
||||||
|
accZero->values.calibrationCompleted = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void accResetFlightDynamicsTrims(void)
|
void accResetFlightDynamicsTrims(void)
|
||||||
|
@ -124,13 +125,12 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
|
||||||
.acc_lpf_hz = 10,
|
.acc_lpf_hz = 10,
|
||||||
.acc_hardware = ACC_DEFAULT,
|
.acc_hardware = ACC_DEFAULT,
|
||||||
.acc_high_fsr = false,
|
.acc_high_fsr = false,
|
||||||
.calibrationCompleted = false,
|
|
||||||
);
|
);
|
||||||
resetRollAndPitchTrims(&instance->accelerometerTrims);
|
resetRollAndPitchTrims(&instance->accelerometerTrims);
|
||||||
resetFlightDynamicsTrims(&instance->accZero);
|
resetFlightDynamicsTrims(&instance->accZero);
|
||||||
}
|
}
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 1);
|
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 2);
|
||||||
|
|
||||||
extern uint16_t InflightcalibratingA;
|
extern uint16_t InflightcalibratingA;
|
||||||
extern bool AccInflightCalibrationMeasurementDone;
|
extern bool AccInflightCalibrationMeasurementDone;
|
||||||
|
|
|
@ -72,7 +72,6 @@ typedef struct accelerometerConfig_s {
|
||||||
bool acc_high_fsr;
|
bool acc_high_fsr;
|
||||||
flightDynamicsTrims_t accZero;
|
flightDynamicsTrims_t accZero;
|
||||||
rollAndPitchTrims_t accelerometerTrims;
|
rollAndPitchTrims_t accelerometerTrims;
|
||||||
bool calibrationCompleted; // has ACC calibration been performed (should not be exposed in settings)
|
|
||||||
} accelerometerConfig_t;
|
} accelerometerConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
|
PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
|
||||||
|
|
|
@ -58,7 +58,7 @@
|
||||||
magDev_t magDev;
|
magDev_t magDev;
|
||||||
mag_t mag; // mag access functions
|
mag_t mag; // mag access functions
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 1);
|
PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 2);
|
||||||
|
|
||||||
void pgResetFn_compassConfig(compassConfig_t *compassConfig)
|
void pgResetFn_compassConfig(compassConfig_t *compassConfig)
|
||||||
{
|
{
|
||||||
|
|
|
@ -36,10 +36,11 @@ typedef struct int16_flightDynamicsTrims_s {
|
||||||
int16_t roll;
|
int16_t roll;
|
||||||
int16_t pitch;
|
int16_t pitch;
|
||||||
int16_t yaw;
|
int16_t yaw;
|
||||||
|
int16_t calibrationCompleted;
|
||||||
} flightDynamicsTrims_def_t;
|
} flightDynamicsTrims_def_t;
|
||||||
|
|
||||||
typedef union flightDynamicsTrims_u {
|
typedef union flightDynamicsTrims_u {
|
||||||
int16_t raw[3];
|
int16_t raw[4];
|
||||||
flightDynamicsTrims_def_t values;
|
flightDynamicsTrims_def_t values;
|
||||||
} flightDynamicsTrims_t;
|
} flightDynamicsTrims_t;
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue