diff --git a/src/main/config/config.c b/src/main/config/config.c index d4771e7bbc..ac42105c26 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -490,7 +490,7 @@ static void resetConf(void) masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; - masterConfig.retarded_arm = 0; // TODO - Cleanup retarded arm support + masterConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support masterConfig.disarm_kill_switch = 1; masterConfig.auto_disarm_delay = 5; masterConfig.small_angle = 25; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index e68817937d..b00e449aa6 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -99,7 +99,7 @@ typedef struct master_t { inputFilteringMode_e inputFilteringMode; // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers. - uint8_t retarded_arm; // allow disarm/arm on throttle down + roll left/right + uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 uint8_t small_angle; diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 6b4dee82e5..8366f96067 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -21,7 +21,8 @@ typedef enum { OK_TO_ARM = (1 << 0), PREVENT_ARMING = (1 << 1), - ARMED = (1 << 2) + ARMED = (1 << 2), + WAS_EVER_ARMED = (1 << 3) } armingFlag_e; extern uint8_t armingFlags; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 5c8872fa2c..a553cb58f9 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -172,7 +172,8 @@ void failsafeUpdateState(void) bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; - if (!receivingRxData) { + // Beep RX lost only if we are not seeing data and we have been armed earlier + if (!receivingRxData && ARMING_FLAG(WAS_EVER_ARMED)) { beeperMode = BEEPER_RX_LOST; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c8496db4a3..85a74d9567 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -311,7 +311,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control gyroError = gyroADC[axis] / 4; error = rc - gyroError; - errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetPidLooptime) >> 12) , -16000, +16000); // WindUp 16 bits is ok here + errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetPidLooptime) >> 11) , -16000, +16000); // WindUp 16 bits is ok here if (ABS(gyroADC[axis]) > (640 * 4)) { errorGyroI[axis] = 0; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 03a108d6f5..2441bf857f 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -130,7 +130,7 @@ rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig) return NOT_CENTERED; } -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch) +void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos @@ -195,15 +195,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat rcDelayCommand = 0; // reset so disarm tone will repeat } } - // Disarm on roll (only when retarded_arm is enabled) - if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) { - if (ARMING_FLAG(ARMED)) - mwDisarm(); - else { - beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held - rcDelayCommand = 0; // reset so disarm tone will repeat - } - } } if (ARMING_FLAG(ARMED)) { @@ -261,12 +252,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat mwArm(); return; } - - if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) { - // Arm via ROLL - mwArm(); - return; - } } if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index c512ed64bb..9674e35803 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -159,7 +159,7 @@ bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch); +void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b9e194c003..91072ef412 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -587,6 +587,7 @@ const clivalue_t valueTable[] = { { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, + { "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } }, { "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, .config.minmax = { 0, 60 } }, { "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, .config.minmax = { 0, 180 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index af529a1704..bd5d8042ba 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -881,9 +881,9 @@ static bool processOutCommand(uint8_t cmdMSP) headSerialReply(3 * PID_ITEM_COUNT); if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < 3; i++) { - serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 255)); + serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 40.0f), 0, 255)); serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 255)); + serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 4000.0f), 0, 255)); } for (i = 3; i < PID_ITEM_COUNT; i++) { if (i == PIDLEVEL) { @@ -1339,9 +1339,9 @@ static bool processInCommand(void) case MSP_SET_PID: if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { for (i = 0; i < 3; i++) { - currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f; + currentProfile->pidProfile.P_f[i] = (float)read8() / 40.0f; currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; - currentProfile->pidProfile.D_f[i] = (float)read8() / 1000.0f; + currentProfile->pidProfile.D_f[i] = (float)read8() / 4000.0f; } for (i = 3; i < PID_ITEM_COUNT; i++) { if (i == PIDLEVEL) { diff --git a/src/main/mw.c b/src/main/mw.c index 2fa5694ee7..ee2ecf2c54 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -358,6 +358,10 @@ void releaseSharedTelemetryPorts(void) { void mwArm(void) { + if (!ARMING_FLAG(WAS_EVER_ARMED) && masterConfig.gyro_cal_on_first_arm) { + gyroSetCalibrationCycles(calculateCalibratingCycles()); + } + if (ARMING_FLAG(OK_TO_ARM)) { if (ARMING_FLAG(ARMED)) { return; @@ -367,6 +371,7 @@ void mwArm(void) } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); + ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); #ifdef BLACKBOX @@ -545,7 +550,7 @@ void processRx(void) } } - processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch); + processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState();