AG_WEIGHT/software/soft_can.c

#include "soft_can.h"
#include "rkfifo.h"
#include "chipflash.h"
#include "math.h"
#include "init.h"
#include "string.h"
#include "my_math.h"
#include "timer.h"

#define MAX_FLITER 2
#define MAX_FRAME_DATA 8
#define CAN_TX_FIFO_SIZE 4
#define CAN_RX_FIFO_SIZE 4

typedef struct
{
    CAN_TxHeaderTypeDef Txheader;
    uint8_t Txdata[MAX_FRAME_DATA];
} Can_Tx;

typedef struct
{
    CAN_RxHeaderTypeDef Rxheader;
    uint8_t Rxdata[MAX_FRAME_DATA];
} Can_Rx;

static ACK_STATUS ack_status = no_ack;

static CAN_FilterTypeDef hCAN_Filter[MAX_FLITER] = {0};

static Can_Tx can_tx_fifo_buff[CAN_TX_FIFO_SIZE] = {0};
static Can_Rx can_rx_fifo_buff[CAN_RX_FIFO_SIZE] = {0};

static rkfifo_t can_tx_kfifo = {0};
static rkfifo_t can_rx_kfifo = {0};

static Can_Tx _can_tx = {0};

static Can_Rx _can_rx = {0};

FORMAT_CAN2PMU *Get_Tx_Msg(void)
{
    return (FORMAT_CAN2PMU *)&_can_tx.Txdata;
}

ACK_STATUS *Get_Ack_Status(void)
{
    return &ack_status;
}

void Set_Ack_Status(ACK_STATUS status)
{
    ack_status = status;
}

static void Can_Fifo_Init(void)
{
    rkfifo_init(&can_tx_kfifo, can_tx_fifo_buff, sizeof(can_tx_fifo_buff), sizeof(Can_Tx));
    rkfifo_init(&can_rx_kfifo, can_rx_fifo_buff, sizeof(can_rx_fifo_buff), sizeof(Can_Rx));
}

static void Set_ExtId(uint32_t ExtId)
{
    _can_tx.Txheader.ExtId = ExtId;
}

static void Filter_Init(uint8_t index, uint32_t FirstId, uint32_t SecondId)
{
    CAN_FilterTypeDef *pfilter = &hCAN_Filter[index];
    pfilter->FilterIdHigh = ((FirstId << 3) & HIGH_MASK) >> 16;
    pfilter->FilterIdLow = ((FirstId << 3) | CAN_ID_EXT | CAN_RTR_DATA) & LOW_MASK;
    pfilter->FilterMaskIdHigh = ((SecondId << 3) & HIGH_MASK) >> 16;
    pfilter->FilterMaskIdLow = ((SecondId << 3) | CAN_ID_EXT | CAN_RTR_DATA) & LOW_MASK;
    pfilter->FilterFIFOAssignment = CAN_FILTER_FIFO0;
    pfilter->FilterBank = index;
    pfilter->FilterMode = CAN_FILTERMODE_IDLIST;
    pfilter->FilterScale = CAN_FILTERSCALE_32BIT;
    pfilter->FilterActivation = ENABLE;
    pfilter->SlaveStartFilterBank = index;
    HAL_CAN_ConfigFilter(&hcan, pfilter);
}

void CAN_Init(uint32_t ExtId)
{
    CAN_TxHeaderTypeDef *p_txhead = &_can_tx.Txheader;

    Can_Fifo_Init();

    p_txhead->IDE = CAN_ID_EXT;
    p_txhead->RTR = CAN_RTR_DATA;
    p_txhead->TransmitGlobalTime = ENABLE;
    p_txhead->DLC = sizeof(_can_tx.Txdata);
    Filter_Init(0, CANRX_ID, CAN_REQ_UPGRADE_ID);
    Set_ExtId(ExtId);
    HAL_CAN_Start(&hcan);
    HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn);

    __HAL_CAN_ENABLE_IT(&hcan, CAN_IT_ERROR | CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE | CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE);
    __HAL_CAN_ENABLE_IT(&hcan, CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_TX_MAILBOX_EMPTY);
    HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING);
    HAL_CAN_ActivateNotification(&hcan, CAN_IT_TX_MAILBOX_EMPTY);
}

