forked from Archive/PX4-Autopilot
sensors/calibration: use params in DF wrappers
Instead of using a uORB topic with the calibration values published in sensors and consumed by the DriverFramework driver wrappers, let's just use the the params directly. This is quite a rough change and needs definitely some cleanup and refactoring.
This commit is contained in:
Julian Oes
parent
aa9d698204
commit
1b5210ca13
|
|
@ -1,10 +0,0 @@
|
|||
uint64 timestamp
|
||||
|
||||
int32 device_id
|
||||
|
||||
float32 x_offset
|
||||
float32 x_scale
|
||||
float32 y_offset
|
||||
float32 y_scale
|
||||
float32 z_offset
|
||||
float32 z_scale
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
uint64 timestamp
|
||||
|
||||
int32 device_id
|
||||
|
||||
float32 x_offset
|
||||
float32 x_scale
|
||||
float32 y_offset
|
||||
float32 y_scale
|
||||
float32 z_offset
|
||||
float32 z_scale
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
uint64 timestamp
|
||||
|
||||
int32 device_id
|
||||
|
||||
float32 x_offset
|
||||
float32 x_scale
|
||||
float32 y_offset
|
||||
float32 y_scale
|
||||
float32 z_offset
|
||||
float32 z_scale
|
||||
|
|
@ -15,3 +15,5 @@ int16 x_raw
|
|||
int16 y_raw
|
||||
int16 z_raw
|
||||
int16 temperature_raw
|
||||
|
||||
uint32 device_id
|
||||
|
|
|
|||
|
|
@ -15,3 +15,5 @@ int16 x_raw
|
|||
int16 y_raw
|
||||
int16 z_raw
|
||||
int16 temperature_raw
|
||||
|
||||
uint32 device_id
|
||||
|
|
|
|||
|
|
@ -10,3 +10,5 @@ float32 temperature
|
|||
int16 x_raw
|
||||
int16 y_raw
|
||||
int16 z_raw
|
||||
|
||||
uint32 device_id
|
||||
|
|
|
|||
|
|
@ -42,7 +42,6 @@
|
|||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <uORB/topics/accel_calibration.h>
|
||||
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
|
@ -55,6 +54,15 @@
|
|||
#include <uORB/topics/sensor_accel.h>
|
||||
#define accel_report sensor_accel_s
|
||||
|
||||
/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
|
||||
struct accel_calibration_s {
|
||||
float x_offset;
|
||||
float x_scale;
|
||||
float y_offset;
|
||||
float y_scale;
|
||||
float z_offset;
|
||||
float z_scale;
|
||||
};
|
||||
/*
|
||||
* ioctl() definitions
|
||||
*
|
||||
|
|
|
|||
|
|
@ -42,7 +42,6 @@
|
|||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <uORB/topics/gyro_calibration.h>
|
||||
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
|
@ -55,6 +54,15 @@
|
|||
#include <uORB/topics/sensor_gyro.h>
|
||||
#define gyro_report sensor_gyro_s
|
||||
|
||||
/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
|
||||
struct gyro_calibration_s {
|
||||
float x_offset;
|
||||
float x_scale;
|
||||
float y_offset;
|
||||
float y_scale;
|
||||
float z_offset;
|
||||
float z_scale;
|
||||
};
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
|
|
|
|||
|
|
@ -40,7 +40,6 @@
|
|||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <uORB/topics/mag_calibration.h>
|
||||
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
|
@ -53,6 +52,15 @@
|
|||
#include <uORB/topics/sensor_mag.h>
|
||||
#define mag_report sensor_mag_s
|
||||
|
||||
/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
|
||||
struct mag_calibration_s {
|
||||
float x_offset;
|
||||
float x_scale;
|
||||
float y_offset;
|
||||
float y_scale;
|
||||
float z_offset;
|
||||
float z_scale;
|
||||
};
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-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
|
||||
|
|
@ -157,7 +157,6 @@ static const int ERROR = -1;
|
|||
static const char *sensor_name = "accel";
|
||||
|
||||
static int32_t device_id[max_accel_sens];
|
||||
static int device_prio_max = 0;
|
||||
static int32_t device_id_primary = 0;
|
||||
|
||||
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
|
||||
|
|
@ -193,9 +192,9 @@ int do_accel_calibration(int mavlink_fd)
|
|||
|
||||
char str[30];
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
/* reset all sensors */
|
||||
for (unsigned s = 0; s < max_accel_sens; s++) {
|
||||
#ifndef __PX4_QURT
|
||||
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
fd = px4_open(str, 0);
|
||||
|
|
@ -212,8 +211,39 @@ int do_accel_calibration(int mavlink_fd)
|
|||
if (res != OK) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
|
||||
}
|
||||
}
|
||||
#else
|
||||
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
|
||||
res = param_set(param_find(str), &accel_scale.x_offset);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
|
||||
res = param_set(param_find(str), &accel_scale.y_offset);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
|
||||
res = param_set(param_find(str), &accel_scale.z_offset);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
|
||||
res = param_set(param_find(str), &accel_scale.x_scale);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
|
||||
res = param_set(param_find(str), &accel_scale.y_scale);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
|
||||
res = param_set(param_find(str), &accel_scale.z_scale);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
float accel_offs[max_accel_sens][3];
|
||||
float accel_T[max_accel_sens][3][3];
|
||||
|
|
@ -267,6 +297,16 @@ int do_accel_calibration(int mavlink_fd)
|
|||
|
||||
failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
|
||||
|
||||
|
||||
PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", i,
|
||||
(double)accel_scale.x_offset,
|
||||
(double)accel_scale.y_offset,
|
||||
(double)accel_scale.z_offset);
|
||||
PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", i,
|
||||
(double)accel_scale.x_scale,
|
||||
(double)accel_scale.y_scale,
|
||||
(double)accel_scale.z_scale);
|
||||
|
||||
/* set parameters */
|
||||
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
|
||||
|
|
@ -280,10 +320,8 @@ int do_accel_calibration(int mavlink_fd)
|
|||
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
|
||||
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
|
||||
#ifndef __PX4_QURT
|
||||
(void)sprintf(str, "CAL_ACC%u_ID", i);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
|
||||
#endif
|
||||
|
||||
if (failed) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
|
||||
|
|
@ -387,10 +425,18 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
|
|||
int32_t prio;
|
||||
orb_priority(worker_data.subs[i], &prio);
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
int device_prio_max = 0;
|
||||
if (prio > device_prio_max) {
|
||||
device_prio_max = prio;
|
||||
device_id_primary = device_id[i];
|
||||
}
|
||||
#else
|
||||
PX4_INFO("found device id: %d", arp.device_id);
|
||||
|
||||
// TODO FIXME: this is hacky but should get the device ID for now
|
||||
device_id[i] = arp.device_id;
|
||||
#endif
|
||||
}
|
||||
|
||||
if (result == calibrate_return_ok) {
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-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
|
||||
|
|
@ -167,12 +167,11 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
gyro_scale_zero.z_offset = 0.0f;
|
||||
gyro_scale_zero.z_scale = 1.0f;
|
||||
|
||||
int device_prio_max = 0;
|
||||
int32_t device_id_primary = 0;
|
||||
|
||||
for (unsigned s = 0; s < max_gyros; s++) {
|
||||
char str[30];
|
||||
#ifndef __PX4_QURT
|
||||
|
||||
// Reset gyro ids to unavailable
|
||||
worker_data.device_id[s] = 0;
|
||||
(void)sprintf(str, "CAL_GYRO%u_ID", s);
|
||||
|
|
@ -181,14 +180,6 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
|
||||
return ERROR;
|
||||
}
|
||||
#else
|
||||
(void)sprintf(str, "CAL_GYRO%u_ID", s);
|
||||
res = param_get(param_find(str), &(worker_data.device_id[s]));
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "[cal] Unable to get CAL_GYRO%u_ID", s);
|
||||
return ERROR;
|
||||
}
|
||||
#endif
|
||||
|
||||
// Reset all offsets to 0 and scales to 1
|
||||
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
|
||||
|
|
@ -205,6 +196,37 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
return ERROR;
|
||||
}
|
||||
}
|
||||
#else
|
||||
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
|
||||
res = param_set(param_find(str), &gyro_scale_zero.x_offset);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
|
||||
res = param_set(param_find(str), &gyro_scale_zero.y_offset);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
|
||||
res = param_set(param_find(str), &gyro_scale_zero.z_offset);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
|
||||
res = param_set(param_find(str), &gyro_scale_zero.x_scale);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
|
||||
res = param_set(param_find(str), &gyro_scale_zero.y_scale);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
|
||||
res = param_set(param_find(str), &gyro_scale_zero.z_scale);
|
||||
if (res != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
|
@ -216,10 +238,20 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
int32_t prio;
|
||||
orb_priority(worker_data.gyro_sensor_sub[s], &prio);
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
int device_prio_max = 0;
|
||||
if (prio > device_prio_max) {
|
||||
device_prio_max = prio;
|
||||
device_id_primary = worker_data.device_id[s];
|
||||
}
|
||||
#else
|
||||
gyro_report report = {};
|
||||
orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &report);
|
||||
//PX4_INFO("found device id: %d", report.device_id);
|
||||
|
||||
// TODO FIXME: this is hacky but should get the device ID for now
|
||||
worker_data.device_id[s] = report.device_id;
|
||||
#endif
|
||||
}
|
||||
|
||||
int cancel_sub = calibrate_cancel_subscribe();
|
||||
|
|
@ -296,10 +328,10 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
(void)sprintf(str, "CAL_GYRO%u_ID", s);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
/* apply new scaling and offsets */
|
||||
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
|
||||
int fd = px4_open(str, 0);
|
||||
|
|
|
|||
|
|
@ -102,7 +102,6 @@ int do_mag_calibration(int mavlink_fd)
|
|||
{
|
||||
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
struct mag_calibration_s mscale_null;
|
||||
mscale_null.x_offset = 0.0f;
|
||||
mscale_null.x_scale = 1.0f;
|
||||
|
|
@ -110,7 +109,6 @@ int do_mag_calibration(int mavlink_fd)
|
|||
mscale_null.y_scale = 1.0f;
|
||||
mscale_null.z_offset = 0.0f;
|
||||
mscale_null.z_scale = 1.0f;
|
||||
#endif
|
||||
|
||||
int result = OK;
|
||||
|
||||
|
|
@ -132,11 +130,35 @@ int do_mag_calibration(int mavlink_fd)
|
|||
break;
|
||||
}
|
||||
#else
|
||||
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
|
||||
result = param_get(param_find(str), &(device_ids[cur_mag]));
|
||||
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.x_offset);
|
||||
if (result != OK) {
|
||||
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
|
||||
break;
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.y_offset);
|
||||
if (result != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.z_offset);
|
||||
if (result != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.x_scale);
|
||||
if (result != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.y_scale);
|
||||
if (result != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
|
||||
result = param_set(param_find(str), &mscale_null.z_scale);
|
||||
if (result != OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -326,12 +348,13 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
|||
px4_pollfd_struct_t fds[max_mags];
|
||||
size_t fd_count = 0;
|
||||
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
if (worker_data->sub_mag[cur_mag] >= 0) {
|
||||
if (worker_data->sub_mag[cur_mag] >= 0 && device_ids[cur_mag] > 0) {
|
||||
fds[fd_count].fd = worker_data->sub_mag[cur_mag];
|
||||
fds[fd_count].events = POLLIN;
|
||||
fd_count++;
|
||||
}
|
||||
}
|
||||
|
||||
int poll_ret = px4_poll(fds, fd_count, 1000);
|
||||
|
||||
if (poll_ret > 0) {
|
||||
|
|
@ -446,24 +469,31 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
|
|||
// Setup subscriptions to mag sensors
|
||||
if (result == calibrate_return_ok) {
|
||||
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
if (device_ids[cur_mag] != 0) {
|
||||
// Mag in this slot is available
|
||||
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
|
||||
if (worker_data.sub_mag[cur_mag] < 0) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
|
||||
result = calibrate_return_error;
|
||||
break;
|
||||
}
|
||||
|
||||
// Get priority
|
||||
int32_t prio;
|
||||
orb_priority(worker_data.sub_mag[cur_mag], &prio);
|
||||
|
||||
if (prio > device_prio_max) {
|
||||
device_prio_max = prio;
|
||||
device_id_primary = device_ids[cur_mag];
|
||||
}
|
||||
// Mag in this slot is available
|
||||
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
|
||||
if (worker_data.sub_mag[cur_mag] < 0) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
|
||||
result = calibrate_return_error;
|
||||
break;
|
||||
}
|
||||
|
||||
// Get priority
|
||||
int32_t prio;
|
||||
orb_priority(worker_data.sub_mag[cur_mag], &prio);
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
if (prio > device_prio_max) {
|
||||
device_prio_max = prio;
|
||||
device_id_primary = device_ids[cur_mag];
|
||||
}
|
||||
#else
|
||||
mag_report report = {};
|
||||
orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report);
|
||||
|
||||
// TODO FIXME: this is hacky but should get the device ID for now
|
||||
device_ids[cur_mag] = report.device_id;
|
||||
PX4_INFO("found device id: %d", report.device_id);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -593,11 +623,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
|
|||
|
||||
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
if (device_ids[cur_mag] != 0) {
|
||||
int fd_mag = -1;
|
||||
struct mag_calibration_s mscale;
|
||||
#ifndef __PX4_QURT
|
||||
int fd_mag = -1;
|
||||
|
||||
// Set new scale
|
||||
|
||||
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
|
||||
fd_mag = px4_open(str, 0);
|
||||
if (fd_mag < 0) {
|
||||
|
|
@ -611,32 +641,44 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
|
|||
result = calibrate_return_error;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (result == calibrate_return_ok) {
|
||||
mscale.x_offset = sphere_x[cur_mag];
|
||||
mscale.y_offset = sphere_y[cur_mag];
|
||||
mscale.z_offset = sphere_z[cur_mag];
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
|
||||
result = calibrate_return_error;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
// Mag device no longer needed
|
||||
if (fd_mag >= 0) {
|
||||
px4_close(fd_mag);
|
||||
}
|
||||
#endif
|
||||
|
||||
if (result == calibrate_return_ok) {
|
||||
bool failed = false;
|
||||
|
||||
PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", cur_mag,
|
||||
(double)mscale.x_offset,
|
||||
(double)mscale.y_offset,
|
||||
(double)mscale.z_offset);
|
||||
PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", cur_mag,
|
||||
(double)mscale.x_scale,
|
||||
(double)mscale.y_scale,
|
||||
(double)mscale.z_scale);
|
||||
|
||||
/* set parameters */
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
|
||||
#endif
|
||||
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
|
||||
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
|
||||
|
|
@ -644,11 +686,12 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
|
|||
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
|
||||
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
|
||||
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
|
||||
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
|
||||
// FIXME: scaling is not used right now
|
||||
//failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
|
||||
//(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
|
||||
//failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
|
||||
//(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
|
||||
//failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
|
||||
|
||||
if (failed) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
|
||||
|
|
|
|||
|
|
@ -100,9 +100,6 @@
|
|||
#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>
|
||||
|
||||
|
|
@ -1530,17 +1527,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g
|
|||
}
|
||||
|
||||
#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, gcal);
|
||||
|
||||
} else {
|
||||
gyro_calibration_pub = orb_advertise(ORB_ID(gyro_calibration), gcal);
|
||||
}
|
||||
|
||||
/* On QURT, the params are read directly in the respective wrappers. */
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
|
@ -1561,17 +1548,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s
|
|||
}
|
||||
|
||||
#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, acal);
|
||||
|
||||
} else {
|
||||
accel_calibration_pub = orb_advertise(ORB_ID(accel_calibration), acal);
|
||||
}
|
||||
|
||||
/* On QURT, the params are read directly in the respective wrappers. */
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
|
@ -1592,17 +1569,7 @@ Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mca
|
|||
}
|
||||
|
||||
#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, mcal);
|
||||
|
||||
} else {
|
||||
mag_calibration_pub = orb_advertise(ORB_ID(mag_calibration), mcal);
|
||||
}
|
||||
|
||||
/* On QURT, the params are read directly in the respective wrappers. */
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -282,12 +282,3 @@ 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);
|
||||
|
|
|
|||
|
|
@ -56,7 +56,7 @@
|
|||
|
||||
#include <drivers/drv_mag.h>
|
||||
|
||||
#include <uORB/topics/mag_calibration.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <board_config.h>
|
||||
//#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
|
|
@ -101,11 +101,16 @@ private:
|
|||
|
||||
orb_advert_t _mag_topic;
|
||||
|
||||
int _mag_calibration_sub;
|
||||
int _param_update_sub;
|
||||
|
||||
struct mag_calibration_s _mag_calibration;
|
||||
|
||||
bool _mag_calibration_set;
|
||||
struct mag_calibration_s {
|
||||
float x_offset;
|
||||
float x_scale;
|
||||
float y_offset;
|
||||
float y_scale;
|
||||
float z_offset;
|
||||
float z_scale;
|
||||
} _mag_calibration;
|
||||
|
||||
int _mag_orb_class_instance;
|
||||
|
||||
|
|
@ -116,10 +121,19 @@ private:
|
|||
DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
|
||||
HMC5883(MAG_DEVICE_PATH),
|
||||
_mag_topic(nullptr),
|
||||
_param_update_sub(-1),
|
||||
_mag_calibration{},
|
||||
_mag_orb_class_instance(-1),
|
||||
_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
|
||||
/*_rotation(rotation)*/
|
||||
{
|
||||
// Set sane default calibration values
|
||||
_mag_calibration.x_scale = 1.0f;
|
||||
_mag_calibration.y_scale = 1.0f;
|
||||
_mag_calibration.z_scale = 1.0f;
|
||||
_mag_calibration.x_offset = 0.0f;
|
||||
_mag_calibration.y_offset = 0.0f;
|
||||
_mag_calibration.z_offset = 0.0f;
|
||||
}
|
||||
|
||||
DfHmc9250Wrapper::~DfHmc9250Wrapper()
|
||||
|
|
@ -139,9 +153,9 @@ int DfHmc9250Wrapper::start()
|
|||
return -1;
|
||||
}
|
||||
|
||||
/* Subscribe to calibration topic. */
|
||||
if (_mag_calibration_sub < 0) {
|
||||
_mag_calibration_sub = orb_subscribe(ORB_ID(mag_calibration));
|
||||
/* Subscribe to param update topic. */
|
||||
if (_param_update_sub < 0) {
|
||||
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
}
|
||||
|
||||
/* Init device and start sensor. */
|
||||
|
|
@ -159,6 +173,9 @@ int DfHmc9250Wrapper::start()
|
|||
return ret;
|
||||
}
|
||||
|
||||
/* Force getting the calibration values. */
|
||||
_update_mag_calibration();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -177,17 +194,65 @@ int DfHmc9250Wrapper::stop()
|
|||
|
||||
void DfHmc9250Wrapper::_update_mag_calibration()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_mag_calibration_sub, &updated);
|
||||
// TODO: replace magic number
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
|
||||
if (updated) {
|
||||
mag_calibration_s new_calibration;
|
||||
orb_copy(ORB_ID(mag_calibration), _mag_calibration_sub, &new_calibration);
|
||||
// TODO: remove printfs and add error counter
|
||||
|
||||
/* Only accept calibration for this device. */
|
||||
if (m_id.dev_id == new_calibration.device_id) {
|
||||
_mag_calibration = new_calibration;
|
||||
_mag_calibration_set = true;
|
||||
char str[30];
|
||||
(void)sprintf(str, "CAL_MAG%u_ID", i);
|
||||
int32_t device_id;
|
||||
int res = param_get(param_find(str), &device_id);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((uint32_t)device_id != m_id.dev_id) {
|
||||
continue;
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
|
||||
res = param_get(param_find(str), &_mag_calibration.x_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
|
||||
res = param_get(param_find(str), &_mag_calibration.y_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
|
||||
res = param_get(param_find(str), &_mag_calibration.z_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
|
||||
res = param_get(param_find(str), &_mag_calibration.x_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_YOFF", i);
|
||||
res = param_get(param_find(str), &_mag_calibration.y_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
|
||||
res = param_get(param_find(str), &_mag_calibration.z_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -196,11 +261,14 @@ void DfHmc9250Wrapper::_update_mag_calibration()
|
|||
int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
|
||||
{
|
||||
/* Check if calibration values are still up-to-date. */
|
||||
_update_mag_calibration();
|
||||
bool updated;
|
||||
orb_check(_param_update_sub, &updated);
|
||||
|
||||
if (!_mag_calibration_set) {
|
||||
// TODO: check the return codes of this function
|
||||
return 0;
|
||||
if (updated) {
|
||||
parameter_update_s parameter_update;
|
||||
orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update);
|
||||
|
||||
_update_mag_calibration();
|
||||
}
|
||||
|
||||
/* Publish mag first. */
|
||||
|
|
@ -220,14 +288,16 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
|
|||
mag_report.x_raw = NAN;
|
||||
mag_report.y_raw = NAN;
|
||||
mag_report.z_raw = NAN;
|
||||
mag_report.x = data.field_x_ga;
|
||||
mag_report.y = data.field_y_ga;
|
||||
mag_report.z = data.field_z_ga;
|
||||
mag_report.x = (data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale;
|
||||
mag_report.y = (data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale;
|
||||
mag_report.z = (data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale;
|
||||
|
||||
// TODO: get these right
|
||||
//mag_report.scaling = -1.0f;
|
||||
//mag_report.range_m_s2 = -1.0f;
|
||||
|
||||
mag_report.device_id = m_id.dev_id;
|
||||
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked)) {
|
||||
|
||||
|
|
|
|||
|
|
@ -57,8 +57,7 @@
|
|||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
|
||||
#include <uORB/topics/gyro_calibration.h>
|
||||
#include <uORB/topics/accel_calibration.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <mpu9250/MPU9250.hpp>
|
||||
#include <DevMgr.hpp>
|
||||
|
|
@ -101,14 +100,25 @@ private:
|
|||
orb_advert_t _accel_topic;
|
||||
orb_advert_t _gyro_topic;
|
||||
|
||||
int _accel_calibration_sub;
|
||||
int _gyro_calibration_sub;
|
||||
int _param_update_sub;
|
||||
|
||||
struct accel_calibration_s _accel_calibration;
|
||||
struct gyro_calibration_s _gyro_calibration;
|
||||
struct accel_calibration_s {
|
||||
float x_offset;
|
||||
float x_scale;
|
||||
float y_offset;
|
||||
float y_scale;
|
||||
float z_offset;
|
||||
float z_scale;
|
||||
} _accel_calibration;
|
||||
|
||||
bool _accel_calibration_set;
|
||||
bool _gyro_calibration_set;
|
||||
struct gyro_calibration_s {
|
||||
float x_offset;
|
||||
float x_scale;
|
||||
float y_offset;
|
||||
float y_scale;
|
||||
float z_offset;
|
||||
float z_scale;
|
||||
} _gyro_calibration;
|
||||
|
||||
int _accel_orb_class_instance;
|
||||
int _gyro_orb_class_instance;
|
||||
|
|
@ -122,18 +132,29 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
|
|||
MPU9250(IMU_DEVICE_PATH),
|
||||
_accel_topic(nullptr),
|
||||
_gyro_topic(nullptr),
|
||||
_accel_calibration_sub(-1),
|
||||
_gyro_calibration_sub(-1),
|
||||
_param_update_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")),
|
||||
_gyro_sample_perf(perf_alloc(PC_ELAPSED, "df_gyro_read"))
|
||||
/*_rotation(rotation)*/
|
||||
{
|
||||
// Set sane default calibration values
|
||||
_accel_calibration.x_scale = 1.0f;
|
||||
_accel_calibration.y_scale = 1.0f;
|
||||
_accel_calibration.z_scale = 1.0f;
|
||||
_accel_calibration.x_offset = 0.0f;
|
||||
_accel_calibration.y_offset = 0.0f;
|
||||
_accel_calibration.z_offset = 0.0f;
|
||||
|
||||
_gyro_calibration.x_scale = 1.0f;
|
||||
_gyro_calibration.y_scale = 1.0f;
|
||||
_gyro_calibration.z_scale = 1.0f;
|
||||
_gyro_calibration.x_offset = 0.0f;
|
||||
_gyro_calibration.y_offset = 0.0f;
|
||||
_gyro_calibration.z_offset = 0.0f;
|
||||
}
|
||||
|
||||
DfMpu9250Wrapper::~DfMpu9250Wrapper()
|
||||
|
|
@ -164,13 +185,9 @@ int DfMpu9250Wrapper::start()
|
|||
return -1;
|
||||
}
|
||||
|
||||
/* Subscribe to calibration topics. */
|
||||
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));
|
||||
/* Subscribe to param update topic. */
|
||||
if (_param_update_sub < 0) {
|
||||
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
}
|
||||
|
||||
/* Init device and start sensor. */
|
||||
|
|
@ -188,6 +205,12 @@ int DfMpu9250Wrapper::start()
|
|||
return ret;
|
||||
}
|
||||
|
||||
PX4_INFO("MPU9250 device id is: %d", m_id.dev_id);
|
||||
|
||||
/* Force getting the calibration values. */
|
||||
_update_accel_calibration();
|
||||
_update_gyro_calibration();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -206,34 +229,130 @@ int DfMpu9250Wrapper::stop()
|
|||
|
||||
void DfMpu9250Wrapper::_update_gyro_calibration()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_gyro_calibration_sub, &updated);
|
||||
// TODO: replace magic number
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
|
||||
if (updated) {
|
||||
gyro_calibration_s new_calibration;
|
||||
orb_copy(ORB_ID(gyro_calibration), _gyro_calibration_sub, &new_calibration);
|
||||
// TODO: remove printfs and add error counter
|
||||
|
||||
/* Only accept calibration for this device. */
|
||||
if (m_id.dev_id == new_calibration.device_id) {
|
||||
_gyro_calibration = new_calibration;
|
||||
_gyro_calibration_set = true;
|
||||
char str[30];
|
||||
(void)sprintf(str, "CAL_GYRO%u_ID", i);
|
||||
int32_t device_id;
|
||||
int res = param_get(param_find(str), &device_id);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((uint32_t)device_id != m_id.dev_id) {
|
||||
continue;
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
|
||||
res = param_get(param_find(str), &_gyro_calibration.x_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
|
||||
res = param_get(param_find(str), &_gyro_calibration.y_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
|
||||
res = param_get(param_find(str), &_gyro_calibration.z_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
|
||||
res = param_get(param_find(str), &_gyro_calibration.x_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
|
||||
res = param_get(param_find(str), &_gyro_calibration.y_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
|
||||
res = param_get(param_find(str), &_gyro_calibration.z_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DfMpu9250Wrapper::_update_accel_calibration()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_accel_calibration_sub, &updated);
|
||||
// TODO: replace magic number
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
|
||||
if (updated) {
|
||||
accel_calibration_s new_calibration;
|
||||
orb_copy(ORB_ID(accel_calibration), _accel_calibration_sub, &new_calibration);
|
||||
// TODO: remove printfs and add error counter
|
||||
|
||||
/* Only accept calibration for this device. */
|
||||
if (m_id.dev_id == new_calibration.device_id) {
|
||||
_accel_calibration = new_calibration;
|
||||
_accel_calibration_set = true;
|
||||
char str[30];
|
||||
(void)sprintf(str, "CAL_ACC%u_ID", i);
|
||||
int32_t device_id;
|
||||
int res = param_get(param_find(str), &device_id);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((uint32_t)device_id != m_id.dev_id) {
|
||||
continue;
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
|
||||
res = param_get(param_find(str), &_accel_calibration.x_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
|
||||
res = param_get(param_find(str), &_accel_calibration.y_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
|
||||
res = param_get(param_find(str), &_accel_calibration.z_scale);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
|
||||
res = param_get(param_find(str), &_accel_calibration.x_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
|
||||
res = param_get(param_find(str), &_accel_calibration.y_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
|
||||
res = param_get(param_find(str), &_accel_calibration.z_offset);
|
||||
|
||||
if (res != OK) {
|
||||
PX4_ERR("Could not access param %s", str);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -241,77 +360,80 @@ void DfMpu9250Wrapper::_update_accel_calibration()
|
|||
int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
||||
{
|
||||
/* Check if calibration values are still up-to-date. */
|
||||
_update_accel_calibration();
|
||||
_update_gyro_calibration();
|
||||
bool updated;
|
||||
orb_check(_param_update_sub, &updated);
|
||||
|
||||
/* Don't publish if we have not received calibration data. */
|
||||
if (_accel_calibration_set) {
|
||||
if (updated) {
|
||||
parameter_update_s parameter_update;
|
||||
orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update);
|
||||
|
||||
/* Publish accel first. */
|
||||
perf_begin(_accel_sample_perf);
|
||||
_update_accel_calibration();
|
||||
_update_gyro_calibration();
|
||||
}
|
||||
|
||||
accel_report accel_report = {};
|
||||
accel_report.timestamp = data.last_read_time_usec;
|
||||
/* Publish accel first. */
|
||||
perf_begin(_accel_sample_perf);
|
||||
|
||||
// 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;
|
||||
accel_report accel_report = {};
|
||||
accel_report.timestamp = data.last_read_time_usec;
|
||||
|
||||
// TODO: get these right
|
||||
accel_report.scaling = -1.0f;
|
||||
accel_report.range_m_s2 = -1.0f;
|
||||
// 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_calibration.x_offset) * _accel_calibration.x_scale;
|
||||
accel_report.y = (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale;
|
||||
accel_report.z = (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale;
|
||||
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked)) {
|
||||
// TODO: get these right
|
||||
accel_report.scaling = -1.0f;
|
||||
accel_report.range_m_s2 = -1.0f;
|
||||
|
||||
if (_accel_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
}
|
||||
accel_report.device_id = m_id.dev_id;
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
/* Don't publish if we have not received calibration data. */
|
||||
if (_gyro_calibration_set) {
|
||||
perf_end(_accel_sample_perf);
|
||||
|
||||
/* Then publish gyro. */
|
||||
perf_begin(_gyro_sample_perf);
|
||||
|
||||
gyro_report gyro_report = {};
|
||||
gyro_report.timestamp = data.last_read_time_usec;
|
||||
/* Then publish gyro. */
|
||||
perf_begin(_gyro_sample_perf);
|
||||
|
||||
// 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;
|
||||
gyro_report gyro_report = {};
|
||||
gyro_report.timestamp = data.last_read_time_usec;
|
||||
|
||||
// TODO: get these right
|
||||
gyro_report.scaling = -1.0f;
|
||||
gyro_report.range_rad_s = -1.0f;
|
||||
// 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_calibration.x_offset) * _gyro_calibration.x_scale;
|
||||
gyro_report.y = (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
|
||||
gyro_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
|
||||
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked)) {
|
||||
// TODO: get these right
|
||||
gyro_report.scaling = -1.0f;
|
||||
gyro_report.range_rad_s = -1.0f;
|
||||
|
||||
if (_gyro_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
|
||||
}
|
||||
gyro_report.device_id = m_id.dev_id;
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
if (_accel_calibration_set || _gyro_calibration_set) {
|
||||
/* Notify anyone waiting for data. */
|
||||
DevMgr::updateNotify(*this);
|
||||
}
|
||||
perf_end(_gyro_sample_perf);
|
||||
|
||||
/* Notify anyone waiting for data. */
|
||||
DevMgr::updateNotify(*this);
|
||||
|
||||
// TODO: check the return codes of this function
|
||||
return 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue