half-steering-basic-framework/modules/power_meter/power_meter.c

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