Feat/acoustics by Wiacer · Pull Request #42 · vortexntnu/vortex-auv-embedded

Wiacer · 2026-04-24T17:58:20Z

It works, but needs a bit of polish

1 FDCAN 1 SPI master 3 SPI slaves 1 Usart

allow cli make clean all build project used to generate compile_json

remove all incompatible *f16.c from CMSIS.DSP remove all incompatible ARM cortex A (neon) from CMSIS_DSP remove all source files that include all source files in the same directory

First iteration, made completely by chatgtp

jorgenfj · 2026-04-24T19:23:46Z

bruh

Wiacer · 2026-04-25T00:56:52Z

bruh

Idk man, cyp ville ha deg med

forrisdahl

I do not know why, but the GitHub browser is insanely slow and difficult to mark where I want to comment, so I will make one larger comment instead.

My general impression of main is that it is big. It has a lot of functions, and it is not immediately clear which are important/used and which are not. I suggested splitting the debug or printf functions into another file. I understand that this might be challenging due to variable scope. The most important thing to see in the main.c file is the main function. I would like the main function to be as early as possible, with other functions defined below it but declared in the header or before main.

I think the code inside the while loop could also be split into functions. It does not have to be many functions, but as it is now, we end up with a lot of indentation. Function names could also make it easier to understand what each step is trying to achieve.

It is a bit random where there are comments and where there are not. For the most important functions, you should have Doxygen-style comments to explain what they do and what they expect.

//NOT TESTED AT TIME OF COMMIT// cleaned and modularized everything all that remains should be to implement dumping functionality and test Features added: CAN interface Proper state machine Stale Data detection

Left to implement: Dump commands Making it not error due to stale data every 10 seconds

Wiacer · 2026-04-27T01:52:20Z

Idk what i'm doing.
Anyway I fixed all the issues i think and majorly rebased and reorganized acoustics.
It errors out and resets every 10 seconds but it works (havent properly tested direction but i assume it works)

…SPI interface would fail. Fixed it. Acoustics is now megagood. Dump still not implemented

Wiacer · 2026-04-27T02:55:41Z

Fixed the erroring out thingy now.
Only optional thing missing is dump functionality but we'll only need it if it doesn't work to begin with

forrisdahl · 2026-04-30T12:10:01Z

+		while(((idxs[0] < dead_space) || (idxs[0] > (WORKSPACE_LEN-(WORKSPACE_OFFSET-1)*BLOCK_LEN))) && max_retries--){
+			float32_t threshold = dsp_min_max_lerp(processing_workspace[0], WORKSPACE_LEN, linear_threshold, n_upper_average, dead_space);
+			idxs[0] = dsp_rl_under_threshold_search(processing_workspace[0],WORKSPACE_LEN, threshold , PROCESSING_PATIENCE);
+			linear_threshold *= 2;
+		}
+		times_of_arrival[0] = (float32_t)idxs[0];
+
+		for(int i = 1; i < N_HYDROPHONES; i++){
+			float32_t linear_threshold = lerp_threshold;
+			uint8_t max_retries = 6;
+			uint32_t dead_space = n_lower_average;
+			idxs[i] = 0;
+			while(((utils_abs_int32(idxs[0] - idxs[i])) > max_idx_difference) && max_retries--){
+				float32_t threshold = dsp_min_max_lerp(processing_workspace[i], WORKSPACE_LEN, linear_threshold, n_upper_average, dead_space);
+				idxs[i] = dsp_rl_under_threshold_search(processing_workspace[i],WORKSPACE_LEN, threshold , PROCESSING_PATIENCE);
+				linear_threshold *= 2;
+			}
+			times_of_arrival[i] = (float32_t)idxs[i];
+		}
+
+		valid_buffers = 0;


Some of these conditions are quite cryptic, not immediately obvious what they are checking. You could have a static inline function to have a human readable name for the conditions.

You probably should split the first part of the function, (the searching?) to its own function. Currently function does a lot of things. Could have one function that finds the TDOA and validates the data and one that solves the position

…-auv-embedded into feat/acoustics

Wiacer · 2026-05-15T22:24:27Z

Aight now there are surely no other problems left

used outside of acoustics.c

forrisdahl · 2026-05-16T17:49:26Z

This looks a lot better, much easier to understand the acoustics processing.

