simon
ee38ce551a
Filter BMA456 logs by configurable LSB threshold; master can set deadzone for local sensor or slaves using ACCEL_DEADZONE (UART) and ESP-NOW broadcast until unicast delivery is restored. Co-authored-by: Cursor <cursoragent@cursor.com>
125 lines
3.9 KiB
C
125 lines
3.9 KiB
C
#include "bosch456.h"
|
|
#include "client_registry.h"
|
|
#include "cmd_accel_deadzone.h"
|
|
#include "cmd_handler.h"
|
|
#include "esp_log.h"
|
|
#include "esp_now_comm.h"
|
|
#include "pb_decode.h"
|
|
#include "uart_messages.pb.h"
|
|
#include "uart_proto.h"
|
|
#include <string.h>
|
|
|
|
static const char *TAG = "[ACCEL_DZ]";
|
|
|
|
static void send_response(uint32_t deadzone, uint32_t client_id, bool success,
|
|
uint32_t slaves_updated) {
|
|
alox_UartMessage response = alox_UartMessage_init_zero;
|
|
response.type = alox_MessageType_ACCEL_DEADZONE;
|
|
response.which_payload = alox_UartMessage_accel_deadzone_response_tag;
|
|
response.payload.accel_deadzone_response.deadzone = deadzone;
|
|
response.payload.accel_deadzone_response.client_id = client_id;
|
|
response.payload.accel_deadzone_response.success = success;
|
|
response.payload.accel_deadzone_response.slaves_updated = slaves_updated;
|
|
|
|
if (uart_send_uart_message(&response) != ESP_OK) {
|
|
ESP_LOGE(TAG, "failed to send response");
|
|
}
|
|
}
|
|
|
|
static esp_err_t push_deadzone_to_slave(const client_info_t *client,
|
|
uint32_t deadzone) {
|
|
if (client == NULL) {
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
esp_err_t err =
|
|
client_registry_set_accel_deadzone(client->id, deadzone);
|
|
if (err != ESP_OK) {
|
|
return err;
|
|
}
|
|
|
|
return esp_now_comm_send_accel_deadzone(client->id, deadzone);
|
|
}
|
|
|
|
static void handle_accel_deadzone(const uint8_t *data, size_t len) {
|
|
alox_UartMessage uart_msg = alox_UartMessage_init_zero;
|
|
alox_AccelDeadzoneRequest req = alox_AccelDeadzoneRequest_init_zero;
|
|
bool have_request = false;
|
|
|
|
if (len > 0) {
|
|
pb_istream_t stream = pb_istream_from_buffer(data, len);
|
|
if (!pb_decode(&stream, alox_UartMessage_fields, &uart_msg)) {
|
|
ESP_LOGW(TAG, "decode failed");
|
|
send_response(BMA456_DEFAULT_ACCEL_DEADZONE, 0, false, 0);
|
|
return;
|
|
}
|
|
if (uart_msg.which_payload ==
|
|
alox_UartMessage_accel_deadzone_request_tag) {
|
|
req = uart_msg.payload.accel_deadzone_request;
|
|
have_request = true;
|
|
}
|
|
}
|
|
|
|
if (!have_request) {
|
|
req.write = false;
|
|
req.client_id = 0;
|
|
req.all_clients = false;
|
|
}
|
|
|
|
if (req.write) {
|
|
if (req.all_clients) {
|
|
size_t n = client_registry_set_accel_deadzone_all(req.deadzone);
|
|
esp_err_t send_err = esp_now_comm_send_accel_deadzone(0, req.deadzone);
|
|
|
|
if (bma456_is_ready()) {
|
|
bma456_set_accel_deadzone(req.deadzone);
|
|
}
|
|
|
|
ESP_LOGI(TAG, "set deadzone %lu broadcast (registry %u)",
|
|
(unsigned long)req.deadzone, (unsigned)n);
|
|
send_response(req.deadzone, 0, send_err == ESP_OK || bma456_is_ready(),
|
|
send_err == ESP_OK ? (uint32_t)n : 0);
|
|
return;
|
|
}
|
|
|
|
if (req.client_id == 0) {
|
|
bma456_set_accel_deadzone(req.deadzone);
|
|
ESP_LOGI(TAG, "set local deadzone %lu (no ESP-NOW; use -client or -all "
|
|
"for slaves)",
|
|
(unsigned long)req.deadzone);
|
|
send_response(req.deadzone, 0, true, 0);
|
|
return;
|
|
}
|
|
|
|
const client_info_t *client = client_registry_find_by_id(req.client_id);
|
|
if (client == NULL) {
|
|
ESP_LOGW(TAG, "client id %lu not found",
|
|
(unsigned long)req.client_id);
|
|
send_response(req.deadzone, req.client_id, false, 0);
|
|
return;
|
|
}
|
|
|
|
esp_err_t err = push_deadzone_to_slave(client, req.deadzone);
|
|
send_response(req.deadzone, req.client_id, err == ESP_OK, err == ESP_OK);
|
|
return;
|
|
}
|
|
|
|
/* Read */
|
|
if (req.all_clients || req.client_id == 0) {
|
|
uint32_t dz = bma456_get_accel_deadzone();
|
|
send_response(dz, 0, true, 0);
|
|
return;
|
|
}
|
|
|
|
uint32_t dz = 0;
|
|
esp_err_t err = client_registry_get_accel_deadzone(req.client_id, &dz);
|
|
send_response(dz, req.client_id, err == ESP_OK, 0);
|
|
}
|
|
|
|
void cmd_accel_deadzone_register(void) {
|
|
if (msg_register_handler(alox_MessageType_ACCEL_DEADZONE,
|
|
handle_accel_deadzone) != ESP_OK) {
|
|
ESP_LOGE(TAG, "register failed");
|
|
}
|
|
}
|