diff --git a/include/bluetooth/mesh/light_ctrl_reg.h b/include/bluetooth/mesh/light_ctrl_reg.h index eb0d05e543c3..7f549bb6e1f6 100644 --- a/include/bluetooth/mesh/light_ctrl_reg.h +++ b/include/bluetooth/mesh/light_ctrl_reg.h @@ -39,11 +39,22 @@ struct bt_mesh_light_ctrl_reg_cfg { /** Common regulator context */ struct bt_mesh_light_ctrl_reg { - /** Initialize the regulator. */ + /** Initialize the regulator. + * + * @param[in] reg Illuminance regulator instance. + */ void (*init)(struct bt_mesh_light_ctrl_reg *reg); - /** Start the regulator. */ - void (*start)(struct bt_mesh_light_ctrl_reg *reg); - /** Stop the regulator. */ + /** Start the regulator. + * + * @param[in] reg Illuminance regulator instance. + * @param[in] lightness Current lightness level + * value equal to @c lightness. + */ + void (*start)(struct bt_mesh_light_ctrl_reg *reg, uint16_t lightness); + /** Stop the regulator. + * + * @param[in] reg Illuminance regulator instance. + */ void (*stop)(struct bt_mesh_light_ctrl_reg *reg); /** Regulator configuration. */ struct bt_mesh_light_ctrl_reg_cfg cfg; diff --git a/include/bluetooth/mesh/light_ctrl_reg_spec.h b/include/bluetooth/mesh/light_ctrl_reg_spec.h index 37b263e86bbf..21789063d574 100644 --- a/include/bluetooth/mesh/light_ctrl_reg_spec.h +++ b/include/bluetooth/mesh/light_ctrl_reg_spec.h @@ -43,11 +43,13 @@ struct bt_mesh_light_ctrl_reg_spec { float i; /** Regulator enabled flag. */ bool enabled; + /* If true, internal integral sum can be negative until it becomes positive. */ + bool neg; }; /** @cond INTERNAL_HIDDEN */ void bt_mesh_light_ctrl_reg_spec_init(struct bt_mesh_light_ctrl_reg *reg); -void bt_mesh_light_ctrl_reg_spec_start(struct bt_mesh_light_ctrl_reg *reg); +void bt_mesh_light_ctrl_reg_spec_start(struct bt_mesh_light_ctrl_reg *reg, uint16_t lightness); void bt_mesh_light_ctrl_reg_spec_stop(struct bt_mesh_light_ctrl_reg *reg); /** @endcond */ diff --git a/include/bluetooth/mesh/light_ctrl_srv.h b/include/bluetooth/mesh/light_ctrl_srv.h index ad4889fa8e6c..bd2fc6fb0c41 100644 --- a/include/bluetooth/mesh/light_ctrl_srv.h +++ b/include/bluetooth/mesh/light_ctrl_srv.h @@ -186,7 +186,7 @@ struct bt_mesh_light_ctrl_srv { struct bt_mesh_light_ctrl_reg *reg; /** Previous regulator value */ uint16_t reg_prev; -#endif +#endif /* CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG */ /** Lightness server instance */ struct bt_mesh_lightness_srv *lightness; /** Extended Generic OnOff server */ diff --git a/subsys/bluetooth/mesh/light_ctrl_reg_spec.c b/subsys/bluetooth/mesh/light_ctrl_reg_spec.c index 1180b24cdf04..674d697baeb5 100644 --- a/subsys/bluetooth/mesh/light_ctrl_reg_spec.c +++ b/subsys/bluetooth/mesh/light_ctrl_reg_spec.c @@ -8,24 +8,19 @@ #define REG_INT CONFIG_BT_MESH_LIGHT_CTRL_REG_SPEC_INTERVAL -static void reg_step(struct k_work *work) -{ - struct k_work_delayable *dwork = k_work_delayable_from_work(work); - struct bt_mesh_light_ctrl_reg_spec *spec_reg = CONTAINER_OF( - dwork, struct bt_mesh_light_ctrl_reg_spec, timer); - - if (!spec_reg->enabled) { - /* The regulator might be disabled asynchronously. */ - return; - } - - k_work_reschedule(&spec_reg->timer, K_MSEC(REG_INT)); +struct reg_terms { + float i; + float p; +}; +static struct reg_terms reg_terms_calc(struct bt_mesh_light_ctrl_reg_spec *spec_reg) +{ float target = bt_mesh_light_ctrl_reg_target_get(&spec_reg->reg); float error = target - spec_reg->reg.measured; /* Accuracy should be in percent and both up and down: */ float accuracy = (spec_reg->reg.cfg.accuracy * target) / (2 * 100.0f); float input; + float kp, ki; if (error > accuracy) { input = error - accuracy; @@ -35,8 +30,6 @@ static void reg_step(struct k_work *work) input = 0.0f; } - float kp, ki; - if (input >= 0) { kp = spec_reg->reg.cfg.kp.up; ki = spec_reg->reg.cfg.ki.up; @@ -45,21 +38,64 @@ static void reg_step(struct k_work *work) ki = spec_reg->reg.cfg.ki.down; } - spec_reg->i += (input * ki) * ((float)REG_INT / (float)MSEC_PER_SEC); - spec_reg->i = CLAMP(spec_reg->i, 0, UINT16_MAX); + return (struct reg_terms){ + .i = ((input) * (ki) * ((float)REG_INT / (float)MSEC_PER_SEC)), + .p = input * kp, + }; +} + +static void reg_step(struct k_work *work) +{ + struct k_work_delayable *dwork = k_work_delayable_from_work(work); + struct bt_mesh_light_ctrl_reg_spec *spec_reg = CONTAINER_OF( + dwork, struct bt_mesh_light_ctrl_reg_spec, timer); + struct reg_terms reg_terms; + + if (!spec_reg->enabled) { + /* The regulator might be disabled asynchronously. */ + return; + } + + k_work_reschedule(&spec_reg->timer, K_MSEC(REG_INT)); + + reg_terms = reg_terms_calc(spec_reg); + spec_reg->i += reg_terms.i; + + if (spec_reg->i >= 0) { + /* Drop the negative flag as soon as the internal sum becomes positive. */ + spec_reg->neg = false; + } + + if (!spec_reg->neg) { + spec_reg->i = CLAMP(spec_reg->i, 0, UINT16_MAX); + } - float p = input * kp; - float output = spec_reg->i + p; + float output = spec_reg->i + reg_terms.p; spec_reg->reg.updated(&spec_reg->reg, output); } -void bt_mesh_light_ctrl_reg_spec_start(struct bt_mesh_light_ctrl_reg *reg) +static void internal_sum_recover(struct bt_mesh_light_ctrl_reg_spec *spec_reg, uint16_t lightness) +{ + struct reg_terms reg_terms; + + reg_terms = reg_terms_calc(spec_reg); + + /* Recalculate the internal sum so that it is equal to the passed lightness level at the + * next regulator step. + */ + spec_reg->i = lightness - reg_terms.i; + /* Allow the internal sum to be negative until it becomes positive. */ + spec_reg->neg = true; +} + +void bt_mesh_light_ctrl_reg_spec_start(struct bt_mesh_light_ctrl_reg *reg, uint16_t lightness) { struct bt_mesh_light_ctrl_reg_spec *spec_reg = CONTAINER_OF( reg, struct bt_mesh_light_ctrl_reg_spec, reg); spec_reg->enabled = true; k_work_schedule(&spec_reg->timer, K_MSEC(REG_INT)); + internal_sum_recover(spec_reg, lightness); } void bt_mesh_light_ctrl_reg_spec_stop(struct bt_mesh_light_ctrl_reg *reg) diff --git a/subsys/bluetooth/mesh/light_ctrl_srv.c b/subsys/bluetooth/mesh/light_ctrl_srv.c index 44f9aafc31dc..46f182989b25 100644 --- a/subsys/bluetooth/mesh/light_ctrl_srv.c +++ b/subsys/bluetooth/mesh/light_ctrl_srv.c @@ -56,25 +56,6 @@ static void restart_timer(struct bt_mesh_light_ctrl_srv *srv, uint32_t delay) k_work_reschedule(&srv->timer, K_MSEC(delay)); } -static void reg_start(struct bt_mesh_light_ctrl_srv *srv) -{ -#if CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG - if (srv->reg && srv->reg->start) { - srv->reg->start(srv->reg); - } -#endif -} - -static void reg_stop(struct bt_mesh_light_ctrl_srv *srv) -{ -#if CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG - if (srv->reg && srv->reg->stop) { - srv->reg->stop(srv->reg); - } -#endif -} - - static inline uint32_t to_centi_lux(const struct sensor_value *lux) { return lux->val1 * 100L + lux->val2 / 10000L; @@ -539,6 +520,24 @@ static void ctrl_enable(struct bt_mesh_light_ctrl_srv *srv) /* Regulator remains stopped until fresh LuxLevel is received. */ } +static void reg_start(struct bt_mesh_light_ctrl_srv *srv) +{ +#if CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG + if (srv->reg && srv->reg->start) { + srv->reg->start(srv->reg, to_linear(light_get(srv))); + } +#endif +} + +static void reg_stop(struct bt_mesh_light_ctrl_srv *srv) +{ +#if CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG + if (srv->reg && srv->reg->stop) { + srv->reg->stop(srv->reg); + } +#endif +} + static void ctrl_disable(struct bt_mesh_light_ctrl_srv *srv) { /* Do not change the light level, disabling the control server just @@ -589,7 +588,7 @@ static void timeout(struct k_work *work) * of the transition: */ if (atomic_test_and_clear_bit(&srv->flags, FLAG_TRANSITION)) { - LOG_DBG("Transition complete"); + LOG_DBG("Transition complete. State: %d", srv->state); /* If the fade wasn't instant, we've already published the * steady state in the state change function. diff --git a/tests/subsys/bluetooth/mesh/light_ctrl/src/main.c b/tests/subsys/bluetooth/mesh/light_ctrl/src/main.c index 2ae74a864650..5fd6f2c21939 100644 --- a/tests/subsys/bluetooth/mesh/light_ctrl/src/main.c +++ b/tests/subsys/bluetooth/mesh/light_ctrl/src/main.c @@ -516,7 +516,7 @@ ZTEST(light_ctrl_test, test_fsm_no_change_by_light_onoff) expect_ctrl_enable(); bt_mesh_light_ctrl_srv_enable(&light_ctrl_srv); /* Start regulator manually to allow the test to check operation */ - light_ctrl_srv.reg->start(light_ctrl_srv.reg); + light_ctrl_srv.reg->start(light_ctrl_srv.reg, 0); /* Wait for transition to completed. */ expected_flags = expected_flags & ~BIT(FLAG_TRANSITION); @@ -726,37 +726,46 @@ ZTEST(light_ctrl_pi_reg_test, test_pi_regulator_after_reset) trigger_pi_reg(14, true); expected_lightness = SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * 14 + light_ctrl_srv.reg->cfg.kp.up; + /* Decremt recalculated value at start of the regulator. */ + expected_lightness -= SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up); zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", expected_lightness, pi_reg_test_ctx.lightness); teardown_pi_reg(NULL); + light_ctrl_srv.reg->cfg.ki.up = 200; + light_ctrl_srv.reg->cfg.kp.up = 80; setup_pi_reg(NULL); - start_reg(0); /* Set different ambient light level than it was before the restart. The regulator should * calculate the output value with internal sum set to zero. */ - send_amb_lux_level(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON - - REG_ACCURACY(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON) - 2); + start_reg(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON - + REG_ACCURACY(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON) - 2); trigger_pi_reg(6, true); expected_lightness = (SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * 6 + light_ctrl_srv.reg->cfg.kp.up) * 2; + /* Decremt recalculated value at start of the regulator. */ + expected_lightness -= SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * 2; zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", expected_lightness, pi_reg_test_ctx.lightness); /* Test that reg.prev is also reset when the controller is disabled. */ teardown_pi_reg(NULL); + light_ctrl_srv.reg->cfg.ki.up = 200; + light_ctrl_srv.reg->cfg.kp.up = 80; setup_pi_reg(NULL); - start_reg(0); /* setup_pi_reg() starts the regulator with the zero ambient light level. The regulator * will drive the lightness high up. + * + * Because it is the first step after starting the regulator, only proportional part will + * be represented in the output value. */ + start_reg(0); trigger_pi_reg(1, true); float input = CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON - REG_ACCURACY(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON); - expected_lightness = SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up * input) + - input * light_ctrl_srv.reg->cfg.kp.up; + expected_lightness = input * light_ctrl_srv.reg->cfg.kp.up; zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", expected_lightness, pi_reg_test_ctx.lightness); @@ -961,8 +970,11 @@ ZTEST(light_ctrl_pi_reg_test, test_target_luxlevel_for_pi_regulator) start_reg(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON - REG_ACCURACY(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON) - 1); trigger_pi_reg(10, true); - expected_lightness = SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * 10 + + expected_lightness = light_ctrl_srv.cfg.light[LIGHT_CTRL_STATE_ON] + + SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * 10 + light_ctrl_srv.reg->cfg.kp.up; + /* Decremt recalculated value at start of the regulator. */ + expected_lightness -= SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up); zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", expected_lightness, pi_reg_test_ctx.lightness); @@ -1028,6 +1040,8 @@ ZTEST(light_ctrl_pi_reg_test, test_linear_output_state) trigger_pi_reg(10, true); expected_lightness = SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * 10 + light_ctrl_srv.reg->cfg.kp.up; + /* Decremt recalculated value at start of the regulator. */ + expected_lightness -= SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up); zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", expected_lightness, pi_reg_test_ctx.lightness); @@ -1059,6 +1073,8 @@ ZTEST(light_ctrl_pi_reg_test, test_amb_light_level_timeout) trigger_pi_reg(int_timeout, true); expected_lightness = SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * int_timeout + light_ctrl_srv.reg->cfg.kp.up; + /* Decremt recalculated value at start of the regulator. */ + expected_lightness -= SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up); zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", expected_lightness, pi_reg_test_ctx.lightness); @@ -1067,8 +1083,7 @@ ZTEST(light_ctrl_pi_reg_test, test_amb_light_level_timeout) /* Run regulator and stop right before the ambient light level timer refresh. */ trigger_pi_reg(int_timeout - 1, false); - expected_lightness = SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * int_timeout * 2 + - light_ctrl_srv.reg->cfg.kp.up; + expected_lightness += SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * int_timeout; zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", expected_lightness, pi_reg_test_ctx.lightness); zassert_not_equal(light_ctrl_srv.reg->measured, 0, "Ambient Light Level is reset"); @@ -1092,5 +1107,70 @@ ZTEST(light_ctrl_pi_reg_test, test_amb_light_level_timeout) expected_lightness, pi_reg_test_ctx.lightness); } +/** + * Test that the regulator correctly recalculates internal sum based on the provided lightness + * value. At the first step, the regulator output should contain the provided lightness value plus + * the proportional part. + */ +ZTEST(light_ctrl_pi_reg_test, test_internal_sum_recalculation) +{ + uint16_t expected_lightness; + float i; + + start_reg(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON - + REG_ACCURACY(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON) - 1); + trigger_pi_reg(1, true); + /* Since the reported ambient lux level is lower than expected, the regulator output will + * be the current On state fsm output value + the proportional part. + */ + expected_lightness = light_ctrl_srv.cfg.light[LIGHT_CTRL_STATE_ON]; + expected_lightness += light_ctrl_srv.reg->cfg.kp.up; + zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", + expected_lightness, pi_reg_test_ctx.lightness); + + /* Trigger the regulator one more time to add integral part to the output */ + trigger_pi_reg(1, true); + /* Since the reported ambient lux level is lower than expected, the regulator output will + * be the current On state fsm output value + one regulator step. + */ + expected_lightness += SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up); + zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", + expected_lightness, pi_reg_test_ctx.lightness); + + /* When input is 0, the regulator should output the previous internal sum because other + * operands (U * T * Ki and U * Kp) evaluates to zero. Therefore, the internal sum should be + * the previous regulator output without proportional part. + */ + send_amb_lux_level(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON); + trigger_pi_reg(1, true); + expected_lightness -= light_ctrl_srv.reg->cfg.kp.up; + zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", + expected_lightness, pi_reg_test_ctx.lightness); + + /* Set the ambient lux level higher than the expected, which results in `input` eq to -1. + * The internal sum should decrease. This will result in the lower regulator output, and + * the fsm output will beat the regulator output. + */ + send_amb_lux_level(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON + + REG_ACCURACY(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON) + 1); + trigger_pi_reg(1, true); + i = expected_lightness - (SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.down) * 1); + expected_lightness = light_ctrl_srv.cfg.light[LIGHT_CTRL_STATE_ON]; + zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", + expected_lightness, pi_reg_test_ctx.lightness); + + /* Set the ambient lux level lower than the expected, which results in `input` eq to 1. + * The internal sum should increment. Now the regulator output should be higher again than + * the fsm output. + */ + send_amb_lux_level(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON - + REG_ACCURACY(CONFIG_BT_MESH_LIGHT_CTRL_SRV_REG_LUX_ON) - 1); + expected_lightness = i + (SUMMATION_STEP(light_ctrl_srv.reg->cfg.ki.up) * 1 + + light_ctrl_srv.reg->cfg.kp.up); + trigger_pi_reg(1, true); + zassert_equal(pi_reg_test_ctx.lightness, expected_lightness, "Expected: %d, got: %d", + expected_lightness, pi_reg_test_ctx.lightness); +} + ZTEST_SUITE(light_ctrl_test, NULL, NULL, setup, teardown, NULL); ZTEST_SUITE(light_ctrl_pi_reg_test, NULL, NULL, setup_pi_reg, teardown_pi_reg, NULL);