diff --git a/plugins/serial2002/serial2002.c b/plugins/serial2002/serial2002.c
index fc90d523260880646882f8a4be77869a2c1903f1..a4a01fd6e489f73aab188e84e0a00f5c897b258c 100644
--- a/plugins/serial2002/serial2002.c
+++ b/plugins/serial2002/serial2002.c
@@ -51,7 +51,7 @@ struct moberg_device_context {
   } port;
   struct remap_analog {
     int count;
-    struct {
+    struct map {
       unsigned char index;
       unsigned long maxdata;
       double min;
@@ -109,13 +109,17 @@ static struct moberg_status analog_in_read(
   struct moberg_channel_context *channel = &analog_in->channel_context;
   struct moberg_device_context *device = channel->device;
   struct serial2002_data data = { 0, 0 };
-  serial2002_poll_channel(device->port.fd,
-                          device->analog_in.map[channel->index].index);
-  struct moberg_status result = serial2002_read(device->port.fd, 1000, &data);
+  struct map map = device->analog_in.map[channel->index];
+  struct moberg_status result = serial2002_poll_channel(
+    device->port.fd, map.index);
+  if (! OK(result)) {
+    goto return_result;
+  }
+  result = serial2002_read(device->port.fd, 1000, &data);
   if (OK(result)) {
-    *value = (data.value * device->analog_in.map[channel->index].delta +
-              device->analog_in.map[channel->index].min);
+    *value = (data.value * map.delta + map.min);
   }
+return_result:
   return result;
 err_einval:
   return MOBERG_ERRNO(EINVAL);
@@ -126,7 +130,18 @@ static struct moberg_status analog_out_write(
   double value)
 {
   fprintf(stderr, "%s\n", __FUNCTION__);
-  return MOBERG_OK;
+
+  struct moberg_channel_context *channel = &analog_out->channel_context;
+  struct moberg_device_context *device = channel->device;
+  struct map map = device->analog_out.map[channel->index];
+  long as_long = value - map.min / map.delta;
+  if (as_long < 0) {
+    value = 0;
+  } else if (as_long > map.maxdata) {
+    as_long = map.maxdata;
+  }
+  struct serial2002_data data = { is_channel, map.index, as_long };
+  return serial2002_write(device->port.fd,  data);
 }
 
 static struct moberg_status digital_in_read(
diff --git a/test/test_io.c b/test/test_io.c
index 09ca9fe398cef4441b2206c8f39faa369ed9179e..680b9c17d0bbc590bb45257591697cf7a9237605 100644
--- a/test/test_io.c
+++ b/test/test_io.c
@@ -4,15 +4,25 @@
 int main(int argc, char *argv[])
 {
   struct moberg *moberg = moberg_new(NULL);
+  if (! moberg) {
+    fprintf(stderr, "NEW failed\n");
+    goto out;
+  }
   struct moberg_analog_in ai0;
   double ai0_value;
   if (! moberg_OK(moberg_analog_in_open(moberg, 0, &ai0))) {
     fprintf(stderr, "OPEN failed\n");
-  }
+    goto free;
+  } 
   if (! moberg_OK(ai0.read(ai0.context, &ai0_value))) { 
     fprintf(stderr, "READ failed\n");
+    goto close;
   }
   fprintf(stderr, "READ ai0: %f\n", ai0_value);
+ close:
   moberg_analog_in_close(moberg, 0, ai0);
+ free:
   moberg_free(moberg);
+ out:
+  return 0;
 }
diff --git a/test/test_moberg4simulink.c b/test/test_moberg4simulink.c
index 40d34cff846407c0066988a32c616f08065f174e..bfe8ee5e7832cfc626129c7f6ad768f31966df9e 100644
--- a/test/test_moberg4simulink.c
+++ b/test/test_moberg4simulink.c
@@ -3,5 +3,11 @@
 int main(int argc, char *argv[])
 {
   struct moberg_analog_in *ain = moberg4simulink_analog_in_open(0);
+  if (!ain) {
+    fprintf(stderr, "OPEN failed\n");
+    goto out;
+  }
   moberg4simulink_analog_in_close(0, ain);
+ out:
+  return 1;
 }