Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

df_imu: added gyro publishing, cleanup

Browse Source
This commit is contained in:
Julian Oes
2016-02-10 09:42:47 +01:00
parent 507b9130fd
commit adb44c2918
1 changed files with 74 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_imu/df_imu.cpp
+74 -25
View File
@@ -33,7 +33,7 @@
/**
* @file df_imu.cpp
* Lightweight driver to access IMU driver of DriverFramework.
* Lightweight driver to access an IMU driver of the DriverFramework.
*
* @author Julian Oes <julian@oes.ch>
*/
@@ -55,17 +55,14 @@
#include <systemlib/err.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <board_config.h>
//#include <mathlib/math/filter/LowPassFilter2p.hpp>
//#include <lib/conversion/rotation.h>
//
#include <mpu9250/MPU9250.hpp>
#include <DevMgr.hpp>
#include <VirtDevObj.hpp>
#define ACCELSIM_DEVICE_PATH_ACCEL "/dev/df_accel"
extern "C" { __EXPORT int df_imu_main(int argc, char *argv[]); }
@@ -79,7 +76,6 @@ public:
DfImu(/*enum Rotation rotation*/);
~DfImu();
//enum Rotation _rotation;
/**
* Start automatic measurement.
@@ -96,44 +92,60 @@ public:
int stop();
private:
virtual int _publish_callback(struct imu_sensor_data &data);
int _publish(struct imu_sensor_data &data);
//enum Rotation _rotation;
orb_advert_t _accel_topic;
orb_advert_t _gyro_topic;
int _accel_orb_class_instance;
int _gyro_orb_class_instance;
perf_counter_t _accel_sample_perf;
perf_counter_t _gyro_sample_perf;
};
DfImu::DfImu(/*enum Rotation rotation*/) :
MPU9250(IMU_DEVICE_PATH),
_accel_topic(nullptr),
_gyro_topic(nullptr),
_accel_orb_class_instance(-1),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read"))
_gyro_orb_class_instance(-1),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")),
_gyro_sample_perf(perf_alloc(PC_ELAPSED, "df_gyro_read"))
/*_rotation(rotation)*/
{
m_id.dev_id_s.bus = 1;
m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}
DfImu::~DfImu()
{
perf_free(_accel_sample_perf);
perf_free(_gyro_sample_perf);
}
int DfImu::start()
{
struct accel_report arp = {};
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
// TODO: don't publish garbage here
accel_report accel_report = {};
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
&_accel_orb_class_instance, ORB_PRIO_DEFAULT);
if (_accel_topic == nullptr) {
PX4_WARN("ADVERT ERR");
PX4_ERR("sensor_accel advert fail");
return -1;
}
/* init device and start sensor */
// TODO: don't publish garbage here
gyro_report gyro_report = {};
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
&_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
if (_gyro_topic == nullptr) {
PX4_ERR("sensor_gyro advert fail");
return -1;
}
/* Init device and start sensor. */
int ret = init();
if (ret != 0) {
@@ -153,21 +165,29 @@ int DfImu::start()
int DfImu::stop()
{
/* stop sensor */
/* Stop sensor. */
int ret = MPU9250::stop();
if (ret != 0) {
PX4_ERR("MPU9250 stop fail: %d", ret);
return ret;
}
return 0;
}
int DfImu::_publish_callback(struct imu_sensor_data &data)
int DfImu::_publish(struct imu_sensor_data &data)
{
accel_report accel_report = {};
/* Publish accel first. */
perf_begin(_accel_sample_perf);
accel_report accel_report = {};
accel_report.timestamp = data.last_read_time_usec;
// TODO: remove these (or get the values)
accel_report.x_raw = NAN;
accel_report.y_raw = NAN;
accel_report.z_raw = NAN;
accel_report.x = data.accel_m_s2_x;
accel_report.y = data.accel_m_s2_y;
accel_report.z = data.accel_m_s2_z;
@@ -176,15 +196,43 @@ int DfImu::_publish_callback(struct imu_sensor_data &data)
accel_report.scaling = -1.0f;
accel_report.range_m_s2 = -1.0f;
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
/* publish it */
if (_accel_topic != nullptr) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
}
perf_end(_accel_sample_perf);
/* notify anyone waiting for data */
/* Then publish gyro. */
perf_begin(_gyro_sample_perf);
gyro_report gyro_report = {};
gyro_report.timestamp = data.last_read_time_usec;
// TODO: remove these (or get the values)
gyro_report.x_raw = NAN;
gyro_report.y_raw = NAN;
gyro_report.z_raw = NAN;
gyro_report.x = data.gyro_rad_s_x;
gyro_report.y = data.gyro_rad_s_y;
gyro_report.z = data.gyro_rad_s_z;
// TODO: get these right
gyro_report.scaling = -1.0f;
gyro_report.range_rad_s = -1.0f;
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
if (_gyro_topic != nullptr) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
}
}
perf_end(_gyro_sample_perf);
/* Notify anyone waiting for data. */
DevMgr::updateNotify(*this);
return 0;
@@ -274,7 +322,8 @@ usage()
//PX4_WARN(" -R rotation");
}
} // namespace
} // namespace df_imu
int
df_imu_main(int argc, char *argv[])