void Can_Tx_Msg(Can_Tx *TxMsg)
{
    uint32_t Tx_MailBox;

    if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan) != 0)
    {
        if (HAL_CAN_AddTxMessage(&hcan, &TxMsg->Txheader, TxMsg->Txdata, &Tx_MailBox) != HAL_OK)
        {
            rkfifo_in(&can_tx_kfifo, &_can_tx, 1);
        }
    }
    else
    {
        rkfifo_in(&can_tx_kfifo, &_can_tx, 1);
    }
}

void CAN1_SCE_IRQHandler()
{
    if (__HAL_CAN_GET_FLAG(&hcan, CAN_FLAG_BOF) != RESET)
    {
        __HAL_CAN_CLEAR_FLAG(&hcan, CAN_FLAG_ERRI);
    }
    if (__HAL_CAN_GET_FLAG(&hcan, CAN_FLAG_EPV) != RESET)
    {
        __HAL_CAN_CLEAR_FLAG(&hcan, CAN_FLAG_ERRI);
    }
    if (__HAL_CAN_GET_FLAG(&hcan, CAN_FLAG_EWG) != RESET)
    {
        __HAL_CAN_CLEAR_FLAG(&hcan, CAN_FLAG_ERRI);
    }
    __HAL_CAN_CLEAR_FLAG(&hcan, CAN_FLAG_ERRI);
}

void Write_To_Flash(uint32_t first_flash, uint32_t end_flash, uint8_t *data, uint8_t size)
{
    flash_ops->Erase_Flash(first_flash, end_flash);
    flash_ops->Write_Flash(first_flash, data, size);
}

static void Write_Check(void)
{
    uint32_t tmp = Value_Check;
    Write_To_Flash(VALUE_FLASH_Check_Addr, VALUE_FLASH_Check_End, (uint8_t *)&tmp, sizeof(tmp));
}

// 擦除配置
static void Restore_Factory_Setting(void)
{
    for (uint8_t i = 0; i < 12; i++)
    {
        flash_ops->Erase_Flash(CAN_FLASH_BASE + ONE_PARAM_SIZE * i, CAN_FLASH_BASE + ONE_PARAM_SIZE * (i + 1));
    }
}

static void Save_GrossWeight(WEIGHING_DEVICE *_device)
{
    uint8_t sensor_num_c = 0;
    Write_Check();
    flash_ops->Erase_Flash(GROSSWEIGHT1_FLASH_BASE, GROSSWEIGHT1_FLASH_END + ONE_PARAM_SIZE);
    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == _device->sensor_num_mask)
        {
            continue;
        }
        struct SENSOR *sensor = _device->sensor[sensor_num_c];
        flash_ops->Write_Flash(GROSSWEIGHT1_FLASH_BASE + sensor_num_c * ONE_WORD_SIZE, (uint8_t *)&sensor->GrossWeight, sizeof(sensor->GrossWeight));
    }
}

static float Get_Single_Weight(struct SENSOR *sensor, uint32_t buf)
{
    return buf * sensor->Scale;
}

static bool Scale_Is_Nan(WEIGHING_DEVICE *_device)
{
    for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == _device->sensor_num_mask)
        {
            continue;
        }
        if (isnan(_device->sensor[sensor_num_c]->Scale) == 1)
        {
            return true;
        }
    }
    return false;
}

static void K_Value_Init(WEIGHING_DEVICE *_device)
{
    for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == _device->sensor_num_mask)
        {
            continue;
        }
        _device->sensor[sensor_num_c]->K.f = 112.0f;
    }
}

