forked from Archive/PX4-Autopilot
sensors: first part of a calibration refactor
This adds uORB messages to publish calibration data by sensors which is then consumed by the sensors. Currently this is only used on Snapdragon and guarded by QURT ifdefs.
This commit is contained in:
Julian Oes
parent
76a8bea1c1
commit
f24b2a701f
|
|
@ -0,0 +1,10 @@
|
|||
uint64 timestamp
|
||||
|
||||
int32 device_id
|
||||
|
||||
float32 x_offset
|
||||
float32 x_scale
|
||||
float32 y_offset
|
||||
float32 y_scale
|
||||
float32 z_offset
|
||||
float32 z_scale
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
uint64 timestamp
|
||||
|
||||
int32 device_id
|
||||
|
||||
float32 x_offset
|
||||
float32 x_scale
|
||||
float32 y_offset
|
||||
float32 y_scale
|
||||
float32 z_offset
|
||||
float32 z_scale
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
uint64 timestamp
|
||||
|
||||
int32 device_id
|
||||
|
||||
float32 x_offset
|
||||
float32 x_scale
|
||||
float32 y_offset
|
||||
float32 y_scale
|
||||
float32 z_offset
|
||||
float32 z_scale
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2016 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
|
||||
|
|
@ -41,7 +41,7 @@
|
|||
* well instead of relying on the sensor_combined topic.
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
* @author Julian Oes <julian@px4.io>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomas@px4.io>
|
||||
* @author Anton Babushkin <anton@px4.io>
|
||||
*/
|
||||
|
|
@ -100,6 +100,9 @@
|
|||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/rc_parameter_map.h>
|
||||
#include <uORB/topics/gyro_calibration.h>
|
||||
#include <uORB/topics/accel_calibration.h>
|
||||
#include <uORB/topics/mag_calibration.h>
|
||||
|
||||
#include <DevMgr.hpp>
|
||||
|
||||
|
|
@ -453,6 +456,36 @@ private:
|
|||
*/
|
||||
void parameter_update_poll(bool forced = false);
|
||||
|
||||
/**
|
||||
* Apply a gyro calibration.
|
||||
*
|
||||
* @param h: reference to the DevHandle in use
|
||||
* @param gscale: the calibration data.
|
||||
* @param device: the device id of the sensor.
|
||||
* @return: true if config is ok
|
||||
*/
|
||||
bool apply_gyro_calibration(DevHandle &h, const struct gyro_scale *gscale, const int device_id);
|
||||
|
||||
/**
|
||||
* Apply a accel calibration.
|
||||
*
|
||||
* @param h: reference to the DevHandle in use
|
||||
* @param ascale: the calibration data.
|
||||
* @param device: the device id of the sensor.
|
||||
* @return: true if config is ok
|
||||
*/
|
||||
bool apply_accel_calibration(DevHandle &h, const struct accel_scale *ascale, const int device_id);
|
||||
|
||||
/**
|
||||
* Apply a mag calibration.
|
||||
*
|
||||
* @param h: reference to the DevHandle in use
|
||||
* @param gscale: the calibration data.
|
||||
* @param device: the device id of the sensor.
|
||||
* @return: true if config is ok
|
||||
*/
|
||||
bool apply_mag_calibration(DevHandle &h, const struct mag_scale *mscale, const int device_id);
|
||||
|
||||
/**
|
||||
* Check for changes in rc_parameter_map
|
||||
*/
|
||||
|
|
@ -1184,7 +1217,6 @@ Sensors::parameter_update_poll(bool forced)
|
|||
|
||||
/* set offset parameters to new values */
|
||||
bool failed;
|
||||
int res;
|
||||
char str[30];
|
||||
unsigned mag_count = 0;
|
||||
unsigned gyro_count = 0;
|
||||
|
|
@ -1193,7 +1225,6 @@ Sensors::parameter_update_poll(bool forced)
|
|||
/* run through all gyro sensors */
|
||||
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
|
||||
|
||||
res = ERROR;
|
||||
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
|
||||
|
||||
DevHandle h;
|
||||
|
|
@ -1243,13 +1274,10 @@ Sensors::parameter_update_poll(bool forced)
|
|||
|
||||
} else {
|
||||
/* apply new scaling and offsets */
|
||||
res = h.ioctl(GYROIOCSSCALE, (long unsigned int)&gscale);
|
||||
config_ok = apply_gyro_calibration(h, &gscale, device_id);
|
||||
|
||||
if (res) {
|
||||
warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
|
||||
|
||||
} else {
|
||||
config_ok = true;
|
||||
if (!config_ok) {
|
||||
warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1265,7 +1293,6 @@ Sensors::parameter_update_poll(bool forced)
|
|||
/* run through all accel sensors */
|
||||
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
|
||||
|
||||
res = ERROR;
|
||||
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
|
||||
|
||||
DevHandle h;
|
||||
|
|
@ -1296,32 +1323,29 @@ Sensors::parameter_update_poll(bool forced)
|
|||
|
||||
/* if the calibration is for this device, apply it */
|
||||
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
|
||||
struct accel_scale gscale = {};
|
||||
struct accel_scale ascale = {};
|
||||
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
|
||||
failed = failed || (OK != param_get(param_find(str), &ascale.x_offset));
|
||||
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.y_offset));
|
||||
failed = failed || (OK != param_get(param_find(str), &ascale.y_offset));
|
||||
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.z_offset));
|
||||
failed = failed || (OK != param_get(param_find(str), &ascale.z_offset));
|
||||
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.x_scale));
|
||||
failed = failed || (OK != param_get(param_find(str), &ascale.x_scale));
|
||||
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.y_scale));
|
||||
failed = failed || (OK != param_get(param_find(str), &ascale.y_scale));
|
||||
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
|
||||
failed = failed || (OK != param_get(param_find(str), &ascale.z_scale));
|
||||
|
||||
if (failed) {
|
||||
warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
|
||||
|
||||
} else {
|
||||
/* apply new scaling and offsets */
|
||||
res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)&gscale);
|
||||
config_ok = apply_accel_calibration(h, &ascale, device_id);
|
||||
|
||||
if (res) {
|
||||
warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
|
||||
|
||||
} else {
|
||||
config_ok = true;
|
||||
if (!config_ok) {
|
||||
warnx(CAL_ERROR_APPLY_CAL_MSG, "accel ", i);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1343,7 +1367,6 @@ Sensors::parameter_update_poll(bool forced)
|
|||
*/
|
||||
_mag_rotation[s] = _board_rotation;
|
||||
|
||||
res = ERROR;
|
||||
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
|
||||
|
||||
DevHandle h;
|
||||
|
|
@ -1377,19 +1400,19 @@ Sensors::parameter_update_poll(bool forced)
|
|||
|
||||
/* if the calibration is for this device, apply it */
|
||||
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
|
||||
struct mag_scale gscale = {};
|
||||
struct mag_scale mscale = {};
|
||||
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
|
||||
failed = failed || (OK != param_get(param_find(str), &mscale.x_offset));
|
||||
(void)sprintf(str, "CAL_MAG%u_YOFF", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.y_offset));
|
||||
failed = failed || (OK != param_get(param_find(str), &mscale.y_offset));
|
||||
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.z_offset));
|
||||
failed = failed || (OK != param_get(param_find(str), &mscale.z_offset));
|
||||
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.x_scale));
|
||||
failed = failed || (OK != param_get(param_find(str), &mscale.x_scale));
|
||||
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.y_scale));
|
||||
failed = failed || (OK != param_get(param_find(str), &mscale.y_scale));
|
||||
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
|
||||
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
|
||||
failed = failed || (OK != param_get(param_find(str), &mscale.z_scale));
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_ROT", i);
|
||||
|
||||
|
|
@ -1451,14 +1474,12 @@ Sensors::parameter_update_poll(bool forced)
|
|||
warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
|
||||
|
||||
} else {
|
||||
|
||||
/* apply new scaling and offsets */
|
||||
res = h.ioctl(MAGIOCSSCALE, (long unsigned int)&gscale);
|
||||
config_ok = apply_mag_calibration(h, &mscale, device_id);
|
||||
|
||||
if (res) {
|
||||
warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
|
||||
|
||||
} else {
|
||||
config_ok = true;
|
||||
if (!config_ok) {
|
||||
warnx(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1493,6 +1514,99 @@ Sensors::parameter_update_poll(bool forced)
|
|||
}
|
||||
}
|
||||
|
||||
bool
|
||||
Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_scale *gscale, const int device_id)
|
||||
{
|
||||
#ifndef __PX4_QURT
|
||||
|
||||
/* On most systems, we can just use the IOCTL call to set the calibration params. */
|
||||
const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gscale);
|
||||
|
||||
if (res) {
|
||||
return false;
|
||||
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
|
||||
#else
|
||||
/* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */
|
||||
|
||||
static orb_advert_t gyro_calibration_pub = nullptr;
|
||||
|
||||
if (gyro_calibration_pub != nullptr) {
|
||||
orb_publish(ORB_ID(gyro_calibration), gyro_calibration_pub, gscale);
|
||||
|
||||
} else {
|
||||
gyro_calibration_pub = orb_advertise(ORB_ID(gyro_calibration), gscale);
|
||||
}
|
||||
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool
|
||||
Sensors::apply_accel_calibration(DevHandle &h, const struct accel_scale *ascale, const int device_id)
|
||||
{
|
||||
#ifndef __PX4_QURT
|
||||
|
||||
/* On most systems, we can just use the IOCTL call to set the calibration params. */
|
||||
const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)ascale);
|
||||
|
||||
if (res) {
|
||||
return false;
|
||||
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
|
||||
#else
|
||||
/* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */
|
||||
|
||||
static orb_advert_t accel_calibration_pub = nullptr;
|
||||
|
||||
if (accel_calibration_pub != nullptr) {
|
||||
orb_publish(ORB_ID(accel_calibration), accel_calibration_pub, ascale);
|
||||
|
||||
} else {
|
||||
accel_calibration_pub = orb_advertise(ORB_ID(accel_calibration), ascale);
|
||||
}
|
||||
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool
|
||||
Sensors::apply_mag_calibration(DevHandle &h, const struct mag_scale *mscale, const int device_id)
|
||||
{
|
||||
#ifndef __PX4_QURT
|
||||
|
||||
/* On most systems, we can just use the IOCTL call to set the calibration params. */
|
||||
const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mscale);
|
||||
|
||||
if (res) {
|
||||
return false;
|
||||
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
|
||||
#else
|
||||
/* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */
|
||||
|
||||
static orb_advert_t mag_calibration_pub = nullptr;
|
||||
|
||||
if (mag_calibration_pub != nullptr) {
|
||||
orb_publish(ORB_ID(mag_calibration), mag_calibration_pub, mscale);
|
||||
|
||||
} else {
|
||||
mag_calibration_pub = orb_advertise(ORB_ID(mag_calibration), mscale);
|
||||
}
|
||||
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::rc_parameter_map_poll(bool forced)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -282,3 +282,12 @@ ORB_DEFINE(ekf2_replay, struct ekf2_replay_s);
|
|||
|
||||
#include "topics/qshell_req.h"
|
||||
ORB_DEFINE(qshell_req, struct qshell_req_s);
|
||||
|
||||
#include "topics/gyro_calibration.h"
|
||||
ORB_DEFINE(gyro_calibration, struct gyro_calibration_s);
|
||||
|
||||
#include "topics/accel_calibration.h"
|
||||
ORB_DEFINE(accel_calibration, struct accel_calibration_s);
|
||||
|
||||
#include "topics/mag_calibration.h"
|
||||
ORB_DEFINE(mag_calibration, struct mag_calibration_s);
|
||||
|
|
|
|||
|
|
@ -57,9 +57,8 @@
|
|||
#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 <uORB/topics/gyro_calibration.h>
|
||||
#include <uORB/topics/accel_calibration.h>
|
||||
|
||||
#include <mpu9250/MPU9250.hpp>
|
||||
#include <DevMgr.hpp>
|
||||
|
|
@ -94,16 +93,28 @@ public:
|
|||
private:
|
||||
int _publish(struct imu_sensor_data &data);
|
||||
|
||||
void _update_accel_calibration();
|
||||
void _update_gyro_calibration();
|
||||
|
||||
//enum Rotation _rotation;
|
||||
|
||||
orb_advert_t _accel_topic;
|
||||
orb_advert_t _gyro_topic;
|
||||
orb_advert_t _accel_topic;
|
||||
orb_advert_t _gyro_topic;
|
||||
|
||||
int _accel_orb_class_instance;
|
||||
int _gyro_orb_class_instance;
|
||||
int _accel_calibration_sub;
|
||||
int _gyro_calibration_sub;
|
||||
|
||||
perf_counter_t _accel_sample_perf;
|
||||
perf_counter_t _gyro_sample_perf;
|
||||
struct accel_calibration_s _accel_calibration;
|
||||
struct gyro_calibration_s _gyro_calibration;
|
||||
|
||||
bool _accel_calibration_set;
|
||||
bool _gyro_calibration_set;
|
||||
|
||||
int _accel_orb_class_instance;
|
||||
int _gyro_orb_class_instance;
|
||||
|
||||
perf_counter_t _accel_sample_perf;
|
||||
perf_counter_t _gyro_sample_perf;
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -111,6 +122,12 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
|
|||
MPU9250(IMU_DEVICE_PATH),
|
||||
_accel_topic(nullptr),
|
||||
_gyro_topic(nullptr),
|
||||
_accel_calibration_sub(-1),
|
||||
_gyro_calibration_sub(-1),
|
||||
_accel_calibration{},
|
||||
_gyro_calibration{},
|
||||
_accel_calibration_set(false),
|
||||
_gyro_calibration_set(false),
|
||||
_accel_orb_class_instance(-1),
|
||||
_gyro_orb_class_instance(-1),
|
||||
_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")),
|
||||
|
|
@ -147,6 +164,14 @@ int DfMpu9250Wrapper::start()
|
|||
return -1;
|
||||
}
|
||||
|
||||
if (_accel_calibration_sub < 0) {
|
||||
_accel_calibration_sub = orb_subscribe(ORB_ID(accel_calibration));
|
||||
}
|
||||
|
||||
if (_gyro_calibration_sub < 0) {
|
||||
_gyro_calibration_sub = orb_subscribe(ORB_ID(gyro_calibration));
|
||||
}
|
||||
|
||||
/* Init device and start sensor. */
|
||||
int ret = init();
|
||||
|
||||
|
|
@ -178,66 +203,114 @@ int DfMpu9250Wrapper::stop()
|
|||
return 0;
|
||||
}
|
||||
|
||||
void DfMpu9250Wrapper::_update_gyro_calibration()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_gyro_calibration_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
gyro_calibration_s new_calibration;
|
||||
orb_copy(ORB_ID(gyro_calibration), _gyro_calibration_sub, &new_calibration);
|
||||
|
||||
/* Only accept calibration for this device. */
|
||||
if (m_id.dev_id == new_calibration.device_id) {
|
||||
_gyro_calibration = new_calibration;
|
||||
_gyro_calibration_set = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DfMpu9250Wrapper::_update_accel_calibration()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_accel_calibration_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
accel_calibration_s new_calibration;
|
||||
orb_copy(ORB_ID(accel_calibration), _accel_calibration_sub, &new_calibration);
|
||||
|
||||
/* Only accept calibration for this device. */
|
||||
if (m_id.dev_id == new_calibration.device_id) {
|
||||
_accel_calibration = new_calibration;
|
||||
_accel_calibration_set = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
||||
{
|
||||
/* Publish accel first. */
|
||||
perf_begin(_accel_sample_perf);
|
||||
/* Check if calibration values are still up-to-date */
|
||||
_update_accel_calibration();
|
||||
_update_gyro_calibration();
|
||||
|
||||
accel_report accel_report = {};
|
||||
accel_report.timestamp = data.last_read_time_usec;
|
||||
/* Don't publish if we have not received calibration data. */
|
||||
if (_accel_calibration_set) {
|
||||
|
||||
// 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;
|
||||
/* Publish accel first. */
|
||||
perf_begin(_accel_sample_perf);
|
||||
|
||||
// TODO: get these right
|
||||
accel_report.scaling = -1.0f;
|
||||
accel_report.range_m_s2 = -1.0f;
|
||||
accel_report accel_report = {};
|
||||
accel_report.timestamp = data.last_read_time_usec;
|
||||
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked)) {
|
||||
// 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 - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
|
||||
accel_report.y = (data.accel_m_s2_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
|
||||
accel_report.z = (data.accel_m_s2_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
|
||||
|
||||
if (_accel_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
// TODO: get these right
|
||||
accel_report.scaling = -1.0f;
|
||||
accel_report.range_m_s2 = -1.0f;
|
||||
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked)) {
|
||||
|
||||
if (_accel_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_accel_sample_perf);
|
||||
}
|
||||
|
||||
perf_end(_accel_sample_perf);
|
||||
/* Don't publish if we have not received calibration data. */
|
||||
if (_gyro_calibration_set) {
|
||||
|
||||
/* Then publish gyro. */
|
||||
perf_begin(_gyro_sample_perf);
|
||||
/* Then publish gyro. */
|
||||
perf_begin(_gyro_sample_perf);
|
||||
|
||||
gyro_report gyro_report = {};
|
||||
gyro_report.timestamp = data.last_read_time_usec;
|
||||
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: 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: 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)) {
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked)) {
|
||||
|
||||
if (_gyro_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
|
||||
if (_gyro_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_gyro_sample_perf);
|
||||
}
|
||||
|
||||
perf_end(_gyro_sample_perf);
|
||||
|
||||
/* Notify anyone waiting for data. */
|
||||
DevMgr::updateNotify(*this);
|
||||
if (_accel_calibration_set || _gyro_calibration_set) {
|
||||
/* Notify anyone waiting for data. */
|
||||
DevMgr::updateNotify(*this);
|
||||
}
|
||||
|
||||
return 0;
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue