Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00
Code Issues Wiki Activity

Logic Conditions framework (#4144)

Browse source 
* CLI for servo mix conditions

* RC channel greater than on servo mixer

* RC channel value based conditions

* MSP2 frames

* Docs update

* mixer condition changed to logic condition and generalized

* basic logic framwork extarcted to separate file'

* minor fixes

* Processing refactoring

* Flight values added to conditions

* Use logic conditions only on > F3

* Make logic conditions a separate entity and link from servo mixer to logic condition

* empty task to periadically compute logic conditions

* Compute logic conditions as task

* Add flags

* CLI logic to configure logic conditions

* MSP frames to get and set logic conditions

* Disabled condition always yelds false

* fixes for F3

* Review changes

* final fix in MSP2_INAV_SERVO_MIXER
This commit is contained in:
Paweł Spychalski 2019-03-05 22:31:28 +01:00 • committed by GitHub
parent 2ea69cf1f8
commit c13a13b172
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
12 changed files with 604 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

1
make/source.mk
View file

@ -10,6 +10,7 @@ COMMON_SRC = \
common/crc.c \
common/encoding.c \
common/filter.c \
common/logic_condition.h \
common/maths.c \
common/calibration.c \
common/memory.c \

241
src/main/common/logic_condition.c Normal file
View file

@ -0,0 +1,241 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include "config/config_reset.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "common/logic_condition.h"
#include "rx/rx.h"
#include "maths.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "navigation/navigation.h"
#include "sensors/battery.h"
#include "sensors/pitotmeter.h"
#include "flight/imu.h"
PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0);
void pgResetFn_logicConditions(logicCondition_t *instance)
{
for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
RESET_CONFIG(logicCondition_t, &instance[i],
.enabled = 0,
.operation = LOGIC_CONDITION_TRUE,
.operandA = {
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
.value = 0
},
.operandB = {
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
.value = 0
},
.flags = 0
);
}
}
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
void logicConditionProcess(uint8_t i) {
if (logicConditions(i)->enabled) {
const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value);
const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value);
logicConditionStates[i].value = logicConditionCompute(logicConditions(i)->operation, operandAValue, operandBValue);
} else {
logicConditionStates[i].value = false;
}
}
int logicConditionCompute(
logicOperation_e operation,
int operandA,
int operandB
) {
switch (operation) {
case LOGIC_CONDITION_TRUE:
return true;
break;
case LOGIC_CONDITION_EQUAL:
return operandA == operandB;
break;
case LOGIC_CONDITION_GREATER_THAN:
return operandA > operandB;
break;
case LOGIC_CONDITION_LOWER_THAN:
return operandA < operandB;
break;
case LOGIC_CONDITION_LOW:
return operandA < 1333;
break;
case LOGIC_CONDITION_MID:
return operandA >= 1333 && operandA <= 1666;
break;
case LOGIC_CONDITION_HIGH:
return operandA > 1666;
break;
default:
return false;
break;
}
}
static int logicConditionGetFlightOperandValue(int operand) {
switch (operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
return constrain((uint32_t)getFlightTime(), 0, 32767);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
return constrain(GPS_distanceToHome, 0, 32767);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
return constrain(getTotalTravelDistance() / 100, 0, 32767);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 10
return getBatteryVoltage();
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10
return getBatteryAverageCellVoltage();
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
return getAmperage();
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
return getMAhDrawn();
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
return gpsSol.numSat;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s
return gpsSol.groundSpeed;
break;
//FIXME align with osdGet3DSpeed
case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s
return (int) sqrtf(sq(gpsSol.groundSpeed) + sq((int)getEstimatedActualVelocity(Z)));
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s
#ifdef USE_PITOT
return constrain(pitot.airSpeed, 0, 32767);
#else
return false;
#endif
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm
return constrain(getEstimatedActualPosition(Z), -32678, 32767);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
return constrain(getEstimatedActualVelocity(Z), 0, 32767);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %
return (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL: // deg
return constrain(attitude.values.roll / 10, -180, 180);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH: // deg
return constrain(attitude.values.pitch / 10, -180, 180);
break;
default:
return 0;
break;
}
}
int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
int retVal = 0;
switch (type) {
case LOGIC_CONDITION_OPERAND_TYPE_VALUE:
retVal = operand;
break;
case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL:
//Extract RC channel raw value
if (operand >= 1 && operand <= 16) {
retVal = rcData[operand - 1];
}
break;
case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT:
retVal = logicConditionGetFlightOperandValue(operand);
break;
default:
break;
}
return retVal;
}
/*
* ConditionId is ordered from 1 while conditions are indexed from 0
* conditionId == 0 is always evaluated at true
*/
int logicConditionGetValue(int8_t conditionId) {
if (conditionId >= 0) {
return logicConditionStates[conditionId].value;
} else {
return true;
}
}
void logicConditionUpdateTask(timeUs_t currentTimeUs) {
for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
logicConditionProcess(i);
}
}

