M451的DSP这么玩PID

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

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

 * [url=home.php?mod=space&uid=895143]@version[/url]  V1.0

 * $Date: 15/09/02 10:04a $

 * @brief

 *         Display how to use M4 DSP PID Controller

 *                      and compare with calcultion without DSP

 * @note

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

 *

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

#include <stdio.h>

#include "M451Series.h"

#include "arm_math.h" 

#define USE_DSP

int i,j,CalTime;

arm_pid_instance_f32 PIDS;

float32_t output[100],ee;

float A0,A1,A2,state[3],Kp=0.4,Ki=0.4,Kd=0,target,ival;

float PID(float in)

{

    float out;

                A0=Kp+Ki;

                A1 = -Kp-(2* Kd);

                A2 = Kd;

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

    out = (A0 * in) + (A1 * state[0]) + (A2 * state[1]) + (state[2]);



    /* Update state */

    state[1] = state[0];

    state[0] = in;

    state[2] = out;



    /* return to application */

    return (out);

}

void SYS_Init(void)

{

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

    /* Init System Clock                                                                                       */

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

    /* Enable HIRC clock */

    CLK_EnableXtalRC(CLK_PWRCTL_HIRCEN_Msk);



    /* Waiting for HIRC clock ready */

    CLK_WaitClockReady(CLK_STATUS_HIRCSTB_Msk);



    /* Switch HCLK clock source to HIRC */

    CLK_SetHCLK(CLK_CLKSEL0_HCLKSEL_HIRC, CLK_CLKDIV0_HCLK(1));



    /* Enable HXT */

    CLK_EnableXtalRC(CLK_PWRCTL_HXTEN_Msk);



    /* Waiting for clock ready */

    CLK_WaitClockReady(CLK_STATUS_HXTSTB_Msk);



    /* Set core clock as PLL_CLOCK from PLL and SysTick source to HCLK/2*/

    CLK_SetCoreClock(72000000);

    CLK_SetSysTickClockSrc(CLK_CLKSEL0_STCLKSEL_HCLK_DIV2);



    /* Enable peripheral clock */

    CLK_EnableModuleClock(UART0_MODULE);

    CLK_EnableModuleClock(TMR0_MODULE);



    /* Peripheral clock source */

    CLK_SetModuleClock(UART0_MODULE, CLK_CLKSEL1_TMR0SEL_HXT, CLK_CLKDIV0_UART(1));

    CLK_SetModuleClock(TMR0_MODULE, CLK_CLKSEL1_TMR0SEL_HXT, 0);



}

void UART_Init(void)

{

    /* Set PD multi-function pins for UART0 RXD, TXD */

    SYS->GPD_MFPL = SYS_GPD_MFPL_PD0MFP_UART0_RXD | SYS_GPD_MFPL_PD1MFP_UART0_TXD;

    /* Reset UART module */

    SYS_ResetModule(UART0_RST);



    /* Configure UART0 and set UART0 Baudrate */

    UART_Open(UART0, 115200);

}



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

/*  Main Function                                                                                          */

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

int32_t main(void)

{                

          uint32_t i; 

                PIDS.Kp=0.4;                                

                PIDS.Ki=0.4;

                PIDS.Kd=0;

                /* Target value*/

                target=500;

                /* Inital value */

                ival=0;

                /* Initial value and target value error */

                ee=target-ival;

        

                /* Unlock protected registers */

    SYS_UnlockReg();



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

    SYS_Init();



    /* Lock protected registers */

    SYS_LockReg();



    /* Init UART for printf */

    UART_Init();

                

                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++)

                {

                        output[i]=arm_pid_f32(&PIDS,ee);

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

                        /* Update error */

                        ee=target-output[i-1];

                }

#else

                

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

                {

                        output[i]=PID(ee);

                        ee=target-output[i-1];

                }

                

#endif

          TIMER_Close(TIMER0);

                CalTime=TIMER_GetCounter(TIMER0);        

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



    while(1);                             /* main function does not return */

}