Mity CPU Platforms » MityDSP-L138 (ARM9 Based Platforms)

/*

 * maindsp.cpp

 *

 *  Created on: Oct 10, 2013

 *      Author: preejith

 */

#include "core/DspUpp.h"

#include "core/DspSyscfg.h"

#include "core/chipconfig.h"

#include "core/cl_ipc_inbound.h"

#include "core/cl_ipc_outbound.h"

#include "core/DspGpio.h"

#include "core/DspIntDispatch.h"

#include "core/DspTimer.h"

#include "core/DspGpio.h"

#include "core/regs_gpio.h"

#include "core/memorymap.h"

#include "core/Gpio.h"

#include <lck.h>

#include <sem.h>

#include <std.h>

#include <tsk.h>

#include <clk.h>

#include <math.h>

#include <vector>

#include <string>

#include <stdio.h>

#include <c62.h>

#include <c6x.h>

#include <hwi.h>

#include <bcache.h>

const long int BUFF_SIZE = 2000;                         //size of buffers used in upp link

//set up buffer for upp

#pragma DATA_ALIGN( 64 );

volatile int16_t buff1[ BUFF_SIZE];//4096 bytes

//for sending data to qt app for display

struct data2bsent

{

	int16_t indicator;

	int16_t integer;//indicator=1

	int16_t state;//indicator=2

	int16_t wal1;//indicator=3

	int16_t wal2;//indicator=4

	int16_t dist_val;	//indicator=5

	int16_t q_value;//indicator=6

	int16_t distension[10];//indicator=7

	int16_t lumen[10];//indicator=8

	int16_t echo[2000];//indicator=9

};

struct data2bsent data_struct;

using namespace std;

using namespace MityDSP;

int numbr=0;

int copy_mun=0;

int peak_index[100];

int valley_index[100];

#define m 10                                            //number of frames i.e. pulses sent from the transducer

#define n 2000                                            //number of samples i.e. output of the ADC

#define w 31                                            //size of the window

#define threshold_noise_removal 0.0001

#define A 51                                            //number of samples to be averaged

#define win_detection 11

#define win_track 21

#define delta 0.1

#define fs    1e8                                            //sampling frequency

#define c 1540000                                        //velocity of ultrasound

#define max_peaks 10

#define dia_min   2                                        //minimum diameter

#define dia_max    10                                        //maximum diameter

#define set_zero 500                                     //number of smaples to be set to zero

#define WMNCC_buffsize    50

#define threshold_negative_corrcheck    0

#define check_cons 3                                     // no of consecutive appro. same wall locations needed in wall detection

#define check_inside 10                                 //no of frames to be captured to calculate 1 wall1 and wall2 location

#define distens_array_size 10000                         //no of points in the distension waveform

#define wmncc_array_size 50                             //no of points for wmncc check

#define wall_det_count 10

int half_window_size = floor(w / 2);

int check=0;                                             //interrupt polling

int16_t cnt=0;

//peak_det vari;

char debug1[]  = "failed to send received buffer to upp\n";

char debug2[]  = "getting instance of DSPuPP\n";

char debug3[]  = "configuring UPP\n";

char debug4[]  = "upp configured\n";

char debug5[]  = "wmncc error back to wall detection\n";

char debug6[]  = "wall detected to tracking\n";

char debug7[]  = "failed to send data\n ";

char debug8[]  = "no buffer received for shared memory\n ";

char debug9[]  = "done tracking\n";

char debug10[] = "failed configuring upp\n";

char debug11[] = "next wall detection\n";

char debug12[] = "interrupt\n";

char debug13[] = "pin high\n";

char debug14[] = "pin low\n";

char debug15[] = "wall id\n";

MityDSP::tcDspUpp* mpDspUpp; //!< Pointer to uPP driver singleton.

const MityDSP::tcDspUpp::teUppChan meRecvChan = tcDspUpp::eeChanA; //!< Constant for receive channel.

MBX_Handle mhRecvMbx = NULL; //!< Handle to receive mailbox.

//envelope smooth and normalize variables

//i have no idea why but these variables have to be global but code doesn't run otherwise

double normal[BUFF_SIZE];                                    //normalized swc

double product[BUFF_SIZE];

double smooth_1[BUFF_SIZE];                                    //sliding window covariance

double smooth_2[BUFF_SIZE];                                    //smoothened swc

// Forward declarations

void init();

void uppngpio();

