diff --git a/src/platforms/posix/drivers/df_imu/df_imu.cpp b/src/platforms/posix/drivers/df_imu/df_imu.cpp index ddf906cd8a..61368ce4f9 100644 --- a/src/platforms/posix/drivers/df_imu/df_imu.cpp +++ b/src/platforms/posix/drivers/df_imu/df_imu.cpp @@ -60,8 +60,12 @@ //#include //#include // -#include "mpu9250/MPU9250.hpp" -#include "DevMgr.hpp" +#include +#include +#include + + +#define ACCELSIM_DEVICE_PATH_ACCEL "/dev/df_accel" extern "C" { __EXPORT int df_imu_main(int argc, char *argv[]); } @@ -69,7 +73,7 @@ extern "C" { __EXPORT int df_imu_main(int argc, char *argv[]); } using namespace DriverFramework; -class DfImu : MPU9250 +class DfImu : public MPU9250 { public: DfImu(/*enum Rotation rotation*/); @@ -94,12 +98,22 @@ public: private: virtual int _publish_callback(struct imu_sensor_data &data); + orb_advert_t _accel_topic; + int _accel_orb_class_instance; + + perf_counter_t _accel_sample_perf; + }; DfImu::DfImu(/*enum Rotation rotation*/) : - MPU9250(IMU_DEVICE_PATH) + MPU9250(IMU_DEVICE_PATH), + _accel_topic(nullptr), + _accel_orb_class_instance(-1), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")) /*_rotation(rotation)*/ { + m_id.dev_id_s.bus = 1; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_MPU9250; } DfImu::~DfImu() @@ -108,36 +122,73 @@ DfImu::~DfImu() int DfImu::start() { - /* start sensor */ - PX4_WARN("DF IMU STARTED=================================================="); - int ret = MPU9250::init(); + struct accel_report arp = {}; + + arp.timestamp = 1234; + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, ORB_PRIO_DEFAULT); + + if (_accel_topic == nullptr) { + PX4_WARN("ADVERT ERR"); + return -1; + } + + /* init device and start sensor */ + int ret = init(); if (ret != 0) { PX4_ERR("MPU9250 init fail: %d", ret); - return -1; + return ret; } ret = MPU9250::start(); if (ret != 0) { PX4_ERR("MPU9250 start fail: %d", ret); - return -1; + return ret; } - return 0; } int DfImu::stop() { /* stop sensor */ - PX4_WARN("DF IMU STOPPED=================================================="); return 0; } int DfImu::_publish_callback(struct imu_sensor_data &data) { - PX4_WARN("publish"); + 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; + + // TODO: get these right + accel_report.scaling = -1.0f; + accel_report.range_m_s2 = -1.0f; + + if (!(m_pub_blocked)) { + /* publish it */ + + if (_accel_topic != nullptr) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } + } + + /* notify anyone waiting for data */ + DevMgr::updateNotify(*this); + return 0; }; @@ -155,7 +206,31 @@ void usage(); int start(/*enum Rotation rotation*/) { g_dev = new DfImu(/*rotation*/); - return g_dev->start(); + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating DfImu object"); + return -1; + } + + int ret = g_dev->start(); + if (ret != 0) { + PX4_ERR("DfImu start failed"); + return ret; + } + + // Open the IMU sensor + DevHandle h; + DevMgr::getHandle(IMU_DEVICE_PATH, h); + if (!h.isValid()) + { + DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", + IMU_DEVICE_PATH, h.getError()); + return -1; + } + + DevMgr::releaseHandle(h); + + return 0; } int stop() diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 9db1b0b0e1..6b424d8239 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -49,6 +49,7 @@ #include #include "get_commands.h" #include "apps.h" +#include "DriverFramework.hpp" using namespace std; @@ -213,6 +214,7 @@ int dspal_entry(int argc, char *argv[]) PX4_INFO("In main\n"); map apps; init_app_map(apps); + DriverFramework::Framework::initialize(); px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands());