Merge pull request #2231 from jgoppert/lidar_lite_pwm_fix

Lidar lite driver fixes.

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -115,8 +115,15 @@ if px4flow start
 then
 fi
 
-if ll40ls start
+if param compare SENS_EN_LL40LS 1
 then
+	if pwm_input start
+	then
+	fi
+	
+	if ll40ls start pwm
+	then
+	fi
 fi
 
 #

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 (use_i2c) {
-
+		if (bus == PX4_I2C_BUS_ONBOARD) {
-	if (*g_dev != nullptr) {
+			if (g_dev_int != nullptr) {
-		delete *g_dev;
+				delete g_dev_int;
-		*g_dev = nullptr;
+				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;
+	LidarLite * g_dev = get_dev(use_i2c, bus);
-
-	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;
-	}
-
 	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);
+	LidarLite * g_dev = get_dev(use_i2c, bus);
-
-	if (g_dev == nullptr) {
-		errx(1, "driver not running");
-	}
-
 	printf("regdump @ %p\n", g_dev);
 	g_dev->print_registers();
-
 	exit(0);
 }
 

src/modules/sdlog2/sdlog2_messages.h

@@ -301,8 +301,8 @@ struct log_DIST_s {
 	uint8_t id;
 	uint8_t type;
 	uint8_t orientation;
-	uint16_t current_distance;
+	float current_distance;
-	uint8_t covariance;
+	float covariance;
 };
 
 /* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
@@ -515,7 +515,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
-	LOG_FORMAT(DIST, "iiff",			"Type,Orientation,Distance,Covariance"),
+	LOG_FORMAT(DIST, "BBBff",			"Id,Type,Orientation,Distance,Covariance"),
 	LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),

src/modules/sensors/sensor_params.c

@@ -1401,3 +1401,12 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000);
  *
  */
 PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000);
+
+/**
+ * Enable Lidar-Lite (LL40LS) pwm driver
+ *
+ * @min 0
+ * @max 1
+ * @group Sensor Enable
+ */
+PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0);