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

Linux: added builtins to show devices and topics

Browse Source 
list_devices will list virtual devices starting with "/dev/".
list_topics will list topics ("/obj/")

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-04-15 20:34:45 -07:00
parent 694427e4ba
commit d2f0572ce6
7 changed files with 43 additions and 746 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/linux_apps.py
+8
View File
@@ -63,6 +63,7 @@ print """
static int shutdown_main(int argc, char *argv[]);
static int list_tasks_main(int argc, char *argv[]);
static int list_devices_main(int argc, char *argv[]);
static int list_topics_main(int argc, char *argv[]);
}
@@ -78,6 +79,7 @@ for app in apps:
print '\tapps["shutdown"] = shutdown_main;'
print '\tapps["list_tasks"] = list_tasks_main;'
print '\tapps["list_devices"] = list_devices_main;'
print '\tapps["list_topics"] = list_topics_main;'
print """
return apps;
}
@@ -108,5 +110,11 @@ static int list_devices_main(int argc, char *argv[])
px4_show_devices();
return 0;
}
static int list_topics_main(int argc, char *argv[])
{
px4_show_topics();
return 0;
}
"""
src/drivers/device/vdev.cpp
+21 -2
View File
@@ -461,16 +461,35 @@ void VDev::showDevices()
int i=0;
printf("Devices:\n");
for (; i<PX4_MAX_DEV; ++i) {
if (devmap[i]) {
if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
printf(" %s\n", devmap[i]->name);
}
}
}
void VDev::showTopics()
{
int i=0;
printf("Devices:\n");
for (; i<PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
printf(" %s\n", devmap[i]->name);
}
}
}
const char *VDev::topicList(unsigned int *next)
{
for (;*next<PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0)
return devmap[(*next)++]->name;
return NULL;
}
const char *VDev::devList(unsigned int *next)
{
for (;*next<PX4_MAX_DEV; (*next)++)
if (devmap[*next])
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0)
return devmap[(*next)++]->name;
return NULL;
}
src/drivers/device/vdev.h
+2
View File
@@ -334,7 +334,9 @@ public:
static VDev *getDev(const char *path);
static void showDevices(void);
static void showTopics(void);
static const char *devList(unsigned int *next);
static const char *topicList(unsigned int *next);
protected:
src/drivers/device/vdev_posix.cpp
+10
View File
@@ -274,10 +274,20 @@ void px4_show_devices()
VDev::showDevices();
}
void px4_show_topics()
{
VDev::showTopics();
}
const char * px4_get_device_names(unsigned int *handle)
{
return VDev::devList(handle);
}
const char * px4_get_topic_names(unsigned int *handle)
{
return VDev::topicList(handle);
}
}
src/modules/commander/gyro_calibration_linux.cpp
-274
View File
@@ -1,274 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gyro_calibration.cpp
*
* Gyroscope calibration routine
*/
#include "gyro_calibration.h"
#include "calibration_messages.h"
#include "commander_helper.h"
#include <px4_posix.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <math.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_gyro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mcu_version.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd)
{
const unsigned max_gyros = 3;
int32_t device_id[3];
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "HOLD STILL");
/* wait for the user to respond */
sleep(2);
struct gyro_scale gyro_scale_zero = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct gyro_scale gyro_scale[max_gyros] = {};
int res = OK;
/* store board ID */
uint32_t mcu_id[3];
mcu_unique_id(&mcu_id[0]);
/* store last 32bit number - not unique, but unique in a given set */
(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
char str[30];
for (unsigned s = 0; s < max_gyros; s++) {
/* ensure all scale fields are initialized tha same as the first struct */
(void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0]));
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
int fd = px4_open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
px4_close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
}
}
unsigned calibration_counter[max_gyros] = { 0 };
const unsigned calibration_count = 5000;
struct gyro_report gyro_report_0 = {};
if (res == OK) {
/* determine gyro mean values */
unsigned poll_errcount = 0;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro[max_gyros];
px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
fds[s].fd = sub_sensor_gyro[s];
fds[s].events = POLLIN;
}
struct gyro_report gyro_report;
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
/* wait blocking for new data */
int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
if (poll_ret > 0) {
for (unsigned s = 0; s < max_gyros; s++) {
bool changed;
orb_check(sub_sensor_gyro[s], &changed);
if (changed) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
if (s == 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0);
}
gyro_scale[s].x_offset += gyro_report.x;
gyro_scale[s].y_offset += gyro_report.y;
gyro_scale[s].z_offset += gyro_report.z;
calibration_counter[s]++;
}
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count);
}
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
res = ERROR;
break;
}
}
for (unsigned s = 0; s < max_gyros; s++) {
px4_close(sub_sensor_gyro[s]);
gyro_scale[s].x_offset /= calibration_counter[s];
gyro_scale[s].y_offset /= calibration_counter[s];
gyro_scale[s].z_offset /= calibration_counter[s];
}
}
if (res == OK) {
/* check offsets */
float xdiff = gyro_report_0.x - gyro_scale[0].x_offset;
float ydiff = gyro_report_0.y - gyro_scale[0].y_offset;
float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
/* maximum allowable calibration error in radians */
const float maxoff = 0.002f;
if (!isfinite(gyro_scale[0].x_offset) ||
!isfinite(gyro_scale[0].y_offset) ||
!isfinite(gyro_scale[0].z_offset) ||
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration");
res = ERROR;
}
}
if (res == OK) {
/* set offset parameters to new values */
bool failed = false;
for (unsigned s = 0; s < max_gyros; s++) {
/* if any reasonable amount of data is missing, skip */
if (calibration_counter[s] < calibration_count / 2) {
continue;
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset)));
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (OK != param_set(param_find(str), &(device_id[s])));
/* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
if (fd < 0) {
failed = true;
continue;
}
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
px4_close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params");
res = ERROR;
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
src/modules/commander/mag_calibration_linux.cpp
-470
View File
@@ -1,470 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_calibration.cpp
*
* Magnetometer calibration routine
*/
#include "mag_calibration.h"
#include "commander_helper.h"
#include "calibration_routines.h"
#include "calibration_messages.h"
#include <px4_posix.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <poll.h>
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static const char *sensor_name = "mag";
static const unsigned max_mags = 3;
int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
int mag_calibration_worker(detect_orientation_return orientation, void* worker_data);
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
unsigned done_count;
int sub_mag[max_mags];
unsigned int calibration_points_perside;
unsigned int calibration_interval_perside_seconds;
uint64_t calibration_interval_perside_useconds;
unsigned int calibration_counter_total;
bool side_data_collected[detect_orientation_side_count];
float* x[max_mags];
float* y[max_mags];
float* z[max_mags];
} mag_worker_data_t;
int do_mag_calibration(int mavlink_fd)
{
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int result = OK;
// Determine which mags are available and reset each
int32_t device_ids[max_mags];
char str[30];
for (size_t i=0; i<max_mags; i++) {
device_ids[i] = 0; // signals no mag
}
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// Reset mag id to mag not available
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag);
break;
}
// Attempt to open mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
int fd = px4_open(str, O_RDONLY);
if (fd < 0) {
continue;
}
// Get device id for this mag
device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
// Reset mag scale
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag);
}
if (result == OK) {
/* calibrate range */
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
}
px4_close(fd);
}
if (result == OK) {
// Calibrate all mags at the same time
result = mag_calibrate_all(mavlink_fd, device_ids);
}
if (result == OK) {
/* auto-save to EEPROM */
result = param_save_default();
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
if (result == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return result;
}
int mag_calibration_worker(detect_orientation_return orientation, void* data)
{
int result = OK;
unsigned int calibration_counter_side;
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
sleep(2);
uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
unsigned poll_errcount = 0;
calibration_counter_side = 0;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter_side < worker_data->calibration_points_perside) {
// Wait clocking for new data on all mags
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) {
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) {
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (worker_data->sub_mag[cur_mag] >= 0) {
struct mag_report mag;
orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x;
worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
}
}
worker_data->calibration_counter_total++;
calibration_counter_side++;
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
"%s %s side calibration: progress <%u>",
sensor_name,
detect_orientation_str(orientation),
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
} else {
poll_errcount++;
}
if (poll_errcount > worker_data->calibration_points_perside * 3) {
result = ERROR;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
break;
}
}
// Mark the opposite side as collected as well. No need to collect opposite side since it
// would generate similar points.
detect_orientation_return alternateOrientation = orientation;
switch (orientation) {
case DETECT_ORIENTATION_TAIL_DOWN:
alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN;
break;
case DETECT_ORIENTATION_NOSE_DOWN:
alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN;
break;
case DETECT_ORIENTATION_LEFT:
alternateOrientation = DETECT_ORIENTATION_RIGHT;
break;
case DETECT_ORIENTATION_RIGHT:
alternateOrientation = DETECT_ORIENTATION_LEFT;
break;
case DETECT_ORIENTATION_UPSIDE_DOWN:
alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP;
break;
case DETECT_ORIENTATION_RIGHTSIDE_UP:
alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN;
break;
case DETECT_ORIENTATION_ERROR:
warnx("Invalid orientation in mag_calibration_worker");
break;
}
worker_data->side_data_collected[alternateOrientation] = true;
mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count);
return result;
}
int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
{
int result = OK;
mag_worker_data_t worker_data;
worker_data.mavlink_fd = mavlink_fd;
worker_data.done_count = 0;
worker_data.calibration_counter_total = 0;
worker_data.calibration_points_perside = 80;
worker_data.calibration_interval_perside_seconds = 20;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Initialize to collect all sides
for (size_t cur_side=0; cur_side<6; cur_side++) {
worker_data.side_data_collected[cur_side] = false;
}
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
worker_data.sub_mag[cur_mag] = -1;
// Initialize to no memory allocated
worker_data.x[cur_mag] = NULL;
worker_data.y[cur_mag] = NULL;
worker_data.z[cur_mag] = NULL;
}
const unsigned int calibration_sides = 3;
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
char str[30];
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
result = ERROR;
}
}
// Setup subscriptions to mag sensors
if (result == 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, "Mag #%u not found, abort", cur_mag);
result = ERROR;
break;
}
}
}
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == OK) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
}
result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (worker_data.sub_mag[cur_mag] >= 0) {
px4_close(worker_data.sub_mag[cur_mag]);
}
}
// Calculate calibration values for each mag
float sphere_x[max_mags];
float sphere_y[max_mags];
float sphere_z[max_mags];
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
if (result == OK) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total,
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag]);
if (!isfinite(sphere_x[cur_mag]) || !isfinite(sphere_y[cur_mag]) || !isfinite(sphere_z[cur_mag])) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag);
result = ERROR;
}
}
}
}
// Data points are no longer needed
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
free(worker_data.x[cur_mag]);
free(worker_data.y[cur_mag]);
free(worker_data.z[cur_mag]);
}
if (result == OK) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;
struct mag_scale mscale;
// Set new scale
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag);
result = ERROR;
}
if (result == OK) {
result = px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag);
result = ERROR;
}
}
if (result == OK) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
result = px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag);
result = ERROR;
}
}
// Mag device no longer needed
if (fd_mag >= 0) {
px4_close(fd_mag);
}
if (result == OK) {
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
(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);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
(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)));
if (failed) {
mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag);
result = ERROR;
} else {
mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
}
}
}
}
}
return result;
}
src/platforms/px4_posix.h
+2
View File
@@ -84,6 +84,8 @@ __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen);
__EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg);
__EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout);
__EXPORT void px4_show_devices(void);
__EXPORT void px4_show_topics(void);
__EXPORT const char * px4_get_device_names(unsigned int *handle);
__EXPORT const char * px4_get_topic_names(unsigned int *handle);
__END_DECLS