半舵旗舰版（功率控制有更改）

modules/power_meter/power_meter.c

@@ -0,0 +1,105 @@
+#include "power_meter.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "bsp_dwt.h"
+
+static void PowerMeterLostCallback(void *meter_ptr)
+{
+    PowerMeterInstance *meter = (PowerMeterInstance *)meter_ptr;
+    if (meter != 0) {
+        meter->online = 0u;
+    }
+}
+
+static void PowerMeterRxCallback(CANInstance *can)
+{
+    if (can == 0 || can->id == 0 || can->rx_len < 4u) {
+        return;
+    }
+
+    PowerMeterInstance *meter = (PowerMeterInstance *)can->id;
+    const uint8_t *data = can->rx_buff;
+    const float scale = meter->scale > 0.0f ? meter->scale : POWER_METER_DEFAULT_SCALE;
+
+    meter->raw_current = (uint16_t)(((uint16_t)data[1] << 8) | data[0]);
+    meter->raw_voltage = (uint16_t)(((uint16_t)data[3] << 8) | data[2]);
+    meter->current = (float)meter->raw_current / scale;
+    meter->voltage = (float)meter->raw_voltage / scale;
+    meter->power = meter->voltage * meter->current;
+    meter->update_count++;
+    meter->last_update_time_ms = DWT_GetTimeline_ms();
+    meter->online = 1u;
+
+    if (meter->daemon != 0) {
+        DaemonReload(meter->daemon);
+    }
+}
+
+PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *config)
+{
+    if (config == 0 || config->can_config.can_handle == 0) {
+        return 0;
+    }
+
+    PowerMeterInstance *meter = (PowerMeterInstance *)malloc(sizeof(PowerMeterInstance));
+    if (meter == 0) {
+        return 0;
+    }
+    memset(meter, 0, sizeof(PowerMeterInstance));
+
+    meter->scale = config->scale > 0.0f ? config->scale : POWER_METER_DEFAULT_SCALE;
+    if (config->can_config.rx_id == 0u) {
+        config->can_config.rx_id = POWER_METER_DEFAULT_CAN_ID;
+    }
+    if (config->can_config.tx_id == 0u) {
+        config->can_config.tx_id = config->can_config.rx_id;
+    }
+
+    config->can_config.id = meter;
+    config->can_config.can_module_callback = PowerMeterRxCallback;
+    meter->can_ins = CANRegister(&config->can_config);
+
+    Daemon_Init_Config_s daemon_config = {
+        .callback = PowerMeterLostCallback,
+        .owner_id = meter,
+        .reload_count = 5u,
+        .init_count = 100u,
+    };
+    meter->daemon = DaemonRegister(&daemon_config);
+
+    return meter;
+}
+
+PowerMeterInstance *PowerMeterInitCan(CAN_HandleTypeDef *can_handle, uint32_t rx_id)
+{
+    PowerMeter_Init_Config_s config = {
+        .can_config = {
+            .can_handle = can_handle,
+            .tx_id = rx_id,
+            .rx_id = rx_id,
+        },
+        .scale = POWER_METER_DEFAULT_SCALE,
+    };
+
+    return PowerMeterInit(&config);
+}
+
+const PowerMeterInstance *PowerMeterGetData(PowerMeterInstance *meter)
+{
+    if (meter == 0) {
+        return 0;
+    }
+
+    meter->online = PowerMeterIsOnline(meter);
+    return meter;
+}
+
+uint8_t PowerMeterIsOnline(PowerMeterInstance *meter)
+{
+    if (meter == 0 || meter->daemon == 0) {
+        return 0u;
+    }
+    return DaemonIsOnline(meter->daemon);
+}