diff --git a/mavros/scripts/mavcmd b/mavros/scripts/mavcmd
index 7d1406fd4..fce962825 100755
--- a/mavros/scripts/mavcmd
+++ b/mavros/scripts/mavcmd
@@ -19,26 +19,15 @@
 
 from __future__ import print_function
 
-import sys
 import argparse
 import threading
 
 import rospy
+from mavros.utils import *
 from mavros.srv import CommandLong, CommandInt, CommandHome, CommandTOL, CommandBool
 from sensor_msgs.msg import NavSatFix
 
 
-def print_if(cond, *args, **kvargs):
-    if cond:
-        print(*args, **kvargs)
-
-
-def fault(*args, **kvargs):
-    kvargs['file'] = sys.stderr
-    print(*args, **kvargs)
-    sys.exit(1)
-
-
 def _check_ret(args, ret):
     if not ret.success:
         fault("Request failed. Check mavros logs. ACK:", ret.result)
diff --git a/mavros/scripts/mavftp b/mavros/scripts/mavftp
index f0c7744b7..23acc0541 100755
--- a/mavros/scripts/mavftp
+++ b/mavros/scripts/mavftp
@@ -19,18 +19,12 @@
 
 from __future__ import print_function
 
-import os
-import sys
 import argparse
 
 import rospy
+from mavros.utils import *
 from mavros.ftp import *
 
-def fault(*args, **kvargs):
-    kvargs['file'] = sys.stdout
-    print(*args, **kvargs)
-    sys.exit(1)
-
 
 def check_ret(operation, ret):
     if not ret.success:
diff --git a/mavros/scripts/mavparam b/mavros/scripts/mavparam
index a27160fd4..c00dba412 100755
--- a/mavros/scripts/mavparam
+++ b/mavros/scripts/mavparam
@@ -19,13 +19,13 @@
 
 from __future__ import print_function
 
-import sys
 import csv
 import time
 import argparse
 
 import roslib; roslib.load_manifest("mavros")
 import rospy
+from mavros.utils import *
 from mavros.srv import ParamPull, ParamPush, ParamGet, ParamSet
 
 
@@ -127,17 +127,6 @@ class QGroundControlParam(ParamFile):
             writer.writerow((sysid, compid, p.param_id, p.param_value, to_type(p.param_value), )) # XXX
 
 
-def print_if(cond, *args, **kvargs):
-    if cond:
-        print(*args, **kvargs)
-
-
-def fault(*args, **kvargs):
-    kvargs['file'] = sys.stdout
-    print(*args, **kvargs)
-    sys.exit(1)
-
-
 def get_param_file_io(args):
     if args.mission_planner:
         print_if(args.verbose, "MissionPlanner format")
diff --git a/mavros/scripts/mavsafety b/mavros/scripts/mavsafety
index 0d68c4b96..c79ba5ffe 100755
--- a/mavros/scripts/mavsafety
+++ b/mavros/scripts/mavsafety
@@ -19,25 +19,14 @@
 
 from __future__ import print_function
 
-import sys
 import argparse
 
 import rospy
+from mavros.utils import *
 from geometry_msgs.msg import PolygonStamped, Point32
 from mavros.srv import CommandBool
 
 
-def print_if(cond, *args, **kvargs):
-    if cond:
-        print(*args, **kvargs)
-
-
-def fault(*args, **kvargs):
-    kvargs['file'] = sys.stderr
-    print(*args, **kvargs)
-    sys.exit(1)
-
-
 def _arm(args, state):
     try:
         arming_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/arming", CommandBool)
diff --git a/mavros/scripts/mavsetp b/mavros/scripts/mavsetp
index 2eb571ba9..845938b7e 100755
--- a/mavros/scripts/mavsetp
+++ b/mavros/scripts/mavsetp
@@ -19,11 +19,11 @@
 
 from __future__ import print_function
 
-import sys
 import argparse
 import math
 
 import rospy
+from mavros.utils import *
 from std_msgs.msg import Header
 from geometry_msgs.msg import TwistStamped, PoseStamped, Vector3, Vector3Stamped, Point, Quaternion
 from tf.transformations import quaternion_from_euler
@@ -32,17 +32,6 @@ from mavros.srv import CommandBool
 _ONCE_DELAY = 3
 
 
-def print_if(cond, *args, **kvargs):
-    if cond:
-        print(*args, **kvargs)
-
-
-def fault(*args, **kvargs):
-    kvargs['file'] = sys.stderr
-    print(*args, **kvargs)
-    sys.exit(1)
-
-
 def publish_once(args, pub, msg):
     pub.publish(msg)
     rospy.sleep(0.2)
diff --git a/mavros/scripts/mavsys b/mavros/scripts/mavsys
index 72b8d64ac..d1871219d 100755
--- a/mavros/scripts/mavsys
+++ b/mavros/scripts/mavsys
@@ -19,27 +19,15 @@
 
 from __future__ import print_function
 
-import sys
 import argparse
 import threading
 
 import rospy
+from mavros.utils import *
 from mavros.msg import State
 from mavros.srv import SetMode, StreamRate, StreamRateRequest
 
 
-def print_if(cond, *args, **kvargs):
-    if cond:
-        print(*args, **kvargs)
-
-
-def fault(*args, **kvargs):
-    kvargs['file'] = sys.stderr
-    print(*args, **kvargs)
-    sys.exit(1)
-
-
-
 def do_mode(args):
     rospy.init_node("mavsys", anonymous=True)
 
diff --git a/mavros/scripts/mavwp b/mavros/scripts/mavwp
index 20b413431..e31090093 100755
--- a/mavros/scripts/mavwp
+++ b/mavros/scripts/mavwp
@@ -19,7 +19,6 @@
 
 from __future__ import print_function
 
-import sys
 import csv
 import time
 import argparse
@@ -27,6 +26,7 @@ import threading
 
 import roslib; roslib.load_manifest("mavros")
 import rospy
+from mavros.utils import *
 from mavros.msg import Waypoint, WaypointList
 from mavros.srv import WaypointPull, WaypointPush, WaypointClear, \
         WaypointSetCurrent, WaypointGOTO
@@ -142,22 +142,10 @@ class QGroundControlWP(WaypointFile):
             ))
 
 
-
-def print_if(cond, *args, **kvargs):
-    if cond:
-        print(*args, **kvargs)
-
-
 def get_wp_file_io(args):
     return QGroundControlWP()
 
 
-def fault(*args, **kvargs):
-    kvargs['file'] = sys.stderr
-    print(*args, **kvargs)
-    sys.exit(1)
-
-
 def _pull(args):
     try:
         pull_cl = rospy.ServiceProxy(args.mavros_ns + "/mission/pull", WaypointPull)
@@ -399,4 +387,3 @@ def main():
 
 if __name__ == '__main__':
     main()
-
diff --git a/mavros/src/mavros/utils.py b/mavros/src/mavros/utils.py
new file mode 100644
index 000000000..2a6df54ac
--- /dev/null
+++ b/mavros/src/mavros/utils.py
@@ -0,0 +1,34 @@
+# -*- python -*-
+# vim:set ts=4 sw=4 et:
+#
+# Copyright 2014 Vladimir Ermakov.
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+# for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with this program; if not, write to the Free Software Foundation, Inc.,
+# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+
+from __future__ import print_function
+
+import os
+import sys
+
+
+def print_if(cond, *args, **kvargs):
+    if cond:
+        print(*args, **kvargs)
+
+
+def fault(*args, **kvargs):
+    kvargs['file'] = sys.stderr
+    print(*args, **kvargs)
+    sys.exit(1)