diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 474db36ef7..ce9e4264a0 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -115,7 +115,11 @@ if px4flow start
 then
 fi
 
-if ll40ls start
+if pwm_input start
+then
+fi
+
+if ll40ls start pwm
 then
 fi
 
diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp
index 8984220625..22cebd207c 100644
--- a/src/drivers/ll40ls/ll40ls.cpp
+++ b/src/drivers/ll40ls/ll40ls.cpp
@@ -36,6 +36,7 @@
  * @author Allyson Kreft
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Ban Siesta <bansiesta@gmail.com>
+ * @author James Goppert <james.goppert@gmail.com>
  *
  * Interface for the PulsedLight Lidar-Lite range finders.
  */
@@ -79,6 +80,7 @@ LidarLiteI2C	*g_dev_int;
 LidarLiteI2C	*g_dev_ext;
 LidarLitePWM	*g_dev_pwm;
 
+LidarLite * get_dev(const bool use_i2c, const int bus);
 void	start(const bool use_i2c, const int bus);
 void	stop(const bool use_i2c, const int bus);
 void	test(const bool use_i2c, const int bus);
@@ -87,6 +89,25 @@ void	info(const bool use_i2c, const int bus);
 void    regdump(const bool use_i2c, const int bus);
 void	usage();
 
+/**
+ * Get the correct device pointer
+ */
+LidarLite * get_dev(const bool use_i2c, const int bus) {
+	LidarLite * g_dev = nullptr;
+	if (use_i2c) {
+		g_dev = static_cast<LidarLite*>(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
+		if (g_dev == nullptr) {
+			errx(1, "i2c driver not running");
+		}
+	} else {
+		g_dev = static_cast<LidarLite*>(g_dev_pwm);
+		if (g_dev == nullptr) {
+			errx(1, "pwm driver not running");
+		}
+	}
+	return g_dev;
+};
+
 /**
  * Start the driver.
  */
@@ -210,6 +231,11 @@ fail:
 		g_dev_ext = nullptr;
 	}
 
+	if (g_dev_pwm != nullptr) {
+		delete g_dev_pwm;
+		g_dev_pwm = nullptr;
+	}
+
 	errx(1, "driver start failed");
 }
 
@@ -218,16 +244,24 @@ fail:
  */
 void stop(const bool use_i2c, const int bus)
 {
-	LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext);
-
-	if (*g_dev != nullptr) {
-		delete *g_dev;
-		*g_dev = nullptr;
-
+	if (use_i2c) {
+		if (bus == PX4_I2C_BUS_ONBOARD) {
+			if (g_dev_int != nullptr) {
+				delete g_dev_int;
+				g_dev_int = nullptr;
+			}
+		} else {
+			if (g_dev_ext != nullptr) {
+				delete g_dev_ext;
+				g_dev_ext = nullptr;
+			}
+		}
 	} else {
-		errx(1, "driver not running");
+		if (g_dev_pwm != nullptr)  {
+			delete g_dev_pwm;
+			g_dev_pwm = nullptr;
+		}
 	}
-
 	exit(0);
 }
 
@@ -346,22 +380,9 @@ reset(const bool use_i2c, const int bus)
 void
 info(const bool use_i2c, const int bus)
 {
-	LidarLite *g_dev = nullptr;
-
-	if (use_i2c) {
-		g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
-
-		if (g_dev == nullptr) {
-			errx(1, "driver not running");
-		}
-
-	} else {
-		g_dev = g_dev_pwm;
-	}
-
+	LidarLite * g_dev = get_dev(use_i2c, bus);
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
-
 	exit(0);
 }
 
@@ -371,15 +392,9 @@ info(const bool use_i2c, const int bus)
 void
 regdump(const bool use_i2c, const int bus)
 {
-	LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
-
-	if (g_dev == nullptr) {
-		errx(1, "driver not running");
-	}
-
+	LidarLite * g_dev = get_dev(use_i2c, bus);
 	printf("regdump @ %p\n", g_dev);
 	g_dev->print_registers();
-
 	exit(0);
 }