Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 07:15:16 +03:00
Code Issues Wiki Activity

Add bitarray support; Migrage rcModeActivationMask to bitArray

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-08-04 00:02:01 +10:00
parent ab33d1eee4
commit c03c7d6bce
21 changed files with 382 additions and 215 deletions
Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -541,6 +541,7 @@ COMMON_SRC = \
build/build_config.c \
build/debug.c \
build/version.c \
common/bitarray.c \
common/encoding.c \
common/filter.c \
common/maths.c \
@ -589,6 +590,7 @@ COMMON_SRC = \
fc/rc_adjustments.c \
fc/rc_controls.c \
fc/rc_curves.c \
fc/rc_modes.c \
fc/runtime_config.c \
fc/stats.c \
flight/failsafe.c \

28
src/main/blackbox/blackbox.c
View file

@ -49,6 +49,7 @@
#include "fc/controlrate_profile.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
@ -383,7 +384,7 @@ typedef struct blackboxGpsState_s {
// This data is updated really infrequently:
typedef struct blackboxSlowState_s {
uint16_t flightModeFlags;
uint32_t flightModeFlags; // extend this data size (from uint16_t)
uint8_t stateFlags;
uint8_t failsafePhase;
bool rxSignalReceived;
@ -391,9 +392,13 @@ typedef struct blackboxSlowState_s {
int32_t hwHealthStatus;
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From rc_controls.c
extern boxBitmask_t rcModeActivationMask;
static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
static uint32_t blackboxLastArmingBeep = 0;
static uint32_t blackboxLastFlightModeFlags = 0;
static struct {
uint32_t headerIndex;
@ -898,7 +903,7 @@ static void writeSlowFrame(void)
*/
static void loadSlowState(blackboxSlowState_t *slow)
{
slow->flightModeFlags = flightModeFlags;
memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
slow->stateFlags = stateFlags;
slow->failsafePhase = failsafePhase();
slow->rxSignalReceived = rxIsReceivingSignal();
@ -1026,6 +1031,7 @@ void blackboxStart(void)
* it finally plays the beep for this arming event.
*/
blackboxLastArmingBeep = getArmingBeepTimeMicros();
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status
blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
}
@ -1418,6 +1424,10 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
case FLIGHT_LOG_EVENT_SYNC_BEEP:
blackboxWriteUnsignedVB(data->syncBeep.time);
break;
case FLIGHT_LOG_EVENT_FLIGHTMODE: // New flightmode flags write
blackboxWriteUnsignedVB(data->flightMode.flags);
blackboxWriteUnsignedVB(data->flightMode.lastFlags);
break;
case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
if (data->inflightAdjustment.floatFlag) {
blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
@ -1453,6 +1463,19 @@ static void blackboxCheckAndLogArmingBeep(void)
}
}
/* monitor the flight mode event status and trigger an event record if the state changes */
static void blackboxCheckAndLogFlightMode(void)
{
// Use != so that we can still detect a change if the counter wraps
if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
eventData.lastFlags = blackboxLastFlightModeFlags;
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
}
}
/*
* Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
* the portion of logged loop iterations.
@ -1498,6 +1521,7 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
writeIntraframe();
} else {
blackboxCheckAndLogArmingBeep();
blackboxCheckAndLogFlightMode();
if (blackboxShouldLogPFrame(blackboxPFrameIndex)) {
/*

7
src/main/blackbox/blackbox_fielddefs.h
View file

@ -111,6 +111,7 @@ typedef enum FlightLogEvent {
FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
FLIGHT_LOG_EVENT_FLIGHTMODE = 30, // Add new event type for flight mode status.
FLIGHT_LOG_EVENT_IMU_FAILURE = 40,
FLIGHT_LOG_EVENT_LOG_END = 255
} FlightLogEvent;
@ -119,6 +120,11 @@ typedef struct flightLogEvent_syncBeep_s {
uint32_t time;
} flightLogEvent_syncBeep_t;
typedef struct flightLogEvent_flightMode_s { // New Event Data type
uint32_t flags;
uint32_t lastFlags;
} flightLogEvent_flightMode_t;
typedef struct flightLogEvent_inflightAdjustment_s {
uint8_t adjustmentFunction;
bool floatFlag;
@ -135,6 +141,7 @@ typedef struct flightLogEvent_loggingResume_s {
typedef union flightLogEventData_u {
flightLogEvent_syncBeep_t syncBeep;
flightLogEvent_flightMode_t flightMode; // New event data
flightLogEvent_inflightAdjustment_t inflightAdjustment;
flightLogEvent_loggingResume_t loggingResume;
} flightLogEventData_t;

38
src/main/common/bitarray.c Executable file
View file

@ -0,0 +1,38 @@
/*
* 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 <stdint.h>
#include <stdbool.h>
#include "bitarray.h"
#define BITARRAY_BIT_OP(array, bit, op) ((array)[(bit) / (sizeof((array)[0]) * 8)] op (1 << ((bit) % (sizeof((array)[0]) * 8))))
bool bitArrayGet(const void *array, unsigned bit)
{
return BITARRAY_BIT_OP((uint32_t*)array, bit, &);
}
void bitArraySet(void *array, unsigned bit)
{
BITARRAY_BIT_OP((uint32_t*)array, bit, |=);
}
void bitArrayClr(void *array, unsigned bit)
{
BITARRAY_BIT_OP((uint32_t*)array, bit, &=~);
}

20
src/main/common/bitarray.h Executable file
View file

@ -0,0 +1,20 @@
/*
* 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/>.
*/
bool bitArrayGet(const void *array, unsigned bit);
void bitArraySet(void *array, unsigned bit);
void bitArrayClr(void *array, unsigned bit);

1
src/main/fc/cli.c
View file

@ -71,6 +71,7 @@ extern uint8_t __config_end;
#include "fc/controlrate_profile.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"

1
src/main/fc/config.c
View file

@ -72,6 +72,7 @@
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/rc_curves.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "navigation/navigation.h"

1
src/main/fc/fc_core.c
View file

@ -53,6 +53,7 @@
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/rc_curves.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"

1
src/main/fc/fc_msp.c
View file

@ -48,6 +48,7 @@
#include "fc/fc_msp.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"

1
src/main/fc/rc_adjustments.h
View file

@ -20,6 +20,7 @@
#include "config/parameter_group.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
//#define USE_INFLIGHT_PROFILE_ADJUSTMENT - not currently enabled

122
src/main/fc/rc_controls.c
View file

@ -42,6 +42,7 @@
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/rc_curves.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
@ -63,23 +64,10 @@
#define AIRMODE_DEADBAND 25
#define MIN_RC_TICK_INTERVAL_MS 20
// true if arming is done via the sticks (as opposed to a switch)
static bool isUsingSticksToArm = true;
// Count of mode activation ranged (per box mode)
static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT];
#ifdef NAV
// true if pilot has any of GPS modes configured
static bool isUsingNAVModes = false;
#endif
stickPositions_e rcStickPositions;
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,
@ -97,21 +85,6 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
.auto_disarm_delay = 5
);
PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0);
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;
}
#if defined(NAV)
bool isUsingNavigationModes(void)
{
return isUsingNAVModes;
}
#endif
bool areSticksInApModePosition(uint16_t ap_mode)
{
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
@ -197,7 +170,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
rcSticks = stTmp;
// perform actions
if (!isUsingSticksToArm) {
if (!isUsingSticksForArming()) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
@ -235,7 +208,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
return;
}
if (isUsingSticksToArm) {
if (isUsingSticksForArming()) {
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
// Dont disarm if fixedwing and motorstop
@ -300,7 +273,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
// Arming by sticks
if (isUsingSticksToArm) {
if (isUsingSticksForArming()) {
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
@ -352,94 +325,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
}
}
bool isModeActivationConditionPresent(boxId_e modeId)
{
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
if (modeActivationConditions(index)->modeId == modeId && IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
return true;
}
}
return false;
}
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
if (!IS_RANGE_USABLE(range)) {
return false;
}
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)
{
// Unfortunately for AND logic it's not enough to simply check if any of the specified channel range conditions are valid for a mode.
// We need to count the total number of conditions specified for each mode, and check that all those conditions are currently valid.
uint8_t activeConditionCountPerMode[CHECKBOX_ITEM_COUNT];
memset(activeConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
if (isRangeActive(modeActivationConditions(index)->auxChannelIndex, &modeActivationConditions(index)->range)) {
// Increment the number of valid conditions for this mode
activeConditionCountPerMode[modeActivationConditions(index)->modeId]++;
}
}
// Disable all modes to begin with
rcModeActivationMask = 0;
// Now see which modes should be enabled
for (int modeIndex = 0; modeIndex < CHECKBOX_ITEM_COUNT; modeIndex++) {
// only modes with conditions specified are considered
if (specifiedConditionCountPerMode[modeIndex] > 0) {
// For AND logic, the specified condition count and valid condition count must be the same.
// For OR logic, the valid condition count must be greater than zero.
if (modeActivationOperatorConfig()->modeActivationOperator == MODE_OPERATOR_AND) {
// AND the conditions
if (activeConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) {
ACTIVATE_RC_MODE(modeIndex);
}
}
else {
// OR the conditions
if (activeConditionCountPerMode[modeIndex] > 0) {
ACTIVATE_RC_MODE(modeIndex);
}
}
}
}
}
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return MIN(ABS(rcData[axis] - midrc), 500);
}
void updateUsedModeActivationConditionFlags(void)
{
memset(specifiedConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
if (IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
specifiedConditionCountPerMode[modeActivationConditions(index)->modeId]++;
}
}
isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
#ifdef NAV
isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
isModeActivationConditionPresent(BOXNAVRTH) ||
isModeActivationConditionPresent(BOXNAVWP);
#endif
}
void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm)
{
modeActivationConditionsMutable(macIndex)->modeId = modeId;
modeActivationConditionsMutable(macIndex)->auxChannelIndex = auxChannelIndex;
modeActivationConditionsMutable(macIndex)->range.startStep = CHANNEL_VALUE_TO_STEP(startPwm);
modeActivationConditionsMutable(macIndex)->range.endStep = CHANNEL_VALUE_TO_STEP(endPwm);
}