/*校准*/
static bool Auto_Calib(WEIGHING_DEVICE *_device, uint32_t buf)
{
    WEIGHING_DEVICE *device = Get_Device_Handle();
    //采样10次取平均
    uint32_t sum[4] = {0}, data[4] = {0}, err_count = 0;

    for(uint8_t i = 0; i < 10; i++)
    {
        for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
        {
            uint32_t temp = 0;
            if (sensor_num_c == device->sensor_num_mask)
            {
                continue;
            }
            temp = Read_Value(device->sensor[sensor_num_c]);

            while (temp== 0)
            {
                temp = Read_Value(device->sensor[sensor_num_c]);
                err_count++;
                if (err_count > 10 /*&& device->sensor_num_mask == 4*/)
                {
                    device->sensor[sensor_num_c]->err_flag = true;
                    device->sensor_num_mask = sensor_num_c;
                    err_count = 0;
                    //test  去皮失败
                    return 0;
                }
            }
            sum[sensor_num_c] += temp;

            if(i == 9)
                data[sensor_num_c] = sum[sensor_num_c] / 10;

            HAL_Delay(13);
        }

    }
    
    Filter_Value(data);

    float allot = 0.f;

    float buff[SENSOR_NUM] = {0.f};
    //计算总重量
    for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        buff[sensor_num_c] = device->sensor[sensor_num_c]->Raw_Value;

        allot += buff[sensor_num_c];
    }
    //计算每个传感器重量占比
    for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        device->sensor[sensor_num_c]->Scale = buff[sensor_num_c] / allot;
    }

    float sum_baseK_weight = 0.0f;
    //计算测量K
    if (!Scale_Is_Nan(_device))
    {
        for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
        {
            if (sensor_num_c == _device->sensor_num_mask)
            {
                continue;
            }
            struct SENSOR *sensor = _device->sensor[sensor_num_c];
            //float tmpfloat = (sensor->Real_Variation / Get_Single_Weight(sensor, buf));

            if (sensor->Real_Variation < 0)
            {
                sensor->Real_Variation = fabsf(sensor->Real_Variation);
            }

            sum_baseK_weight += sensor->Real_Variation / sensor-> base_k ;
            //if (tmpfloat <= MAX_K && tmpfloat >= MIN_K)
            {
                //sensor->K.f = tmpfloat;
            }
            // else //test
            // {
            //     return false;
            // }
            
        }
        _device->correct_k = sum_baseK_weight / buf;
        Write_To_Flash(K1_VALUE_FLASH_BASE, K1_VALUE_FLASH_END, (uint8_t *)&_device->correct_k, sizeof(_device->correct_k));
        
        return true;
    }
    return false;
}

/*获取E2指令的K值*/
static float Get_K(FORMAT_RXMSG *msg)
{
    uint16_t tmpshort = 0;
    float tmpfloat = 0.0f;
    memcpy(&tmpshort, &msg->Data, sizeof(tmpshort));

    tmpfloat = tmpshort;

    if(tmpshort > 1000)
    {
        tmpfloat = tmpfloat /  10.0f;
    }
    if (tmpfloat <= MAX_K && tmpfloat >= MIN_K)
    {
        return (float)tmpfloat;
    }
    return 112.0f;
}

