/*
 * @Author: DaMing zxm5337@163.com
 * @Date: 2024-04-10 11:25:26
 * @LastEditors: 张小明 zxm5337@163.com
 * @LastEditTime: 2024-05-30 08:16:15
 * @FilePath: \Proxi_CheckBoard\User\App\bldc.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <string.h>
#include <math.h>
#include "FreeRTOS.h"
#include "task.h"
#include "bldc.h"
#include "tim.h"
#include "communication.h"
#include "ctr_logic.h"

freq_t freq_opto;
freq_t freq_namur;
bldc_t bldc_A;

void fun_ini_freq(freq_t *freq)
{
    if (!freq)
        return;
    memset(freq, 0, sizeof(freq_t));

    HAL_TIM_Base_Start_IT(&htim9);
    __HAL_TIM_SET_CAPTUREPOLARITY(&htim9, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_FALLING);
    HAL_TIM_IC_Start_IT(&htim9, TIM_CHANNEL_2);

    HAL_TIM_Base_Start_IT(&htim10);
    __HAL_TIM_SET_CAPTUREPOLARITY(&htim10, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
    HAL_TIM_IC_Start_IT(&htim10, TIM_CHANNEL_1);
}

static void fun_zero_freq(freq_t *freq)
{
    if (!freq)
        return;

    uint32_t *delay_out = &freq->delay_out;

    if (*delay_out > 0)
        (*delay_out)--;
    else
        memset(freq, 0, sizeof(freq_t));
}

// 周期：66000us
// t = 66000us = 66ms = 0.066s
// f = 1/0.066 = 1000/66 = 15.15
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (!htim)
        return;

    freq_t *freq_dev;
    uint32_t channel;

    if (htim == &htim9)
    {
        channel = TIM_CHANNEL_2;
        freq_dev = &freq_namur;
    }
    else if (htim == &htim10)
    {
        channel = TIM_CHANNEL_1;
        freq_dev = &freq_opto;
    }

    uint32_t *count = &freq_dev->count;
    uint32_t *count2 = &freq_dev->count2;
    uint32_t *duty = &freq_dev->duty;
    float32 *freq = &freq_dev->freq;
    float32 *pre_freq = &freq_dev->pre_freq;
    float32 *velocity = &freq_dev->velocity;
    uint32_t *number = &freq_dev->number;
    uint32_t *update_cnt = &freq_dev->update_cnt;
    uint32_t *delay_out = &freq_dev->delay_out;

    *delay_out = DELAY_5S;

    if (*number < 1)
        (*number)++;
    else
        *number = 0;

    if (*number == 1)
    {
        __HAL_TIM_SET_COUNTER(htim, 0);
        *update_cnt = 0;
    }
    else if (*number == 0)
    {
        *count = HAL_TIM_ReadCapturedValue(htim, channel);
        *duty = (*count + (*update_cnt) * 65535) * SYS_CLOCK;
        *freq = 1000000.0f / (*duty); // hz

        if (*freq > BLDC_DEFAULT_SPEED / 60 + 5)
            *freq = BLDC_DEFAULT_SPEED / 60 + 5;

        if (fabs(*freq - *pre_freq) > A_FREQ)
        {
            *freq = *pre_freq;
            (*count2)++;
            if (*count2 > 5)
            {
                *pre_freq = *freq;
                *count2 = 0;
            }
        }
        else
        {
            *count2 = 0;
            *freq = *pre_freq * 0.8f + *freq * 0.2f;
            *pre_freq = *freq;
        }

        *velocity = *freq * 60.0f;
    }
}

// 电调驱动
// 周期：20ms（50hz）
// 脉宽：1-2ms
// pwm：5%-10%
// 电机速度：0-60000r/min
// 电机频率：0-1khz

static void fun_get_pwm_pulse(bldc_t *bldc)
{
    if (!bldc)
        return;

    float32 *pwm = &bldc->pwm;
    uint16_t *pulse = &bldc->pulse;
    uint16_t velocity_pv = bldc->velocity_pv;
    uint16_t *velocity_sv = &bldc->velocity_sv;

    bldc_A.velocity_pv = freq_opto.velocity;
    bldc_A.frequency_pv = bldc_A.velocity_pv / 60.0f;

    if (velocity_pv < *velocity_sv)
    {
        (*pwm) += PWM_STEP;
    }
    else
    {
        *velocity_sv = 0;
        (*pwm) -= PWM_STEP;
    }

    *pwm = *pwm < BLDC_PWM_MIN ? BLDC_PWM_MIN : *pwm > BLDC_PWM_MAX ? BLDC_PWM_MIN
                                                                    : *pwm;
    *pulse = (*pwm) * 200;
}

static void fun_set_pwm_pulse(bldc_t *bldc)
{
    if (!bldc)
        return;

    float32 *pwm = &bldc->pwm;
    uint16_t *pulse = &bldc->pulse;

    if ((*pwm >= 5) && (*pwm <= 10))
    {
        __HAL_TIM_SET_COMPARE(&htim11, TIM_CHANNEL_1, *pulse);
        HAL_TIM_PWM_Start(&htim11, TIM_CHANNEL_1);
    }
    else
    {
        HAL_TIM_PWM_Stop(&htim11, TIM_CHANNEL_1);
    }
}

static void fun_proc_seal_flag(freq_t *freq, bldc_t *bldc)
{
    if (!freq)
        return;

    GPIO_PinState pin_state;
    uint32_t *delay_out = &freq->delay_out;
    BOOL *seal_flag = &bldc->seal_flag;
    uint16_t *velocity_sv = &bldc->velocity_sv;

    pin_state = HAL_GPIO_ReadPin(SEAL_IN_GPIO_Port, SEAL_IN_Pin);
    if (pin_state == GPIO_PIN_RESET)
        *seal_flag = TRUE;
    else
        *seal_flag = FALSE;

    if (!(*seal_flag))
    {
        *velocity_sv = 0;
        *delay_out = 0;
    }
}

void fun_ctr_bldc(bldc_t *bldc)
{
    if (!bldc)
        return;

    fun_zero_freq(&freq_namur);
    fun_zero_freq(&freq_opto);
    fun_proc_seal_flag(&freq_namur, bldc);
    fun_proc_seal_flag(&freq_opto, bldc);
    fun_get_pwm_pulse(bldc);
    fun_set_pwm_pulse(bldc);
}