Faster sensor bus resets on timeouts, massively reworked fixed wing app, tested

3 files changed, 81 insertions, 70 deletions

diff --git a/nuttx/configs/px4fmu/include/nsh_romfsimg.h b/nuttx/configs/px4fmu/include/nsh_romfsimg.h
index 799670c4d..9965277c7 100644
--- a/nuttx/configs/px4fmu/include/nsh_romfsimg.h
+++ b/nuttx/configs/px4fmu/include/nsh_romfsimg.h
@@ -234,7 +234,7 @@ unsigned char romfs_img[] = {
   0x69, 0x73, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x46, 0x41, 0x49, 0x4c,
   0x45, 0x44, 0x2e, 0x22, 0x0a, 0x23, 0x09, 0x72, 0x65, 0x62, 0x6f, 0x6f,
   0x74, 0x0a, 0x23, 0x66, 0x69, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x0e, 0xf2,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xcd, 0x52, 0xce, 0xe0, 0xfc,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xcc, 0x52, 0xce, 0xe0, 0xfd,
   0x72, 0x63, 0x2e, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e,
   0x65, 0x00, 0x00, 0x00, 0x23, 0x21, 0x6e, 0x73, 0x68, 0x0a, 0x23, 0x0a,
   0x23, 0x20, 0x46, 0x6c, 0x69, 0x67, 0x68, 0x74, 0x20, 0x73, 0x74, 0x61,
@@ -248,76 +248,76 @@ unsigned char romfs_img[] = {
   0x34, 0x46, 0x4d, 0x55, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70,
   0x2e, 0x2e, 0x2e, 0x22, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
   0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x4f, 0x52, 0x42, 0x0a,
-  0x23, 0x0a, 0x23, 0x75, 0x6f, 0x72, 0x62, 0x20, 0x73, 0x74, 0x61, 0x72,
-  0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74,
-  0x20, 0x74, 0x68, 0x65, 0x20, 0x73, 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73,
-  0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x73, 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63,
-  0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x73,
-  0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20,
-  0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x4d, 0x41, 0x56, 0x4c, 0x69, 0x6e,
-  0x6b, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x6d, 0x61, 0x76, 0x6c, 0x69, 0x6e,
-  0x6b, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, 0x2f, 0x74, 0x74,
-  0x79, 0x53, 0x30, 0x20, 0x2d, 0x62, 0x20, 0x35, 0x37, 0x36, 0x30, 0x30,
-  0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72,
-  0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e,
-  0x64, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58,
-  0x20, 0x74, 0x68, 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64,
-  0x20, 0x62, 0x65, 0x20, 0x27, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64,
-  0x65, 0x72, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23,
-  0x0a, 0x23, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, 0x72, 0x20,
+  0x23, 0x0a, 0x75, 0x6f, 0x72, 0x62, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74,
+  0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20,
+  0x74, 0x68, 0x65, 0x20, 0x73, 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x2e,
+  0x0a, 0x23, 0x0a, 0x23, 0x73, 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f,
+  0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x73, 0x65,
+  0x6e, 0x73, 0x6f, 0x72, 0x73, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53,
+  0x74, 0x61, 0x72, 0x74, 0x20, 0x4d, 0x41, 0x56, 0x4c, 0x69, 0x6e, 0x6b,
+  0x0a, 0x23, 0x0a, 0x23, 0x20, 0x6d, 0x61, 0x76, 0x6c, 0x69, 0x6e, 0x6b,
+  0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, 0x2f, 0x74, 0x74, 0x79,
+  0x53, 0x30, 0x20, 0x2d, 0x62, 0x20, 0x35, 0x37, 0x36, 0x30, 0x30, 0x20,
   0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74,
-  0x20, 0x74, 0x68, 0x65, 0x20, 0x61, 0x74, 0x74, 0x69, 0x74, 0x75, 0x64,
-  0x65, 0x20, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, 0x6f, 0x72, 0x0a,
-  0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73,
-  0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x27,
-  0x3c, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x3e, 0x20, 0x73, 0x74,
-  0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x61, 0x74, 0x74,
-  0x69, 0x74, 0x75, 0x64, 0x65, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61,
-  0x74, 0x6f, 0x72, 0x5f, 0x62, 0x6d, 0x20, 0x26, 0x0a, 0x23, 0x70, 0x6f,
-  0x73, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d,
-  0x61, 0x74, 0x6f, 0x72, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20,
-  0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x66, 0x69,
-  0x78, 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x63, 0x6f, 0x6e,
-  0x74, 0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23,
-  0x20, 0x58, 0x58, 0x58, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20,
-  0x74, 0x68, 0x69, 0x73, 0x20, 0x62, 0x65, 0x20, 0x6c, 0x6f, 0x6f, 0x6b,
-  0x69, 0x6e, 0x67, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x63, 0x6f, 0x6e, 0x66,
-  0x69, 0x67, 0x75, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x74, 0x6f,
-  0x20, 0x64, 0x65, 0x63, 0x69, 0x64, 0x65, 0x0a, 0x23, 0x20, 0x77, 0x68,
-  0x65, 0x74, 0x68, 0x65, 0x72, 0x20, 0x74, 0x68, 0x65, 0x20, 0x62, 0x6f,
-  0x61, 0x72, 0x64, 0x20, 0x69, 0x73, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69,
-  0x67, 0x75, 0x72, 0x65, 0x64, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x66, 0x69,
-  0x78, 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x75, 0x73, 0x65,
-  0x3f, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68,
-  0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65,
-  0x20, 0x27, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, 0x69, 0x6e, 0x67, 0x5f,
-  0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, 0x73, 0x74, 0x61, 0x72,
-  0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x66, 0x69, 0x78, 0x65, 0x64,
-  0x77, 0x69, 0x6e, 0x67, 0x5f, 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c,
-  0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x43, 0x6f, 0x6e, 0x66,
-  0x69, 0x67, 0x75, 0x72, 0x65, 0x20, 0x46, 0x4d, 0x55, 0x20, 0x66, 0x6f,
-  0x72, 0x20, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65,
-  0x20, 0x6d, 0x6f, 0x64, 0x65, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58,
-  0x58, 0x20, 0x61, 0x72, 0x67, 0x75, 0x6d, 0x65, 0x6e, 0x74, 0x73, 0x3f,
-  0x0a, 0x23, 0x0a, 0x23, 0x70, 0x78, 0x34, 0x66, 0x6d, 0x75, 0x20, 0x73,
-  0x74, 0x61, 0x72, 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
-  0x61, 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, 0x6e, 0x67, 0x20,
-  0x66, 0x6f, 0x72, 0x20, 0x61, 0x20, 0x47, 0x50, 0x53, 0x2e, 0x0a, 0x23,
+  0x20, 0x74, 0x68, 0x65, 0x20, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64,
+  0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20,
+  0x74, 0x68, 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20,
+  0x62, 0x65, 0x20, 0x27, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65,
+  0x72, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a,
+  0x23, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, 0x72, 0x20, 0x26,
+  0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20,
+  0x74, 0x68, 0x65, 0x20, 0x61, 0x74, 0x74, 0x69, 0x74, 0x75, 0x64, 0x65,
+  0x20, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, 0x6f, 0x72, 0x0a, 0x23,
   0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, 0x20,
-  0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x6e,
-  0x65, 0x65, 0x64, 0x20, 0x74, 0x6f, 0x20, 0x62, 0x65, 0x20, 0x62, 0x61,
-  0x63, 0x6b, 0x67, 0x72, 0x6f, 0x75, 0x6e, 0x64, 0x65, 0x64, 0x0a, 0x23,
-  0x0a, 0x23, 0x67, 0x70, 0x73, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65,
-  0x76, 0x2f, 0x74, 0x74, 0x79, 0x53, 0x33, 0x20, 0x2d, 0x6d, 0x20, 0x61,
-  0x6c, 0x6c, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
-  0x61, 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x20,
-  0x74, 0x6f, 0x20, 0x6d, 0x69, 0x63, 0x72, 0x6f, 0x53, 0x44, 0x20, 0x69,
-  0x66, 0x20, 0x77, 0x65, 0x20, 0x63, 0x61, 0x6e, 0x0a, 0x23, 0x0a, 0x73,
-  0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e,
-  0x64, 0x2f, 0x72, 0x63, 0x2e, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67,
-  0x0a, 0x0a, 0x65, 0x63, 0x68, 0x6f, 0x20, 0x22, 0x5b, 0x69, 0x6e, 0x69,
-  0x74, 0x5d, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x20, 0x64,
-  0x6f, 0x6e, 0x65, 0x22, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a,
+  0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x27, 0x3c,
+  0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x3e, 0x20, 0x73, 0x74, 0x61,
+  0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x61, 0x74, 0x74, 0x69,
+  0x74, 0x75, 0x64, 0x65, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74,
+  0x6f, 0x72, 0x5f, 0x62, 0x6d, 0x20, 0x26, 0x0a, 0x23, 0x70, 0x6f, 0x73,
+  0x69, 0x74, 0x69, 0x6f, 0x6e, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61,
+  0x74, 0x6f, 0x72, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53,
+  0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x66, 0x69, 0x78,
+  0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x63, 0x6f, 0x6e, 0x74,
+  0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20,
+  0x58, 0x58, 0x58, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x74,
+  0x68, 0x69, 0x73, 0x20, 0x62, 0x65, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69,
+  0x6e, 0x67, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69,
+  0x67, 0x75, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x74, 0x6f, 0x20,
+  0x64, 0x65, 0x63, 0x69, 0x64, 0x65, 0x0a, 0x23, 0x20, 0x77, 0x68, 0x65,
+  0x74, 0x68, 0x65, 0x72, 0x20, 0x74, 0x68, 0x65, 0x20, 0x62, 0x6f, 0x61,
+  0x72, 0x64, 0x20, 0x69, 0x73, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69, 0x67,
+  0x75, 0x72, 0x65, 0x64, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x66, 0x69, 0x78,
+  0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x75, 0x73, 0x65, 0x3f,
+  0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69,
+  0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20,
+  0x27, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, 0x69, 0x6e, 0x67, 0x5f, 0x63,
+  0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74,
+  0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77,
+  0x69, 0x6e, 0x67, 0x5f, 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20,
+  0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x43, 0x6f, 0x6e, 0x66, 0x69,
+  0x67, 0x75, 0x72, 0x65, 0x20, 0x46, 0x4d, 0x55, 0x20, 0x66, 0x6f, 0x72,
+  0x20, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65, 0x20,
+  0x6d, 0x6f, 0x64, 0x65, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58,
+  0x20, 0x61, 0x72, 0x67, 0x75, 0x6d, 0x65, 0x6e, 0x74, 0x73, 0x3f, 0x0a,
+  0x23, 0x0a, 0x23, 0x70, 0x78, 0x34, 0x66, 0x6d, 0x75, 0x20, 0x73, 0x74,
+  0x61, 0x72, 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61,
+  0x72, 0x74, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, 0x6e, 0x67, 0x20, 0x66,
+  0x6f, 0x72, 0x20, 0x61, 0x20, 0x47, 0x50, 0x53, 0x2e, 0x0a, 0x23, 0x0a,
+  0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, 0x20, 0x73,
+  0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x6e, 0x65,
+  0x65, 0x64, 0x20, 0x74, 0x6f, 0x20, 0x62, 0x65, 0x20, 0x62, 0x61, 0x63,
+  0x6b, 0x67, 0x72, 0x6f, 0x75, 0x6e, 0x64, 0x65, 0x64, 0x0a, 0x23, 0x0a,
+  0x23, 0x67, 0x70, 0x73, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76,
+  0x2f, 0x74, 0x74, 0x79, 0x53, 0x33, 0x20, 0x2d, 0x6d, 0x20, 0x61, 0x6c,
+  0x6c, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61,
+  0x72, 0x74, 0x20, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x20, 0x74,
+  0x6f, 0x20, 0x6d, 0x69, 0x63, 0x72, 0x6f, 0x53, 0x44, 0x20, 0x69, 0x66,
+  0x20, 0x77, 0x65, 0x20, 0x63, 0x61, 0x6e, 0x0a, 0x23, 0x0a, 0x73, 0x68,
+  0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64,
+  0x2f, 0x72, 0x63, 0x2e, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x0a,
+  0x0a, 0x65, 0x63, 0x68, 0x6f, 0x20, 0x22, 0x5b, 0x69, 0x6e, 0x69, 0x74,
+  0x5d, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x20, 0x64, 0x6f,
+  0x6e, 0x65, 0x22, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a,
   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x76, 0x8d, 0x9c, 0xa3, 0x80,
   0x72, 0x63, 0x53, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
   0x00, 0x00, 0x00, 0x00, 0x23, 0x21, 0x6e, 0x73, 0x68, 0x0a, 0x23, 0x0a,
diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
index 0e6dbe2ac..2a6d04306 100644
--- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c
+++ b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
@@ -204,7 +204,7 @@ read_values(int16_t *data)
 			int hmc_status = hmc5883l_read_reg(ADDR_STATUS);
 			if (hmc_status < 0)
 			  {
-			  	if (ret == ETIMEDOUT) hmc5883l_reset();
+			  	if (hmc_status == ETIMEDOUT) hmc5883l_reset();
 			  	ret = hmc_status;
 			  }
 			else
diff --git a/nuttx/configs/px4fmu/src/drv_ms5611.c b/nuttx/configs/px4fmu/src/drv_ms5611.c
index 7fea65159..c7e91c5ea 100644
--- a/nuttx/configs/px4fmu/src/drv_ms5611.c
+++ b/nuttx/configs/px4fmu/src/drv_ms5611.c
@@ -123,6 +123,16 @@ static FAR struct {
 
 static int ms5611_read_prom(void);
 
+int ms5611_reset()
+{
+	int ret;
+	printf("[ms5611 drv] Resettet I2C2 BUS\n");
+	up_i2cuninitialize(ms5611_dev.i2c);
+	ms5611_dev.i2c = up_i2cinitialize(2);
+	I2C_SETFREQUENCY(ms5611_dev.i2c, 400000);
+	return ret;
+}
+
 static bool
 read_values(float *data)
 {
@@ -279,6 +289,7 @@ read_values(float *data)
 	else
 	{
 		errno = -ret;
+		if (errno == ETIMEDOUT || ret == ETIMEDOUT) ms5611_reset();
 		return ret;
 	}
 }