/*设置E2指令的K值并写入FLASH默认配置,可用到F2指令*/
static void Save_K(FORMAT_RXMSG *msg, uint8_t sensor)
{
    WEIGHING_DEVICE *device = Get_Device_Handle();

    struct SENSOR *Sensor_tmp = device->sensor[sensor];
    uint32_t tmpint = 0;
    memcpy(&tmpint, msg->Data, sizeof(tmpint));
    Write_Check(); // 写入检验位

    /*E2指令才获取K值，F2自动校准使用校准后的K值*/
    if (msg->Code == 0xE2)
    {
        switch (tmpint)
        {
        case 0x1: //重量标定
            Sensor_tmp->base_k = Sensor_tmp->Real_Variation / 1000;
            if (Sensor_tmp->base_k >= MAX_K || Sensor_tmp->base_k < MIN_K)
            {
                Sensor_tmp->base_k = 75.0f;
            }
            Write_To_Flash(SENSOR1_INIT_VALUE_BASE + sensor * ONE_PARAM_SIZE, SENSOR1_INIT_VALUE_END + sensor * ONE_PARAM_SIZE, (uint8_t *)&Sensor_tmp->base_k, sizeof(Sensor_tmp->base_k));
            break;
        case 0x3://指定抛弃第N个传感器
            /*指定弃置传感器的话不能改变传感器数量，使用mask作为标志位屏蔽某个传感器*/
            device->sensor_num_mask = sensor;
            flash_ops->Erase_Flash(SENSOR_NUM_RECORD_BASE, SENSOR_NUM_RECORD_END);
            Sensor_tmp->err_flag = false;
            flash_ops->Write_Flash(SENSOR_NUM_RECORD_BASE, (uint8_t *)&device->sensor_num_mask, sizeof(device->sensor_num_mask));
            return;
        case 0x4://恢复使用4个传感器·
            // param->sensor_num = SENSOR_NUM;
            flash_ops->Erase_Flash(SENSOR_NUM_RECORD_BASE, SENSOR_NUM_RECORD_END);
            device->sensor_num_mask = SENSOR_NUM;
            flash_ops->Write_Flash(SENSOR_NUM_RECORD_BASE, (uint8_t *)&device->sensor_num_mask, sizeof(device->sensor_num_mask));
            break;
        case 0x29A:
            if (sensor == 3)
            {
                Restore_Factory_Setting();
                NVIC_SystemReset();
            }
            break;
        default:
            Sensor_tmp->base_k = Get_K(msg);
            Write_To_Flash(SENSOR1_INIT_VALUE_BASE + sensor * ONE_PARAM_SIZE, SENSOR1_INIT_VALUE_END + sensor * ONE_PARAM_SIZE, (uint8_t *)&Sensor_tmp->base_k, sizeof(Sensor_tmp->base_k));
            break;
        }
    }

    //Write_To_Flash(SENSOR1_INIT_VALUE_BASE + sensor * ONE_PARAM_SIZE, SENSOR1_INIT_VALUE_END + sensor * ONE_PARAM_SIZE, (uint8_t *)&Sensor_tmp->base_k, sizeof(Sensor_tmp->base_k));
}

/*去皮重，重设零点*/
static bool Peeling_Calib(void)
{
    WEIGHING_DEVICE *device = Get_Device_Handle();

    //采样10次取平均
    uint32_t sum[4] = {0}, data[4] = {0}, err_count = 0;

    for(uint8_t i = 0; i < 10; i++)
    {
        
        for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
        {
            uint32_t temp = 0;
            if (sensor_num_c == device->sensor_num_mask)
            {
                continue;
            }
            temp = Read_Value(device->sensor[sensor_num_c]);

            while (temp== 0)
            {
                temp = Read_Value(device->sensor[sensor_num_c]);
                err_count++;
                if (err_count > 10 /*&& device->sensor_num_mask == 4*/)
                {
                    device->sensor[sensor_num_c]->err_flag = true;
                    device->sensor_num_mask = sensor_num_c;
                    err_count = 0;
                    //test  去皮失败
                    return 0;
                }
            }
            sum[sensor_num_c] += temp;

            if(i == 9)
                 data[sensor_num_c] = sum[sensor_num_c] / 10;
            HAL_Delay(13);
        }
    }
    
    Filter_Value(data);

    for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        device->sensor[sensor_num_c]->GrossWeight = device->sensor[sensor_num_c]->Raw_Value;
    }

    Save_GrossWeight(device);

    return 1;
}

/*称重校准*/

static void Weight_Calib(FORMAT_RXMSG *msg)
{
    uint32_t tmpint;
    uint8_t sensor_num_c;
    WEIGHING_DEVICE *device = Get_Device_Handle();

    memcpy(&tmpint, msg->Data, sizeof(tmpint));

    if (false == Auto_Calib(device, tmpint))
    {
        K_Value_Init(device);
    }

    Write_Check();

    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        Save_K(msg, sensor_num_c); // 写入FLASH	默认配置
    }
}