int handleInboundMessage( void *apBuffer, uint32_t anLength, void *apUserArg ) ;

void HandleInterrupt();

void envelope_smooth_normalise(int16_t* ptr,int array_size);

void peakdetect(double* input,int array_size,double delt);

void debugPrint( char *buffer );

void integerPrint(int* intMsg);

void send_data();

tcDspTimer myTimer;

int16_t* lpMessageBuffer = NULL;

MityDSP::tcDspGpio* gpDspGpio;

tcDspUpp::tsMbxMsg lsRecvMbxMsg;

// Object for sending debug messages (these are received and printed to stdout by tcDspApp)

tcCL_IPCOutbound*     gpDebug;

// Object for sending GPPMSGQ1 messages that the ARM will receive

tcCL_IPCOutbound*     gpOutbound;

// Object for receiving DSPMSGQ0 messages that the DSP will receive

tcCL_IPCInbound*     gpInbound;

//*********uPP configuration record***********//

static const tcDspUpp::tsDspUppConfig gsUppConfig =

{

		4, // Interrupt level. Must be between 4 and 15.

		9, // Chan A DMA thread handling priority

		9, // Chan B DMA thread handling priority

		tcDspUpp::eeReceive, // Directionality of Channel A.

		tcDspUpp::eeDisabled, // Directionality of Channel B.

		tcDspUpp::ee14Bit, // Chan A data bit width.

		tcDspUpp::ee16Bit, // Chan B data bit width.

		false, // Use XData[7:0] for ChanA[15:8] even if ChanB is

		// disabled. See Table 3 in uPP User's Guide for details.

		4, // Size of MBX for Channel A.

		4, // Size of MBX for Channel B.

		0, // Clock divider for Channel A (37.5 MHz output clock)

		// (only in transmit mode). See Section 2.1.1 in uPP User's

		// Guide for details. Value must be between 1 and 16.

		0, // Clock divider for Channel B

		// (only in transmit mode). See Section 2.1.1 in uPP User's

		// Guide for details. Value must be between 1 and 16.

		tcDspUpp::ee256Bytes, // Chan A Transmit Thresh

		tcDspUpp::ee64Bytes, // Chan A Receive Thresh

		tcDspUpp::ee256Bytes, // Chan B Transmit Thresh

		tcDspUpp::ee256Bytes, // Chan B Receive Thresh

		false, // Do not use Chan A start signal

		false, // Do not use Chan B start signal

		//tcDspUpp::eeUPP_2xTXCLK, // Using external 2xTxClk for Transmit

		tcDspUpp::eePLL0_SYSCLK2,

		0 // uPP DMA Master Priority

};

/**

* Main routine for DSPAPP

*/

int main(int argc, char* argv[])

{

   // initialize the DSPLink system

   tcCL_IPCInit::GetInstance();

   // Launch an initialization task

   TSK_Attrs* lpAttrs = new TSK_Attrs;

   *lpAttrs           = TSK_ATTRS;

   lpAttrs->name      = "Initialize";

   lpAttrs->stacksize = 8192*4;

   lpAttrs->priority  = 5;

   TSK_create((Fxn)init,lpAttrs);

    return 0;

}

/**

* Private Constructor - initializes upp and gpio

*/

void uppngpio()

{

   //////////////////////////

   ///////UPP CONFIG////////

   //////////////////////////

   // Get instance of DspUpp

   debugPrint(debug2);

   mpDspUpp = tcDspUpp::getInstance();

   // Configure uPP

   debugPrint(debug3);

   if (0 != mpDspUpp->initialize(&gsUppConfig))

   {

       debugPrint(debug10);

   }

   else

   {

       debugPrint(debug4);

       // Get the receive and transmit mailboxes

       mhRecvMbx = mpDspUpp->getMBX(meRecvChan);

   }

   //send buffers to upp to fill

   mpDspUpp->receive(meRecvChan,(uint8_t*)buff1,4096 ) ;

   MBX_pend(mhRecvMbx, &lsRecvMbxMsg, SYS_FOREVER);

   BCACHE_inv(lsRecvMbxMsg.pBufPtr,4096,true);

   /////////////////////////////

   /////////GPIO CONFIG/////////

   /////////////////////////////

   //interrupt stuff

   //interrupt level= 6 bank0 interrupt event = 65

   HWI_eventMap(6, 65);

   //register the interrupt handler

   HWI_dispatchPlug(6, (Fxn)HandleInterrupt, -1, NULL);

   //setup interrupt pin

   gpDspGpio = tcDspGpio::GetInstance();

   tcDspSyscfg::SetPinMuxConfig(GPIO0_6);

   gpDspGpio->ConfigurePin(0,6,false,0,true,false);

   //setup rising edge interrupt

   gpDspGpio->mpGpioRegs->BANKPAIR[0].SET_RIS_TRIG |= (1<<6);

   gpDspGpio->mpGpioRegs->BANKPAIR[0].CLR_FAL_TRIG |= (1<<6);

   //eanble interrupt

   C62_enableIER(1 << 6);

}

