fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 04:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move all drivers to multi pub/sub API

Browse Source
This commit is contained in:
Lorenz Meier
2015-01-29 16:33:53 +01:00
parent cc7a00b96e
commit d851a630d8
8 changed files with 87 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/l3gd20/l3gd20.cpp
+20 -20
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@@ -33,7 +33,7 @@
/**
* @file l3gd20.cpp
* Driver for the ST L3GD20 MEMS gyro connected via SPI.
* Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
*
* Note: With the exception of the self-test feature, the ST L3G4200D is
* also supported by this driver.
@@ -179,6 +179,12 @@ static const int ERROR = -1;
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
#endif
@@ -221,7 +227,7 @@ private:
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
orb_id_t _orb_id;
int _orb_class_instance;
int _class_instance;
unsigned _current_rate;
@@ -273,6 +279,13 @@ private:
*/
void disable_i2c();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_orb_id(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -456,20 +469,6 @@ L3GD20::init()
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_orb_id = ORB_ID(sensor_gyro2);
break;
}
reset();
measure();
@@ -478,7 +477,8 @@ L3GD20::init()
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(_orb_id, &grp);
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic < 0) {
debug("failed to create sensor_gyro publication");
@@ -1050,7 +1050,7 @@ L3GD20::measure()
/* publish for subscribers */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_orb_id, _gyro_topic, &report);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;