Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ll40ls cleanup and create PX4Rangerfinder helper class #12567

Merged
merged 1 commit into from
Jul 31, 2019
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion src/drivers/distance_sensor/ll40ls/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,9 +35,13 @@ px4_add_module(
MAIN ll40ls
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
STACK_MAIN
1500
SRCS
ll40ls.cpp
LidarLite.cpp
LidarLiteI2C.cpp
LidarLitePWM.cpp
DEPENDS
drivers_rangefinder
)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @dagar, can you add STACK_MAIN 1200 to the CMakeLists.txt? (Or allocate more if you prefer.)

89 changes: 30 additions & 59 deletions src/drivers/distance_sensor/ll40ls/LidarLite.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,64 +41,35 @@

#include "LidarLite.h"

int LidarLite::ioctl(device::file_t *filp, int cmd, unsigned long arg)
LidarLite::LidarLite(uint8_t rotation) :
_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls: read")),
_sample_interval_perf(perf_alloc(PC_ELAPSED, "ll40ls: interval")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls: comms errors")),
_sensor_resets(perf_alloc(PC_COUNT, "ll40ls: resets")),
_sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls: zero resets"))
{
switch (cmd) {

case SENSORIOCSPOLLRATE: {
switch (arg) {

/* zero would be bad */
case 0:
return -EINVAL;

/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);

/* set interval for next measurement to minimum legal value */
_measure_interval = (LL40LS_CONVERSION_INTERVAL);

/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}

return OK;
}

/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);

/* convert hz to tick interval via microseconds */
unsigned interval = (1000000 / arg);

/* check against maximum rate */
if (interval < (LL40LS_CONVERSION_INTERVAL)) {
return -EINVAL;
}

/* update interval for next measurement */
_measure_interval = interval;

/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}

return OK;
}
}
}

case SENSORIOCRESET:
reset_sensor();
return OK;
_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V3);
_px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian
}

default:
return -EINVAL;
}
LidarLite::~LidarLite()
{
perf_free(_sample_perf);
perf_free(_sample_interval_perf);
perf_free(_comms_errors);
perf_free(_sensor_resets);
perf_free(_sensor_zero_resets);
};

void
LidarLite::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_sample_interval_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_sensor_resets);
perf_print_counter(_sensor_zero_resets);
printf("poll interval: %u \n", get_measure_interval());
}
49 changes: 21 additions & 28 deletions src/drivers/distance_sensor/ll40ls/LidarLite.h
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,7 +41,8 @@
#pragma once

#include <drivers/device/device.h>
#include <drivers/drv_range_finder.h>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <perf/perf_counter.h>

/* Device limits */
#define LL40LS_MIN_DISTANCE (0.05f)
@@ -57,51 +58,43 @@
class LidarLite
{
public:
LidarLite() = default;
virtual ~LidarLite() = default;
LidarLite(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~LidarLite();

virtual int init() = 0;

virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);

virtual void start() = 0;

virtual void stop() = 0;

/**
* @brief
* Diagnostics - print some basic information about the driver.
*/
virtual void print_info() = 0;
void print_info();

/**
* @brief
* print registers to console
*/
virtual void print_registers() = 0;

virtual const char *get_dev_name() = 0;
virtual void print_registers() {};

protected:
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE
* and LL40LS_MAX_DISTANCE_V3
*/
void set_minimum_distance(const float min) { _min_distance = min; }
void set_maximum_distance(const float max) { _max_distance = max; }
float get_minimum_distance() const { return _min_distance; }
float get_maximum_distance() const { return _max_distance; }

uint32_t getMeasureInterval() const { return _measure_interval; }
uint32_t get_measure_interval() const { return _measure_interval; }

virtual int measure() = 0;
virtual int collect() = 0;

virtual int measure() = 0;
virtual int collect() = 0;
virtual int reset_sensor() { return PX4_ERROR; };

virtual int reset_sensor() = 0;
PX4Rangefinder _px4_rangefinder;

perf_counter_t _sample_perf;
perf_counter_t _sample_interval_perf;
perf_counter_t _comms_errors;
perf_counter_t _sensor_resets;
perf_counter_t _sensor_zero_resets;

private:
float _min_distance{LL40LS_MIN_DISTANCE};
float _max_distance{LL40LS_MAX_DISTANCE_V3};
uint32_t _measure_interval{0};

uint32_t _measure_interval{LL40LS_CONVERSION_INTERVAL};
};
Loading