void ipc_setup()

{

   // Message to ARM core.

       char lpReturnMessage[] = "DSP Initialization finished.";

       // Buffer for return message

       char* lpMessageBuffer = NULL;

       // Create the outbound debug link

       gpDebug = new tcCL_IPCOutbound("debug");

       // Create the inbound link for messages to the DSP

       gpInbound = new tcCL_IPCInbound();

       // Create the outbound controller for sending messages to the ARM

       gpOutbound = new tcCL_IPCOutbound("GPPMSGQ1");

       if (NULL != gpInbound)

       {

    	   gpInbound->Open("DSPMSGQ0", 8);

           // Register a callback function to handle messages from the ARM

              gpInbound->RegisterCallback(handleInboundMessage, (void*)NULL);

       }

       // Now that initialization is complete, let the ARM know with a message

       // Obtain a dsplink buffer for the return message

       lpMessageBuffer = (char*)gpOutbound->GetBuffer(strlen(lpReturnMessage) + 1);

       // Make sure we received a valid buffer

       if (NULL != lpMessageBuffer)

       {

           // Copy our message to the buffer

           strcpy(lpMessageBuffer, lpReturnMessage);

           // Send the message back to the ARM

           gpOutbound->SendMessage(lpMessageBuffer);

       }

}

//interrupt routine

void HandleInterrupt()

{

   if (!check)

   {

	   mpDspUpp->receive(meRecvChan,(uint8_t*)buff1,4096 );

	   MBX_pend(mhRecvMbx, &lsRecvMbxMsg, 0);

	   check=1;

   }

}

/**

* handles messages received from the arm

* @param apBuffer buffer location of message

* @param anLength length of buffer used

* @param apUserArg user given arguments

* @return 0;

*/

int handleInboundMessage(void* apBuffer, uint32_t anLength, void* apUserArg)

{

     return 0;

}

void init()

{

   // initialise communication protocols with qt

   ipc_setup();

    // initialise upp and gpio for interrupt

    uppngpio();

    while(1)

    {

        if ((check==1))

        {

        	//send data to display every 10 frames

        	cnt++;

        	 if(cnt%10==0)

        	 {

        		 memcpy((int16_t*)data_struct.echo, (int16_t*)buff1 ,sizeof(int16_t)*(BUFF_SIZE));

        		 send_data();

        	 }

        	 if(cnt==50)

        		 cnt=0;

        	//carry out processing

        	envelope_smooth_normalise((int16_t*)buff1,BUFF_SIZE);

        	//invalidate the cache

        	BCACHE_inv(lsRecvMbxMsg.pBufPtr,4096,true);

        	check=0;

        }

    }

}

//finds envelope smoothens the curve twice n normalises

void envelope_smooth_normalise(int16_t* ptr,int array_size)

