diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 95eed3223..fe3739538 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -68,7 +68,7 @@ // safe band to allow a bit of calibration error or trim offset (in microseconds) #define CONNECTION_OFFSET 250 -#define ASSISTEDCONTROL_DEADBAND_MINIMUM 0.02f // minimum value for a well bahaved Tx. +#define ASSISTEDCONTROL_DEADBAND_MINIMUM 2 // minimum value for a well bahaved Tx, in percent. // Private types @@ -87,7 +87,7 @@ static void receiverTask(void *parameters); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); static bool validInputRange(int16_t min, int16_t max, uint16_t value); -static void applyDeadband(float *value, float deadband); +static void applyDeadband(float *value, uint8_t deadband); static void SettingsUpdatedCb(UAVObjEvent *ev); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES @@ -95,7 +95,7 @@ static uint8_t isAssistedFlightMode(uint8_t position); #endif #ifdef USE_INPUT_LPF -static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, float deadband, float dT); +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, uint8_t deadband, float dT); #endif #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 @@ -450,7 +450,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1; } - float deadband_checked = settings.Deadband; + uint8_t deadband_checked = settings.Deadband; #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES // AssistedControl must have deadband set for pitch/roll hence // we default to a higher value for badly behaved TXs and also enforce a minimum value @@ -470,7 +470,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) #endif // PIOS_EXCLUDE_ADVANCED_FEATURES // Apply deadband for Roll/Pitch/Yaw stick inputs - if (deadband_checked > 0.0f) { + if (deadband_checked > 0) { applyDeadband(&cmd.Roll, deadband_checked); applyDeadband(&cmd.Pitch, deadband_checked); applyDeadband(&cmd.Yaw, deadband_checked); @@ -494,7 +494,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) { cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; - if (settings.Deadband > 0.0f) { + if (settings.Deadband > 0) { applyDeadband(&cmd.Collective, settings.Deadband); } #ifdef USE_INPUT_LPF @@ -790,14 +790,16 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value) /** * @brief Apply deadband to Roll/Pitch/Yaw channels */ -static void applyDeadband(float *value, float deadband) +static void applyDeadband(float *value, uint8_t deadband) { - if (fabsf(*value) < deadband) { + float floatDeadband = ((float)deadband) * 0.01f; + + if (fabsf(*value) < floatDeadband) { *value = 0.0f; } else if (*value > 0.0f) { - *value = (*value - deadband) / (1.0f - deadband); + *value = (*value - floatDeadband) / (1.0f - floatDeadband); } else { - *value = (*value + deadband) / (1.0f - deadband); + *value = (*value + floatDeadband) / (1.0f - floatDeadband); } } @@ -805,16 +807,17 @@ static void applyDeadband(float *value, float deadband) /** * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel */ -static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, float deadband, float dT) +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, uint8_t deadband, float dT) { float rt = (float)ManualControlSettingsResponseTimeToArray((*responseTime))[channel]; if (rt > 0.0f) { inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); + float floatDeadband = ((float)deadband) * 0.01f; // avoid a long tail of non-zero data. if we have deadband, once the filtered result reduces to 1/10th // of deadband revert to 0. We downstream rely on this to know when sticks are centered. - if (deadband > 0.0f && fabsf(inputFiltered[channel]) < deadband / 10.0f) { + if (floatDeadband > 0.0f && fabsf(inputFiltered[channel]) < floatDeadband * 0.1f) { inputFiltered[channel] = 0.0f; } *value = inputFiltered[channel]; diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 8fc084da2..b1ff2fc13 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -45,6 +45,8 @@ typedef struct { StabilizationSettingsData settings; StabilizationBankData stabBank; float gyro_alpha; + float floatThrustPIDScaleCurve[STABILIZATIONBANK_THRUSTPIDSCALECURVE_NUMELEM]; + float acroInsanityFactors[STABILIZATIONBANK_ACROINSANITYFACTOR_NUMELEM]; struct { float min_thrust; float max_thrust; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 0e3a93282..3786b62f4 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -152,11 +152,11 @@ static pid_scaler create_pid_scaler(int axis) const pid_curve_scaler curve_scaler = { .x = get_pid_scale_source_value(), .points = { - { 0.00f, stabSettings.stabBank.ThrustPIDScaleCurve[0] }, - { 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] }, - { 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] }, - { 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] }, - { 1.00f, stabSettings.stabBank.ThrustPIDScaleCurve[4] } + { 0.00f, stabSettings.floatThrustPIDScaleCurve[0] }, + { 0.25f, stabSettings.floatThrustPIDScaleCurve[1] }, + { 0.50f, stabSettings.floatThrustPIDScaleCurve[2] }, + { 0.75f, stabSettings.floatThrustPIDScaleCurve[3] }, + { 1.00f, stabSettings.floatThrustPIDScaleCurve[4] } } }; @@ -302,15 +302,11 @@ static void stabilizationInnerloopTask() -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t], StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] ); - const float acroFactors[] = { - stabSettings.stabBank.AcroInsanityFactor.Roll, - stabSettings.stabBank.AcroInsanityFactor.Pitch, - stabSettings.stabBank.AcroInsanityFactor.Yaw - }; + pid_scaler ascaler = create_pid_scaler(t); ascaler.i *= boundf(1.0f - (1.5f * fabsf(stickinput[t])), 0.0f, 1.0f); // this prevents Integral from getting too high while controlled manually float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT); - float factor = fabsf(stickinput[t]) * acroFactors[t]; + float factor = fabsf(stickinput[t]) * stabSettings.acroInsanityFactors[t]; actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate; } break; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 0247e89e6..ba841b196 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -381,6 +381,13 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f; stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f; stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor)); + + for (int i = 0; i < STABILIZATIONSETTINGSBANK1_THRUSTPIDSCALECURVE_NUMELEM; i++) { + stabSettings.floatThrustPIDScaleCurve[i] = (float)(stabSettings.stabBank.ThrustPIDScaleCurve[i]) * 0.01f; + } + stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f; + stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f; + stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f; } /** diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index 89f83a184..198194bda 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -88,6 +88,7 @@ // Private functions static void updatePIDs(UAVObjEvent *ev); static uint8_t update(float *var, float val); +static uint8_t updateUint16(uint16_t *var, float val); static uint8_t updateUint8(uint8_t *var, float val); static uint8_t updateInt8(int8_t *var, float val); static float scale(float val, float inMin, float inMax, float outMin, float outMax); @@ -273,7 +274,7 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.RollRatePID.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLRATERESP: - needsUpdateBank |= update(&bank.ManualRate.Roll, value); + needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: needsUpdateBank |= update(&bank.RollPI.Kp, value); @@ -300,7 +301,7 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value); break; case TXPIDSETTINGS_PIDS_PITCHRATERESP: - needsUpdateBank |= update(&bank.ManualRate.Pitch, value); + needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: needsUpdateBank |= update(&bank.PitchPI.Kp, value); @@ -331,8 +332,8 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP: - needsUpdateBank |= update(&bank.ManualRate.Roll, value); - needsUpdateBank |= update(&bank.ManualRate.Pitch, value); + needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value); + needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: needsUpdateBank |= update(&bank.RollPI.Kp, value); @@ -363,7 +364,7 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.YawRatePID.ILimit, value); break; case TXPIDSETTINGS_PIDS_YAWRATERESP: - needsUpdateBank |= update(&bank.ManualRate.Yaw, value); + needsUpdateBank |= updateUint16(&bank.ManualRate.Yaw, value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: needsUpdateBank |= update(&bank.YawPI.Kp, value); @@ -394,14 +395,14 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateStab |= update(&stab.GyroTau, value); break; case TXPIDSETTINGS_PIDS_ACROROLLFACTOR: - needsUpdateBank |= update(&bank.AcroInsanityFactor.Roll, value); + needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value); break; case TXPIDSETTINGS_PIDS_ACROPITCHFACTOR: - needsUpdateBank |= update(&bank.AcroInsanityFactor.Pitch, value); + needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value); break; case TXPIDSETTINGS_PIDS_ACROROLLPITCHFACTOR: - needsUpdateBank |= update(&bank.AcroInsanityFactor.Roll, value); - needsUpdateBank |= update(&bank.AcroInsanityFactor.Pitch, value); + needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value); + needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value); break; case TXPIDSETTINGS_PIDS_ACCELTAU: needsUpdateAtt |= update(&att.AccelTau, value); @@ -534,6 +535,21 @@ static uint8_t update(float *var, float val) return 0; } +/** + * Updates var using val if needed. + * \returns 1 if updated, 0 otherwise + */ +static uint8_t updateUint16(uint16_t *var, float val) +{ + uint16_t roundedVal = (uint16_t)roundf(val); + + if (*var != roundedVal) { + *var = roundedVal; + return 1; + } + return 0; +} + /** * Updates var using val if needed. * \returns 1 if updated, 0 otherwise diff --git a/ground/gcs/src/plugins/config/configinputwidget.cpp b/ground/gcs/src/plugins/config/configinputwidget.cpp index 837225e4b..3e8cd43ed 100644 --- a/ground/gcs/src/plugins/config/configinputwidget.cpp +++ b/ground/gcs/src/plugins/config/configinputwidget.cpp @@ -155,8 +155,8 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ++index; } - addWidgetBinding("ManualControlSettings", "Deadband", ui->deadband, 0, 0.01f); - addWidgetBinding("ManualControlSettings", "DeadbandAssistedControl", ui->assistedControlDeadband, 0, 0.01f); + addWidgetBinding("ManualControlSettings", "Deadband", ui->deadband, 0, 1); + addWidgetBinding("ManualControlSettings", "DeadbandAssistedControl", ui->assistedControlDeadband, 0, 1); connect(ui->configurationWizard, SIGNAL(clicked()), this, SLOT(goToWizard())); connect(ui->stackedWidget, SIGNAL(currentChanged(int)), this, SLOT(disableWizardButton(int))); diff --git a/ground/gcs/src/plugins/config/configstabilizationwidget.cpp b/ground/gcs/src/plugins/config/configstabilizationwidget.cpp index 6f9cab145..02ee9fd9f 100644 --- a/ground/gcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/gcs/src/plugins/config/configstabilizationwidget.cpp @@ -265,7 +265,7 @@ void ConfigStabilizationWidget::updateThrottleCurveFromObject() QList curve; for (quint32 i = 0; i < field->getNumElements(); i++) { - curve.append(field->getValue(i).toDouble()); + curve.append(field->getValue(i).toDouble() / 100); } ui->thrustPIDScalingCurve->setCurve(&curve); @@ -290,7 +290,7 @@ void ConfigStabilizationWidget::updateObjectFromThrottleCurve() QList curve = ui->thrustPIDScalingCurve->getCurve(); for (quint32 i = 0; i < field->getNumElements(); i++) { - field->setValue(curve.at(i), i); + field->setValue(curve.at(i) * 100, i); } field = stabBank->getField("EnableThrustPIDScaling"); @@ -361,7 +361,7 @@ void ConfigStabilizationWidget::resetThrottleCurveToDefault() QList curve; for (quint32 i = 0; i < field->getNumElements(); i++) { - curve.append(field->getValue(i).toDouble()); + curve.append(field->getValue(i).toDouble() / 100); } ui->thrustPIDScalingCurve->setCurve(&curve); diff --git a/ground/gcs/src/plugins/config/input.ui b/ground/gcs/src/plugins/config/input.ui index fd770ccfe..3d55eea17 100644 --- a/ground/gcs/src/plugins/config/input.ui +++ b/ground/gcs/src/plugins/config/input.ui @@ -224,13 +224,13 @@ Stick deadband in percents of full range (0-10), zero to disable - 1 + 0 10.000000000000000 - 0.100000000000000 + 1.000000000000000 @@ -247,7 +247,7 @@ Assisted Control stick deadband in percents of full range (2-12) for use with GPSAssist. This can not be disabled. - 1 + 0 2.000000000000000 diff --git a/ground/gcs/src/plugins/config/stabilization.ui b/ground/gcs/src/plugins/config/stabilization.ui index 5e54d3e92..811b6735d 100644 --- a/ground/gcs/src/plugins/config/stabilization.ui +++ b/ground/gcs/src/plugins/config/stabilization.ui @@ -299,7 +299,7 @@ margin-top: -1px; objname:StabilizationSettingsBankX fieldname:AcroInsanityFactor - scale:0.01 + scale:1 buttongroup:77 element:Roll @@ -333,7 +333,7 @@ margin-top: -1px; objname:StabilizationSettingsBankX fieldname:AcroInsanityFactor - scale:0.01 + scale:1 buttongroup:77 element:Roll @@ -398,7 +398,7 @@ margin-top: -1px; objname:StabilizationSettingsBankX fieldname:AcroInsanityFactor - scale:0.01 + scale:1 buttongroup:77 element:Pitch @@ -432,7 +432,7 @@ margin-top: -1px; objname:StabilizationSettingsBankX fieldname:AcroInsanityFactor - scale:0.01 + scale:1 buttongroup:77 element:Pitch diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 9aebacbfd..6900ce5ad 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -15,9 +15,9 @@ - + - + diff --git a/shared/uavobjectdefinition/stabilizationbank.xml b/shared/uavobjectdefinition/stabilizationbank.xml index eb8d0443e..4f703d6e9 100644 --- a/shared/uavobjectdefinition/stabilizationbank.xml +++ b/shared/uavobjectdefinition/stabilizationbank.xml @@ -5,8 +5,8 @@ - - + + @@ -16,12 +16,12 @@ - + - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index 0de049167..7d8842006 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -5,8 +5,8 @@ - - + + @@ -16,12 +16,12 @@ - + - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index 4617fec8b..8c1d7554e 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -5,8 +5,8 @@ - - + + @@ -16,12 +16,12 @@ - + - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index 04e2c6c50..ad7ac0e41 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -5,8 +5,8 @@ - - + + @@ -16,12 +16,12 @@ - + - +