Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

ll40ls: astyle

Browse Source
This commit is contained in:
Ban Siesta
2015-05-24 09:44:10 +01:00
parent 3efaeabd5b
commit e67f681935
7 changed files with 667 additions and 628 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ll40ls/LidarLite.cpp
+78 -78
View File
@@ -43,130 +43,130 @@
#include <nuttx/clock.h>
LidarLite::LidarLite() :
_min_distance(LL40LS_MIN_DISTANCE),
_max_distance(LL40LS_MAX_DISTANCE),
_measure_ticks(0)
_min_distance(LL40LS_MIN_DISTANCE),
_max_distance(LL40LS_MAX_DISTANCE),
_measure_ticks(0)
{
//ctor
//ctor
}
LidarLite::~LidarLite()
{
//dtor
//dtor
}
void LidarLite::set_minimum_distance(const float min)
{
_min_distance = min;
_min_distance = min;
}
void LidarLite::set_maximum_distance(const float max)
{
_max_distance = max;
_max_distance = max;
}
float LidarLite::get_minimum_distance() const
{
return _min_distance;
return _min_distance;
}
float LidarLite::get_maximum_distance() const
{
return _max_distance;
return _max_distance;
}
uint32_t LidarLite::getMeasureTicks() const
uint32_t LidarLite::getMeasureTicks() const
{
return _measure_ticks;
return _measure_ticks;
}
int LidarLite::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* check against maximum rate */
if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
return (1000 / _measure_ticks);
case SENSORIOCRESET:
reset_sensor();
return OK;
case SENSORIOCRESET:
reset_sensor();
return OK;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return OK;
}
break;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return OK;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return OK;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return OK;
}
break;
default:
return -EINVAL;
}
default:
return -EINVAL;
}
}
src/drivers/ll40ls/LidarLite.h
+33 -33
View File
@@ -38,7 +38,7 @@
*
* Generic interface driver for the PulsedLight Lidar-Lite range finders.
*/
#pragma once
#pragma once
#include <drivers/device/device.h>
#include <drivers/drv_range_finder.h>
@@ -56,50 +56,50 @@
class LidarLite
{
public:
LidarLite();
LidarLite();
virtual ~LidarLite();
virtual ~LidarLite();
virtual int init() = 0;
virtual int init() = 0;
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual void start() = 0;
virtual void start() = 0;
virtual void stop() = 0;
virtual void stop() = 0;
/**
* @brief
* Diagnostics - print some basic information about the driver.
*/
virtual void print_info() = 0;
/**
* @brief
* Diagnostics - print some basic information about the driver.
*/
virtual void print_info() = 0;
/**
* @brief
* print registers to console
*/
virtual void print_registers() = 0;
/**
* @brief
* print registers to console
*/
virtual void print_registers() = 0;
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
*/
void set_minimum_distance(const float min);
void set_maximum_distance(const float max);
float get_minimum_distance() const;
float get_maximum_distance() const;
/**
* 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
*/
void set_minimum_distance(const float min);
void set_maximum_distance(const float max);
float get_minimum_distance() const;
float get_maximum_distance() const;
uint32_t getMeasureTicks() const;
uint32_t getMeasureTicks() const;
virtual int measure() = 0;
virtual int collect() = 0;
virtual int measure() = 0;
virtual int collect() = 0;
virtual int reset_sensor() = 0;
virtual int reset_sensor() = 0;
private:
float _min_distance;
float _max_distance;
uint32_t _measure_ticks;
float _min_distance;
float _max_distance;
uint32_t _measure_ticks;
};
src/drivers/ll40ls/LidarLiteI2C.cpp
+374 -360
View File
File diff suppressed because it is too large Load Diff
src/drivers/ll40ls/LidarLiteI2C.h
+69 -69
View File
@@ -73,91 +73,91 @@ class RingBuffer;
class LidarLiteI2C : public LidarLite, public device::I2C
{
public:
LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR);
virtual ~LidarLiteI2C();
LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR);
virtual ~LidarLiteI2C();
virtual int init() override;
virtual int init() override;
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg) override;
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg) override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info() override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info() override;
/**
* print registers to console
*/
void print_registers() override;
/**
* print registers to console
*/
void print_registers() override;
protected:
virtual int probe();
virtual int read_reg(uint8_t reg, uint8_t &val);
virtual int probe();
virtual int read_reg(uint8_t reg, uint8_t &val);
int measure() override;
int reset_sensor() override;
int measure() override;
int reset_sensor() override;
private:
work_s _work;
RingBuffer *_reports;
bool _sensor_ok;
bool _collect_phase;
int _class_instance;
work_s _work;
RingBuffer *_reports;
bool _sensor_ok;
bool _collect_phase;
int _class_instance;
orb_advert_t _range_finder_topic;
orb_advert_t _range_finder_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
perf_counter_t _sensor_resets;
perf_counter_t _sensor_zero_resets;
uint16_t _last_distance;
uint16_t _zero_counter;
uint64_t _acquire_time_usec;
volatile bool _pause_measurements;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
perf_counter_t _sensor_resets;
perf_counter_t _sensor_zero_resets;
uint16_t _last_distance;
uint16_t _zero_counter;
uint64_t _acquire_time_usec;
volatile bool _pause_measurements;
/**< the bus the device is connected to */
int _bus;
/**< the bus the device is connected to */
int _bus;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
private:
LidarLiteI2C(const LidarLiteI2C &copy) = delete;
LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete;
LidarLiteI2C(const LidarLiteI2C &copy) = delete;
LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete;
};
src/drivers/ll40ls/LidarLitePWM.cpp
+52 -51
View File
@@ -48,40 +48,40 @@ static const int ERROR = -1;
#endif
LidarLitePWM::LidarLitePWM() :
_terminateRequested(false),
_pwmSub(-1),
_pwm{},
_rangePub(-1),
_range{}
_terminateRequested(false),
_pwmSub(-1),
_pwm{},
_rangePub(-1),
_range{}
{
}
int LidarLitePWM::init()
{
_pwmSub = orb_subscribe(ORB_ID(pwm_input));
_pwmSub = orb_subscribe(ORB_ID(pwm_input));
if(_pwmSub == -1) {
return ERROR;
}
if (_pwmSub == -1) {
return ERROR;
}
_range.type = RANGE_FINDER_TYPE_LASER;
_range.valid = false;
_rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range);
_range.type = RANGE_FINDER_TYPE_LASER;
_range.valid = false;
_rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range);
return OK;
return OK;
}
void LidarLitePWM::print_info()
{
printf("poll interval: %u ticks\n", getMeasureTicks());
printf("distance: %ucm (0x%04x)\n",
(unsigned)_range.distance, (unsigned)_range.distance);
printf("poll interval: %u ticks\n", getMeasureTicks());
printf("distance: %ucm (0x%04x)\n",
(unsigned)_range.distance, (unsigned)_range.distance);
}
void LidarLitePWM::print_registers()
{
printf("Not supported in PWM mode\n");
printf("Not supported in PWM mode\n");
}
void LidarLitePWM::start()
@@ -91,52 +91,53 @@ void LidarLitePWM::start()
void LidarLitePWM::stop()
{
//TODO: stop measurement task
_terminateRequested = true;
//TODO: stop measurement task
_terminateRequested = true;
}
int LidarLitePWM::measure()
{
int result = OK;
int result = OK;
_range.error_count = _pwm.error_count;
_range.maximum_distance = get_maximum_distance();
_range.minimum_distance = get_minimum_distance();
_range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite
_range.distance_vector[0] = _range.distance;
_range.just_updated = 0;
_range.valid = true;
_range.error_count = _pwm.error_count;
_range.maximum_distance = get_maximum_distance();
_range.minimum_distance = get_minimum_distance();
_range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite
_range.distance_vector[0] = _range.distance;
_range.just_updated = 0;
_range.valid = true;
//TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0)
if(_range.distance <= 0.0f) {
_range.valid = false;
_range.error_count++;
result = ERROR;
}
//TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0)
if (_range.distance <= 0.0f) {
_range.valid = false;
_range.error_count++;
result = ERROR;
}
orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
return result;
return result;
}
int LidarLitePWM::collect()
{
//Check PWM
bool update;
orb_check(_pwmSub, &update);
if(update) {
orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm);
_range.timestamp = hrt_absolute_time();
return OK;
}
//Check PWM
bool update;
orb_check(_pwmSub, &update);
//Timeout readings after 0.2 seconds and mark as invalid
if(hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT*2) {
_range.timestamp = hrt_absolute_time();
_range.valid = false;
orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
return ERROR;
}
if (update) {
orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm);
_range.timestamp = hrt_absolute_time();
return OK;
}
return EAGAIN;
//Timeout readings after 0.2 seconds and mark as invalid
if (hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT * 2) {
_range.timestamp = hrt_absolute_time();
_range.valid = false;
orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
return ERROR;
}
return EAGAIN;
}
src/drivers/ll40ls/LidarLitePWM.h
+22 -22
View File
@@ -47,37 +47,37 @@
class LidarLitePWM : public LidarLite
{
public:
LidarLitePWM();
LidarLitePWM();
int init() override;
int init() override;
void start() override;
void start() override;
void stop() override;
void stop() override;
/**
* @brief
* Diagnostics - print some basic information about the driver.
*/
void print_info() override;
/**
* @brief
* Diagnostics - print some basic information about the driver.
*/
void print_info() override;
/**
* @brief
* print registers to console
*/
void print_registers() override;
/**
* @brief
* print registers to console
*/
void print_registers() override;
protected:
int measure() override;
int measure() override;
int collect() override;
int collect() override;
void task_main_trampoline(int argc, char *argv[]);
void task_main_trampoline(int argc, char *argv[]);
private:
bool _terminateRequested;
int _pwmSub;
pwm_input_s _pwm;
orb_advert_t _rangePub;
range_finder_report _range;
bool _terminateRequested;
int _pwmSub;
pwm_input_s _pwm;
orb_advert_t _rangePub;
range_finder_report _range;
};
src/drivers/ll40ls/ll40ls.cpp
+39 -15
View File
@@ -90,12 +90,16 @@ void start(int bus)
{
/* create the driver, attempt expansion bus first */
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev_ext != nullptr)
if (g_dev_ext != nullptr) {
errx(0, "already started external");
}
g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
delete g_dev_ext;
g_dev_ext = nullptr;
if (bus == PX4_I2C_BUS_EXPANSION) {
goto fail;
}
@@ -103,11 +107,15 @@ void start(int bus)
}
#ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */
if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr)
if (g_dev_int != nullptr) {
errx(0, "already started internal");
}
g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
/* tear down the failing onboard instance */
delete g_dev_int;
@@ -117,44 +125,54 @@ void start(int bus)
goto fail;
}
}
if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
#endif
/* set the poll rate to default, starts automatic data collection */
if (g_dev_int != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
if (fd == -1) {
goto fail;
}
if (fd == -1) {
goto fail;
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (ret < 0) {
goto fail;
}
}
}
if (g_dev_ext != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
if (fd == -1) {
goto fail;
}
if (fd == -1) {
goto fail;
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (ret < 0) {
goto fail;
}
}
}
exit(0);
fail:
if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
delete g_dev_int;
g_dev_int = nullptr;
}
if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
delete g_dev_ext;
g_dev_ext = nullptr;
@@ -168,7 +186,8 @@ fail:
*/
void stop(int bus)
{
LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext);
LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext);
if (*g_dev != nullptr) {
delete *g_dev;
*g_dev = nullptr;
@@ -191,7 +210,7 @@ test(int bus)
struct range_finder_report report;
ssize_t sz;
int ret;
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
int fd = open(path, O_RDONLY);
@@ -254,7 +273,7 @@ test(int bus)
void
reset(int bus)
{
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
int fd = open(path, O_RDONLY);
if (fd < 0) {
@@ -278,7 +297,8 @@ reset(int bus)
void
info(int bus)
{
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) {
errx(1, "driver not running");
}
@@ -295,7 +315,8 @@ info(int bus)
void
regdump(int bus)
{
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) {
errx(1, "driver not running");
}
@@ -328,13 +349,16 @@ ll40ls_main(int argc, char *argv[])
while ((ch = getopt(argc, argv, "XI")) != EOF) {
switch (ch) {
#ifdef PX4_I2C_BUS_ONBOARD
case 'I':
bus = PX4_I2C_BUS_ONBOARD;
break;
#endif
case 'X':
bus = PX4_I2C_BUS_EXPANSION;
break;
default:
ll40ls::usage();
exit(0);