{

   int j,k,k1,k2;

   double sum1=0;

   double sum2=0;

   double maxi=0;

   double mini=0;

   for(j=0;j<array_size;j++)

   {

       if((ptr[j]<250)&&(ptr[j]>-250))

    	   ptr[j]=0;

	   product[j] = ptr[j] * ptr[j];

        normal[j]=0;

   }

   j=half_window_size;

   for(k=0;k<=2*half_window_size;k++)

       sum1=sum1+product[k];

   smooth_1[j]=sum1/w;

   for(j=half_window_size+1;j<array_size-half_window_size;j++)

   {

       k1=j-half_window_size;

       k2=j+half_window_size;

       sum1 = sum1 - product[k1-1]+product[k2];

       smooth_1[j]=sum1/w;

   }

   sum1=0;

   sum2=0;

   for(j=0;j<half_window_size;j++)

   {

       sum1+=product[j];

       sum2+=product[array_size-1-j];

       smooth_1[j] = sum1/(j+1);

       smooth_1[array_size-1-j]=sum2/(j+1);

   }

   j=half_window_size;

   sum1=0;

   for(k=0;k<=2*half_window_size;k++)

       sum1=sum1+smooth_1[k];

   smooth_2[j]=sum1/w;

   mini=smooth_2[j];

   maxi=smooth_2[j];

   for(j=half_window_size+1;j<array_size-half_window_size;j++)

   {

       k1=j-half_window_size;

       k2=j+half_window_size;

      sum1 = sum1 - smooth_1[k1-1]+smooth_1[k2];

       smooth_2[j]=sum1/w;

       if(smooth_2[j]>maxi)

           maxi = smooth_2[j];

       else if(smooth_2[j]<mini)

           mini = smooth_2[j];

   }

   for (j = half_window_size; j < array_size-half_window_size; j++)

   {

       normal[j] = (smooth_2[j] - mini) / (maxi - mini);

       if (normal[j] < threshold_noise_removal)

           normal[j] = 0;

   }

}

//////////////peak detection

void peakdetect(double* input,int array_size,double delt)

{

   numbr=0;

   int pp_pos = 0;

   int chk=0;

   int pv_pos = 0;

   double pp_val = input[0];

   double pv_val = input[0];

   int nmbr = 0;

   int j = 0;

   for (j = 0; j < array_size; j++)

   {

       if (input[j] >= pp_val)

       {

           pp_pos = j;

           pp_val = input[j];

           chk=0;

           continue;

       }

       else if (pp_val - input[j] < delt)

       {

    	   chk=0;

    	   continue;

       }

       else

       {

           peak_index[nmbr] = pp_pos;

           chk=1;

           pv_pos = j;

           pv_val = input[j];

       }

       while (j < array_size)

       {

           j++;

           if (input[j] < pv_val)

           {

               pv_val = input[j];

           }

           else if (input[j] - pv_val < delt)

           {

           }

           else

           {

               valley_index[nmbr] = pv_pos;

               chk=2;

               nmbr++;

               pp_pos = j;

               pp_val = input[j];

               break;

           }

       }

   }

   if (chk!=1)

	   nmbr--;

//   nmbr++;

   numbr=nmbr;

}

/**

* prints a character array through a debug IPC object

* output is: "[DSP] <msg>"

* @param pMsg - character array of message to print

*/

void debugPrint(char* pMsg)

{

   // The length of the message to be sent

   int len = strlen(pMsg);

   // Pointer to dsplink buffer where to write the message

   char* pBuffer;

   // Make sure the debug IPC outbound object has been initialized

   if (gpDebug == NULL)

       return;

   // Get a buffer for the message

   pBuffer = (char *)gpDebug->GetBuffer(len+1);

   // Check that the buffer is valid

   if (pBuffer)

   {

      // Copy the message to the buffer

       strcpy(pBuffer, pMsg);

       // Send the message

       gpDebug->SendMessage(pBuffer);

   }

}

//prints integer output

void integerPrint(int* intMsg)

{

   int* lpMessageBuffer = NULL;

   int bytes=sizeof(int);

      // Make sure the intOutbound IPC outbound object has been initialized

   if (gpOutbound == NULL)

       return;

   // Get a buffer for the message

   lpMessageBuffer = (int *)gpOutbound->GetBuffer(200);

   // Check that the buffer is valid

   if (lpMessageBuffer!=NULL)

   {

//	   int *Msg =lpMessageBuffer;

       // Copy the message to the buffer

       memmove(lpMessageBuffer, intMsg ,bytes);

       // Send the message

       gpOutbound->SendMessage(lpMessageBuffer);

   }

   else

       debugPrint(debug8);

}

void send_data()

{

	 int* lpMessageBuffer = NULL;

	   // Make sure the intOutbound IPC outbound object has been initialized

	   if (gpOutbound == NULL)

	       return;

	   // Get a buffer for the message

	   lpMessageBuffer = (int *)gpOutbound->GetBuffer(sizeof(data_struct));

	   // Check that the buffer is valid

	   if (lpMessageBuffer!=NULL)

	   {

	       // Copy the message to the buffer

	       memmove(lpMessageBuffer, &data_struct,sizeof(data_struct));

	       // Send the message

	       gpOutbound->SendMessage(lpMessageBuffer);

	   }

	   else

	       debugPrint(debug8);

}