diff --git a/firmware/controllers/core/logic_expression.cpp b/firmware/controllers/core/logic_expression.cpp index fc4a3b47b8..edd847910e 100644 --- a/firmware/controllers/core/logic_expression.cpp +++ b/firmware/controllers/core/logic_expression.cpp @@ -31,23 +31,78 @@ LECalculator::LECalculator() { first = NULL; } +static bool float2bool(float v) { + return v != 0; +} + +void LECalculator::doJob(LEElement *element) { + switch (element->action) { + + case LE_NUMERIC_VALUE: + stack.push(element->fValue); + break; + case LE_OPERATOR_AND: { + float v1 = stack.pop(); + float v2 = stack.pop(); + + stack.push(float2bool(v1) && float2bool(v2)); + } + break; + case LE_OPERATOR_OR: { + float v1 = stack.pop(); + float v2 = stack.pop(); + + stack.push(float2bool(v1) || float2bool(v2)); + } + break; + case LE_OPERATOR_LESS: { + float v1 = stack.pop(); + float v2 = stack.pop(); + + stack.push(v1 < v2); + } + break; + case LE_OPERATOR_MORE: { + float v1 = stack.pop(); + float v2 = stack.pop(); + + stack.push(v1 > v2); + } + break; + case LE_OPERATOR_LESS_OR_EQUAL: { + float v1 = stack.pop(); + float v2 = stack.pop(); + + stack.push(v1 <= v2); + } + break; + case LE_OPERATOR_MORE_OR_EQUAL: { + float v1 = stack.pop(); + float v2 = stack.pop(); + + stack.push(v1 >= v2); + } + break; + default: + firmwareError("Not implemented: %d", element->action); + + } + +} + float LECalculator::getValue() { LEElement *element = first; stack.reset(); - while(element != NULL) { - - stack.push(element->fValue); - - + while (element != NULL) { + doJob(element); element = element->next; } return stack.pop(); } - void LECalculator::add(LEElement *element) { if (first == NULL) { first = element; diff --git a/firmware/controllers/core/logic_expression.h b/firmware/controllers/core/logic_expression.h index 226b00afd0..a3e34cd4a9 100644 --- a/firmware/controllers/core/logic_expression.h +++ b/firmware/controllers/core/logic_expression.h @@ -16,6 +16,8 @@ typedef enum { LE_NUMERIC_VALUE, LE_OPERATOR_LESS, LE_OPERATOR_MORE, + LE_OPERATOR_LESS_OR_EQUAL, + LE_OPERATOR_MORE_OR_EQUAL, LE_OPERATOR_AND, LE_OPERATOR_OR, @@ -47,6 +49,7 @@ public: LEElement *first; private: + void doJob(LEElement *element); FLStack stack; }; diff --git a/unit_tests/test_logic_expression.cpp b/unit_tests/test_logic_expression.cpp index 633e64207e..fadb55d3b6 100644 --- a/unit_tests/test_logic_expression.cpp +++ b/unit_tests/test_logic_expression.cpp @@ -30,6 +30,7 @@ void testLogicExpressions(void) { LEElement value3; value3.init(LE_OPERATOR_AND); c.add(&value3); + assertEqualsM("123 and 321", 1.0, c.getValue()); /** * fuel_pump = (time_since_boot < 4 seconds) OR (rpm > 0)