Hi, it's me again! Sad, yes! I manage to write code to execute the trajectory simulation. This code I test in Qt work fine. But I have some trouble to implement it in nios because of the Interval Timer. I can't seem to make it work.

So I make a timer and start running if the start_moving signal is 1. And everytime the timer update the target will move a distance equivalent to v*t (with t = 0.2s fixed). I tried to use interrupt, everytime the timer finished a cycle of t it will call the function timer_isr to calculate the position of the target. But somehow it didn't work. It only displayed the first position and stop there. Can anyone give me some pointer how to fix it.

Code:
/*system module*/
#include <stdio.h>
#include "alt_types.h"
#include "system.h"
#include <sys/alt_irq.h>
#include "math.h"
#include "float.h"
#include <priv/alt_legacy_irq.h>
/*user define module*/
#include "timer_drv.h"
#include "avalon_gpio.h"


/*register file*/
#include "altera_avalon_timer_regs.h"
#include "altera_avalon_pio_regs.h"

typedef struct flash_type{
    int pause;
    int start_moving;
    int diving;
    int rocket;
} cmd_type;

  double const pi = 3.141592653589793238463;

  double H0, vg, S0;
  signed P;
  double q_target;
  cmd_type cmd = {0,0,0,0};
  double h_old, S_old, Hr_old, h_current, S_current, Hr_current;
  double betaT, epsT, dT, betaR, epsR, dR;


  alt_u32 isr_timer_base;

void get_input(double* H0, double* vg, double* S0, double* P, double* q_target)
{
    //target parameters
    *H0 = pio_read(H_TARGET_BASE);
    *vg = pio_read(V_TARGET_BASE);
    *S0 = pio_read(S_TARGET_BASE)*1000;
    *P  = pio_read(P_TARGET_BASE);
    *q_target = pio_read(Q_TARGET_BASE) * 0.06;
}

void get_command(cmd_type* cmd)
{
    cmd->start_moving = pio_read(START_MOVING_BASE)&0x1;
    cmd->diving = pio_read(DIVING_BASE)&0x1;
    cmd->rocket = pio_read(ROCKET_BASE)&0x1;
}
void get_Coordinate(double* h_old, double* S_old, double* Hr_old,
                    double* S_current, double* h_current, double* Hr_current,
                    double* BetaT, double* epsT, double* dT,
                    double* BetaR, double* epsR, double* dR)
{
    double Qcr = q_target*pi/180;
    double t = 0;
    double vr = 700;
    double diving_angle = 11.3*pi/180;
    double Hr = 0;

    double S, H, X, Y;
    double d, Beta, BetaOutR, epsOut1;
    double dt, Betat, epst, dr, Betar, epsr = 0;
    S = *S_old - vg*t;
    if (cmd.start_moving != 0)
        t = 0.2;
    else t = 0;
    //Parallel flying
    if (P!=0)
    {
        H = *h_old;
        X = S*sin(Qcr) -  P*cos(Qcr);
        Y = P*sin(Qcr) + S*cos(Qcr);
        if((Y!=0))
        {
            Beta = atan2(X,Y);
            if (X>0)
            {
               BetaOutR = Beta;
            }
            else
            {
               BetaOutR = Beta + 2*pi;
            }
        }
        else {
            if((X>0)) {BetaOutR = pi/2;}
            else BetaOutR = 3*pi/2;
        }
        d = fabs(X*sin(BetaOutR) + Y*cos(BetaOutR));
        if (d != 0)
        {
            epsOut1 = atan2(H,d);
        }
        else {epsOut1 = pi/2;}

    //Output calculation:
    dt = (H*sin(epsOut1) + d*cos(epsOut1))/1000;
    Betat = BetaOutR*180/pi;
    epst = epsOut1*180/pi;
    dr = 0; Betar = 0; epsr = 0;
    }
    else //diving
    {
        d = fabs(S);
        H = *h_old;
        Hr = *Hr_old;
        if (d!=0) {
            if ((S-20000) <= 100) {
                  //enable rocket
               }
            if (cmd.rocket == 1)
            {
                if ((S<15000))
                {
                    if ((H>=3000))
                    {H = *h_old - vg*tan(diving_angle)*t;}
                    else if (H<=2000) {
                        H = *h_old + vg*tan(diving_angle)*t;
                    } else { H = *h_old;}
                    Hr = H0 - vr*tan(diving_angle)*t;
                    epsr = 11.3;
                    if (Hr <0) { Hr = 0;}
                }
             }
                 else {
                          Hr = 0;
                          H = *h_old;
                        }
                epsOut1 = atan2(H,d);
                dt = (H*sin(epsOut1) + d*cos(epsOut1))/1000;
                dr = (Hr/sin(diving_angle))/1000;
            }

        else {
            epsOut1 = pi/2;
            }
        if (S>0)
        {
            Betat = q_target;
            Betar = q_target;
        } else
            {
            Betar = q_target + 180;
            Betat = q_target + 180;
            }
            epsr = 11.3;
            epst =  epsOut1*180/pi;
        }
    *S_current = S;
    *h_current = H;
    *Hr_current = Hr;
    *dT = dt;
    *BetaT = Betat;
    *epsT = epst;
    *dR = dr;
    *BetaR = Betar;
    *epsR = epsr;

}

