DSP program for the Surfboard hardware (PCB to be open sourced) http://www.avbotz.com/ourauv/electrical/signal-processing/
Dependencies: MODDMA SimpleIOMacros mbed-dsp mbed
surfboard.cpp@1:f69ec4c889ff, 2013-08-02 (annotated)
- Committer:
- avbotz
- Date:
- Fri Aug 02 02:25:12 2013 +0000
- Revision:
- 1:f69ec4c889ff
- Parent:
- 0:2381a319fc35
Initial commit. Not working. MODDMA makes DMA setup slow, and the mbed SPI peripheral corrupts data sometimes. I will write my own SPI and DMA classes and see how it goes.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
avbotz | 0:2381a319fc35 | 1 | // I think we should move this to the offline compiler so we can get loop unrolling. -- Kevin |
avbotz | 0:2381a319fc35 | 2 | // TODO: code size is small (4% of available space) so we should totally do loop unrolling |
avbotz | 0:2381a319fc35 | 3 | |
avbotz | 0:2381a319fc35 | 4 | #include "surfboard.h" |
avbotz | 0:2381a319fc35 | 5 | |
avbotz | 0:2381a319fc35 | 6 | // Might have to use IO Macros library for fast digital I/O http://mbed.org/users/AjK/code/SimpleIOMacros/ |
avbotz | 0:2381a319fc35 | 7 | // less abstracted InterruptIn http://mbed.org/forum/mbed/topic/2326/ |
avbotz | 0:2381a319fc35 | 8 | //InterruptIn drdy(p8); // active low |
avbotz | 0:2381a319fc35 | 9 | // interruptin too slow http://mbed.org/forum/mbed/topic/2326/ |
avbotz | 0:2381a319fc35 | 10 | DigitalOut sync(p14); // active low |
avbotz | 0:2381a319fc35 | 11 | |
avbotz | 0:2381a319fc35 | 12 | //Serial submarine(/*tx*/ p28, /*rx*/ p27); |
avbotz | 0:2381a319fc35 | 13 | Serial submarine(USBTX, USBRX); |
avbotz | 0:2381a319fc35 | 14 | // From sub to hydrophone board: |
avbotz | 0:2381a319fc35 | 15 | // r = start running |
avbotz | 0:2381a319fc35 | 16 | // s = stop running |
avbotz | 0:2381a319fc35 | 17 | // From hydrophone board to sub: |
avbotz | 0:2381a319fc35 | 18 | // h = hello world |
avbotz | 0:2381a319fc35 | 19 | // b = turned off because battery low |
avbotz | 0:2381a319fc35 | 20 | |
avbotz | 0:2381a319fc35 | 21 | DigitalOut avdd_ctl(p15); |
avbotz | 0:2381a319fc35 | 22 | |
avbotz | 0:2381a319fc35 | 23 | AnalogIn abat_sense(p16); |
avbotz | 0:2381a319fc35 | 24 | // We don't want the battery to go below 6 V. |
avbotz | 0:2381a319fc35 | 25 | // TODO: read the battery after each IIR batch? |
avbotz | 0:2381a319fc35 | 26 | // Calculate what the minimum battery voltage should be after it goes through |
avbotz | 0:2381a319fc35 | 27 | // the voltage divider and mbed AnalogIn ADC. |
avbotz | 0:2381a319fc35 | 28 | #define ABAT_RATIO ((2.21f / 6.95f) / 3.3f) |
avbotz | 0:2381a319fc35 | 29 | #define ABAT_THRESH_LO (ABAT_RATIO * 6.0f) |
avbotz | 0:2381a319fc35 | 30 | #define ABAT_THRESH_HI (ABAT_RATIO * 6.5f) |
avbotz | 0:2381a319fc35 | 31 | Ticker abat_tick; |
avbotz | 0:2381a319fc35 | 32 | |
avbotz | 0:2381a319fc35 | 33 | // On Surfboard v1.0, setting these does not actually control the power because |
avbotz | 0:2381a319fc35 | 34 | // of the modifications on the board. |
avbotz | 0:2381a319fc35 | 35 | DigitalOut clkvdd(p9), iovdd(p10); |
avbotz | 0:2381a319fc35 | 36 | |
avbotz | 0:2381a319fc35 | 37 | volatile bool fresh_data = false; |
avbotz | 0:2381a319fc35 | 38 | |
avbotz | 0:2381a319fc35 | 39 | // lights for debugging |
avbotz | 0:2381a319fc35 | 40 | // Replacing with SimpleIOMacros.h because each toggle takes ~300 ns (!!) |
avbotz | 0:2381a319fc35 | 41 | DigitalOut led1(LED1), // blinking: wait for run command. solid: ready |
avbotz | 0:2381a319fc35 | 42 | led2(LED2), // in function get_data() |
avbotz | 0:2381a319fc35 | 43 | led3(LED3), // toggles when IIR filter is run |
avbotz | 0:2381a319fc35 | 44 | led4(LED4); // toggles on fresh data |
avbotz | 0:2381a319fc35 | 45 | // Counts how many times we toggled the LED |
avbotz | 0:2381a319fc35 | 46 | uint32_t led1num, led2num, led3num, led4num; |
avbotz | 0:2381a319fc35 | 47 | Ticker* led1ticks[2]; |
avbotz | 0:2381a319fc35 | 48 | |
avbotz | 0:2381a319fc35 | 49 | int i_timestamps[NUM_CHANNELS]; |
avbotz | 0:2381a319fc35 | 50 | |
avbotz | 0:2381a319fc35 | 51 | // For saving received data to disk |
avbotz | 0:2381a319fc35 | 52 | LocalFileSystem local("local"); |
avbotz | 0:2381a319fc35 | 53 | bool log_data_enabled = true; |
avbotz | 0:2381a319fc35 | 54 | |
avbotz | 0:2381a319fc35 | 55 | // Call this function to reset the mbed |
avbotz | 0:2381a319fc35 | 56 | extern "C" void mbed_reset(); |
avbotz | 0:2381a319fc35 | 57 | |
avbotz | 0:2381a319fc35 | 58 | int main() |
avbotz | 0:2381a319fc35 | 59 | { |
avbotz | 0:2381a319fc35 | 60 | pre_setup(); // For safety, this must be the first thing that runs |
avbotz | 0:2381a319fc35 | 61 | |
avbotz | 0:2381a319fc35 | 62 | /*for (int i = 0; i < sizeof(parsed)/sizeof(uint16_t); i++) |
avbotz | 0:2381a319fc35 | 63 | { |
avbotz | 0:2381a319fc35 | 64 | *(((int16_t*)parsed) + i) = 12345; |
avbotz | 0:2381a319fc35 | 65 | }*/ |
avbotz | 0:2381a319fc35 | 66 | |
avbotz | 0:2381a319fc35 | 67 | wait(5); |
avbotz | 0:2381a319fc35 | 68 | |
avbotz | 0:2381a319fc35 | 69 | led1ticks[0] = new Ticker(); |
avbotz | 0:2381a319fc35 | 70 | led1ticks[1] = new Ticker(); |
avbotz | 0:2381a319fc35 | 71 | |
avbotz | 0:2381a319fc35 | 72 | led1 = 1; |
avbotz | 0:2381a319fc35 | 73 | led1ticks[0]->attach(&led1_cb, 1.0); |
avbotz | 0:2381a319fc35 | 74 | wait_ms(60); |
avbotz | 0:2381a319fc35 | 75 | led1ticks[1]->attach(&led1_cb, 1.0); |
avbotz | 0:2381a319fc35 | 76 | led1 = 0; |
avbotz | 0:2381a319fc35 | 77 | |
avbotz | 0:2381a319fc35 | 78 | submarine.baud(115200); |
avbotz | 0:2381a319fc35 | 79 | submarine.putc('h'); |
avbotz | 0:2381a319fc35 | 80 | submarine.printf("%d ", sizeof(q15_t)); |
avbotz | 0:2381a319fc35 | 81 | |
avbotz | 0:2381a319fc35 | 82 | // Wait until we are told to turn on |
avbotz | 0:2381a319fc35 | 83 | //while (submarine.getc() != 'r'); |
avbotz | 0:2381a319fc35 | 84 | |
avbotz | 0:2381a319fc35 | 85 | submarine.attach(&submarine_cb); |
avbotz | 0:2381a319fc35 | 86 | led1ticks[0]->detach(); |
avbotz | 0:2381a319fc35 | 87 | led1ticks[1]->detach(); |
avbotz | 0:2381a319fc35 | 88 | |
avbotz | 0:2381a319fc35 | 89 | setup(); |
avbotz | 0:2381a319fc35 | 90 | |
avbotz | 0:2381a319fc35 | 91 | while (true) |
avbotz | 0:2381a319fc35 | 92 | { |
avbotz | 0:2381a319fc35 | 93 | if (fresh_data) |
avbotz | 0:2381a319fc35 | 94 | { |
avbotz | 0:2381a319fc35 | 95 | fresh_data = false; |
avbotz | 0:2381a319fc35 | 96 | |
avbotz | 0:2381a319fc35 | 97 | LED4_TOGGLE; |
avbotz | 0:2381a319fc35 | 98 | |
avbotz | 0:2381a319fc35 | 99 | if (log_data_enabled /*&& read_blocks > 3*/) |
avbotz | 0:2381a319fc35 | 100 | { |
avbotz | 0:2381a319fc35 | 101 | // Disable DRDY interrupt to prevent the buffer from changing |
avbotz | 0:2381a319fc35 | 102 | // while we save it. Writing to disk takes a long time. |
avbotz | 0:2381a319fc35 | 103 | // drdy.rise(NULL); |
avbotz | 0:2381a319fc35 | 104 | // Only log data once |
avbotz | 0:2381a319fc35 | 105 | log_data_enabled = false; |
avbotz | 0:2381a319fc35 | 106 | |
avbotz | 0:2381a319fc35 | 107 | FILE *fp = fopen("/local/out.csv", "w"); |
avbotz | 0:2381a319fc35 | 108 | |
avbotz | 0:2381a319fc35 | 109 | fprintf(fp, "Index,Ch1,Ch2,Ch3,Ch4\n"); |
avbotz | 0:2381a319fc35 | 110 | for (int i = 0; i < BLOCK_SIZE; i++) |
avbotz | 0:2381a319fc35 | 111 | { |
avbotz | 0:2381a319fc35 | 112 | fprintf(fp, "%d", i); |
avbotz | 0:2381a319fc35 | 113 | for (int j = 0; j < NUM_CHANNELS; j++) |
avbotz | 0:2381a319fc35 | 114 | { |
avbotz | 0:2381a319fc35 | 115 | fprintf(fp, ",%02X%02X%02X", parsed[write_block^1][j][i][0], parsed[write_block^1][j][i][1], parsed[write_block^1][j][i][2]); |
avbotz | 0:2381a319fc35 | 116 | //int16_t n = parsed[write_block^1][j][i]; |
avbotz | 0:2381a319fc35 | 117 | //fprintf(fp, ",%X", n); |
avbotz | 0:2381a319fc35 | 118 | } |
avbotz | 0:2381a319fc35 | 119 | fprintf(fp, "\n"); |
avbotz | 0:2381a319fc35 | 120 | } |
avbotz | 0:2381a319fc35 | 121 | |
avbotz | 0:2381a319fc35 | 122 | fclose(fp); |
avbotz | 0:2381a319fc35 | 123 | |
avbotz | 0:2381a319fc35 | 124 | // drdy.rise(&get_data); |
avbotz | 0:2381a319fc35 | 125 | } |
avbotz | 0:2381a319fc35 | 126 | |
avbotz | 0:2381a319fc35 | 127 | /*for (char i = 0; i < NUM_CHANNELS; i++) |
avbotz | 0:2381a319fc35 | 128 | { |
avbotz | 0:2381a319fc35 | 129 | arm_iir_lattice_q15(iir_filter+i, parsed[write_block^1][i], filtered, BLOCK_SIZE); |
avbotz | 0:2381a319fc35 | 130 | i_timestamps[i] = find_timestamp_tdoa(filtered); |
avbotz | 0:2381a319fc35 | 131 | }*/ |
avbotz | 0:2381a319fc35 | 132 | // hyperbolic positioning |
avbotz | 0:2381a319fc35 | 133 | // send it to the sub |
avbotz | 0:2381a319fc35 | 134 | } |
avbotz | 0:2381a319fc35 | 135 | } |
avbotz | 0:2381a319fc35 | 136 | } |
avbotz | 0:2381a319fc35 | 137 | |
avbotz | 0:2381a319fc35 | 138 | void pre_setup() |
avbotz | 0:2381a319fc35 | 139 | { |
avbotz | 0:2381a319fc35 | 140 | |
avbotz | 0:2381a319fc35 | 141 | } |
avbotz | 0:2381a319fc35 | 142 | |
avbotz | 0:2381a319fc35 | 143 | int setup() |
avbotz | 0:2381a319fc35 | 144 | { |
avbotz | 0:2381a319fc35 | 145 | int retValue = 0; |
avbotz | 0:2381a319fc35 | 146 | retValue += setup_ads1274(); |
avbotz | 0:2381a319fc35 | 147 | retValue += setup_dma(); |
avbotz | 0:2381a319fc35 | 148 | |
avbotz | 0:2381a319fc35 | 149 | led1 = 1; // Debug |
avbotz | 0:2381a319fc35 | 150 | |
avbotz | 0:2381a319fc35 | 151 | // Should be at the end of setup function, in case ADS1274 tries to send data |
avbotz | 0:2381a319fc35 | 152 | //drdy.rise(&get_data); |
avbotz | 0:2381a319fc35 | 153 | event_irq_init(); |
avbotz | 0:2381a319fc35 | 154 | |
avbotz | 0:2381a319fc35 | 155 | return retValue; |
avbotz | 0:2381a319fc35 | 156 | } |
avbotz | 0:2381a319fc35 | 157 | |
avbotz | 0:2381a319fc35 | 158 | // Return values |
avbotz | 0:2381a319fc35 | 159 | // 0: setup finished normally |
avbotz | 0:2381a319fc35 | 160 | // 1: setup aborted because battery voltage was too low |
avbotz | 0:2381a319fc35 | 161 | int setup_ads1274() |
avbotz | 0:2381a319fc35 | 162 | { |
avbotz | 0:2381a319fc35 | 163 | // Don't start up if the battery is too low |
avbotz | 0:2381a319fc35 | 164 | abat_cb(); |
avbotz | 0:2381a319fc35 | 165 | abat_tick.attach(&abat_cb, 1); |
avbotz | 0:2381a319fc35 | 166 | |
avbotz | 0:2381a319fc35 | 167 | // Sequence the power-on of the ADC |
avbotz | 0:2381a319fc35 | 168 | iovdd = 1; |
avbotz | 0:2381a319fc35 | 169 | __nop(); |
avbotz | 0:2381a319fc35 | 170 | avdd_ctl = 1; |
avbotz | 0:2381a319fc35 | 171 | // Give it some time, just in case. |
avbotz | 0:2381a319fc35 | 172 | wait_ms(5); |
avbotz | 0:2381a319fc35 | 173 | // Now we can start driving pins without destroying the ADS1274 |
avbotz | 0:2381a319fc35 | 174 | // Turn on sampling clock (CLK) |
avbotz | 0:2381a319fc35 | 175 | clkvdd = 1; |
avbotz | 0:2381a319fc35 | 176 | |
avbotz | 0:2381a319fc35 | 177 | // ADS1274 needs 2^18 clk cycles + 129 conversions before data are valid |
avbotz | 0:2381a319fc35 | 178 | // minimum wait time = 2^18 / f_clk + 129 / f_data = 2^18/27MHz + 129/0.1MHz = 11 ms |
avbotz | 0:2381a319fc35 | 179 | wait_ms(11 + 1); |
avbotz | 0:2381a319fc35 | 180 | |
avbotz | 0:2381a319fc35 | 181 | // Datasheet suggested resetting the ads1274 after start up to synchronize outputs |
avbotz | 0:2381a319fc35 | 182 | // Need to put it low for at least 1 CLK period |
avbotz | 0:2381a319fc35 | 183 | // On mbed, this is 96 MHz/27MHz = 3.556 instruction cycles |
avbotz | 0:2381a319fc35 | 184 | sync = 1; |
avbotz | 0:2381a319fc35 | 185 | // Drive *sync to logic low for about 4 instruction cycles |
avbotz | 0:2381a319fc35 | 186 | sync = 0; |
avbotz | 0:2381a319fc35 | 187 | __nop(); __nop(); __nop(); __nop(); // wait 4 cycles |
avbotz | 0:2381a319fc35 | 188 | sync = 1; |
avbotz | 0:2381a319fc35 | 189 | // Wait 1/f_data = 1/0.1MHz = 10 us for new data to be valid |
avbotz | 0:2381a319fc35 | 190 | wait_us(10 + 1); |
avbotz | 0:2381a319fc35 | 191 | |
avbotz | 0:2381a319fc35 | 192 | return 0; // success |
avbotz | 0:2381a319fc35 | 193 | } |
avbotz | 0:2381a319fc35 | 194 | |
avbotz | 0:2381a319fc35 | 195 | void teardown() |
avbotz | 0:2381a319fc35 | 196 | { |
avbotz | 0:2381a319fc35 | 197 | // Disable interrupts. Not sure if this is necessary, but I don't want samples to be read while we shut down |
avbotz | 0:2381a319fc35 | 198 | __disable_irq(); |
avbotz | 0:2381a319fc35 | 199 | |
avbotz | 0:2381a319fc35 | 200 | teardown_ads1274(); |
avbotz | 0:2381a319fc35 | 201 | teardown_dma(); |
avbotz | 0:2381a319fc35 | 202 | } |
avbotz | 0:2381a319fc35 | 203 | |
avbotz | 0:2381a319fc35 | 204 | void teardown_ads1274() |
avbotz | 0:2381a319fc35 | 205 | { |
avbotz | 0:2381a319fc35 | 206 | // Turn off peripherals, then turn off ADS1274 |
avbotz | 0:2381a319fc35 | 207 | // Turn off CLK |
avbotz | 0:2381a319fc35 | 208 | clkvdd = 0; |
avbotz | 0:2381a319fc35 | 209 | __nop(); __nop(); |
avbotz | 0:2381a319fc35 | 210 | // Turn off op amps and analog power to ADS1274 |
avbotz | 0:2381a319fc35 | 211 | avdd_ctl = 0; |
avbotz | 0:2381a319fc35 | 212 | // Turn off IO power |
avbotz | 0:2381a319fc35 | 213 | iovdd = 0; |
avbotz | 0:2381a319fc35 | 214 | } |
avbotz | 0:2381a319fc35 | 215 | |
avbotz | 0:2381a319fc35 | 216 | // Called when the submarine sends a message to us |
avbotz | 0:2381a319fc35 | 217 | void submarine_cb() |
avbotz | 0:2381a319fc35 | 218 | { |
avbotz | 0:2381a319fc35 | 219 | // Loop as long as there are more characters to be read |
avbotz | 0:2381a319fc35 | 220 | while (submarine.readable()) |
avbotz | 0:2381a319fc35 | 221 | { |
avbotz | 0:2381a319fc35 | 222 | // If we are told to stop |
avbotz | 0:2381a319fc35 | 223 | if (submarine.getc() == 's') |
avbotz | 0:2381a319fc35 | 224 | { |
avbotz | 0:2381a319fc35 | 225 | // Shut everything down |
avbotz | 0:2381a319fc35 | 226 | teardown(); |
avbotz | 0:2381a319fc35 | 227 | // Reset the mbed |
avbotz | 0:2381a319fc35 | 228 | mbed_reset(); |
avbotz | 0:2381a319fc35 | 229 | } |
avbotz | 0:2381a319fc35 | 230 | } |
avbotz | 0:2381a319fc35 | 231 | } |
avbotz | 0:2381a319fc35 | 232 | |
avbotz | 0:2381a319fc35 | 233 | // Called every few seconds to check the battery status |
avbotz | 0:2381a319fc35 | 234 | void abat_cb() |
avbotz | 0:2381a319fc35 | 235 | { |
avbotz | 0:2381a319fc35 | 236 | if (abat_sense < ABAT_THRESH_LO) |
avbotz | 0:2381a319fc35 | 237 | { |
avbotz | 0:2381a319fc35 | 238 | // Uh oh, stop the hydrophone board |
avbotz | 0:2381a319fc35 | 239 | teardown(); |
avbotz | 0:2381a319fc35 | 240 | |
avbotz | 0:2381a319fc35 | 241 | submarine.putc('b'); |
avbotz | 0:2381a319fc35 | 242 | led1 = led2 = led3 = led4 = 0; |
avbotz | 0:2381a319fc35 | 243 | |
avbotz | 0:2381a319fc35 | 244 | // Wait until someone changes the battery |
avbotz | 0:2381a319fc35 | 245 | int num_hi = 0, loop_count = 0;; |
avbotz | 0:2381a319fc35 | 246 | while (num_hi < 100) |
avbotz | 0:2381a319fc35 | 247 | { |
avbotz | 0:2381a319fc35 | 248 | if (abat_sense > ABAT_THRESH_HI) |
avbotz | 0:2381a319fc35 | 249 | { |
avbotz | 0:2381a319fc35 | 250 | num_hi++; |
avbotz | 0:2381a319fc35 | 251 | } |
avbotz | 0:2381a319fc35 | 252 | else if (num_hi > 0) |
avbotz | 0:2381a319fc35 | 253 | { |
avbotz | 0:2381a319fc35 | 254 | num_hi--; |
avbotz | 0:2381a319fc35 | 255 | } |
avbotz | 0:2381a319fc35 | 256 | |
avbotz | 0:2381a319fc35 | 257 | if (loop_count == 32000) |
avbotz | 0:2381a319fc35 | 258 | { |
avbotz | 0:2381a319fc35 | 259 | led1 = led2 = led3 = led4 = !led1; |
avbotz | 0:2381a319fc35 | 260 | loop_count = 0; |
avbotz | 0:2381a319fc35 | 261 | } |
avbotz | 0:2381a319fc35 | 262 | else |
avbotz | 0:2381a319fc35 | 263 | { |
avbotz | 0:2381a319fc35 | 264 | loop_count++; |
avbotz | 0:2381a319fc35 | 265 | } |
avbotz | 0:2381a319fc35 | 266 | } |
avbotz | 0:2381a319fc35 | 267 | |
avbotz | 0:2381a319fc35 | 268 | // On battery replacement, wait for the user to plug the battery in all the way |
avbotz | 0:2381a319fc35 | 269 | wait_ms(3000); |
avbotz | 0:2381a319fc35 | 270 | |
avbotz | 0:2381a319fc35 | 271 | mbed_reset(); |
avbotz | 0:2381a319fc35 | 272 | } |
avbotz | 0:2381a319fc35 | 273 | } |
avbotz | 0:2381a319fc35 | 274 | |
avbotz | 0:2381a319fc35 | 275 | void led1_cb() |
avbotz | 0:2381a319fc35 | 276 | { |
avbotz | 0:2381a319fc35 | 277 | led1 = !led1; |
avbotz | 0:2381a319fc35 | 278 | } |