/*改变CAN通信波特率*/
static void Change_CAN_Baud(FORMAT_RXMSG *msg)
{
    uint16_t baudflag;

    memcpy(&baudflag, msg->Data, sizeof(baudflag));

    if (baudflag != 0x1F4 && baudflag != 0x3E8)
    {
        return;
    }

    Write_To_Flash(CAN_FLASH_BASE, CAN_FLASH_END, (uint8_t *)&baudflag, sizeof(baudflag));

    HAL_CAN_DeInit(&hcan);

    if (baudflag == 0x1F4)
    {
        MX_CAN_Init(8); // 500K
        CAN_Init(CANTX_ID);
    }
    else
    {
        MX_CAN_Init(4); // 1000K
        CAN_Init(CANTX_ID);
    }
}

/*写完flag重启*/
static void Soft_Reset(void)
{
    uint8_t buf[8] = {0xFE, 0xFD, 0xFC, 0xFB, 0xFA, 0xF9, 0xF8, 0xF7};

    Write_To_Flash(RESET_FLASH_BASE, RESET_FLASH_END, buf, sizeof(buf));

    __set_FAULTMASK(1); // 软重启，主函数做了读取RCC_FLAG_SFTRST发送应答报文
    NVIC_SystemReset(); // 加上bootloader后读取标志位方法不可用，改为读写片内flash();
}

static void Reset_Device_Param(void)
{
    Restore_Factory_Setting();
    Soft_Reset();
}
uint8_t k_set_num_test = 0;
bool factory_calibration = false;
/*解析PMU消息*/
void Can_Rx_Decode(void)
{
    Can_Rx *parse = &_can_rx;

    while (rkfifo_out(&can_rx_kfifo, parse, 1) == 1)
    {
        FORMAT_RXMSG *msg = (FORMAT_RXMSG *)parse->Rxdata;
        if (parse->Rxheader.ExtId == CAN_REQ_UPGRADE_ID && msg->Code == WS_ID) // 跳BOOTLOADER并将APP大小写入FLASH
        {
            buf2int((int *)&ws_info._app_bin_size, (unsigned char *)msg->Data);
            Write_Upgrade_Flag();
            __set_FAULTMASK(1);
            NVIC_SystemReset();
        }

        switch (parse->Rxdata[0])
        {
        case 0xF1: // 去皮校准
            if(Peeling_Calib() == 1)
                ack_status = ack_f1;
            else
                ack_status = no_ack;
            break;
        case 0xF2: // 重量校准
            Weight_Calib(msg);
            ack_status = ack_f2;
            break;
        case 0xF3: // 更改波特率
            Change_CAN_Baud(msg);
            break;
        case 0xF4: // 恢复出厂设置
        {
            WEIGHING_DEVICE *device = Get_Device_Handle();
        //Reset_Device_Param();
            device->correct_k = 1;
            Write_To_Flash(K1_VALUE_FLASH_BASE, K1_VALUE_FLASH_END, (uint8_t *)&device->correct_k, sizeof(device->correct_k));
            ack_status = ack_f4;
            break;
        }
        case 0xF6: // 软件版本
            ack_status = ack_f6;
            break;
        case 0xE1: // 查询K值
            ack_status = ack_e1;
            break;
        case 0xE2: // 设置K值
            Save_K(msg, msg->Num);
            ack_status = ack_e2;
            k_set_num_test = msg->Num;
            Can_Ack_Pmu();
            ack_status = ack_e1;

            break;
        case 0xF5:
            break;
        case 0xF7:
            break;
        case 0xE3:
            break;
        case 0xE4:
            break;
        case 0xE5:
            break;
        case 0xE6:
            break;
        case 0xE7://出厂K标定
        {
            WEIGHING_DEVICE *device = Get_Device_Handle();
            
            for(uint8_t i = 0; i < 4; i++)
            {
                struct SENSOR *Sensor_tmp = device->sensor[i];
                Sensor_tmp->base_k = Sensor_tmp->Real_Variation / 1000;
                if (Sensor_tmp->base_k >= MAX_K || Sensor_tmp->base_k < MIN_K)
                {
                    Sensor_tmp->base_k = 76.0f;
                }
                Write_To_Flash(SENSOR1_INIT_VALUE_BASE + i * ONE_PARAM_SIZE, SENSOR1_INIT_VALUE_END + i * ONE_PARAM_SIZE, (uint8_t *)&Sensor_tmp->base_k, sizeof(Sensor_tmp->base_k));
            }
            ack_status = ack_e7;
            factory_calibration = true;
            break;
        }
        default:
            break;
        }
    }
    Can_Ack_Pmu();
}

