Bluetooth: Mesh: Recalculate regulator internal sum

Recalculate internal sum of the regulator to mitigate rapid change of
the regulator output when lightness value is non zero.
Add ramp up time to the pi regulator to artificially delay the
regulator.

Signed-off-by: Pavel Vasilyev <pavel.vasilyev@nordicsemi.no>

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;

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 */
 

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 */

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)

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.

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);