fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

teraranger: add possibility to specify orientation and adapt to new defaults

Browse Source
This commit is contained in:
ChristophTobler 2017-09-19 13:22:29 +02:00 committed by ChristophTobler
parent a4d37f7120
commit 286d8f2bff
1 changed files with 33 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/drivers/teraranger/teraranger.cpp
View File

@ -40,6 +40,7 @@
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_getopt.h>
#include <drivers/device/i2c.h>
@ -101,7 +102,8 @@
class TERARANGER : public device::I2C
{
public:
TERARANGER(int bus = TERARANGER_BUS, int address = TRONE_BASEADDR);
TERARANGER(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
int bus = TERARANGER_BUS, int address = TRONE_BASEADDR);
virtual ~TERARANGER();
virtual int init();
@ -118,6 +120,7 @@ protected:
virtual int probe();
private:
uint8_t _rotation;
float _min_distance;
float _max_distance;
work_s _work;
@ -227,8 +230,9 @@ static uint8_t crc8(uint8_t *p, uint8_t len)
*/
extern "C" __EXPORT int teraranger_main(int argc, char *argv[]);
TERARANGER::TERARANGER(int bus, int address) :
TERARANGER::TERARANGER(uint8_t rotation, int bus, int address) :
I2C("TERARANGER", TERARANGER_DEVICE_PATH, bus, address, 100000),
_rotation(rotation),
_min_distance(-1.0f),
_max_distance(-1.0f),
_reports(nullptr),
@ -631,7 +635,7 @@ TERARANGER::collect()
report.timestamp = hrt_absolute_time();
/* there is no enum item for a combined LASER and ULTRASOUND which it should be */
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = 8;
report.orientation = _rotation;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
@ -765,7 +769,7 @@ namespace teraranger
TERARANGER *g_dev;
void start();
void start(uint8_t rotation);
void stop();
void test();
void reset();
@ -775,7 +779,7 @@ void info();
* Start the driver.
*/
void
start()
start(uint8_t rotation)
{
int fd;
@ -784,7 +788,7 @@ start()
}
/* create the driver */
g_dev = new TERARANGER(TERARANGER_BUS);
g_dev = new TERARANGER(rotation, TERARANGER_BUS);
if (g_dev == nullptr) {
@ -945,38 +949,55 @@ info()
int
teraranger_main(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
PX4_INFO("Setting sensor orientation to %d", (int)rotation);
break;
default:
PX4_WARN("Unknown option!");
}
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
teraranger::start();
if (!strcmp(argv[myoptind], "start")) {
teraranger::start(rotation);
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
if (!strcmp(argv[myoptind], "stop")) {
teraranger::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
if (!strcmp(argv[myoptind], "test")) {
teraranger::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
if (!strcmp(argv[myoptind], "reset")) {
teraranger::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
teraranger::info();
}