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

df trone wrapper: update for new orientation convention

Browse Source 
add possibility to specify distance sensor orientation
This commit is contained in:
ChristophTobler
2017-09-25 08:52:21 +02:00
committed by ChristophTobler
parent de0c196b2b
commit 58e09faf9b
1 changed files with 16 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp
+16 -8
View File
@@ -76,7 +76,7 @@ using namespace DriverFramework;
class DfTROneWrapper : public TROne class DfTROneWrapper : public TROne
{ {
public: public:
DfTROneWrapper(); DfTROneWrapper(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
~DfTROneWrapper(); ~DfTROneWrapper();
@@ -97,6 +97,8 @@ public:
private: private:
int _publish(struct range_sensor_data &data); int _publish(struct range_sensor_data &data);
uint8_t _rotation;
orb_advert_t _range_topic; orb_advert_t _range_topic;
int _orb_class_instance; int _orb_class_instance;
@@ -105,8 +107,9 @@ private:
}; };
DfTROneWrapper::DfTROneWrapper(/*enum Rotation rotation*/) : DfTROneWrapper::DfTROneWrapper(uint8_t rotation) :
TROne(TRONE_DEVICE_PATH), TROne(TRONE_DEVICE_PATH),
_rotation(rotation),
_range_topic(nullptr), _range_topic(nullptr),
_orb_class_instance(-1) _orb_class_instance(-1)
{ {
@@ -169,7 +172,7 @@ int DfTROneWrapper::_publish(struct range_sensor_data &data)
d.id = 0; // TODO set proper ID d.id = 0; // TODO set proper ID
d.orientation = 0; // TODO no idea what to put here d.orientation = _rotation;
d.covariance = 0.0f; d.covariance = 0.0f;
@@ -190,16 +193,16 @@ namespace df_trone_wrapper
DfTROneWrapper *g_dev = nullptr; DfTROneWrapper *g_dev = nullptr;
int start(); int start(uint8_t rotation);
int stop(); int stop();
int info(); int info();
int probe(); int probe();
void usage(); void usage();
int start() int start(uint8_t rotation)
{ {
PX4_ERR("start"); PX4_ERR("start");
g_dev = new DfTROneWrapper(); g_dev = new DfTROneWrapper(rotation);
if (g_dev == nullptr) { if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfTROneWrapper object"); PX4_ERR("failed instantiating DfTROneWrapper object");
@@ -272,7 +275,7 @@ probe()
int ret; int ret;
if (g_dev == nullptr) { if (g_dev == nullptr) {
ret = start(); ret = start(distance_sensor_s::ROTATION_DOWNWARD_FACING);
if (ret) { if (ret) {
PX4_ERR("Failed to start"); PX4_ERR("Failed to start");
@@ -309,10 +312,15 @@ df_trone_wrapper_main(int argc, char *argv[])
int ret = 0; int ret = 0;
int myoptind = 1; int myoptind = 1;
const char *myoptarg = NULL; const char *myoptarg = NULL;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
/* jump over start/off/etc and look at options first */ /* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) { switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
break;
default: default:
df_trone_wrapper::usage(); df_trone_wrapper::usage();
@@ -329,7 +337,7 @@ df_trone_wrapper_main(int argc, char *argv[])
if (!strcmp(verb, "start")) { if (!strcmp(verb, "start")) {
ret = df_trone_wrapper::start(/*rotation*/); ret = df_trone_wrapper::start(rotation);
} }
else if (!strcmp(verb, "stop")) { else if (!strcmp(verb, "stop")) {