From 1c6ea1d3970a382ee6f9d0760fae61e692472ecc Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 26 Aug 2014 21:28:23 +0100 Subject: [PATCH] Fix ANGLE mode and HORIZON mode resetting the errorAngle when they are both attempted to be enabled at the same time. Angle mode now takes precedence over horizon mode. Fix using aux settings that are not applicable to in-use aux channels - prior to this it was possible to configure aux4 and then switch to RX_SERIAL using a 7 channel system (3 aux channels) and aux4 would still have been processed. --- src/main/config/runtime_config.c | 9 ------ src/main/config/runtime_config.h | 27 ------------------ src/main/io/beeper.c | 4 +++ src/main/io/rc_controls.c | 41 ++++++++++++++++++++++++++++ src/main/io/rc_controls.h | 29 ++++++++++++++++++++ src/main/mw.c | 47 ++++++++++---------------------- src/main/rx/rx.c | 4 +++ src/main/rx/rx.h | 1 + src/main/telemetry/telemetry.c | 3 ++ 9 files changed, 97 insertions(+), 68 deletions(-) diff --git a/src/main/config/runtime_config.c b/src/main/config/runtime_config.c index e0f3a0ed9..803486f51 100644 --- a/src/main/config/runtime_config.c +++ b/src/main/config/runtime_config.c @@ -24,15 +24,6 @@ uint8_t armingFlags = 0; uint8_t stateFlags = 0; uint16_t flightModeFlags = 0; -// each entry in the array is a bitmask, 3 bits per aux channel (only aux 1 to 4), aux1 is first, each bit corresponds to an rc channel reading -// bit 1 - stick LOW -// bit 2 - stick MIDDLE -// bit 3 - stick HIGH -// an option is enabled when ANY channel has an appropriate reading corresponding to the bit. -// an option is disabled when NO channel has an appropriate reading corresponding to the bit. -// example: 110000000001 - option is only enabled when AUX1 is LOW or AUX4 is MEDIUM or HIGH. -uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; - static uint32_t enabledSensors = 0; bool sensors(uint32_t mask) diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 2a96c5d5c..676e924a2 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -17,33 +17,6 @@ #pragma once -enum { - BOXARM = 0, - BOXANGLE, - BOXHORIZON, - BOXBARO, - BOXMAG, - BOXHEADFREE, - BOXHEADADJ, - BOXCAMSTAB, - BOXCAMTRIG, - BOXGPSHOME, - BOXGPSHOLD, - BOXPASSTHRU, - BOXBEEPERON, - BOXLEDMAX, - BOXLEDLOW, - BOXLLIGHTS, - BOXCALIB, - BOXGOV, - BOXOSD, - BOXTELEMETRY, - BOXAUTOTUNE, - CHECKBOX_ITEM_COUNT -}; - -extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; - // FIXME some of these are flight modes, some of these are general status indicators typedef enum { OK_TO_ARM = (1 << 0), diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index eb038c014..95cdb079a 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -23,6 +23,10 @@ #include "drivers/system.h" #include "flight/failsafe.h" #include "sensors/sensors.h" + +#include "rx/rx.h" +#include "io/rc_controls.h" + #include "config/runtime_config.h" #include "config/config.h" diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index c79067aa8..c3d070f4c 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -43,6 +43,15 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW +// each entry in the array is a bitmask, 3 bits per aux channel (only aux 1 to 4), aux1 is first, each bit corresponds to an rc channel reading +// bit 1 - stick LOW +// bit 2 - stick MIDDLE +// bit 3 - stick HIGH +// an option is enabled when ANY channel has an appropriate reading corresponding to the bit. +// an option is disabled when NO channel has an appropriate reading corresponding to the bit. +// example: 110000000001 - option is only enabled when AUX1 is LOW or AUX4 is MEDIUM or HIGH. +uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; + bool areSticksInApModePosition(uint16_t ap_mode) { return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode; @@ -210,3 +219,35 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } } + +#define MAX_AUX_STATE_CHANNELS 8 + +void updateRcOptions(uint32_t *activate) +{ + // Check AUX switches + + // auxState is a bitmask, 3 bits per channel. aux1 is first. + // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8 + // + // the three bits are as follows: + // bit 1 is SET when the stick is less than 1300 + // bit 2 is SET when the stick is between 1300 and 1700 + // bit 3 is SET when the stick is above 1700 + // if the value is 1300 or 1700 NONE of the three bits are set. + + int i; + uint32_t auxState = 0; + + for (i = 0; i < rxRuntimeConfig.auxChannelCount && i < MAX_AUX_STATE_CHANNELS; i++) { + uint32_t temp = (rcData[AUX1 + i] < 1300) << (3 * i) | + (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | + (rcData[AUX1 + i] > 1700) << (3 * i + 2); + + if (i >= 4 && i < 8) { + temp <<= 16; + } + auxState |= temp; + } + for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) + rcOptions[i] = (auxState & activate[i]) > 0; +} diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 01726be6a..77cdf2b4c 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,6 +17,33 @@ #pragma once +enum { + BOXARM = 0, + BOXANGLE, + BOXHORIZON, + BOXBARO, + BOXMAG, + BOXHEADFREE, + BOXHEADADJ, + BOXCAMSTAB, + BOXCAMTRIG, + BOXGPSHOME, + BOXGPSHOLD, + BOXPASSTHRU, + BOXBEEPERON, + BOXLEDMAX, + BOXLEDLOW, + BOXLLIGHTS, + BOXCALIB, + BOXGOV, + BOXOSD, + BOXTELEMETRY, + BOXAUTOTUNE, + CHECKBOX_ITEM_COUNT +}; + +extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; + typedef enum rc_alias { ROLL = 0, PITCH, @@ -66,3 +93,5 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, uint32_t *activate, bool retarded_arm); +void updateRcOptions(uint32_t *activate); + diff --git a/src/main/mw.c b/src/main/mw.c index e79d3e6dd..cdd230350 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -439,9 +439,6 @@ void executePeriodicTasks(void) void processRx(void) { - int i; - uint32_t auxState = 0; - calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time @@ -474,30 +471,14 @@ void processRx(void) updateInflightCalibrationState(); } - // Check AUX switches + updateRcOptions(currentProfile->activate); - // auxState is a bitmask, 3 bits per channel. aux1 is first. - // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8 - // - // the three bits are as follows: - // bit 1 is SET when the stick is less than 1300 - // bit 2 is SET when the stick is between 1300 and 1700 - // bit 3 is SET when the stick is above 1700 - // if the value is 1300 or 1700 NONE of the three bits are set. - - for (i = 0; i < 4; i++) { - auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | - (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | - (rcData[AUX1 + i] > 1700) << (3 * i + 2); - auxState |= ((rcData[AUX5 + i] < 1300) << (3 * i) | - (1300 < rcData[AUX5 + i] && rcData[AUX5 + i] < 1700) << (3 * i + 1) | - (rcData[AUX5 + i] > 1700) << (3 * i + 2)) << 16; - } - for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) - rcOptions[i] = (auxState & currentProfile->activate[i]) > 0; + bool canUseHorizonMode = true; if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode + canUseHorizonMode = false; + if (!FLIGHT_MODE(ANGLE_MODE)) { resetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); @@ -506,15 +487,17 @@ void processRx(void) DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } - if (rcOptions[BOXHORIZON]) { - DISABLE_FLIGHT_MODE(ANGLE_MODE); - if (!FLIGHT_MODE(HORIZON_MODE)) { - resetErrorAngle(); - ENABLE_FLIGHT_MODE(HORIZON_MODE); - } - } else { - DISABLE_FLIGHT_MODE(HORIZON_MODE); - } + if (rcOptions[BOXHORIZON] && canUseHorizonMode) { + + DISABLE_FLIGHT_MODE(ANGLE_MODE); + + if (!FLIGHT_MODE(HORIZON_MODE)) { + resetErrorAngle(); + ENABLE_FLIGHT_MODE(HORIZON_MODE); + } + } else { + DISABLE_FLIGHT_MODE(HORIZON_MODE); + } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 607174daf..fc52985bb 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -99,6 +99,8 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo } #endif +#define STICK_CHANNEL_COUNT 4 + void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe) { uint8_t i; @@ -123,6 +125,8 @@ void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe) if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc); } + + rxRuntimeConfig.auxChannelCount = rxRuntimeConfig.channelCount - STICK_CHANNEL_COUNT; } #ifdef SERIAL_RX diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index b2b6ea025..3629c6c0d 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -62,6 +62,7 @@ typedef struct rxConfig_s { typedef struct rxRuntimeConfig_s { uint8_t channelCount; // number of rc channels as reported by current input driver + uint8_t auxChannelCount; } rxRuntimeConfig_t; extern rxRuntimeConfig_t rxRuntimeConfig; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index ae4963348..4442d73e6 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -29,6 +29,9 @@ #include "drivers/serial_softserial.h" #include "io/serial.h" +#include "rx/rx.h" +#include "io/rc_controls.h" + #include "config/runtime_config.h" #include "config/config.h"