95
src/main/fc/rc_controls.h
View file

@ -19,47 +19,6 @@
#include "config/parameter_group.h"
typedef enum {
BOXARM = 0,
BOXANGLE = 1,
BOXHORIZON = 2,
BOXNAVALTHOLD = 3, // old BOXBARO
BOXHEADINGHOLD = 4, // old MAG
BOXHEADFREE = 5,
BOXHEADADJ = 6,
BOXCAMSTAB = 7,
BOXNAVRTH = 8, // old GPSHOME
BOXNAVPOSHOLD = 9, // old GPSHOLD
BOXPASSTHRU = 10,
BOXBEEPERON = 11,
BOXLEDLOW = 12,
BOXLLIGHTS = 13,
BOXNAVLAUNCH = 14,
BOXOSD = 15,
BOXTELEMETRY = 16,
BOXBLACKBOX = 17,
BOXFAILSAFE = 18,
BOXNAVWP = 19,
BOXAIRMODE = 20,
BOXHOMERESET = 21,
BOXGCSNAV = 22,
BOXKILLSWITCH = 23, // old HEADING LOCK
BOXSURFACE = 24,
BOXFLAPERON = 25,
BOXTURNASSIST = 26,
BOXAUTOTRIM = 27,
BOXAUTOTUNE = 28,
BOXCAMERA1 = 29,
BOXCAMERA2 = 30,
BOXCAMERA3 = 31,
CHECKBOX_ITEM_COUNT
} boxId_e;
extern uint32_t rcModeActivationMask;
#define IS_RC_MODE_ACTIVE(modeId) ((1 << (modeId)) & rcModeActivationMask)
#define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId))
typedef enum rc_alias {
ROLL = 0,
PITCH,
@ -103,48 +62,6 @@ typedef enum {
THR_HI = (2 << (2 * THROTTLE))
} stickPositions_e;
#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
#define CHANNEL_RANGE_MIN 900
#define CHANNEL_RANGE_MAX 2100
#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step)
#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25)
#define MIN_MODE_RANGE_STEP 0
#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25)
// steps are 25 apart
// a value of 0 corresponds to a channel value of 900 or less
// a value of 48 corresponds to a channel value of 2100 or more
// 48 steps between 900 and 1200
typedef struct channelRange_s {
uint8_t startStep;
uint8_t endStep;
} channelRange_t;
typedef struct modeActivationCondition_s {
boxId_e modeId;
uint8_t auxChannelIndex;
channelRange_t range;
} modeActivationCondition_t;
#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
typedef enum {
MODE_OPERATOR_OR, // default
MODE_OPERATOR_AND
} modeActivationOperator_e;
typedef struct modeActivationOperatorConfig_s {
modeActivationOperator_e modeActivationOperator;
} modeActivationOperatorConfig_t;
PG_DECLARE(modeActivationOperatorConfig_t, modeActivationOperatorConfig);
extern int16_t rcCommand[4];
typedef struct rcControlsConfig_s {
@ -168,21 +85,9 @@ PG_DECLARE(armingConfig_t, armingConfig);
stickPositions_e getRcStickPositions(void);
bool checkStickPosition(stickPositions_e stickPos);
bool areUsingSticksToArm(void);
bool areSticksInApModePosition(uint16_t ap_mode);
throttleStatus_e calculateThrottleStatus(void);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm);
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void);
bool isUsingSticksForArming(void);
bool isUsingNavigationModes(void);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
bool isModeActivationConditionPresent(boxId_e modeId);
void updateUsedModeActivationConditionFlags(void);
void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);