104
src/main/common/logic_condition.h Normal file
View file

@ -0,0 +1,104 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include "config/parameter_group.h"
#include "common/time.h"
#define MAX_LOGIC_CONDITIONS 8
typedef enum {
LOGIC_CONDITION_TRUE = 0, // 0
LOGIC_CONDITION_EQUAL, // 1
LOGIC_CONDITION_GREATER_THAN, // 2
LOGIC_CONDITION_LOWER_THAN, // 3
LOGIC_CONDITION_LOW, // 4
LOGIC_CONDITION_MID, // 5
LOGIC_CONDITION_HIGH, // 6
LOGIC_CONDITION_LAST
} logicOperation_e;
typedef enum logicOperandType_s {
LOGIC_CONDITION_OPERAND_TYPE_VALUE = 0,
LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL,
LOGIC_CONDITION_OPERAND_TYPE_FLIGHT,
LOGIC_CONDITION_OPERAND_TYPE_LAST
} logicOperandType_e;
typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s
LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m
LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m
LOGIC_CONDITION_OPERAND_FLIGHT_RSSI,
LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10
LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10
LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100
LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS,
LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s
LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s
LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s
LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm
LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s
LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // %
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH, // deg
} logicFlightOperands_e;
typedef enum {
LOGIC_CONDITION_FLAG_LATCH = 1 << 0,
} logicConditionFlags_e;
typedef struct logicOperand_s {
logicOperandType_e type;
int32_t value;
} logicOperand_t;
typedef struct logicCondition_s {
uint8_t enabled;
logicOperation_e operation;
logicOperand_t operandA;
logicOperand_t operandB;
uint8_t flags;
} logicCondition_t;
PG_DECLARE_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions);
typedef struct logicConditionState_s {
int value;
uint8_t flags;
} logicConditionState_t;
void logicConditionProcess(uint8_t i);
int logicConditionCompute(
logicOperation_e operation,
int operandA,
int operandB
);
int logicConditionGetOperandValue(logicOperandType_e type, int operand);
int logicConditionGetValue(int8_t conditionId);
void logicConditionUpdateTask(timeUs_t currentTimeUs);

5
src/main/config/parameter_group_ids.h
View file

@ -76,7 +76,7 @@
#define PG_VTX_CONFIG 54
#define PG_ELERES_CONFIG 55
#define PG_TEMP_SENSOR_CONFIG 56
#define PG_CF_END 57
#define PG_CF_END 56
// Driver configuration
//#define PG_DRIVER_PWM_RX_CONFIG 100
@ -103,7 +103,8 @@
#define PG_DISPLAY_CONFIG 1013
#define PG_LIGHTS_CONFIG 1014
#define PG_PINIOBOX_CONFIG 1015
#define PG_INAV_END 1015
#define PG_LOGIC_CONDITIONS 1016
#define PG_INAV_END 1016
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

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

