FFT and nRF24 transmission / LaserGame 1A
Dependencies: mbed nRF24L01P mbed-dsp
main.cpp@3:627354c99ba0, 2022-03-21 (annotated)
- Committer:
- villemejane
- Date:
- Mon Mar 21 15:08:26 2022 +0000
- Revision:
- 3:627354c99ba0
- Parent:
- 1:47d90ce030a3
FFT and nRF24 transmission / LaserGame 1A
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
villemejane | 1:47d90ce030a3 | 1 | /****************************************************************************/ |
villemejane | 1:47d90ce030a3 | 2 | /* FFT d'un signal - Nucleo */ |
villemejane | 1:47d90ce030a3 | 3 | /****************************************************************************/ |
villemejane | 1:47d90ce030a3 | 4 | /* LEnsE / Julien VILLEMEJANE / Institut d'Optique Graduate School */ |
villemejane | 1:47d90ce030a3 | 5 | /****************************************************************************/ |
villemejane | 1:47d90ce030a3 | 6 | /* Brochage */ |
villemejane | 1:47d90ce030a3 | 7 | /* TO COMPLETE */ |
villemejane | 1:47d90ce030a3 | 8 | /****************************************************************************/ |
villemejane | 1:47d90ce030a3 | 9 | /* Test réalisé sur Nucléo-L476RG */ |
villemejane | 1:47d90ce030a3 | 10 | /****************************************************************************/ |
villemejane | 1:47d90ce030a3 | 11 | |
villemejane | 0:96c89b4dc711 | 12 | #include "mbed.h" |
villemejane | 0:96c89b4dc711 | 13 | #include "arm_math.h" |
villemejane | 0:96c89b4dc711 | 14 | #include "dsp.h" |
villemejane | 0:96c89b4dc711 | 15 | #include "arm_common_tables.h" |
villemejane | 0:96c89b4dc711 | 16 | #include "arm_const_structs.h" |
villemejane | 3:627354c99ba0 | 17 | #include "nRF24L01P.h" |
villemejane | 0:96c89b4dc711 | 18 | |
villemejane | 3:627354c99ba0 | 19 | #define TRANSFER_SIZE 8 |
villemejane | 0:96c89b4dc711 | 20 | #define SAMPLES 512 /* 256 real party and 256 imaginary parts */ |
villemejane | 0:96c89b4dc711 | 21 | #define FFT_SIZE SAMPLES / 2 /* FFT size is always the same size as we have samples, so 256 in our case */ |
villemejane | 0:96c89b4dc711 | 22 | |
villemejane | 0:96c89b4dc711 | 23 | float32_t Input[SAMPLES]; |
villemejane | 0:96c89b4dc711 | 24 | float32_t Output[FFT_SIZE]; |
villemejane | 0:96c89b4dc711 | 25 | bool trig=0; |
villemejane | 0:96c89b4dc711 | 26 | int indice = 0; |
villemejane | 0:96c89b4dc711 | 27 | |
villemejane | 3:627354c99ba0 | 28 | DigitalOut myled(D13); |
villemejane | 3:627354c99ba0 | 29 | AnalogIn myADC(A0); |
villemejane | 3:627354c99ba0 | 30 | Serial debug_pc(USBTX, USBRX); |
villemejane | 3:627354c99ba0 | 31 | Ticker timer; |
villemejane | 3:627354c99ba0 | 32 | |
villemejane | 3:627354c99ba0 | 33 | // nRF24 |
villemejane | 3:627354c99ba0 | 34 | char k; |
villemejane | 3:627354c99ba0 | 35 | char dataToSend[TRANSFER_SIZE] = {0xAA, 0x01, 0x10, 0xF0,0xAA, 0x01, 0x10, 0xF0}; |
villemejane | 3:627354c99ba0 | 36 | char dataReceived[TRANSFER_SIZE] = {0}; |
villemejane | 3:627354c99ba0 | 37 | char rxDataCnt; |
villemejane | 3:627354c99ba0 | 38 | |
villemejane | 3:627354c99ba0 | 39 | nRF24L01P nRF24_mod(D11, D12, D13, D10, D9, PB_8); |
villemejane | 3:627354c99ba0 | 40 | // MOSI, MISO, SCK, CSN, CE, IRQ |
villemejane | 3:627354c99ba0 | 41 | |
villemejane | 3:627354c99ba0 | 42 | #define largeur_signal_carre_equipe1 4300 /* a corriger */ |
villemejane | 3:627354c99ba0 | 43 | #define largeur_signal_carre_equipe2 40 /* a corriger */ |
villemejane | 3:627354c99ba0 | 44 | #define epsilon 200 |
villemejane | 3:627354c99ba0 | 45 | |
villemejane | 3:627354c99ba0 | 46 | /* definition des entrees des 3 capteurs: epaule droit, gauche et ventre */ |
villemejane | 3:627354c99ba0 | 47 | AnalogIn cap_ventre(A0); |
villemejane | 3:627354c99ba0 | 48 | AnalogIn cap_epaule_droit(A1); |
villemejane | 3:627354c99ba0 | 49 | AnalogIn cap_epaule_gauche(A2); |
villemejane | 3:627354c99ba0 | 50 | |
villemejane | 3:627354c99ba0 | 51 | float32_t Input_ventre[SAMPLES]; |
villemejane | 3:627354c99ba0 | 52 | float32_t Output_ventre[FFT_SIZE]; |
villemejane | 3:627354c99ba0 | 53 | |
villemejane | 3:627354c99ba0 | 54 | float32_t Input_epaule_droit[SAMPLES]; |
villemejane | 3:627354c99ba0 | 55 | float32_t Output_epaule_droit[FFT_SIZE]; |
villemejane | 3:627354c99ba0 | 56 | |
villemejane | 3:627354c99ba0 | 57 | float32_t Input_epaule_gauche[SAMPLES]; |
villemejane | 3:627354c99ba0 | 58 | float32_t Output_epaule_gauche[FFT_SIZE]; |
villemejane | 3:627354c99ba0 | 59 | |
villemejane | 3:627354c99ba0 | 60 | int nbr_pts_equipe1; |
villemejane | 3:627354c99ba0 | 61 | int nbr_pts_equipe2; |
villemejane | 3:627354c99ba0 | 62 | |
villemejane | 3:627354c99ba0 | 63 | // Fonction d'initialisation du module BT nRF24L01 |
villemejane | 3:627354c99ba0 | 64 | void initNRF24(){ |
villemejane | 3:627354c99ba0 | 65 | nRF24_mod.powerUp(); |
villemejane | 3:627354c99ba0 | 66 | wait_us(100000); |
villemejane | 3:627354c99ba0 | 67 | nRF24_mod.setAirDataRate(NRF24L01P_DATARATE_250_KBPS); |
villemejane | 3:627354c99ba0 | 68 | nRF24_mod.setRfFrequency(2400); |
villemejane | 3:627354c99ba0 | 69 | wait_us(100000); |
villemejane | 3:627354c99ba0 | 70 | debug_pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", nRF24_mod.getRfFrequency() ); |
villemejane | 3:627354c99ba0 | 71 | debug_pc.printf( "nRF24L01+ Output power : %d dBm\r\n", nRF24_mod.getRfOutputPower() ); |
villemejane | 3:627354c99ba0 | 72 | debug_pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", nRF24_mod.getAirDataRate() ); |
villemejane | 3:627354c99ba0 | 73 | debug_pc.printf( "Transfers are grouped into %d characters\r\n", TRANSFER_SIZE ); |
villemejane | 3:627354c99ba0 | 74 | nRF24_mod.setTransferSize( TRANSFER_SIZE ); |
villemejane | 3:627354c99ba0 | 75 | nRF24_mod.setReceiveMode(); |
villemejane | 3:627354c99ba0 | 76 | nRF24_mod.enable(); |
villemejane | 3:627354c99ba0 | 77 | } |
villemejane | 3:627354c99ba0 | 78 | |
villemejane | 3:627354c99ba0 | 79 | // Fonction de test du module BT nRF24L01 |
villemejane | 3:627354c99ba0 | 80 | void testNRF24(void){ |
villemejane | 3:627354c99ba0 | 81 | /* Lecture donnée depuis nRF24 */ |
villemejane | 3:627354c99ba0 | 82 | if ( nRF24_mod.readable() ) { |
villemejane | 3:627354c99ba0 | 83 | |
villemejane | 3:627354c99ba0 | 84 | // ...read the data into the receive buffer |
villemejane | 3:627354c99ba0 | 85 | rxDataCnt = nRF24_mod.read( NRF24L01P_PIPE_P0, dataReceived, TRANSFER_SIZE); |
villemejane | 3:627354c99ba0 | 86 | |
villemejane | 3:627354c99ba0 | 87 | // Display the receive buffer contents via the host serial link |
villemejane | 3:627354c99ba0 | 88 | debug_pc.printf("\tD = "); |
villemejane | 3:627354c99ba0 | 89 | for ( int i = 0; i < rxDataCnt; i++ ) { |
villemejane | 3:627354c99ba0 | 90 | debug_pc.printf(" %x \t", dataReceived[i]); |
villemejane | 3:627354c99ba0 | 91 | } |
villemejane | 3:627354c99ba0 | 92 | debug_pc.printf("\r\n"); |
villemejane | 3:627354c99ba0 | 93 | } |
villemejane | 3:627354c99ba0 | 94 | /* Transmission donnée depuis nRF24 */ |
villemejane | 3:627354c99ba0 | 95 | if(dataReceived[0] == 0){ |
villemejane | 3:627354c99ba0 | 96 | nRF24_mod.setRfFrequency(2400); |
villemejane | 3:627354c99ba0 | 97 | nRF24_mod.write( NRF24L01P_PIPE_P0, dataToSend, TRANSFER_SIZE ); |
villemejane | 3:627354c99ba0 | 98 | debug_pc.printf( "SENDED\r\n"); |
villemejane | 3:627354c99ba0 | 99 | } |
villemejane | 3:627354c99ba0 | 100 | } |
villemejane | 3:627354c99ba0 | 101 | |
villemejane | 3:627354c99ba0 | 102 | // echantillonnage |
villemejane | 0:96c89b4dc711 | 103 | void sample(){ |
villemejane | 0:96c89b4dc711 | 104 | myled = 1; |
villemejane | 0:96c89b4dc711 | 105 | if(indice < SAMPLES){ |
villemejane | 3:627354c99ba0 | 106 | Input_ventre[indice] = cap_ventre.read() - 0.5f; |
villemejane | 3:627354c99ba0 | 107 | Input_epaule_droit[indice] = cap_ventre.read() - 0.5f; |
villemejane | 3:627354c99ba0 | 108 | Input_epaule_droit[indice] = cap_ventre.read() - 0.5f; |
villemejane | 3:627354c99ba0 | 109 | |
villemejane | 3:627354c99ba0 | 110 | //Real part NB removing DC offset |
villemejane | 3:627354c99ba0 | 111 | Input_ventre[indice + 1] = 0; |
villemejane | 3:627354c99ba0 | 112 | Input_epaule_droit[indice + 1] = 0; |
villemejane | 3:627354c99ba0 | 113 | Input_epaule_gauche[indice + 1] = 0; |
villemejane | 3:627354c99ba0 | 114 | |
villemejane | 3:627354c99ba0 | 115 | //Imaginary Part set to zero |
villemejane | 0:96c89b4dc711 | 116 | indice += 2; |
villemejane | 0:96c89b4dc711 | 117 | } |
villemejane | 0:96c89b4dc711 | 118 | else{ trig = 0; } |
villemejane | 0:96c89b4dc711 | 119 | myled = 0; |
villemejane | 0:96c89b4dc711 | 120 | } |
villemejane | 0:96c89b4dc711 | 121 | |
villemejane | 0:96c89b4dc711 | 122 | int main() { |
villemejane | 3:627354c99ba0 | 123 | float maxValue_ventre; // Max FFT value is stored here (la largeur du caré envoyé par la led) |
villemejane | 3:627354c99ba0 | 124 | float maxValue_epaule_droit; |
villemejane | 3:627354c99ba0 | 125 | float maxValue_epaule_gauche; |
villemejane | 3:627354c99ba0 | 126 | |
villemejane | 3:627354c99ba0 | 127 | uint32_t maxIndex_ventre; // Index in Output array where max value is |
villemejane | 3:627354c99ba0 | 128 | uint32_t maxIndex_epaule_droit; |
villemejane | 3:627354c99ba0 | 129 | uint32_t maxIndex_epaule_gauche; |
villemejane | 3:627354c99ba0 | 130 | |
villemejane | 3:627354c99ba0 | 131 | initNRF24(); |
villemejane | 0:96c89b4dc711 | 132 | while(1) { |
villemejane | 0:96c89b4dc711 | 133 | if(trig == 0){ |
villemejane | 0:96c89b4dc711 | 134 | timer.detach(); |
villemejane | 0:96c89b4dc711 | 135 | // Init the Complex FFT module, intFlag = 0, doBitReverse = 1 |
villemejane | 0:96c89b4dc711 | 136 | //NB using predefined arm_cfft_sR_f32_lenXXX, in this case XXX is 256 |
villemejane | 3:627354c99ba0 | 137 | arm_cfft_f32(&arm_cfft_sR_f32_len256, Input_ventre, 0, 1); |
villemejane | 3:627354c99ba0 | 138 | arm_cfft_f32(&arm_cfft_sR_f32_len256, Input_epaule_droit, 0, 1); |
villemejane | 3:627354c99ba0 | 139 | arm_cfft_f32(&arm_cfft_sR_f32_len256, Input_epaule_gauche, 0, 1); |
villemejane | 0:96c89b4dc711 | 140 | |
villemejane | 0:96c89b4dc711 | 141 | // Complex Magniture Module put results into Output(Half size of the Input) |
villemejane | 3:627354c99ba0 | 142 | arm_cmplx_mag_f32(Input_ventre, Output_ventre, FFT_SIZE); |
villemejane | 3:627354c99ba0 | 143 | arm_cmplx_mag_f32(Input_ventre, Output_epaule_droit, FFT_SIZE); |
villemejane | 3:627354c99ba0 | 144 | arm_cmplx_mag_f32(Input_ventre, Output_epaule_gauche, FFT_SIZE); |
villemejane | 3:627354c99ba0 | 145 | |
villemejane | 3:627354c99ba0 | 146 | Output_ventre[0] = 0; |
villemejane | 3:627354c99ba0 | 147 | Output_epaule_droit[0] = 0; |
villemejane | 3:627354c99ba0 | 148 | Output_epaule_gauche[0] = 0; |
villemejane | 0:96c89b4dc711 | 149 | Output[0] = 0; |
villemejane | 3:627354c99ba0 | 150 | //Calculates maxValue and returns corresponding value |
villemejane | 3:627354c99ba0 | 151 | arm_max_f32(Output_ventre, FFT_SIZE/2, &maxValue_ventre, &maxIndex_ventre); |
villemejane | 3:627354c99ba0 | 152 | arm_max_f32(Output_epaule_droit, FFT_SIZE/2, &maxValue_epaule_droit, &maxIndex_epaule_droit); |
villemejane | 3:627354c99ba0 | 153 | arm_max_f32(Output_epaule_gauche, FFT_SIZE/2, &maxValue_epaule_gauche, &maxIndex_epaule_gauche); |
villemejane | 3:627354c99ba0 | 154 | |
villemejane | 3:627354c99ba0 | 155 | //Envoie du nombre de point au ordinateur par bluetooth |
villemejane | 3:627354c99ba0 | 156 | dataToSend[1] = 100; |
villemejane | 3:627354c99ba0 | 157 | nRF24_mod.write( NRF24L01P_PIPE_P0, dataToSend, TRANSFER_SIZE ); |
villemejane | 3:627354c99ba0 | 158 | |
villemejane | 3:627354c99ba0 | 159 | // Calcul des points |
villemejane | 3:627354c99ba0 | 160 | //Tire sur un coéquipier : |
villemejane | 3:627354c99ba0 | 161 | if (maxValue_ventre<(largeur_signal_carre_equipe1+epsilon) && maxValue_ventre>(largeur_signal_carre_equipe1-epsilon) ) |
villemejane | 3:627354c99ba0 | 162 | { |
villemejane | 3:627354c99ba0 | 163 | nbr_pts_equipe1=nbr_pts_equipe1-20; // -20pts pour l'équipe du tireur |
villemejane | 3:627354c99ba0 | 164 | } |
villemejane | 3:627354c99ba0 | 165 | if (maxValue_epaule_droit<(largeur_signal_carre_equipe1+epsilon) && maxValue_epaule_droit>(largeur_signal_carre_equipe1-epsilon) ) |
villemejane | 3:627354c99ba0 | 166 | { |
villemejane | 3:627354c99ba0 | 167 | nbr_pts_equipe1=nbr_pts_equipe1-20; // -20pts pour l'équipe du tireur |
villemejane | 0:96c89b4dc711 | 168 | } |
villemejane | 3:627354c99ba0 | 169 | if (maxValue_epaule_gauche<(largeur_signal_carre_equipe1+epsilon) && maxValue_epaule_gauche>(largeur_signal_carre_equipe1-epsilon) ) |
villemejane | 3:627354c99ba0 | 170 | { |
villemejane | 3:627354c99ba0 | 171 | nbr_pts_equipe1=nbr_pts_equipe1-20; // -20pts pour l'équipe du tireur |
villemejane | 3:627354c99ba0 | 172 | } |
villemejane | 3:627354c99ba0 | 173 | |
villemejane | 3:627354c99ba0 | 174 | //Tire sur le ventre |
villemejane | 3:627354c99ba0 | 175 | if (maxValue_ventre<(largeur_signal_carre_equipe2+epsilon) && maxValue_ventre>(largeur_signal_carre_equipe2-epsilon)) |
villemejane | 3:627354c99ba0 | 176 | { |
villemejane | 3:627354c99ba0 | 177 | nbr_pts_equipe1=nbr_pts_equipe1-10; //Tire sur le ventre : -10pts pour l'équipe de la victime |
villemejane | 3:627354c99ba0 | 178 | nbr_pts_equipe2=nbr_pts_equipe2+50; //Tire sur le ventre : +50pts pour l'équipe du tireur |
villemejane | 3:627354c99ba0 | 179 | } |
villemejane | 3:627354c99ba0 | 180 | |
villemejane | 3:627354c99ba0 | 181 | //Tire sur l'épaule |
villemejane | 3:627354c99ba0 | 182 | if ((maxValue_epaule_droit<(largeur_signal_carre_equipe2+epsilon))&& (maxValue_epaule_droit>(largeur_signal_carre_equipe2-epsilon)) || ((maxValue_epaule_gauche<(largeur_signal_carre_equipe2+epsilon))&& (maxValue_epaule_gauche>(largeur_signal_carre_equipe2-epsilon)))) |
villemejane | 3:627354c99ba0 | 183 | { |
villemejane | 3:627354c99ba0 | 184 | nbr_pts_equipe1=nbr_pts_equipe1-10; //Tire sur l'épaule : -10pts pour l'équipe de la victime |
villemejane | 3:627354c99ba0 | 185 | nbr_pts_equipe2=nbr_pts_equipe2+100; //Tire sur l'épaule : +100pts pour l'équipe du tireur |
villemejane | 3:627354c99ba0 | 186 | } |
villemejane | 3:627354c99ba0 | 187 | |
villemejane | 3:627354c99ba0 | 188 | // Relance de l'acquisition |
villemejane | 0:96c89b4dc711 | 189 | trig = 1; |
villemejane | 0:96c89b4dc711 | 190 | indice = 0; |
villemejane | 0:96c89b4dc711 | 191 | timer.attach_us(&sample,40); //20us 50KHz sampling rate |
villemejane | 0:96c89b4dc711 | 192 | } |
villemejane | 0:96c89b4dc711 | 193 | } |
villemejane | 0:96c89b4dc711 | 194 | } |