mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 00:05:33 +03:00
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).
360 lines
11 KiB
C
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);
|
|
}
|