1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +03:00
betaflight/src/main/fc/rc_controls.c
Hydra 723831b579 SPRACINGF3NEO - Use RTC6705 clk hack.
CF/BF - Fix typo in max7456DmaInProgress.  Always define an
implementation even when there is no TX-DMA.

CF/BF - Ensure that max7456 communication does not disrupt RTC6705
communication when using the same SPI port for both.

CF/BF - Port RTC6705 to use the VTX API.

* Support VTX channel/band/power on OSD for ALL vtx APIs - previously it only supported RTC6705.
* Remove all FEATURE_VTX usage.
* Remove all `#define VTX` usage.

Note that due to the async nature of tramp/smartaudio it's not possible
to know the current band/channel after requesting a change (as the
request hasn't been processed) so it makes no sense to try and save the
config - and on the tramp/smartaudio vtx the vtx maintains it's state.
So on an RTX6705 equipped FC the user should now MANUALLY save the
config using the save config stick position.

CF/BF - Move CMS for RTC6705 into vtx_rtc6705.c for consistency with
other VTX CMS menus.

CF/BF - Update RTC6705 via SOFT SPI to use VTX API.

* Achieved by simply aligning the API at the driver level and removing
all legacy conditional compilation checks.

CF/BF - Use new IO for RTC6705 CLK Hack

CF/BF - Port VTX button from CF v1.x.

Features:
* Channel Cycle
* Band Cycle
* Power Cycle
* Save Settings
* Works with any VTX using VTX API.

CF/BF - Allow support for Internal and External VTX - External takes
precedence when configured.

CF/BF - Fix display of VTX power in CMS.

* Issue occured when CMS was transitioned from integer to list of
strings.

CF/BF - Disable SMARTAUDIO and TRAMP on the SPRacingF3NEO due to flash
size.

CF/BF - Cleanup const usage of bass-by-value parameters.  Cleanup
naming.

CF/BF - Cleanup naming.  Removing noise-words. Improving consistency.

CF/BF - Improve readability by further naming cleanups.

CF/BF - Remove some magic numbers in the RTC6705 VTX code.

CF/BF - Use braces on some procedural macros.

CF/BF - Improve code readability by renaming rtc6705SetChannel to
rtc6705SetBandAndChannel.

CF/BF - Remove editor mishap.

CF/BF - Make the rtc6705Dev variable static.

CF/BF - remove duplicate state variablertc6705Dev_t - vtxDevice_t had
all we needed.  Use brackets on some procedural style macros.  Make all
the vtx vtable static const.

CF/BF - making some declarations const.

CF/BF - Fix vtx_power maximum value (out by one).
2017-04-21 11:49:31 +12:00

360 lines
11 KiB
C

/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "blackbox/blackbox.h"
#include "build/build_config.h"
#include "common/axis.h"
#include "common/maths.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/fc_rc.h"
#include "fc/runtime_config.h"
#include "io/gps.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "io/vtx_control.h"
#include "io/dashboard.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "rx/rx.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "flight/failsafe.h"
static pidProfile_t *pidProfile;
// true if arming is done via the sticks (as opposed to a switch)
static bool isUsingSticksToArm = true;
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.deadband = 0,
.yaw_deadband = 0,
.alt_hold_deadband = 40,
.alt_hold_fast_change = 1,
.yaw_control_reversed = false,
);
PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0);
PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
.gyro_cal_on_first_arm = 0, // TODO - Cleanup retarded arm support
.disarm_kill_switch = 1,
.auto_disarm_delay = 5
);
PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
bool isAntiGravityModeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
}
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;
}
bool areSticksInApModePosition(uint16_t ap_mode)
{
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
}
throttleStatus_e calculateThrottleStatus(void)
{
if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)))
return THROTTLE_LOW;
} else {
if (rcData[THROTTLE] < rxConfig()->mincheck)
return THROTTLE_LOW;
}
return THROTTLE_HIGH;
}
void processRcStickPositions(throttleStatus_e throttleStatus)
{
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
static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it
uint8_t stTmp = 0;
int i;
// ------------------ STICKS COMMAND HANDLER --------------------
// checking sticks positions
for (i = 0; i < 4; i++) {
stTmp >>= 2;
if (rcData[i] > rxConfig()->mincheck)
stTmp |= 0x80; // check for MIN
if (rcData[i] < rxConfig()->maxcheck)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) {
if (rcDelayCommand < 250)
rcDelayCommand++;
} else
rcDelayCommand = 0;
rcSticks = stTmp;
// perform actions
if (!isUsingSticksToArm) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
if (throttleStatus == THROTTLE_LOW) {
if (ARMING_FLAG(OK_TO_ARM)) {
mwArm();
}
}
} else {
// Disarming via ARM BOX
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (armingConfig()->disarm_kill_switch) {
mwDisarm();
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm();
}
}
}
}
}
if (rcDelayCommand != 20) {
return;
}
if (isUsingSticksToArm) {
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
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)) {
// actions during armed
return;
}
// actions during not armed
i = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroSetCalibrationCycles();
#ifdef GPS
if (feature(FEATURE_GPS)) {
GPS_reset_home_position();
}
#endif
#ifdef BARO
if (sensors(SENSOR_BARO))
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif
return;
}
if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
// Inflight ACC Calibration
handleInflightCalibrationStickPosition();
return;
}
// Multiple configuration profiles
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
i = 1;
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
i = 2;
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
i = 3;
if (i) {
changePidProfile(i - 1);
return;
}
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
if (isUsingSticksToArm) {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
mwArm();
return;
}
}
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
// Calibrating Acc
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
return;
}
if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
// Calibrating Mag
ENABLE_STATE(CALIBRATE_MAG);
return;
}
// Accelerometer Trim
rollAndPitchTrims_t accelerometerTrimsDelta;
memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
bool shouldApplyRollAndPitchTrimDelta = false;
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
accelerometerTrimsDelta.values.pitch = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
accelerometerTrimsDelta.values.pitch = -2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
accelerometerTrimsDelta.values.roll = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
accelerometerTrimsDelta.values.roll = -2;
shouldApplyRollAndPitchTrimDelta = true;
}
if (shouldApplyRollAndPitchTrimDelta) {
applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
rcDelayCommand = 0; // allow autorepetition
return;
}
#ifdef USE_DASHBOARD
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
dashboardDisablePageCycling();
}
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
dashboardEnablePageCycling();
}
#endif
#ifdef VTX_CONTROL
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) {
vtxIncrementBand();
}
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) {
vtxDecrementBand();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) {
vtxIncrementChannel();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) {
vtxDecrementChannel();
}
#endif
}
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
{
uint8_t index;
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
return true;
}
}
return false;
}
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
if (!IS_RANGE_USABLE(range)) {
return false;
}
const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
return (channelValue >= 900 + (range->startStep * 25) &&
channelValue < 900 + (range->endStep * 25));
}
void updateActivatedModes(void)
{
rcModeActivationMask = 0;
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index);
if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
ACTIVATE_RC_MODE(modeActivationCondition->modeId);
}
}
}
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return MIN(ABS(rcData[axis] - midrc), 500);
}
void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse)
{
pidProfile = pidProfileToUse;
isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);
}