From 9c29475ba474caf673ee4f47f13798715b12002d Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 30 Mar 2017 12:12:20 +1300 Subject: [PATCH] Changed float parameters to int16 in 'cli.c'. Fixed buffer length used for 'ftoa'. Got rid of magic numbers, replaced 'digitalIdlePercent' with 'digitalIdleValue'. --- src/main/blackbox/blackbox.c | 12 ++-- src/main/cms/cms_menu_misc.c | 8 +-- src/main/common/typeconversion.h | 2 + src/main/config/parameter_group.h | 3 + src/main/fc/cli.c | 92 ++++++++----------------------- src/main/fc/fc_msp.c | 12 ++-- src/main/fc/fc_rc.c | 2 +- src/main/flight/altitude.c | 4 +- src/main/flight/mixer.c | 14 ++--- src/main/flight/mixer.h | 2 +- src/main/flight/pid.c | 10 ++-- src/main/flight/pid.h | 14 ++--- src/main/sensors/barometer.c | 8 +-- src/main/sensors/barometer.h | 6 +- src/main/telemetry/frsky.c | 4 +- src/main/telemetry/telemetry.h | 4 +- 16 files changed, 77 insertions(+), 120 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 58b33cead..a0c06a8c2 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1284,13 +1284,13 @@ static bool blackboxWriteSysinfo() // Betaflight PID controller parameters BLACKBOX_PRINT_HEADER_LINE("anti_gravity_thresh", "%d", currentPidProfile->itermThrottleThreshold); - BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", castFloatBytesToInt(currentPidProfile->itermAcceleratorGain)); + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain); BLACKBOX_PRINT_HEADER_LINE("setpoint_relax_ratio", "%d", currentPidProfile->setpointRelaxRatio); BLACKBOX_PRINT_HEADER_LINE("d_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight); - BLACKBOX_PRINT_HEADER_LINE("yaw_accel_limit", "%d", castFloatBytesToInt(currentPidProfile->yawRateAccelLimit)); - BLACKBOX_PRINT_HEADER_LINE("accel_limit", "%d", castFloatBytesToInt(currentPidProfile->rateAccelLimit)); - BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", castFloatBytesToInt(currentPidProfile->pidSumLimit)); - BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", castFloatBytesToInt(currentPidProfile->pidSumLimitYaw)); + BLACKBOX_PRINT_HEADER_LINE("yaw_accel_limit", "%d", currentPidProfile->yawRateAccelLimit); + BLACKBOX_PRINT_HEADER_LINE("accel_limit", "%d", currentPidProfile->rateAccelLimit); + BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit); + BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw); // End of Betaflight controller parameters BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband); @@ -1314,7 +1314,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate); - BLACKBOX_PRINT_HEADER_LINE("digital_idle_percent", "%d", (int)(motorConfig()->digitalIdleOffsetPercent * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("digital_idle_value", "%d", motorConfig()->digitalIdleOffsetValue); BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode); BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index fe5bb9d3b..2703d55a3 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -90,14 +90,14 @@ CMS_Menu cmsx_menuRcPreview = { }; static uint16_t motorConfig_minthrottle; -static uint8_t motorConfig_digitalIdleOffsetPercent; +static uint8_t motorConfig_digitalIdleOffsetValue; static uint8_t voltageSensorADCConfig_vbatscale; static uint8_t batteryConfig_vbatmaxcellvoltage; static long cmsx_menuMiscOnEnter(void) { motorConfig_minthrottle = motorConfig()->minthrottle; - motorConfig_digitalIdleOffsetPercent = 10 * motorConfig()->digitalIdleOffsetPercent; + motorConfig_digitalIdleOffsetValue = motorConfig()->digitalIdleOffsetValue / 10; voltageSensorADCConfig_vbatscale = voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale; batteryConfig_vbatmaxcellvoltage = batteryConfig()->vbatmaxcellvoltage; return 0; @@ -108,7 +108,7 @@ static long cmsx_menuMiscOnExit(const OSD_Entry *self) UNUSED(self); motorConfigMutable()->minthrottle = motorConfig_minthrottle; - motorConfigMutable()->digitalIdleOffsetPercent = motorConfig_digitalIdleOffsetPercent / 10.0f; + motorConfigMutable()->digitalIdleOffsetValue = 10 * motorConfig_digitalIdleOffsetValue; voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = voltageSensorADCConfig_vbatscale; batteryConfigMutable()->vbatmaxcellvoltage = batteryConfig_vbatmaxcellvoltage; return 0; @@ -119,7 +119,7 @@ static OSD_Entry menuMiscEntries[]= { "-- MISC --", OME_Label, NULL, NULL, 0 }, { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig_minthrottle, 1000, 2000, 1 }, 0 }, - { "DIGITAL IDLE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &motorConfig_digitalIdleOffsetPercent, 0, 200, 1, 100 }, 0 }, + { "DIGITAL IDLE", OME_UINT16, NULL, &(OSD_UINT16_t) { &motorConfig_digitalIdleOffsetValue, 0, 2000, 1 }, 0 }, { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &voltageSensorADCConfig_vbatscale, 1, 250, 1 }, 0 }, { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatmaxcellvoltage, 10, 50, 1 }, 0 }, { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, diff --git a/src/main/common/typeconversion.h b/src/main/common/typeconversion.h index 62b437875..0d9041cdf 100644 --- a/src/main/common/typeconversion.h +++ b/src/main/common/typeconversion.h @@ -16,6 +16,8 @@ */ #pragma once +#define FTOA_BUFFER_LENGTH 11 + void uli2a(unsigned long int num, unsigned int base, int uc, char *bf); void li2a(long num, char *bf); void ui2a(unsigned int num, unsigned int base, int uc, char *bf); diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 3ac0f0946..a16f66b79 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -233,6 +233,9 @@ extern const uint8_t __pg_resetdata_end[]; } \ /**/ +#define CONVERT_PARAMETER_TO_FLOAT(param) (0.001f * param) +#define CONVERT_PARAMETER_TO_PERCENT(param) (0.01f * param) + const pgRegistry_t* pgFind(pgn_t pgn); void pgLoad(const pgRegistry_t* reg, int profileIndex, const void *from, int size, int version); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1499a506b..2ea7f41b2 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -465,8 +465,6 @@ typedef enum { VAR_INT8 = (1 << VALUE_TYPE_OFFSET), VAR_UINT16 = (2 << VALUE_TYPE_OFFSET), VAR_INT16 = (3 << VALUE_TYPE_OFFSET), - //VAR_UINT32 = (4 << VALUE_TYPE_OFFSET), - VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), // 0x05 // value section, bits 4-5 MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), @@ -549,9 +547,9 @@ static const clivalue_t valueTable[] = { #ifdef BARO { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) }, { "baro_tab_size", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_sample_count) }, - { "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0 , 1 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) }, - { "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0 , 1 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_cf_vel) }, - { "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0 , 1 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_cf_alt) }, + { "baro_noise_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) }, + { "baro_cf_vel", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_cf_vel) }, + { "baro_cf_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_cf_alt) }, #endif // PG_RX_CONFIG @@ -599,7 +597,7 @@ static const clivalue_t valueTable[] = { { "max_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, maxthrottle) }, { "min_command", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, mincommand) }, #ifdef USE_DSHOT - { "digital_idle_percent", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 20 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, digitalIdleOffsetPercent) }, + { "digital_idle_value", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, digitalIdleOffsetValue) }, #endif { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useUnsyncedPwm) }, { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmProtocol) }, @@ -745,16 +743,16 @@ static const clivalue_t valueTable[] = { { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) }, { "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, - { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 1, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) }, + { "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) }, { "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 254 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) }, - { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 0.1f, 50.0f }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, - { "accel_limit", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 0.1f, 50.0f }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, + { "yaw_accel_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, + { "accel_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) }, - { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 0.1, 1.0 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) }, - { "pidsum_limit_yaw", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 0.1, 1.0 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) }, + { "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) }, + { "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PITCH]) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PITCH]) }, @@ -797,8 +795,8 @@ static const clivalue_t valueTable[] = { { "tlm_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_switch) }, { "tlm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inversion) }, { "sport_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, sportHalfDuplex) }, - { "frsky_default_lat", VAR_FLOAT | MASTER_VALUE, .config.minmax = { -90.0, 90.0 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLatitude) }, - { "frsky_default_long", VAR_FLOAT | MASTER_VALUE, .config.minmax = { -180.0, 180.0 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLongitude) }, + { "frsky_default_lat", VAR_INT16 | MASTER_VALUE, .config.minmax = { -9000, 9000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLatitude) }, + { "frsky_default_long", VAR_INT16 | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLongitude) }, { "frsky_gps_format", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, FRSKY_FORMAT_NMEA }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_coordinate_format) }, { "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_UNIT }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_unit) }, { "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_vfas_precision) }, @@ -1058,7 +1056,6 @@ static void cliPrintf(const char *format, ...) static void printValuePointer(const clivalue_t *var, const void *valuePointer, uint32_t full) { int32_t value = 0; - char buf[8]; switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: @@ -1076,19 +1073,6 @@ static void printValuePointer(const clivalue_t *var, const void *valuePointer, u case VAR_INT16: value = *(int16_t *)valuePointer; break; - -/* not currently used - case VAR_UINT32: - value = *(uint32_t *)valuePointer; - break; */ - - case VAR_FLOAT: - cliPrintf("%s", ftoa(*(float *)valuePointer, buf)); - if (full && (var->type & VALUE_MODE_MASK) == MODE_DIRECT) { - cliPrintf(" %s", ftoa((float)var->config.minmax.min, buf)); - cliPrintf(" %s", ftoa((float)var->config.minmax.max, buf)); - } - return; // return from case for float only } switch(var->type & VALUE_MODE_MASK) { @@ -1123,16 +1107,8 @@ static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptr case VAR_INT16: result = *(int16_t *)ptr == *(int16_t *)ptrDefault; break; - -/* not currently used - case VAR_UINT32: - result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; - break;*/ - - case VAR_FLOAT: - result = *(float *)ptr == *(float *)ptrDefault; - break; } + return result; } @@ -1471,33 +1447,19 @@ static void cliPrintVarRange(const clivalue_t *var) } } -typedef union { - int32_t int_value; - float float_value; -} int_float_value_t; - -static void cliSetVar(const clivalue_t *var, const int_float_value_t value) +static void cliSetVar(const clivalue_t *var, const int32_t value) { void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: case VAR_INT8: - *(int8_t *)ptr = value.int_value; + *(int8_t *)ptr = value; break; case VAR_UINT16: case VAR_INT16: - *(int16_t *)ptr = value.int_value; - break; - -/* not currently used - case VAR_UINT32: - *(uint32_t *)ptr = value.int_value; - break; */ - - case VAR_FLOAT: - *(float *)ptr = (float)value.float_value; + *(int16_t *)ptr = value; break; } } @@ -2052,9 +2014,9 @@ static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer { const char *format = "mmix %d %s %s %s %s\r\n"; char buf0[8]; - char buf1[8]; - char buf2[8]; - char buf3[8]; + char buf1[FTOA_BUFFER_LENGTH]; + char buf2[FTOA_BUFFER_LENGTH]; + char buf3[FTOA_BUFFER_LENGTH]; for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { if (customMotorMixer[i].throttle == 0.0f) break; @@ -3599,22 +3561,12 @@ static void cliSet(char *cmdline) if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { bool changeValue = false; - int_float_value_t tmp = { 0 }; + int32_t value = 0; switch (valueTable[i].type & VALUE_MODE_MASK) { case MODE_DIRECT: { - int32_t value = 0; - float valuef = 0; - value = atoi(eqptr); - valuef = fastA2F(eqptr); - - if (valuef >= valueTable[i].config.minmax.min && valuef <= valueTable[i].config.minmax.max) { // note: compare float value - - if ((valueTable[i].type & VALUE_TYPE_MASK) == VAR_FLOAT) - tmp.float_value = valuef; - else - tmp.int_value = value; + if (value >= valueTable[i].config.minmax.min && value <= valueTable[i].config.minmax.max) { changeValue = true; } } @@ -3626,7 +3578,7 @@ static void cliSet(char *cmdline) matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; if (matched) { - tmp.int_value = tableValueIndex; + value = tableValueIndex; changeValue = true; } } @@ -3635,7 +3587,7 @@ static void cliSet(char *cmdline) } if (changeValue) { - cliSetVar(val, tmp); + cliSetVar(val, value); cliPrintf("%s set to ", valueTable[i].name); cliPrintVar(val, 0); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 474440ac7..845e1228c 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1176,7 +1176,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm); sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol); sbufWriteU16(dst, motorConfig()->dev.motorPwmRate); - sbufWriteU16(dst, (uint16_t)lrintf(motorConfig()->digitalIdleOffsetPercent * 100)); + sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue); sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); //!!TODO gyro_isr_update to be added pending decision //sbufWriteU8(dst, gyroConfig()->gyro_isr_update); @@ -1206,8 +1206,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved - sbufWriteU16(dst, (uint16_t)lrintf(currentPidProfile->rateAccelLimit * 10)); - sbufWriteU16(dst, (uint16_t)lrintf(currentPidProfile->yawRateAccelLimit * 10)); + sbufWriteU16(dst, currentPidProfile->rateAccelLimit); + sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit); sbufWriteU8(dst, currentPidProfile->levelAngleLimit); sbufWriteU8(dst, currentPidProfile->levelSensitivity); break; @@ -1538,7 +1538,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src); if (sbufBytesRemaining(src) >= 2) { - motorConfigMutable()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; + motorConfigMutable()->digitalIdleOffsetValue = sbufReadU16(src); } if (sbufBytesRemaining(src)) { gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src); @@ -1586,8 +1586,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) sbufReadU8(src); // reserved sbufReadU8(src); // reserved sbufReadU8(src); // reserved - currentPidProfile->rateAccelLimit = sbufReadU16(src) / 10.0f; - currentPidProfile->yawRateAccelLimit = sbufReadU16(src) / 10.0f; + currentPidProfile->rateAccelLimit = sbufReadU16(src); + currentPidProfile->yawRateAccelLimit = sbufReadU16(src); if (dataSize > 17) { currentPidProfile->levelAngleLimit = sbufReadU8(src); currentPidProfile->levelSensitivity = sbufReadU8(src); diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 6f7522fee..a22c3affd 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -167,7 +167,7 @@ static void scaleRcCommandToFpvCamAngle(void) { const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; if(ABS(rcCommandSpeed) > throttleVelocityThreshold) - pidSetItermAccelerator(currentPidProfile->itermAcceleratorGain); + pidSetItermAccelerator(0.0001f * currentPidProfile->itermAcceleratorGain); else pidSetItermAccelerator(1.0f); } diff --git a/src/main/flight/altitude.c b/src/main/flight/altitude.c index 0cbb6f781..bf5cad28e 100644 --- a/src/main/flight/altitude.c +++ b/src/main/flight/altitude.c @@ -259,7 +259,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) - accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) + accAlt = accAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt) + (float)baro.BaroAlt * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt)); // complementary filter for altitude estimation (baro & acc) vel += vel_acc; estimatedAltitude = accAlt; } @@ -289,7 +289,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay - vel = vel * barometerConfig()->baro_cf_vel + (float)baroVel * (1.0f - barometerConfig()->baro_cf_vel); + vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel)); int32_t vel_tmp = lrintf(vel); // set vario diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b698cdfbd..c3fd48e4d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -96,7 +96,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) #endif motorConfig->maxthrottle = 2000; motorConfig->mincommand = 1000; - motorConfig->digitalIdleOffsetPercent = 4.5f; + motorConfig->digitalIdleOffsetValue = 450; int motorIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { @@ -350,11 +350,11 @@ void initEscEndpoints(void) { if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; if (feature(FEATURE_3D)) - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); + motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAM_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); else - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); + motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAM_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); motorOutputHigh = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAM_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); // TODO - Not working yet !! Mixer requires some throttle rescaling changes deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; } else #endif @@ -542,13 +542,13 @@ void mixTable(pidProfile_t *pidProfile) // Calculate and Limit the PIDsum scaledAxisPIDf[FD_ROLL] = constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, - -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + -CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit), CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit)); scaledAxisPIDf[FD_PITCH] = constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, - -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + -CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit), CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit)); scaledAxisPIDf[FD_YAW] = constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, - -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw); + -CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit), CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimitYaw)); // Calculate voltage compensation const float vbatCompensationFactor = (pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 5f88ff3ae..d370d85d0 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -107,7 +107,7 @@ PG_DECLARE(mixerConfig_t, mixerConfig); typedef struct motorConfig_s { motorDevConfig_t dev; - float digitalIdleOffsetPercent; + uint16_t digitalIdleOffsetValue; // Idle value for DShot protocol, full motor output = 10000 uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5b86e0d53..7452a5a91 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -116,10 +116,10 @@ void resetPidProfile(pidProfile_t *pidProfile) .levelSensitivity = 55, .setpointRelaxRatio = 100, .dtermSetpointWeight = 60, - .yawRateAccelLimit = 10.0f, - .rateAccelLimit = 0.0f, + .yawRateAccelLimit = 100, + .rateAccelLimit = 0, .itermThrottleThreshold = 350, - .itermAcceleratorGain = 1.0f + .itermAcceleratorGain = 1000 ); } @@ -242,8 +242,8 @@ void pidInitConfig(const pidProfile_t *pidProfile) { levelGain = pidProfile->P8[PIDLEVEL] / 10.0f; horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f; horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL]; - maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; - maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; + maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT; + maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 666bfb173..ce8acddfb 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -24,8 +24,8 @@ #define PID_CONTROLLER_BETAFLIGHT 1 #define PID_MIXER_SCALING 1000.0f #define PID_SERVO_MIXER_SCALING 0.7f -#define PIDSUM_LIMIT 0.5f -#define PIDSUM_LIMIT_YAW 0.4f +#define PIDSUM_LIMIT 500 +#define PIDSUM_LIMIT_YAW 400 // Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float #define PTERM_SCALE 0.032029f @@ -68,8 +68,8 @@ typedef struct pidProfile_s { uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation - float pidSumLimit; - float pidSumLimitYaw; + uint16_t pidSumLimit; + uint16_t pidSumLimitYaw; uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. @@ -78,11 +78,11 @@ typedef struct pidProfile_s { // Betaflight PID controller parameters uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms - float itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit + uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) - float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms - float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms + uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms + uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms } pidProfile_t; #if FLASH_SIZE <= 128 diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index a0572cf91..d282af5da 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -49,9 +49,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, .baro_hardware = 1, .baro_sample_count = 21, - .baro_noise_lpf = 0.6f, - .baro_cf_vel = 0.985f, - .baro_cf_alt = 0.965f + .baro_noise_lpf = 600, + .baro_cf_vel = 985, + .baro_cf_alt = 965 ); #ifdef BARO @@ -229,7 +229,7 @@ int32_t baroCalculateAltitude(void) if (isBaroCalibrationComplete()) { BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; - baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise + baro.BaroAlt = lrintf((float)baro.BaroAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf) + (float)BaroAlt_tmp * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf))); // additional LPF to reduce baro noise } else { baro.BaroAlt = 0; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index b753ef5ea..fbc43e75d 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -33,9 +33,9 @@ typedef enum { typedef struct barometerConfig_s { uint8_t baro_hardware; // Barometer hardware to use uint8_t baro_sample_count; // size of baro filter array - float baro_noise_lpf; // additional LPF to reduce baro noise - float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) - float baro_cf_alt; // apply CF to use ACC for height estimation + uint16_t baro_noise_lpf; // additional LPF to reduce baro noise + uint16_t baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) + uint16_t baro_cf_alt; // apply CF to use ACC for height estimation } barometerConfig_t; PG_DECLARE(barometerConfig_t, barometerConfig); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index c2c6fb25d..df292e928 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -328,8 +328,8 @@ static void sendFakeLatLong(void) // Heading is only displayed on OpenTX if non-zero lat/long is also sent int32_t coord[2] = {0,0}; - coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); - coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); + coord[LAT] = ((0.01f * telemetryConfig()->gpsNoFixLatitude) * GPS_DEGREES_DIVIDER); + coord[LON] = ((0.01f * telemetryConfig()->gpsNoFixLongitude) * GPS_DEGREES_DIVIDER); sendLatLong(coord); } diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 0fc00cae5..82676fec3 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -38,8 +38,8 @@ typedef enum { } frskyUnit_e; typedef struct telemetryConfig_s { - float gpsNoFixLatitude; - float gpsNoFixLongitude; + int16_t gpsNoFixLatitude; + int16_t gpsNoFixLongitude; uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inversion; // also shared with smartport inversion uint8_t sportHalfDuplex;