Project

General

Profile

Download (15 KB)

RE: UPP acquisition issue ยป maindsp.cpp

Preejith S P, 10/18/2013 10:12 AM

 
/*
* 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);
}
    (1-1/1)
    Go to top