Working version

descrambler.c

@@ -10,27 +10,27 @@
  */
 
 /* Include files */
-#include <math.h>
-#include <string.h>
 #include "descrambler.h"
 
 /* Function Definitions */
-void descramble(double *value, double t, const double B1[9], const double A1[9],
-                 const double B2[19], const double A2[19], double Z1[8], double
+
+static float b[9];
+static float a[9];
+static float a1;
+static int k;
+static float zf[8];
+//static double b_b[19];
+//static double b_a[19];
+//static double b_zf[18];
+
+void descramble(float *value, float sine, const float B1[9], const float A1[9],
+                 const float B2[19], const float A2[19], float Z1[8], float
                  Z2[18])
 {
-  double b[9];
-  double a[9];
-  double a1;
-  int k;
-  double zf[8];
-  double b_b[19];
-  double b_a[19];
-  double b_zf[18];
 
   /*  Filter to remove 8kHz tone */
-  memcpy(&b[0], &B1[0], 9U * sizeof(double));
-  memcpy(&a[0], &A1[0], 9U * sizeof(double));
+  memcpy(&b[0], &B1[0], 9U * sizeof(float));
+  memcpy(&a[0], &A1[0], 9U * sizeof(float));
   a1 = A1[0];
   if ((!(A1[0] == 0.0)) && (A1[0] != 1.0)) {
     for (k = 0; k < 9; k++) {
@@ -42,7 +42,7 @@ void descramble(double *value, double t, const double B1[9], const double A1[9],
     a[0] = 1.0;
   }
 
-  memset(&zf[0], 0, sizeof(double) << 3);
+  memset(&zf[0], 0, sizeof(float) << 3);
   a1 = Z1[0] + *value * b[0];
   for (k = 0; k < 7; k++) {
     zf[k] = Z1[k + 1];
@@ -51,31 +51,31 @@ void descramble(double *value, double t, const double B1[9], const double A1[9],
   for (k = 0; k < 8; k++) {
     Z1[k] = (zf[k] + *value * b[k + 1]) + -a1 * a[k + 1];
   }
-
-  /*  Shifting by 7kHz */
-  *value = a1 * 2.0 * sin(6.2831853071795862 * t * 7000.0);
-
-  /*  Low pass filter */
-  memcpy(&b_b[0], &B2[0], 19U * sizeof(double));
-  memcpy(&b_a[0], &A2[0], 19U * sizeof(double));
-  a1 = A2[0];
-  if ((!(A2[0] == 0.0)) && (A2[0] != 1.0)) {
-    for (k = 0; k < 19; k++) {
-      b_b[k] /= a1;
-    }
-    for (k = 0; k < 18; k++) {
-      b_a[k + 1] /= a1;
-    }
-    b_a[0] = 1.0;
-  }
-
-  memset(&b_zf[0], 0, 18U * sizeof(double));
-  a1 = Z2[0] + *value * b_b[0];
-  memcpy(&b_zf[0], &Z2[1], 17U * sizeof(double));
-  for (k = 0; k < 18; k++) {
-    Z2[k] = (b_zf[k] + *value * b_b[k + 1]) + -a1 * b_a[k + 1];
-  }
   *value = a1;
+  /*  Shifting by 7kHz */
+//  *value = a1 * sine;
+//
+//  /*  Low pass filter */
+//  memcpy(&b_b[0], &B2[0], 19U * sizeof(double));
+//  memcpy(&b_a[0], &A2[0], 19U * sizeof(double));
+//  a1 = A2[0];
+//  if ((!(A2[0] == 0.0)) && (A2[0] != 1.0)) {
+//    for (k = 0; k < 19; k++) {
+//      b_b[k] /= a1;
+//    }
+//    for (k = 0; k < 18; k++) {
+//      b_a[k + 1] /= a1;
+//    }
+//    b_a[0] = 1.0;
+//  }
+//
+//  memset(&b_zf[0], 0, 18U * sizeof(double));
+//  a1 = Z2[0] + *value * b_b[0];
+//  memcpy(&b_zf[0], &Z2[1], 17U * sizeof(double));
+//  for (k = 0; k < 18; k++) {
+//    Z2[k] = (b_zf[k] + *value * b_b[k + 1]) + -a1 * b_a[k + 1];
+//  }
+//  *value = a1;
 }
 
 /* End of code generation (descrambler.c) */

descrambler.h

@@ -15,9 +15,9 @@
 /* Include files */
 
 /* Function Declarations */
-extern void descramble(double *value, double t, const double B1[9], const
-  double A1[9], const double B2[19], const double A2[19], double Z1[8], double
-  Z2[18]);
+extern void descramble(float *value, float t, const float B1[9], const
+                       float A1[9], const float B2[19], const float A2[19],
+                       float Z1[8], float Z2[18]);
 
 #endif
 

main.c

@@ -6,32 +6,42 @@
 #include <math.h>
 #include <descrambler.h>
 
-// Sampling frequency
-const int fs = 48000;
+#define SINE_SIZE 50
+#define FS 50000
 
-
-// Timer for calculate sine wave
-static double Timer = 0;
 // Coefficients for bandstop filter
-const double B1[] = {0.7755, -2.5985, 6.3671, -9.6188, 11.5650, -9.6188, 6.3671, -2.5985, 0.7755};
-const double A1[] = {1.0000, -3.1835, 7.4065, -10.6464, 12.1861, -9.6677, 6.1083, -2.3849, 0.6809};
+//const double B1[] = {0.7755, -2.5985, 6.3671, -9.6188, 11.5650, -9.6188, 6.3671, -2.5985, 0.7755};
+//const double A1[] = {1.0000, -3.1835, 7.4065, -10.6464, 12.1861, -9.6677, 6.1083, -2.3849, 0.6809};
+const float B1[] = {0.9208, 0.1952, 0.9208};
+const float A1[] = {1.0000, 0.1952, 0.8415};
+//const float A1[] = {1.0000, -1.0457, 0.9510};
+const float A2[] = {1.0000, -1.5610, 0.6414};
 // Coefficients for lowpass filter
-const double B2[] = {0.0000, 0.0001, 0.0005, 0.0025, 0.0093, 0.0260, 0.0564, 0.0967, 0.1330, 0.1477, 0.1330, 0.0967, 0.0564, 0.0260, 0.0093, 0.0025, 0.0005, 0.0001, 0.0000};
-const double A2[] = {0.0001, -0.0014, 0.0096, -0.0423, 0.1340, -0.3250, 0.6243, -0.9708, 1.2386, -1.3057, 1.1399, -0.8221, 0.4862, -0.2326, 0.0881, -0.0255, 0.0053, -0.0007, 0.0000};
+//const float B2[] = {0.0000, 0.0001, 0.0005, 0.0025, 0.0093, 0.0260, 0.0564, 0.0967, 0.1330, 0.1477, 0.1330, 0.0967, 0.0564, 0.0260, 0.0093, 0.0025, 0.0005, 0.0001, 0.0000};
+//const float A2[] = {0.0001, -0.0014, 0.0096, -0.0423, 0.1340, -0.3250, 0.6243, -0.9708, 1.2386, -1.3057, 1.1399, -0.8221, 0.4862, -0.2326, 0.0881, -0.0255, 0.0053, -0.0007, 0.0000};
 // Time delays for both filters
-static double Z1[8];
-static double Z2[18];
+
+static float Z1[2][3];
+static float Z2[2][3];
+
+float sine[SINE_SIZE];
+unsigned int sine_index = 0;
 
 int main(void){
     /* Halting the Watchdog */
     MAP_WDT_A_holdTimer();
     
+    for(;sine_index<=SINE_SIZE;sine_index++){
+//        sine[sine_index] = 2.0 * sin(6.2831853071795862 * 7000.0 * sine_index * 20e-6);
+//        sine[sine_index] = 1024 * sin(sine_index * 1.4661); // for 30kHz sampling
+        sine[sine_index] = (1+sin(sine_index * 0.8796f))/2.0f;
+//        sine[sine_index] = sin(6.2831853071795862 * 7000 * sine_index * 20e-6);
+    }/* Fill sine array */
     //![Simple CS Config]
     /* Configuring pins for peripheral/crystal usage */
-    MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_PJ,
-            GPIO_PIN3 | GPIO_PIN2, GPIO_PRIMARY_MODULE_FUNCTION);
+    MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_PJ, GPIO_PIN3 | GPIO_PIN2, GPIO_PRIMARY_MODULE_FUNCTION);
 
-    /* Configure P5.6 and P5.7 to their analog functions to output VREF */
+    /* Configure P5.6 and P5.7 to their analogue functions to output VREF */
     P5SEL0 |= BIT6 | BIT7;
     P5SEL1 |= BIT6 | BIT7;
 
@@ -49,14 +59,13 @@ int main(void){
             GPIO_PIN2 | GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);
 
     /* Set P6.0 as output */
-    MAP_GPIO_setAsOutputPin(GPIO_PORT_P1, GPIO_PIN0);
+    MAP_GPIO_setAsOutputPin(GPIO_PORT_P6, GPIO_PIN0);
 
-    /* Set all 8 pins of P2 as output */
-    MAP_GPIO_setAsOutputPin(GPIO_PORT_P2, GPIO_PIN0 | GPIO_PIN1 | GPIO_PIN2 | GPIO_PIN3 | GPIO_PIN4 | GPIO_PIN5 | GPIO_PIN6 | GPIO_PIN7);
+    /* Set all 8 pins of P4 as output */
+    MAP_GPIO_setAsOutputPin(GPIO_PORT_P4, GPIO_PIN0 | GPIO_PIN1 | GPIO_PIN2 | GPIO_PIN3 | GPIO_PIN4 | GPIO_PIN5 | GPIO_PIN6 | GPIO_PIN7);
 
     /* Configuring GPIOs (4.3 MCLK) */
-    MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P4, GPIO_PIN3,
-    GPIO_PRIMARY_MODULE_FUNCTION);
+//    MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P4, GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);
 
     /* Setting the external clock frequency. This API is optional, but will
      * come in handy if the user ever wants to use the getMCLK/getACLK/etc
@@ -74,9 +83,9 @@ int main(void){
     /* Initialising MCLK to HFXT (effectively 48MHz) */
     MAP_CS_initClockSignal(CS_MCLK, CS_HFXTCLK_SELECT, CS_CLOCK_DIVIDER_1);
 
-    /* Set SysTick to 48MHz/1000=48kHz*/
+    /* Set SysTick to 48MHz/1200=40kHz*/
     MAP_SysTick_enableModule();
-    MAP_SysTick_setPeriod(1000);
+    MAP_SysTick_setPeriod(960); // 960 for 50kHz
     MAP_Interrupt_enableSleepOnIsrExit();
     MAP_SysTick_enableInterrupt();
 
@@ -97,6 +106,7 @@ int main(void){
     /* Configuring Sample Timer */
     MAP_ADC14_enableSampleTimer(ADC_MANUAL_ITERATION);
 
+
      /* Enabling MASTER interrupts */
     MAP_Interrupt_enableMaster();
 
@@ -105,19 +115,72 @@ int main(void){
     }
 }
 
+float bandstop_filter(float value){
+//    Z1[0] = Z1[1];
+//    Z1[], Z1[2], value};
+//    Z2[] = {Z2[1], Z2[2], (Z1[0] + Z1[2]) -   1.080f * Z1[1] + (-0.775f * Z2[1]) + (  0.959f * Z2[2])};
+    Z1[0][0] = Z1[0][1];
+    Z1[0][1] = Z1[0][2];
+    Z1[0][2] = value;
+    Z1[1][0] = Z1[1][1];
+    Z1[1][1] = Z1[1][2];
+//    Z2[2] =   (Z1[0] + Z1[2]) -   1.080f * Z1[1] + (-0.775f * Z2[0]) + (  0.959f * Z2[1]);
+    Z1[1][2] = (Z1[0][0] + Z1[0][2]) - A1[0] * Z1[0][1] + (-1*A1[1] * Z1[1][0]) + (A1[2] * Z1[1][1]);
+    return Z1[1][2];
+}
+
+//float bandstop_filter2(float value){
+////    Z1[0] = Z1[1];
+////    Z1[], Z1[2], value};
+////    Z2[] = {Z2[1], Z2[2], (Z1[0] + Z1[2]) -   1.080f * Z1[1] + (-0.775f * Z2[1]) + (  0.959f * Z2[2])};
+//    Z2[0][0] = Z2[0][1];
+//    Z2[0][1] = Z2[0][2];
+//    Z2[0][2] = value;
+//    Z2[1][0] = Z2[1][1];
+//    Z2[1][1] = Z2[1][2];
+////    Z2[2] =   (Z2[0] + Z2[2]) -   1.080f * Z2[1] + (-0.775f * Z2[0]) + (  0.959f * Z2[1]);
+//    Z2[1][2] =   (Z2[0][0] + Z2[0][2]) - A2[0] * Z2[0][1] + (-1*A2[1] * Z2[1][0]) + (A2[2] * Z2[1][1]);
+//    return Z2[1][2];
+//}
+float lowpass_filter(float value) {
+    Z2[0][0] = Z2[0][1];
+    Z2[0][1] = Z2[0][2];
+    Z2[0][2] = value / 3.589405582f;
+    Z2[1][0] = Z2[1][1];
+    Z2[1][1] = Z2[1][2];
+//    Z2[1][2] = (Z2[0][0] + Z2[0][2]) + 2 * Z2[0][1]  + ( -0.4918122372 * Z2[1][0]) + (  1.3072850288 * Z2[1][1]);;
+    Z2[1][2] = (Z2[0][0] + Z2[0][2]) + 2 * Z2[0][1] + ( -0.5869195081f * Z2[1][0]) + (  1.4754804436f * Z2[1][1]);
+
+//            (Z2[0][0] + Z2[0][2]) -   1.4547966009f * Z2[0][1] + ( -0.2905268567f * Z2[1][0]) + (  0.9387270423f * Z2[1][1]);
+    return Z2[1][2];
+}
+
+
 void SysTick_Handler(void)
 {
-    double buffer = ADC14_getResult(ADC_MEM0);  //Get the conversion result.
+    P6OUT |= BIT0;
+    float buffer = ADC14_getResult(ADC_MEM0);  //Get the conversion result.
+
+
+    buffer = bandstop_filter(buffer);// * sine[sine_index];
+//    buffer -= 512;
+    buffer = buffer * sine[sine_index];
+//    buffer += 512;
+    buffer = lowpass_filter(buffer);
+
+//    float buffer = 256*(1+sine[sine_index]);
 
-    descramble(&buffer, Timer, B1, A1, B2, A2, Z1, Z2);
+    P4OUT = buffer / 4;  //We do this because the ADC is set to use 10 bits but P2OUT is only 8 bits.
 
-    Timer += 1/fs;
-    P2OUT = buffer / 4;  //We do this because the ADC is set to use 10 bits but P2OUT is only 8 bits.
+    /* calculating sine index */
+    sine_index++;
+    if (sine_index >= SINE_SIZE) sine_index = 0;
 
     /* Enabling/Toggling Conversion */
     MAP_ADC14_enableConversion();
     MAP_ADC14_toggleConversionTrigger();
     MAP_ADC14_toggleConversionTrigger();
+    P6OUT &= ~BIT0;
 }
 
 void ADC14_IRQHandler(void)