1 files changed, 43 insertions, 8 deletions

diff --git a/core/libdivecomputer.c b/core/libdivecomputer.c
index c935ecbce..819db0226 100644
--- a/core/libdivecomputer.c
+++ b/core/libdivecomputer.c
@@ -307,6 +307,47 @@ static void handle_gasmix(struct divecomputer *dc, struct sample *sample, int id
 	current_gas_index = idx;
 }
 
+/*
+ * Adding a cylinder pressure sample field is not quite as trivial as it
+ * perhaps should be.
+ *
+ * We try to keep the same sensor index for the same sensor, so that even
+ * if the dive computer doesn't give pressure information for every sample,
+ * we don't move pressure information around between the different sensor
+ * indexes.
+ *
+ * The "prepare_sample()" function will always copy the sensor indices
+ * from the previous sample, so the indexes are pre-populated (but the
+ * pressures obviously are not)
+ */
+static void add_sample_pressure(struct sample *sample, int sensor, int mbar)
+{
+	int idx;
+
+	if (!mbar)
+		return;
+
+	/* Do we already have a slot for this sensor */
+	for (idx = 0; idx < MAX_SENSORS; idx++) {
+		if (sensor != sample->sensor[idx])
+			continue;
+		sample->pressure[idx].mbar = mbar;
+		return;
+	}
+
+	/* Pick the first unused index if we couldn't reuse one */
+	for (idx = 0; idx < MAX_SENSORS; idx++) {
+		if (sample->pressure[idx].mbar)
+			continue;
+		sample->sensor[idx] = sensor;
+		sample->pressure[idx].mbar = mbar;
+		return;
+	}
+
+	/* We do not have enough slots for the pressure samples. */
+	/* Should we warn the user about dropping pressure data? */
+}
+
 void
 sample_cb(dc_sample_type_t type, dc_sample_value_t value, void *userdata)
 {
@@ -352,15 +393,9 @@ sample_cb(dc_sample_type_t type, dc_sample_value_t value, void *userdata)
 	case DC_SAMPLE_DEPTH:
 		sample->depth.mm = lrint(value.depth * 1000);
 		break;
-	case DC_SAMPLE_PRESSURE: {
-		int sensoridx = 0;
-		/* Do we already have a pressure reading? */
-		if (sample->pressure[0].mbar)
-			sensoridx = 1;
-		sample->sensor[sensoridx] = value.pressure.tank;
-		sample->pressure[sensoridx].mbar = lrint(value.pressure.value * 1000);
+	case DC_SAMPLE_PRESSURE:
+		add_sample_pressure(sample, value.pressure.tank, lrint(value.pressure.value * 1000));
 		break;
-	}
 	case DC_SAMPLE_GASMIX:
 		handle_gasmix(dc, sample, value.gasmix);
 		break;