static void Can_Txmsg_Init(void)
{
    FORMAT_CAN2PMU *msg = (FORMAT_CAN2PMU *)_can_tx.Txdata;
    _can_tx.Txheader.ExtId = CANTX_ID;
    memset(msg, 0, sizeof(FORMAT_CAN2PMU));
}

static void Can_Ackmsg_Init(FORMAT_ACKMSG *msg)
{
    _can_tx.Txheader.ExtId = CAN_ACK_ID;
    msg->Code = ack_status;
    memset(msg->Data, 0, sizeof(msg->Data));
    msg->CRCH = 0xFE;
    msg->CRCL = 0xFE;
    msg->Num = 0;
}

void Can_Ack_Pmu(void)
{
    FORMAT_ACKMSG *msg = (FORMAT_ACKMSG *)_can_tx.Txdata;

    if (ack_status == no_ack)
    {
        return;
    }
    WEIGHING_DEVICE *device = Get_Device_Handle();
    uint16_t tmpshort = 0;
    uint32_t tmpint = 0;

    Can_Ackmsg_Init(msg);

    switch (ack_status)
    {
    case ack_f1:
    case ack_f4:
        break;

    case ack_f2:
        tmpint = device->Weight_current;
        memcpy(&msg->Data, &tmpint, sizeof(tmpint));
        rkfifo_in(&can_tx_kfifo, &_can_tx, 1);

        Can_Txmsg_Init();
        ack_status = ack_e1;

        return;

    case ack_f6:

        tmpint = SOFT_VER;
        msg->Num = HARD_VER;
        memcpy(msg->Data, &tmpint, sizeof(tmpint));
        break;

    case ack_e1:
        for(uint8_t i = 0; i < 4;i++)
        {
            if(  isnan(device->sensor[i]->base_k))
            {
                device->sensor[i]->base_k = 88;
            }
        }
        tmpshort = (uint16_t)device->sensor[0]->base_k;
        memcpy(msg->Data, &tmpshort, sizeof(tmpshort));

        tmpshort = (uint16_t)device->sensor[1]->base_k;
        memcpy(msg->Data + 2, &tmpshort, sizeof(tmpshort));
        msg->Num = 0x01;

        rkfifo_in(&can_tx_kfifo, &_can_tx, 1);

        tmpshort = ((uint16_t)device->sensor[2]->base_k) & 0x00FF;
        memcpy(msg->Data, &tmpshort, sizeof(tmpshort));

        tmpshort = ((uint16_t)device->sensor[3]->base_k) & 0xFF;
        memcpy(msg->Data + 2, &tmpshort, sizeof(tmpshort));
        msg->Num = 0x02;

        break;

    case ack_e2:

        tmpshort = (uint16_t)device->sensor[0]->K.f;
        memcpy(msg->Data, &tmpshort, sizeof(tmpshort));
        msg->Num = _can_rx.Rxdata[7];

        break;

    default:
        break;
    }
    ack_status = no_ack;
    rkfifo_in(&can_tx_kfifo, &_can_tx, 1);
    Can_Txmsg_Init();
}

void can2Pmu_3hz_info(void)
{
    WEIGHING_DEVICE *device = Get_Device_Handle();
    CAN_TxHeaderTypeDef Txheader;
    char Data[8] = {0};
    Txheader.ExtId = CANTX_ID2;
    Txheader.RTR = CAN_RTR_DATA;
    Txheader.IDE = CAN_ID_EXT;
    Txheader.DLC = 8;

    uint16_t temp = 0;
    temp = device->sensor[0]->Real_Weight / 10 ;
    memcpy(&Data[0],(uint16_t*)&temp,2);
    temp = device->sensor[1]->Real_Weight / 10 ;
    memcpy(&Data[2],(int16_t*)&temp,2);
    temp = device->sensor[2]->Real_Weight / 10 ;
    memcpy(&Data[4],(int16_t*)&temp,2);
    temp = device->sensor[3]->Real_Weight / 10 ;
    memcpy(&Data[6],(int16_t*)&temp,2);

    // if(device->sensor_num_mask == 3)
    // {
    //     Data[6] = 0;
    //     Data[7] = 0;
    // } 
    uint32_t time2 = HAL_GetTick();
    uint32_t TxMailBox0 = 0;
    while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan) == 0 && HAL_GetTick() - time2 < 5)
      ;
    HAL_CAN_AddTxMessage(&hcan, &Txheader, Data, &TxMailBox0);
}

