From 59557735748974230d531fd020e688ccaafba4fc Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 25 May 2015 18:29:27 -0400 Subject: [PATCH 1/8] Lidar lite driver fixes. Conflicts: src/modules/uORB/Subscription.cpp --- ROMFS/px4fmu_common/init.d/rc.sensors | 6 ++- src/drivers/ll40ls/ll40ls.cpp | 73 ++++++++++++++++----------- 2 files changed, 49 insertions(+), 30 deletions(-) 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 * @author Ban Siesta + * @author James Goppert * * 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(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(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); } From 0b703096e736738f891aeeead9d13ca3c29dfc4c Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 26 May 2015 02:48:24 -0400 Subject: [PATCH 2/8] Fixed distance sensor log error. --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 062ab88f4f..9614e86acd 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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, "iiiff", "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"), From 853a22db180070bf9a4a12070639fdfa5605de28 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 26 May 2015 21:44:55 +0530 Subject: [PATCH 3/8] ll40ls : conditional startup param --- src/modules/sensors/sensor_params.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 6a127df0ea..6ed03f6606 100644 --- a/src/modules/sensors/sensor_params.c +++ b/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_ENABLE_LL40LS, 0); From 42358a60d139852376ee40bfe25c3a52ac4d0f12 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 26 May 2015 21:51:33 +0530 Subject: [PATCH 4/8] ll40ls : conditional startup --- ROMFS/px4fmu_common/init.d/rc.sensors | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 474db36ef7..c749233139 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -115,8 +115,15 @@ if px4flow start then fi -if ll40ls start +if param compare SENS_LL40LS_ENABLE 1 then + if pwm_input start + then + fi + + if ll40ls start pwm + then + fi fi # From d06bdc044577583356c5ea6e74696cda59a706ed Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 26 May 2015 21:54:19 +0530 Subject: [PATCH 5/8] ll40ls : fix param typo --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index c749233139..245b9cd040 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -115,7 +115,7 @@ if px4flow start then fi -if param compare SENS_LL40LS_ENABLE 1 +if param compare SENS_ENABLE_LL40LS 1 then if pwm_input start then From 6f309ba6252d1d45beac96430ba68450707b5f66 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 26 May 2015 20:02:20 -0400 Subject: [PATCH 6/8] Distance sensor log fix. --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9614e86acd..ec42618757 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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, "iiiff", "Id,Type,Orientation,Distance,Covariance"), + LOG_FORMAT(DIST, "BBBHB", "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"), From 3cc84b1a15df2ad4b4ce0f981b37971864bfa912 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 26 May 2015 21:19:44 -0400 Subject: [PATCH 7/8] Correct ll40ls param enable length. --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- src/modules/sensors/sensor_params.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 245b9cd040..536cfca91a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -115,7 +115,7 @@ if px4flow start then fi -if param compare SENS_ENABLE_LL40LS 1 +if param compare SENS_EN_LL40LS 1 then if pwm_input start then diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 6ed03f6606..c8ad86afa7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1409,4 +1409,4 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); * @max 1 * @group Sensor Enable */ -PARAM_DEFINE_INT32(SENS_ENABLE_LL40LS, 0); +PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); From 57f5db544c386c0db8feb2e9fab19874a8d4e542 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 27 May 2015 00:00:33 -0400 Subject: [PATCH 8/8] Distance sensor log precision fix. --- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index ec42618757..9cf37683ae 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/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; - uint8_t covariance; + float current_distance; + 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, "BBBHB", "Id,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"),