@ -1488,7 +1488,7 @@ static void cliServo(char *cmdline)
static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixers, const servoMixer_t *defaultCustomServoMixers)
{
const char *format = "smix %d %d %d %d %d";
const char *format = "smix %d %d %d %d %d %d";
for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) {
const servoMixer_t customServoMixer = customServoMixers[i];
if (customServoMixer.rate == 0) {
@ -1501,14 +1501,23 @@ static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixer
equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel
&& customServoMixer.inputSource == customServoMixerDefault.inputSource
&& customServoMixer.rate == customServoMixerDefault.rate
&& customServoMixer.speed == customServoMixerDefault.speed;
&& customServoMixer.speed == customServoMixerDefault.speed
#ifdef USE_LOGIC_CONDITIONS
&& customServoMixer.conditionId == customServoMixerDefault.conditionId
#endif
;
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
i,
customServoMixerDefault.targetChannel,
customServoMixerDefault.inputSource,
customServoMixerDefault.rate,
customServoMixerDefault.speed
customServoMixerDefault.speed,
#ifdef USE_LOGIC_CONDITIONS
customServoMixer.conditionId
#else
0
#endif
);
}
cliDumpPrintLinef(dumpMask, equalsDefault, format,
@ -1516,7 +1525,12 @@ static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixer
customServoMixer.targetChannel,
customServoMixer.inputSource,
customServoMixer.rate,
customServoMixer.speed
customServoMixer.speed,
#ifdef USE_LOGIC_CONDITIONS
customServoMixer.conditionId
#else
0
#endif
);
}
}
@ -1524,7 +1538,7 @@ static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixer
static void cliServoMix(char *cmdline)
{
char * saveptr;
int args[8], check = 0;
int args[6], check = 0;
uint8_t len = strlen(cmdline);
if (len == 0) {
@ -1533,7 +1547,7 @@ static void cliServoMix(char *cmdline)
// erase custom mixer
pgResetCopy(customServoMixersMutable(0), PG_SERVO_MIXER);
} else {
enum {RULE = 0, TARGET, INPUT, RATE, SPEED, ARGS_COUNT};
enum {RULE = 0, TARGET, INPUT, RATE, SPEED, CONDITION, ARGS_COUNT};
char *ptr = strtok_r(cmdline, " ", &saveptr);
while (ptr != NULL && check < ARGS_COUNT) {
args[check++] = fastA2I(ptr);
@ -1546,15 +1560,21 @@ static void cliServoMix(char *cmdline)
}
int32_t i = args[RULE];
if (i >= 0 && i < MAX_SERVO_RULES &&
if (
i >= 0 && i < MAX_SERVO_RULES &&
args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS &&
args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT &&
args[RATE] >= -1000 && args[RATE] <= 1000 &&
args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED) {
args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED &&
args[CONDITION] >= -1 && args[CONDITION] < MAX_LOGIC_CONDITIONS
) {
customServoMixersMutable(i)->targetChannel = args[TARGET];
customServoMixersMutable(i)->inputSource = args[INPUT];
customServoMixersMutable(i)->rate = args[RATE];
customServoMixersMutable(i)->speed = args[SPEED];
#ifdef USE_LOGIC_CONDITIONS
customServoMixersMutable(i)->conditionId = args[CONDITION];
#endif
cliServoMix("");
} else {
cliShowParseError();
@ -1562,6 +1582,110 @@ static void cliServoMix(char *cmdline)
}
}
#ifdef USE_LOGIC_CONDITIONS
static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions)
{
const char *format = "logic %d %d %d %d %d %d %d %d";
for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
const logicCondition_t logic = logicConditions[i];
bool equalsDefault = false;
if (defaultLogicConditions) {
logicCondition_t defaultValue = defaultLogicConditions[i];
equalsDefault =
logic.enabled == defaultValue.enabled &&
logic.operation == defaultValue.operation &&
logic.operandA.type == defaultValue.operandA.type &&
logic.operandA.value == defaultValue.operandA.value &&
logic.operandB.type == defaultValue.operandB.type &&
logic.operandB.value == defaultValue.operandB.value &&
logic.flags == defaultValue.flags;
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
i,
logic.enabled,
logic.operation,
logic.operandA.type,
logic.operandA.value,
logic.operandB.type,
logic.operandB.value,
logic.flags
);
}
cliDumpPrintLinef(dumpMask, equalsDefault, format,
i,
logic.enabled,
logic.operation,
logic.operandA.type,
logic.operandA.value,
logic.operandB.type,
logic.operandB.value,
logic.flags
);
}
}
static void cliLogic(char *cmdline) {
char * saveptr;
int args[8], check = 0;
uint8_t len = strlen(cmdline);
if (len == 0) {
printLogic(DUMP_MASTER, logicConditions(0), NULL);
} else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
pgResetCopy(logicConditionsMutable(0), PG_SERVO_MIXER);
} else {
enum {
INDEX = 0,
ENABLED,
OPERATION,
OPERAND_A_TYPE,
OPERAND_A_VALUE,
OPERAND_B_TYPE,
OPERAND_B_VALUE,
FLAGS,
ARGS_COUNT
};
char *ptr = strtok_r(cmdline, " ", &saveptr);
while (ptr != NULL && check < ARGS_COUNT) {
args[check++] = fastA2I(ptr);
ptr = strtok_r(NULL, " ", &saveptr);
}
if (ptr != NULL || check != ARGS_COUNT) {
cliShowParseError();
return;
}
int32_t i = args[INDEX];
if (
i >= 0 && i < MAX_LOGIC_CONDITIONS &&
args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST &&
args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 &&
args[OPERAND_B_TYPE] >= 0 && args[OPERAND_B_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
args[OPERAND_B_VALUE] >= -1000000 && args[OPERAND_B_VALUE] <= 1000000 &&
args[FLAGS] >= 0 && args[FLAGS] <= 255
) {
logicConditionsMutable(i)->enabled = args[ENABLED];
logicConditionsMutable(i)->operation = args[OPERATION];
logicConditionsMutable(i)->operandA.type = args[OPERAND_A_TYPE];
logicConditionsMutable(i)->operandA.value = args[OPERAND_A_VALUE];
logicConditionsMutable(i)->operandB.type = args[OPERAND_B_TYPE];
logicConditionsMutable(i)->operandB.value = args[OPERAND_B_VALUE];
logicConditionsMutable(i)->flags = args[FLAGS];
cliLogic("");
} else {
cliShowParseError();
}
}
}
#endif
#ifdef USE_SDCARD
static void cliWriteBytes(const uint8_t *buffer, int count)
@ -2725,6 +2849,11 @@ static void printConfig(const char *cmdline, bool doDiff)
cliPrintHashLine("servo");
printServo(dumpMask, servoParams_CopyArray, servoParams(0));
#ifdef USE_LOGIC_CONDITIONS
cliPrintHashLine("logic");
printLogic(dumpMask, logicConditions_CopyArray, logicConditions(0));
#endif
cliPrintHashLine("feature");
printFeature(dumpMask, &featureConfig_Copy, featureConfig());
@ -2913,9 +3042,14 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", "<id> [baud] [mode] : passthrough to serial", cliSerialPassthrough),
#endif
CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
#ifdef USE_LOGIC_CONDITIONS
CLI_COMMAND_DEF("logic", "configure logic conditions",
"<rule> <enabled> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>\r\n"
"\treset\r\n", cliLogic),
#endif
CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
CLI_COMMAND_DEF("smix", "servo mixer",
"<rule> <servo> <source> <rate> <speed>\r\n"
"<rule> <servo> <source> <rate> <speed> <conditionId>\r\n"
"\treset\r\n", cliServoMix),
#ifdef USE_SDCARD
CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),

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