What remains now is to clean up your branch. First of all I see that there are telemetry files I accidentally pushed on the branch that needs to be removed.

I think the old version of the firmware can be removed to save space. Both the old version and the new version includes CMSIS library which adds a lot more files to your branch than needs to be. In addition since I do not think we have done any changes cmsis library we should look for a way to avoid having it in our repo.

Most importantly i found and removed the test data that was still in the repo (i think)

forrisdahl

Hope your summer is going well so far! Here are some suggestions for improvement

forrisdahl · 2026-06-29T16:34:16Z

+float32_t find_idx_of_arrival_in_buffer(uint8_t buffer_idx){
+	float32_t linear_threshold = lerp_threshold;
+	uint8_t max_retries = find_idx_of_arrival_max_retries;
+	idxs[buffer_idx] = 0;
+
+	while(is_idx_out_of_bounds(buffer_idx) && max_retries--){
+		float32_t threshold = dsp_min_max_lerp(processing_workspace[buffer_idx], WORKSPACE_LEN, linear_threshold, n_upper_average, dead_space);
+		idxs[buffer_idx] = dsp_rl_under_threshold_search(processing_workspace[buffer_idx],WORKSPACE_LEN, threshold , PROCESSING_PATIENCE);
+		linear_threshold *= 2;
+	}
+	return idxs[buffer_idx];
+}
+
+bool are_idxs_of_arrival_valid(uint8_t valid_buffers_array[N_HYDROPHONES]){
+	uint8_t valid_buffers = 0;
+
+	for(int i = 1; i < N_HYDROPHONES; i++){
+		bool valid = !regular_idx_out_of_bounds(idxs[i]);
+		valid_buffers += valid;
+		valid_buffers_array[i] &= valid;
+	}
+
+	return valid_buffers >= (MINIMUM_VALID_BUFFERS - 1);
+}


find_idx_of_arrival_in_buffer only process one index whereas are_idxs_of_arrival_valid processes all indexes. I think both functions should either process all indexes or process one index

forrisdahl · 2026-06-29T16:37:27Z

+    for (uint32_t i = 1; i < signal_len; i++)
+    {
+        float32_t key = sorted[i];
+        int32_t j = (int32_t)i - 1;
+        while (j >= 0 && sorted[j] > key)
+        {
+            sorted[j + 1] = sorted[j];
+            j--;
+        }
+        sorted[j + 1] = key;
+    }


Could make this a static inline function, would express intent clearer and reduce the length of this function

forrisdahl · 2026-06-29T16:42:50Z

+    arm_add_f32(                          /* low + t*(high-low)          */
+        &low_mean,                        /* not a vector call, so we    */
+        &(float32_t){t *          		  /* use scalar arithmetic below */
+            (high_mean - low_mean)},
+        &result, 1);


This is quite confusing. Would instead of doing this inside the function call you instead

&(float32_t){t * (high_mean - low_mean)}

float32_t temp = t * (high_mean - low_mean) arm_add_f32(&low_mean, &temp, &result, 1)

You should give it a better name than temp, but I was too lazy to come up with something clever

forrisdahl · 2026-06-29T16:44:34Z