159
src/main/fc/rc_modes.c Executable file
View file

@ -0,0 +1,159 @@
/*
* 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 "rc_modes.h"
#include "common/bitarray.h"
#include "common/maths.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "rx/rx.h"
static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT];
static bool isUsingSticksToArm = true;
#ifdef NAV
static bool isUsingNAVModes = false;
#endif
boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0);
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;
}
#if defined(NAV)
bool isUsingNavigationModes(void)
{
return isUsingNAVModes;
}
#endif
bool IS_RC_MODE_ACTIVE(boxId_e boxId)
{
return bitArrayGet(&rcModeActivationMask, boxId);
}
void rcModeUpdate(boxBitmask_t *newState)
{
rcModeActivationMask = *newState;
}
bool isModeActivationConditionPresent(boxId_e modeId)
{
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
if (modeActivationConditions(index)->modeId == modeId && IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
return true;
}
}
return false;
}
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range)
{
if (!IS_RANGE_USABLE(range)) {
return false;
}
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)
{
// Disable all modes to begin with
boxBitmask_t newMask;
memset(&newMask, 0, sizeof(newMask));
// Unfortunately for AND logic it's not enough to simply check if any of the specified channel range conditions are valid for a mode.
// We need to count the total number of conditions specified for each mode, and check that all those conditions are currently valid.
uint8_t activeConditionCountPerMode[CHECKBOX_ITEM_COUNT];
memset(activeConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
if (isRangeActive(modeActivationConditions(index)->auxChannelIndex, &modeActivationConditions(index)->range)) {
// Increment the number of valid conditions for this mode
activeConditionCountPerMode[modeActivationConditions(index)->modeId]++;
}
}
// Now see which modes should be enabled
for (int modeIndex = 0; modeIndex < CHECKBOX_ITEM_COUNT; modeIndex++) {
// only modes with conditions specified are considered
if (specifiedConditionCountPerMode[modeIndex] > 0) {
// For AND logic, the specified condition count and valid condition count must be the same.
// For OR logic, the valid condition count must be greater than zero.
if (modeActivationOperatorConfig()->modeActivationOperator == MODE_OPERATOR_AND) {
// AND the conditions
if (activeConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) {
bitArraySet(&newMask, modeIndex);
}
}
else {
// OR the conditions
if (activeConditionCountPerMode[modeIndex] > 0) {
bitArraySet(&newMask, modeIndex);
}
}
}
}
rcModeUpdate(&newMask);
}
void updateUsedModeActivationConditionFlags(void)
{
memset(specifiedConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
if (IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
specifiedConditionCountPerMode[modeActivationConditions(index)->modeId]++;
}
}
isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
#ifdef NAV
isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
isModeActivationConditionPresent(BOXNAVRTH) ||
isModeActivationConditionPresent(BOXNAVWP);
#endif
}
void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm)
{
modeActivationConditionsMutable(macIndex)->modeId = modeId;
modeActivationConditionsMutable(macIndex)->auxChannelIndex = auxChannelIndex;
modeActivationConditionsMutable(macIndex)->range.startStep = CHANNEL_VALUE_TO_STEP(startPwm);
modeActivationConditionsMutable(macIndex)->range.endStep = CHANNEL_VALUE_TO_STEP(endPwm);
}

114
src/main/fc/rc_modes.h Executable file
View file

@ -0,0 +1,114 @@
/*
* 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/>.
*/
#pragma once
#include <stdbool.h>
#include "config/parameter_group.h"
typedef enum {
BOXARM = 0,
BOXANGLE = 1,
BOXHORIZON = 2,
BOXNAVALTHOLD = 3, // old BOXBARO
BOXHEADINGHOLD = 4, // old MAG
BOXHEADFREE = 5,
BOXHEADADJ = 6,
BOXCAMSTAB = 7,
BOXNAVRTH = 8, // old GPSHOME
BOXNAVPOSHOLD = 9, // old GPSHOLD
BOXPASSTHRU = 10,
BOXBEEPERON = 11,
BOXLEDLOW = 12,
BOXLLIGHTS = 13,
BOXNAVLAUNCH = 14,
BOXOSD = 15,
BOXTELEMETRY = 16,
BOXBLACKBOX = 17,
BOXFAILSAFE = 18,
BOXNAVWP = 19,
BOXAIRMODE = 20,
BOXHOMERESET = 21,
BOXGCSNAV = 22,
BOXKILLSWITCH = 23, // old HEADING LOCK
BOXSURFACE = 24,
BOXFLAPERON = 25,
BOXTURNASSIST = 26,
BOXAUTOTRIM = 27,
BOXAUTOTUNE = 28,
BOXCAMERA1 = 29,
BOXCAMERA2 = 30,
BOXCAMERA3 = 31,
CHECKBOX_ITEM_COUNT
} boxId_e;
// type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior
typedef struct boxBitmask_s { uint32_t bits[(CHECKBOX_ITEM_COUNT + 31) / 32]; } boxBitmask_t;
#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
#define CHANNEL_RANGE_MIN 900
#define CHANNEL_RANGE_MAX 2100
#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step)
#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25)
#define MIN_MODE_RANGE_STEP 0
#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25)
#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
// steps are 25 apart
// a value of 0 corresponds to a channel value of 900 or less
// a value of 48 corresponds to a channel value of 2100 or more
// 48 steps between 900 and 1200
typedef struct channelRange_s {
uint8_t startStep;
uint8_t endStep;
} channelRange_t;
typedef struct modeActivationCondition_s {
boxId_e modeId;
uint8_t auxChannelIndex;
channelRange_t range;
} modeActivationCondition_t;
typedef enum {
MODE_OPERATOR_OR, // default
MODE_OPERATOR_AND
} modeActivationOperator_e;
typedef struct modeActivationOperatorConfig_s {
modeActivationOperator_e modeActivationOperator;
} modeActivationOperatorConfig_t;
PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
PG_DECLARE(modeActivationOperatorConfig_t, modeActivationOperatorConfig);
bool IS_RC_MODE_ACTIVE(boxId_e boxId);
void rcModeUpdate(boxBitmask_t *newState);
bool isModeActivationConditionPresent(boxId_e modeId);
bool isUsingSticksForArming(void);
bool isUsingNavigationModes(void);
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void);
void updateUsedModeActivationConditionFlags(void);
void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);

1
src/main/flight/failsafe.c
View file

@ -37,6 +37,7 @@
#include "fc/fc_core.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/controlrate_profile.h"

1
src/main/flight/servos.c
View file

@ -41,6 +41,7 @@
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"

1
src/main/io/beeper.c
View file

@ -40,6 +40,7 @@
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "config/feature.h"

1
src/main/io/ledstrip.c
View file

@ -44,6 +44,7 @@
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"

1
src/main/navigation/navigation.c
View file

@ -37,6 +37,7 @@
#include "fc/fc_core.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"

1
src/main/rx/rx.c
View file

@ -42,6 +42,7 @@
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "flight/failsafe.h"

1
src/main/telemetry/telemetry.c
View file

@ -32,6 +32,7 @@
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "io/serial.h"