M480利用内核DSP库函数实现PID功能

/**************************************************************************//**

 * [url=home.php?mod=space&uid=288409]@file[/url]     main.c

 * @brief

 *           Display how to use M4 DSP PID Controller

 *           compare with calcultion time without DSP

 * @note

 * Copyright (C) 2019 Nuvoton Technology Corp. All rights reserved.

 *****************************************************************************/

#include <stdio.h>

#include "NuMicro.h"

#include "arm_math.h"



/*---------------------------------------------------------------------------*/

/* Global variables                                                          */

/*---------------------------------------------------------------------------*/

#define USE_DSP



uint32_t u32CalTime;

arm_pid_instance_f32 PIDS;

float32_t doutput[100], derr;

float fA0, fA1, fA2, afstate[3], fKp = 0.4, fKi = 0.4, fKd = 0, ftarget, fival;



/*---------------------------------------------------------------------------*/

/* Functions                                                                 */

/*---------------------------------------------------------------------------*/

float PID(float fin)

{

    float fout;



    fA0 = fKp + fKi;

    fA1 = -fKp - (2 * fKd);

    fA2 = fKd;

    /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]  */

    fout = (fA0 * fin) + (fA1 * afstate[0]) + (fA2 * afstate[1]) + (afstate[2]);



    /* Update state */

    afstate[1] = afstate[0];

    afstate[0] = fin;

    afstate[2] = fout;



    /* return to application */

    return (fout);

}



void SYS_Init(void)

{

    /*---------------------------------------------------------------------------*/

    /* Init System Clock                                                         */

    /*---------------------------------------------------------------------------*/

    /* Unlock protected registers */

    SYS_UnlockReg();



    /* Set XT1_OUT(PF.2) and XT1_IN(PF.3) to input mode */

    PF->MODE &= ~(GPIO_MODE_MODE2_Msk | GPIO_MODE_MODE3_Msk);



    /* Enable External XTAL (4~24 MHz) */

    CLK_EnableXtalRC(CLK_PWRCTL_HXTEN_Msk);



    /* Waiting for 12MHz clock ready */

    CLK_WaitClockReady(CLK_STATUS_HXTSTB_Msk);



    /* Set core clock as PLL_CLOCK from PLL */

    CLK_SetCoreClock(FREQ_192MHZ);



    /* Set both PCLK0 and PCLK1 as HCLK/2 */

    CLK->PCLKDIV = CLK_PCLKDIV_PCLK0DIV2 | CLK_PCLKDIV_PCLK1DIV2;



    /* Enable IP clock */

    CLK_EnableModuleClock(UART0_MODULE);

    CLK_EnableModuleClock(TMR0_MODULE);



    /* Select IP clock source */

    CLK_SetModuleClock(UART0_MODULE, CLK_CLKSEL1_UART0SEL_HXT, CLK_CLKDIV0_UART0(1));

    CLK_SetModuleClock(TMR0_MODULE, CLK_CLKSEL1_TMR0SEL_HXT, 0);



    /* Update System Core Clock */

    /* User can use SystemCoreClockUpdate() to calculate SystemCoreClock. */

    SystemCoreClockUpdate();





    /* Set GPB multi-function pins for UART0 RXD and TXD */

    SYS->GPB_MFPH &= ~(SYS_GPB_MFPH_PB12MFP_Msk | SYS_GPB_MFPH_PB13MFP_Msk);

    SYS->GPB_MFPH |= (SYS_GPB_MFPH_PB12MFP_UART0_RXD | SYS_GPB_MFPH_PB13MFP_UART0_TXD);



    /* Lock protected registers */

    SYS_LockReg();



}



void UART_Init(void)

{

    /* Configure UART0 and set UART0 Baudrate */

    UART_Open(UART0, 115200);

}



int32_t main(void)

{

    uint32_t i;



    /* Set PID parameters */

    PIDS.Kp = 0.4;

    PIDS.Ki = 0.4;

    PIDS.Kd = 0;



    /* Target value*/

    ftarget = 500;

    /* Inital value */

    fival = 0;

    /* Initial value and target value error */

    derr = ftarget - fival;



    /* Unlock protected registers */

    SYS_UnlockReg();



    /* Init System, peripheral clock and multi-function I/O */

    SYS_Init();



    /* Init UART for printf */

    UART_Init();



    /*

        This sample code is used to show how to use ARM Cortex-M4 DSP library.

        And displays function calculation time.

    */



    printf("\n\n");

    printf("+----------------------------------------+\n");

    printf("|      M480_M4_DSP_PID Sample Code       |\n");

    printf("+----------------------------------------+\n");



    TIMER_Open(TIMER0, TIMER_CONTINUOUS_MODE, 1);

    TIMER_Start(TIMER0);



#ifdef USE_DSP

    /* Initial DSP PID controller function */

    arm_pid_init_f32(&PIDS, 0);



    /* Calculate PID controller function 100 times */

    for (i = 1; i < 100; i++)

    {

        doutput[i] = arm_pid_f32(&PIDS, derr);

        //printf("%0.2f\n",doutput[i]);

        /* Update error */

        derr = ftarget - doutput[i - 1];

    }



#else



    for (i = 1; i < 100; i++)

    {

        doutput[i] = PID(derr);

        derr = ftarget - doutput[i - 1];

    }



#endif



    TIMER_Close(TIMER0);

    u32CalTime = TIMER_GetCounter(TIMER0);



    /* Display function calculation time */

    printf("Calculation time is %d \n", u32CalTime);



    while (1);

}