void timer_isr(void* context, alt_u8 id)
{
    timer_clear_tick(isr_timer_base);
    if (cmd.start_moving != 0)
        {get_Coordinate(&h_old, &S_old, &Hr_old,
                       &h_current, &S_current, &Hr_current,
                       &betaT,&epsT,&dT,
                       &betaR,&epsR, &dR);
        h_old = h_current;
        S_old = S_current;
        Hr_old = Hr_current;}
    else
        {return;}
    printf("Azimuth target is %f: \n", betaT);
    printf("Elevation target is %f: \n", epsT);
    printf("Range target is %f: \n", dT);
    printf("Azimuth rocket is %f: \n", betaR);
    printf("Elevation rocket is %f: \n", epsR);
    printf("Range rocket is %f: \n", dR);
}
int main()
{
  printf("Hello from Nios II!\n");

  get_input(&H0, &vg, &S0, &P, &q_target);
  get_Coordinate(&H0, &S0, &Hr_old,
                     &h_current, &S_current, &Hr_current,
                     &betaT, &epsT, &dT,
                     &betaR, &epsR, &dR);
   // period = 0.2s;
  isr_timer_base = SYS_TIMER_BASE;

  //register ISR
  alt_irq_register(SYS_TIMER_IRQ, NULL,(void*) timer_isr);
  timer_wr_prd(SYS_TIMER_BASE, 10000000);   //start timer with period = 0.2s
 
  //starting position
  printf("Azimuth target is %f: \n", betaT);
  printf("Elevation target is %f: \n", epsT);
  printf("Range target is %f: \n", dT);
  printf("Azimuth rocket is %f: \n", betaR);
  printf("Elevation rocket is %f: \n", epsR);
  printf("Range rocket is %f: \n", dR);

  //main loop
  while(1)
  {
      get_command(&cmd);
  }
       

return 0;
}
Here is the timer driver avalon_gpio.h

Code:
/*file inclusion */
#include "alt_types.h"
#include "io.h"


/********************************************************************
 * constant definitions
 *******************************************************************/
#define PIO_DATA_REG_OFT 0     //data register address offset
#define PIO_DIRT_REG_OFT 1    //direction register address offset
#define PIO_INTM_REG_OFT 2    //interrupt mask register address offset
#define PIO_EDGE_REG_OFT 3    //edge capture register address offset


/********************************************************************
 * macro definitions
 *******************************************************************/
/*read/write PIO data register */
#define pio_read(base)            IORD(base, PIO_DATA_REG_OFT)
#define pio_write(base, data)    IOWR(base, PIO_DATA_REG_OFT, data)
/*read/clear pushbutton edge capture register*/
/*must write 0xf if the write-individual bit option is used is SOPC*/
#define btn_read(base)  IORD(base, PIO_EDGE_REG_OFT)
#define btn_clear(base) IOWR(base, PIO_EDGE_REG_OFT, 0xf)
#define btn_is_pressed(base) (IORD(base,PIO_EDGE_REG_OFT) != 0)
/********************************************************************
 * function prototypes
 *******************************************************************/
alt_u8 sseg_conv_hex(int hex);
void sseg_disp_ptn(alt_u32 base, alt_u8 *ptn);
The timer_drv.h
Code:
/****************************************************************
 * Module: Demo timer driver header
 * File:    timer_drv.h
 ***************************************************************/
/*file inclusion */
#include "alt_types.h"
#include "io.h"

/****************************************************************
 * constant definitions
 ***************************************************************/
#define TIMER_STAT_REG_OFT 0      //status register address offset
#define TIMER_CTRL_REG_OFT 1    //control register address offset
#define TIMER_PRDL_REG_OFT 2    //period reg (lower 16 bits) addr offset
#define TIMER_PRDH_REG_OFT 3    //period reg (upper 16 bits) addr offset

/*check "to" field for ms tick */
#define timer_read_tick(base) (IORD(base, TIMER_STAT_REG_OFT) & 0x01)

#define timer_clear_tick(base) IOWR(base, TIMER_STAT_REG_OFT, 0)

void timer_wr_prd(alt_u32 timer_base, alt_u32 prd);
Code:
/************************************************************************ * Module:    Demo timer driver functions
 * File:    timer_drv.c
 ***********************************************************************/
/*file inclusion*/
#include "timer_drv.h"


/************************************************************************
 * function: timer_wr_prd()
 * purpose:     write timer timeout period and configure/start timer
 * argument:
 *     timer-base: base address of timer-stamp timer
 *     prd: timeout period value
 * return:
 * note:
 ************************************************************************/
void timer_wr_prd(alt_u32 timer_base, alt_u32 prd)
{
    alt_u16 high, low;

    /*unpack 32-bit timeout period into two 16-bit half words */
    high = (alt_u16) (prd>>16);
    low = (alt_u16) (prd & 0x0000ffff);
    /*write timeout period*/
    IOWR(timer_base, TIMER_PRDH_REG_OFT, high);
    IOWR(timer_base, TIMER_PRDL_REG_OFT, low);
    /*configure timer to start, continuous mode; enable interrupt */
    IOWR(timer_base, TIMER_CTRL_REG_OFT, 0x0007);
}