void CAN2PMU_Send(void)
{
    WEIGHING_DEVICE *device = Get_Device_Handle();

    static FORMAT_CAN2PMU *msg = (FORMAT_CAN2PMU *)_can_tx.Txdata;

    uint32_t tmp = (uint32_t)(fabsf(device->Weight_current));

    device->Weight_last = device->Weight_current;

    memcpy(msg->Weight, &tmp, sizeof(tmp));

    _can_tx.Txheader.ExtId = CANTX_ID;
    Can_Tx_Msg(&_can_tx);

    

}

void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan)
{
    if (hcan->Instance == CAN1)
    {
        Can_Tx can_tx;
        uint32_t mailbox = 0;
        if (rkfifo_out(&can_tx_kfifo, &can_tx, 1) == 1)
        {
            HAL_CAN_AddTxMessage(hcan, &can_tx.Txheader, can_tx.Txdata, &mailbox);
        }
    }
}

// void CAN_Tx_Empty_Callback(CAN_HandleTypeDef *hcan)
// {
//     if (hcan->Instance == CAN1)
//     {
//         Can_Tx can_tx;
//         uint32_t mailbox = 0;
//         if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_TME0) != RESET)
//         {
//             if (rkfifo_out(&can_tx_kfifo, &can_tx, 1) == 1)
//             {
//                 HAL_CAN_AddTxMessage(hcan, &can_tx.Txheader, can_tx.Txdata, &mailbox);
//             }
//         }
//         hcan->Instance->TSR = CAN_TSR_RQCP0 | CAN_TSR_RQCP1 | CAN_TSR_RQCP2;
//     }
// }

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_FF0) != RESET)
    {
        __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_FF0);
    }
    else if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_FOV0) != RESET)
    {
        __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_FOV0);
    }
    if (hcan->Instance == CAN1)
    {
        Can_Rx can_rx;

        HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &can_rx.Rxheader, (uint8_t *)&can_rx.Rxdata);

        rkfifo_in(&can_rx_kfifo, &can_rx, 1);
    }
}

#if 0
	if(fitted_data.Fitted_Count == 0)
	{
		Set_Fitted_Data(Param,buf);
	}

	if(fitted_data.Fitted_Count != 0 && fitted_data.Fitted_Count < 8)
	{
		Set_Fitted_Data(Param,buf);
		for(sensor_num_c = 0; sensor_num_c < SENSOR_NUM;sensor_num_c++)
		{
			Apply_Least_Squares(Param->sensor[sensor_num_c]);
		}
	}

	fitted_data.Fitted_Count++;	//count + 1 作为样本数
	
	if(fitted_data.Fitted_Count >= 7)
	{
		fitted_data.Fitted_Count = 0;
	}
#endif

#if 0
void Set_Fitted_Data(WEIGHING_PARAM* param ,uint32_t buf)
{
		for(uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM;sensor_num_c++)
		{
			struct SENSOR* sensor = param->sensor[sensor_num_c];
			fitted_data.RealWeight[sensor_num_c][fitted_data.Fitted_Count] = Get_Single_Weight(sensor, buf);
			fitted_data.Real_Variation[sensor_num_c][fitted_data.Fitted_Count] = sensor->Real_Variation;
		}
}

void Apply_Least_Squares(struct SENSOR* sensor)
{
	Least_Squares(fitted_data.Fitted_Count + 1, fitted_data.RealWeight[sensor->Num],fitted_data.Real_Variation[sensor->Num], &sensor->K.f, &sensor->intercept.f);
}
#endif