@ -463,6 +463,32 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, 0);
}
break;
case MSP2_INAV_SERVO_MIXER:
for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, customServoMixers(i)->targetChannel);
sbufWriteU8(dst, customServoMixers(i)->inputSource);
sbufWriteU16(dst, customServoMixers(i)->rate);
sbufWriteU8(dst, customServoMixers(i)->speed);
#ifdef USE_LOGIC_CONDITIONS
sbufWriteU8(dst, customServoMixers(i)->conditionId);
#else
sbufWriteU8(dst, 0);
#endif
}
break;
#ifdef USE_LOGIC_CONDITIONS
case MSP2_INAV_LOGIC_CONDITIONS:
for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
sbufWriteU8(dst, logicConditions(i)->enabled);
sbufWriteU8(dst, logicConditions(i)->operation);
sbufWriteU8(dst, logicConditions(i)->operandA.type);
sbufWriteU32(dst, logicConditions(i)->operandA.value);
sbufWriteU8(dst, logicConditions(i)->operandB.type);
sbufWriteU32(dst, logicConditions(i)->operandB.value);
sbufWriteU8(dst, logicConditions(i)->flags);
}
break;
#endif
case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
sbufWriteU16(dst, customMotorMixer(i)->throttle * 1000);
@ -1824,6 +1850,37 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR;
break;
case MSP2_INAV_SET_SERVO_MIXER:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 7) && (tmp_u8 < MAX_SERVO_RULES)) {
customServoMixersMutable(tmp_u8)->targetChannel = sbufReadU8(src);
customServoMixersMutable(tmp_u8)->inputSource = sbufReadU8(src);
customServoMixersMutable(tmp_u8)->rate = sbufReadU16(src);
customServoMixersMutable(tmp_u8)->speed = sbufReadU8(src);
#ifdef USE_LOGIC_CONDITIONS
customServoMixersMutable(tmp_u8)->conditionId = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
loadCustomServoMixer();
} else
return MSP_RESULT_ERROR;
break;
#ifdef USE_LOGIC_CONDITIONS
case MSP2_INAV_SET_LOGIC_CONDITIONS:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 13) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) {
logicConditionsMutable(tmp_u8)->enabled = sbufReadU8(src);
logicConditionsMutable(tmp_u8)->operation = sbufReadU8(src);
logicConditionsMutable(tmp_u8)->operandA.type = sbufReadU8(src);
logicConditionsMutable(tmp_u8)->operandA.value = sbufReadU32(src);
logicConditionsMutable(tmp_u8)->operandB.type = sbufReadU8(src);
logicConditionsMutable(tmp_u8)->operandB.value = sbufReadU32(src);
logicConditionsMutable(tmp_u8)->flags = sbufReadU8(src);
} else
return MSP_RESULT_ERROR;
break;
#endif
case MSP2_COMMON_SET_MOTOR_MIXER:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) {

12
src/main/fc/fc_tasks.c
View file

@ -26,6 +26,7 @@
#include "common/axis.h"
#include "common/color.h"
#include "common/utils.h"
#include "common/logic_condition.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/compass/compass.h"
@ -337,6 +338,9 @@ void fcTasksInit(void)
#ifdef USE_RCDEVICE
setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
#endif
#ifdef USE_LOGIC_CONDITIONS
setTaskEnabled(TASK_LOGIC_CONDITIONS, true);
#endif
}
cfTask_t cfTasks[TASK_COUNT] = {
@ -542,4 +546,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef USE_LOGIC_CONDITIONS
[TASK_LOGIC_CONDITIONS] = {
.taskName = "LOGIC",
.taskFunc = logicConditionUpdateTask,
.desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
};

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

@ -62,7 +62,22 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
.tri_unarmed_servo = 1
);
PG_REGISTER_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 0);
PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1);
void pgResetFn_customServoMixers(servoMixer_t *instance)
{
for (int i = 0; i < MAX_SERVO_RULES; i++) {
RESET_CONFIG(servoMixer_t, &instance[i],
.targetChannel = 0,
.inputSource = 0,
.rate = 0,
.speed = 0
#ifdef USE_LOGIC_CONDITIONS
,.conditionId = -1
#endif
);
}
}
PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 2);
@ -276,6 +291,16 @@ void servoMixer(float dT)
// mix servos according to rules
for (int i = 0; i < servoRuleCount; i++) {
/*
* Check if conditions for a rule are met, not all conditions apply all the time
*/
#ifdef USE_LOGIC_CONDITIONS
if (!logicConditionGetValue(currentServoMixer[i].conditionId)) {
continue;
}
#endif
const uint8_t target = currentServoMixer[i].targetChannel;
const uint8_t from = currentServoMixer[i].inputSource;

4
src/main/flight/servos.h
View file

@ -18,6 +18,7 @@
#pragma once
#include "config/parameter_group.h"
#include "common/logic_condition.h"
#define MAX_SUPPORTED_SERVOS 8
@ -99,6 +100,9 @@ typedef struct servoMixer_s {
uint8_t inputSource; // input channel for this rule
int16_t rate; // range [-1000;+1000] ; can be used to adjust a rate 0-1000% and a direction
uint8_t speed; // reduces the speed of the rule, 0=unlimited speed
#ifdef USE_LOGIC_CONDITIONS
int8_t conditionId;
#endif
} servoMixer_t;
#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS)

5
src/main/msp/msp_protocol_v2_inav.h
View file

@ -52,3 +52,8 @@
#define MSP2_INAV_TEMP_SENSOR_CONFIG 0x201C
#define MSP2_INAV_SET_TEMP_SENSOR_CONFIG 0x201D
#define MSP2_INAV_TEMPERATURES 0x201E
#define MSP2_INAV_SERVO_MIXER 0x2020
#define MSP2_INAV_SET_SERVO_MIXER 0x2021
#define MSP2_INAV_LOGIC_CONDITIONS 0x2022
#define MSP2_INAV_SET_LOGIC_CONDITIONS 0x2023

4
src/main/scheduler/scheduler.h
View file

@ -110,7 +110,9 @@ typedef enum {
#ifdef USE_VTX_CONTROL
TASK_VTXCTRL,
#endif
#ifdef USE_LOGIC_CONDITIONS
TASK_LOGIC_CONDITIONS,
#endif
/* Count of real tasks */
TASK_COUNT,

4
src/main/target/common.h
View file

@ -145,6 +145,10 @@
#define USE_VTX_TRAMP
#define USE_VTX_FFPV
#ifndef STM32F3 //F3 series does not have enoug RAM to support logic conditions
#define USE_LOGIC_CONDITIONS
#endif
//Enable DST calculations
#define RTC_AUTOMATIC_DST
// Wind estimator