victor@3: /*
victor@3:  * render.cpp
victor@3:  *
victor@3:  *  Created on: Oct 24, 2014
victor@3:  *      Author: Andrew McPherson and Victor Zappi
victor@3:  */
victor@3: 
victor@3: 
andrewm@56: #include <BeagleRT.h>	// to schedule lower prio parallel process
victor@3: #include <rtdk.h>
victor@3: #include <cmath>
victor@3: #include <stdio.h>
victor@3: #include "SampleData.h"
victor@3: 
victor@3: SampleData gSampleData;	// User defined structure to get complex data from main
victor@3: int gReadPtr;			// Position of last read sample from file
victor@3: 
victor@3: // filter vars
victor@3: float gLastX[2];
victor@3: float gLastY[2];
victor@3: double lb0, lb1, lb2, la1, la2 = 0.0;
victor@3: 
victor@3: // communication vars between the 2 auxiliary tasks
victor@3: int gChangeCoeff = 0;
victor@3: int gFreqDelta = 0;
victor@3: 
victor@3: void initialise_filter(float freq);
victor@3: 
victor@3: void calculate_coeff(float cutFreq);
victor@3: 
victor@3: bool initialise_aux_tasks();
victor@3: 
victor@3: // Task for handling the update of the frequencies using the matrix
victor@3: AuxiliaryTask gChangeCoeffTask;
victor@3: 
victor@3: void check_coeff();
victor@3: 
victor@3: // Task for handling the update of the frequencies using the matrix
victor@3: AuxiliaryTask gInputTask;
victor@3: 
victor@3: void read_input();
victor@3: 
victor@3: 
victor@3: extern float gCutFreq;
victor@3: 
victor@3: 
andrewm@56: // setup() is called once before the audio rendering starts.
victor@3: // Use it to perform any initialisation and allocation which is dependent
victor@3: // on the period size or sample rate.
victor@3: //
victor@3: // userData holds an opaque pointer to a data structure that was passed
victor@3: // in from the call to initAudio().
victor@3: //
victor@3: // Return true on success; returning false halts the program.
victor@3: 
andrewm@56: bool setup(BeagleRTContext *context, void *userData)
victor@3: {
victor@3: 
victor@3: 	// Retrieve a parameter passed in from the initAudio() call
victor@3: 	gSampleData = *(SampleData *)userData;
victor@3: 
victor@3: 	gReadPtr = -1;
victor@3: 
victor@3: 	initialise_filter(200);
victor@3: 
victor@3: 	// Initialise auxiliary tasks
victor@3: 	if(!initialise_aux_tasks())
victor@3: 		return false;
victor@3: 
victor@3: 	return true;
victor@3: }
victor@3: 
victor@3: // render() is called regularly at the highest priority by the audio engine.
victor@3: // Input and output are given from the audio hardware and the other
victor@3: // ADCs and DACs (if available). If only audio is available, numMatrixFrames
victor@3: // will be 0.
victor@3: 
andrewm@52: void render(BeagleRTContext *context, void *userData)
victor@3: {
andrewm@52: 	for(unsigned int n = 0; n < context->audioFrames; n++) {
victor@3: 		float sample = 0;
victor@3: 		float out = 0;
victor@3: 
victor@3: 		// If triggered...
victor@3: 		if(gReadPtr != -1)
victor@3: 			sample += gSampleData.samples[gReadPtr++];	// ...read each sample...
victor@3: 
victor@3: 		if(gReadPtr >= gSampleData.sampleLen)
victor@3: 			gReadPtr = -1;
victor@3: 
victor@3: 		out = lb0*sample+lb1*gLastX[0]+lb2*gLastX[1]-la1*gLastY[0]-la2*gLastY[1];
victor@3: 
victor@3: 		gLastX[1] = gLastX[0];
victor@3: 		gLastX[0] = out;
victor@3: 		gLastY[1] = gLastY[0];
victor@3: 		gLastY[0] = out;
victor@3: 
andrewm@52: 		for(unsigned int channel = 0; channel < context->audioChannels; channel++)
andrewm@52: 			context->audioOut[n * context->audioChannels + channel] = out;	// ...and put it in both left and right channel
victor@3: 
victor@3: 	}
victor@3: 
victor@3: 	// Request that the lower-priority tasks run at next opportunity
andrewm@52: 	BeagleRT_scheduleAuxiliaryTask(gChangeCoeffTask);
andrewm@52: 	BeagleRT_scheduleAuxiliaryTask(gInputTask);
victor@3: }
victor@3: 
victor@3: // First calculation of coefficients
victor@3: 
victor@3: void initialise_filter(float freq)
victor@3: {
victor@3: 	calculate_coeff(freq);
victor@3: }
victor@3: 
victor@3: 
victor@3: // Calculate the filter coefficients
victor@3: // second order low pass butterworth filter
victor@3: 
victor@3: void calculate_coeff(float cutFreq)
victor@3: {
victor@3: 	// Initialise any previous state (clearing buffers etc.)
victor@3: 	// to prepare for calls to render()
victor@3: 	float sampleRate = 44100;
victor@3: 	double f = 2*M_PI*cutFreq/sampleRate;
victor@3: 	double denom = 4+2*sqrt(2)*f+f*f;
victor@3: 	lb0 = f*f/denom;
victor@3: 	lb1 = 2*lb0;
victor@3: 	lb2 = lb0;
victor@3: 	la1 = (2*f*f-8)/denom;
victor@3: 	la2 = (f*f+4-2*sqrt(2)*f)/denom;
victor@3: 	gLastX[0] = gLastX [1] = 0;
victor@3: 	gLastY[0] = gLastY[1] = 0;
victor@3: 
victor@3: }
victor@3: 
victor@3: 
victor@3: // Initialise the auxiliary tasks
victor@3: // and print info
victor@3: 
victor@3: bool initialise_aux_tasks()
victor@3: {
andrewm@52: 	if((gChangeCoeffTask = BeagleRT_createAuxiliaryTask(&check_coeff, 90, "beaglert-check-coeff")) == 0)
victor@3: 		return false;
victor@3: 
andrewm@52: 	if((gInputTask = BeagleRT_createAuxiliaryTask(&read_input, 50, "beaglert-read-input")) == 0)
victor@3: 		return false;
victor@3: 
victor@3: 	rt_printf("Press 'a' to trigger sample, 's' to stop\n");
victor@3: 	rt_printf("Press 'z' to low down cut-off freq of 100 Hz, 'x' to raise it up\n");
victor@3: 	rt_printf("Press 'q' to quit\n");
victor@3: 
victor@3: 	return true;
victor@3: }
victor@3: 
victor@3: // Check if cut-off freq has been changed
victor@3: // and new coefficients are needed
victor@3: 
victor@3: void check_coeff()
victor@3: {
victor@3: 	if(gChangeCoeff == 1)
victor@3: 	{
victor@3: 		gCutFreq += gFreqDelta;
victor@3: 		gCutFreq = gCutFreq < 0 ? 0 : gCutFreq;
victor@3: 		gCutFreq = gCutFreq > 22050 ? 22050 : gCutFreq;
victor@3: 
victor@3: 		rt_printf("Cut-off frequency: %f\n", gCutFreq);
victor@3: 
victor@3: 		calculate_coeff(gCutFreq);
victor@3: 		gChangeCoeff = 0;
victor@3: 	}
victor@3: }
victor@3: 
victor@3: // This is a lower-priority call to periodically read keyboard input
victor@3: // and trigger samples. By placing it at a lower priority,
victor@3: // it has minimal effect on the audio performance but it will take longer to
victor@3: // complete if the system is under heavy audio load.
victor@3: 
victor@3: void read_input()
victor@3: {
victor@3: 	// This is not a real-time task!
victor@3: 	// Cos getchar is a system call, not handled by Xenomai.
victor@3: 	// This task will be automatically down graded.
victor@3: 
victor@3: 	char keyStroke = '.';
victor@3: 
victor@3: 	keyStroke =	getchar();
victor@3: 	while(getchar()!='\n'); // to read the first stroke
victor@3: 
victor@3: 	switch (keyStroke)
victor@3: 	{
victor@3: 		case 'a':
victor@3: 			gReadPtr = 0;
victor@3: 			break;
victor@3: 		case 's':
victor@3: 			gReadPtr = -1;
victor@3: 			break;
victor@3: 		case 'z':
victor@3: 			gChangeCoeff = 1;
victor@3: 			gFreqDelta = -100;
victor@3: 			break;
victor@3: 		case 'x':
victor@3: 			gChangeCoeff = 1;
victor@3: 			gFreqDelta = 100;
victor@3: 			break;
victor@3: 		case 'q':
victor@3: 			gShouldStop = true;
victor@3: 			break;
victor@3: 		default:
victor@3: 			break;
victor@3: 	}
victor@3: }
victor@3: 
victor@3: 
victor@3: 
andrewm@56: // cleanup() is called once at the end, after the audio has stopped.
andrewm@56: // Release any resources that were allocated in setup().
victor@3: 
andrewm@56: void cleanup(BeagleRTContext *context, void *userData)
victor@3: {
victor@3: 	delete[] gSampleData.samples;
victor@3: }