+static bool is_vector_too_long(float32_t vec[3]){
+	float32_t sum = 0.0f;
+	for(int i = 0; i < 3; i++) sum += utils_abs_f32(vec[i]);
+	return sum > 1.732050807569f;


Very specific number, probably should use a define here

forrisdahl · 2026-06-29T16:50:44Z

+void hydrophone_interface_init(float32_t new_hydrophone_positions[N_HYDROPHONES][3]){
+
+	for(int i = 0; i < N_HYDROPHONES; i++){
+		hydrophone_valid[i] = false;
+		hydrophone_positions[i][0] = new_hydrophone_positions[i][0];
+		hydrophone_positions[i][1] = new_hydrophone_positions[i][1];
+		hydrophone_positions[i][2] = new_hydrophone_positions[i][2];
+	}
+
+	float32_t biggest_distance = 0;
+	for(int i = 1; i < N_HYDROPHONES; i++){
+		float32_t dist = utils_distance_3d(hydrophone_positions[0],hydrophone_positions[i]);
+		if(dist > biggest_distance){
+			biggest_distance = dist;
+		}
+	}
+	{
+		float32_t idx_distance = (biggest_distance/(WAVE_SPEED)*SAMPLING_FREQUENCY) + BLOCK_LEN/4;
+		uint16_t n = (uint16_t)ceilf(idx_distance);
+		n--;
+		n |= n >> 1;
+		n |= n >> 2;
+		n |= n >> 4;
+		n |= n >> 8;
+		n |= n >> 16;
+		n++;
+		max_idx_difference = n;
+	}
+
+
+	struct ad7606_pins pins = {
+			.cs = {CS},
+			.busy = {BUSY},
+			.frstdata = {FRSTDATA},
+			.convst = {CONVST},
+	};
+
+	union ad7606_spi spi = {
+		.by_name = {
+			.douta = DOUTA,
+			.doutb = DOUTB,
+			.doutc = DOUTC,
+			.doutd = DOUTD,
+			.doute = DOUTE,
+			.doutf = NULL,
+			.doutg = NULL,
+			.douth = DOUTH,
+			.sdi   = MASTER_SPI,
+		}
+	};
+
+	struct ad7606_config config = {
+		.status_header = false,
+		.external_oversampling_clock = false,
+		.dout_format = AD7606_DOUT_8,
+		.operation_mode = AD7606_OPERATION_NORMAL,
+	};
+
+	struct ad7606_channel channels[8];
+	for(int i = 0; i < 8; i++){
+//			AD7606_MUX_CTRL_TEMP,
+//			AD7606_MUX_CTRL_2V5_REF,
+//			AD7606_MUX_CTRL_1V8_ALDO,
+//			AD7606_MUX_CTRL_1V8_DLDO,
+//			AD7606_MUX_CTRL_V_DRIVE,
+//			AD7606_MUX_CTRL_A_GND,
+//			AD7606_MUX_CTRL_AV_CC;
+		AD7606_CHANNEL_MUX_CTRL mux_ctrl = AD7606_MUX_CTRL_A_IN; // = (i != 8) ? (AD7606_MUX_CTRL_A_IN) : (AD7606_MUX_CTRL_TEMP);
+		AD7606_CHANNEL_RANGE range = AD7606_RANGE_SE_PM_12_5V;
+		switch(i){
+			case(2):
+				range = AD7606_RANGE_SE_PM_2_5V;
+			break;
+			case(5):
+				mux_ctrl = AD7606_MUX_CTRL_A_GND;
+				range = AD7606_RANGE_SE_PM_2_5V;
+			break;
+			case(6):
+				mux_ctrl = AD7606_MUX_CTRL_AV_CC;
+				range = AD7606_RANGE_SE_0_TO_5V;
+			break;
+			case(7):
+				mux_ctrl = AD7606_MUX_CTRL_TEMP;
+				range = AD7606_RANGE_SE_PM_2_5V;
+			break;
+		}
+	    struct ad7606_channel ch = {
+	        .open_detect    = false,
+	        .high_bandwidth = true,
+	        .range          = range,
+			.gain 			= 0,
+			.phase 			= 0,
+			.offset 		= 0x80,
+			.mux_ctrl 		= mux_ctrl,
+	    };
+	    channels[i] = ch;
+	}
+
+	struct ad7606_oversampling oversampling = {
+			.oversampling_ratio = 3, // 2^N oversampling
+			.oversampling_padding = 0,
+	};
+
+	struct ad7606_digital_diagnostics digital_diagnostics = {
+			.rom_CRC_err_en = true,
+			.mm_CRC_err_en = false,
+			.int_CRC_err_en = false,
+			.spi_write_err_en = false,
+			.spi_read_err_en = false,
+			.busy_stuck_high_err_en = true,
+			.clk_fs_os_en = false,
+			.interface_check_en = false,
+	};
+
+	ADC_settings.config = config;
+	ADC_settings.digital_diagnostics = digital_diagnostics;
+	ADC_settings.oversampling = oversampling;
+
+	for(int i = 0; i < 8; i++){
+		ADC_settings.channels[i] = channels[i];
+	}
+	my_ADC.cooked = true;
+	ad7606_init(&my_ADC, &ADC_regs, pins, spi, &ADC_settings, &diagnostics_sample);
+//	if(verbose){
+//		printf("ADC initialized. Status register:\t");
+//		print_binary(ad7606_check_status(&my_ADC),8);
+//		printf("\r\n");
+//
+//		printf("Digital diagnostics error register:\t");
+//		print_binary(ad7606_check_digital_error(&my_ADC),8);
+//		printf("\r\n");
+//	}
+
+	uint8_t interface_check_result[8];
+	ad7606_check_interface(&my_ADC, interface_check_result);
+	if(!hydrophone_valid[0]){
+		for(int i = 0; i < N_HYDROPHONES; i++){
+			hydrophone_valid[i] = interface_check_result[i];
+		}
+	}
+//	if(verbose){
+//		printf("Interface check result:\r\n");
+//		for(int i = 0; i < 8; i++){
+//			printf("Channel V%d: ",i+1);
+//		  switch(interface_check_result[i]){
+//		  case 0xFF:
+//			  printf("Not configured");
+//			  break;
+//		  case 0:
+//			  printf("Fail");
+//			  break;
+//		  case 1:
+//			  printf("Pass");
+//			break;
+//		  default:
+//			printf("Unknown result");
+//			break;
+//		  }
+//		  printf("\t\t\t");
+//		  if(i%4 == 3) printf("\r\n");
+//		}
+//		printf("\r\n");
+//	}
+
+	int lengths[8] = {
+			BUFFER_LEN,
+			BUFFER_LEN,
+			BUFFER_LEN,
+			BUFFER_LEN,
+			BUFFER_LEN,
+			0,
+			0,
+			0,
+	};
+	int16_t* buffers[8] = {NULL};
+	for(int i = 0; i < 5; i++) buffers[i] = (int16_t*)&hydrophone_buffers[i][0][0];
+
+	ad7606_enter_adc_mode(&my_ADC);
+
+	for(int i = 0; i < 5; i++){
+		utils_clear_array_q15(hydrophone_buffers[i][0], BUFFER_LEN);
+	}
+
+	ad7606_init_output_buffers_DMA(&my_ADC, buffers, lengths);
+	HAL_MDMA_RegisterCallback(&hmdma_mdma_channel0_sw_0, HAL_MDMA_XFER_CPLT_CB_ID,  MyMDMA_TransferCompleteCallback);
+	HAL_MDMA_RegisterCallback(&hmdma_mdma_channel0_sw_0, HAL_MDMA_XFER_ERROR_CB_ID, MyMDMA_ErrorCallback);
+	ad7606_dma_spi_init(&my_ADC, &hdma_spi6_rx, diagnostics_buffer, BLOCK_LEN);
+
+	mdma_half = 0;
+	mdma_done_flag = false;
+}


Would clean this function up a bit, remove the commented code or have them in the code. Possible to have them conditially using #ifdef if you do not want them in your code all the time.

forrisdahl · 2026-06-29T16:51:43Z

+		float32_t idx_distance = (biggest_distance/(WAVE_SPEED)*SAMPLING_FREQUENCY) + BLOCK_LEN/4;
+		uint16_t n = (uint16_t)ceilf(idx_distance);
+		n--;
+		n |= n >> 1;
+		n |= n >> 2;
+		n |= n >> 4;
+		n |= n >> 8;
+		n |= n >> 16;
+		n++;


Quite interesting trick, not immideately obvious what is going on here, so again I would recommend using static inline function to express intent clearer

forrisdahl added 30 commits October 18, 2025 12:41

chore: add stm32 files to gitignore

a0fd211

feat: add acoustic stm32cubeide project

0c864ad

feat: add stm32 config

f675a47

1 FDCAN 1 SPI master 3 SPI slaves 1 Usart

feat: add stm32cubeide generated drivers

2ff419c

fix: temporary disable cyclomatic compexity

c725ffa

allow cli make clean all build project used to generate compile_json

feat: add ads8555 driver

8741ad5

feat: add CMSIS_DSP in project

6a0b0b0

remove all incompatible *f16.c from CMSIS.DSP remove all incompatible ARM cortex A (neon) from CMSIS_DSP remove all source files that include all source files in the same directory

fix: remove all f16.h

559da65

fix: add ARM_MATH_CM7 flag

88efb20

fix: remove *.s from .gitignore

fc2ba9b

feat: add startup .S file that was previously ignored

8610488

feat: add dsp

dfb1b72

First iteration, made completely by chatgtp

feat: add multilateration

97903d9

fix: buttterworth function used same pointer as source in destination

db9441d

refactor: remove redundant (void)

d0f8d56

fix: butterworth io_o output to out_u instead of out_i

3e79fac

feat: convert multilateration to use arm_math

6aeacb7

feat: add deinterleave_fast function

fd6c222

feat: add dsp_decimate function using arm_fir_decimate

841f79a

feat: add fir_decimate to init and struct

394658c

refactor: remove if statements from dsp init

cdbadd3

fix: add dsp_decimate to dsp.h, remove old decimate declaration

5492fae

feat: add dsp_matched_filter function

8fb756f

feat: improve deinterleave function

bb5a284

refactor: replace ml_init with memset to 0

d619e4a

fix: remove remaining float16 files

55a3a42

fix: remove inline from deinterleave function

78c08d9

refactor: simplify dsp_init function by using macros for constants

3157b4b

style: chane input from n to size

1a9955d

feat: ml_solve using cholesky instead of inverse

36d2dea

Wiacer added 2 commits April 6, 2026 13:54

acoustics works

546460e

feat: many small improvements

949c0f2

Wiacer requested review from Q3rkses, forrisdahl and jorgenfj April 24, 2026 17:58

Wiacer self-assigned this Apr 24, 2026

forrisdahl requested changes Apr 26, 2026

View reviewed changes

Comment thread acoustics/acoustics_firmware2/Core/Src/main.c Outdated

Comment thread acoustics/acoustics_firmware2/Core/Src/main.c Outdated

Comment thread acoustics/acoustics_firmware2/Core/Src/main.c Outdated

Wiacer added 2 commits April 26, 2026 22:47

MAJOR refactor: polished acoustics

d062194

//NOT TESTED AT TIME OF COMMIT// cleaned and modularized everything all that remains should be to implement dumping functionality and test Features added: CAN interface Proper state machine Stale Data detection

feat: Got it working baby.

0d23eca

Left to implement: Dump commands Making it not error due to stale data every 10 seconds

Wiacer closed this Apr 27, 2026

Wiacer reopened this Apr 27, 2026

fix: temperature monitoring ADC was firing an interrupt so often the …

82c1b0d

…SPI interface would fail. Fixed it. Acoustics is now megagood. Dump still not implemented

forrisdahl reviewed Apr 30, 2026

View reviewed changes

Wiacer added 3 commits May 15, 2026 21:48

added ros2 live viewer

85b9f6b

refactor: Cleaned up the big ugly function

4447c49

Merge branch 'feat/acoustics' of https://github.com/vortexntnu/vortex…

d4daab4

…-auv-embedded into feat/acoustics

Wiacer added 2 commits May 16, 2026 14:28

polish: removed functions and variables from acoustics.h that weren't

82e3d90

used outside of acoustics.c

changed some function names and stuff

6682e72

Wiacer added 5 commits June 7, 2026 19:01

Ensured that the system can't get stuck in the can state

e16992e

Removed firmware old and got rid of "2" in name

fbdd24c

removed and moved a bunch of stuff

cdbd341

Most importantly i found and removed the test data that was still in the repo (i think)

Made the sim guide comprehensible

84dee5e

Attempting to ignore CMSIS library stuff

b137577

forrisdahl requested changes Jun 29, 2026

View reviewed changes

Uh oh!

Conversation

Wiacer commented Apr 24, 2026

Uh oh!

jorgenfj commented Apr 24, 2026

Uh oh!

Wiacer commented Apr 25, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

forrisdahl left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Wiacer commented Apr 27, 2026

Uh oh!

Wiacer commented Apr 27, 2026

Uh oh!

forrisdahl Apr 30, 2026

Choose a reason for hiding this comment

Uh oh!

Wiacer commented May 15, 2026

Uh oh!

forrisdahl commented May 16, 2026

Uh oh!

forrisdahl left a comment

Choose a reason for hiding this comment

Uh oh!

forrisdahl Jun 29, 2026

Choose a reason for hiding this comment

Uh oh!

forrisdahl Jun 29, 2026

Choose a reason for hiding this comment

Uh oh!

forrisdahl Jun 29, 2026

Choose a reason for hiding this comment

Uh oh!

forrisdahl Jun 29, 2026

Choose a reason for hiding this comment

Uh oh!

forrisdahl Jun 29, 2026

Choose a reason for hiding this comment

Uh oh!

forrisdahl Jun 29, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Wiacer commented Apr 25, 2026 •

edited

Loading