mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 14:33:34 +08:00
Merged fmuv2_bringup
This commit is contained in:
Lorenz Meier
+101
-84
@@ -2,6 +2,7 @@
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Author: Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -35,6 +36,7 @@
|
||||
/**
|
||||
* @file config.c
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* config tool.
|
||||
*/
|
||||
@@ -69,27 +71,15 @@ config_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 2) {
|
||||
if (!strcmp(argv[1], "gyro")) {
|
||||
if (argc >= 3) {
|
||||
do_gyro(argc - 2, argv + 2);
|
||||
} else {
|
||||
errx(1, "not enough parameters.");
|
||||
}
|
||||
do_gyro(argc - 2, argv + 2);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "accel")) {
|
||||
if (argc >= 3) {
|
||||
do_accel(argc - 2, argv + 2);
|
||||
} else {
|
||||
errx(1, "not enough parameters.");
|
||||
}
|
||||
do_accel(argc - 2, argv + 2);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "mag")) {
|
||||
if (argc >= 3) {
|
||||
do_mag(argc - 2, argv + 2);
|
||||
} else {
|
||||
errx(1, "not enough parameters.");
|
||||
}
|
||||
do_mag(argc - 2, argv + 2);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -100,6 +90,7 @@ static void
|
||||
do_gyro(int argc, char *argv[])
|
||||
{
|
||||
int fd;
|
||||
int ret;
|
||||
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
|
||||
@@ -109,44 +100,45 @@ do_gyro(int argc, char *argv[])
|
||||
|
||||
} else {
|
||||
|
||||
if (argc >= 2) {
|
||||
if (argc == 2 && !strcmp(argv[0], "sampling")) {
|
||||
|
||||
char* end;
|
||||
int i = strtol(argv[1],&end,10);
|
||||
/* set the gyro internal sampling rate up to at least i Hz */
|
||||
ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
if (!strcmp(argv[0], "sampling")) {
|
||||
if (ret)
|
||||
errx(ret,"sampling rate could not be set");
|
||||
|
||||
/* set the accel internal sampling rate up to at leat i Hz */
|
||||
ioctl(fd, GYROIOCSSAMPLERATE, i);
|
||||
} else if (argc == 2 && !strcmp(argv[0], "rate")) {
|
||||
|
||||
} else if (!strcmp(argv[0], "rate")) {
|
||||
/* set the driver to poll at i Hz */
|
||||
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
/* set the driver to poll at i Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, i);
|
||||
} else if (!strcmp(argv[0], "range")) {
|
||||
if (ret)
|
||||
errx(ret,"pollrate could not be set");
|
||||
|
||||
/* set the range to i dps */
|
||||
ioctl(fd, GYROIOCSRANGE, i);
|
||||
}
|
||||
} else if (argc == 2 && !strcmp(argv[0], "range")) {
|
||||
|
||||
} else if (argc > 0) {
|
||||
/* set the range to i dps */
|
||||
ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
if(!strcmp(argv[0], "check")) {
|
||||
int ret = ioctl(fd, GYROIOCSELFTEST, 0);
|
||||
if (ret)
|
||||
errx(ret,"range could not be set");
|
||||
|
||||
if (ret) {
|
||||
warnx("gyro self test FAILED! Check calibration:");
|
||||
struct gyro_scale scale;
|
||||
ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
|
||||
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
|
||||
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
|
||||
} else {
|
||||
warnx("gyro calibration and self test OK");
|
||||
}
|
||||
} else if(argc == 1 && !strcmp(argv[0], "check")) {
|
||||
ret = ioctl(fd, GYROIOCSELFTEST, 0);
|
||||
|
||||
if (ret) {
|
||||
warnx("gyro self test FAILED! Check calibration:");
|
||||
struct gyro_scale scale;
|
||||
ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
|
||||
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
|
||||
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
|
||||
} else {
|
||||
warnx("gyro calibration and self test OK");
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
|
||||
errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
|
||||
}
|
||||
|
||||
int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
|
||||
@@ -165,6 +157,7 @@ static void
|
||||
do_mag(int argc, char *argv[])
|
||||
{
|
||||
int fd;
|
||||
int ret;
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
|
||||
@@ -174,31 +167,52 @@ do_mag(int argc, char *argv[])
|
||||
|
||||
} else {
|
||||
|
||||
if (argc > 0) {
|
||||
if (argc == 2 && !strcmp(argv[0], "sampling")) {
|
||||
|
||||
if (!strcmp(argv[0], "check")) {
|
||||
int ret = ioctl(fd, MAGIOCSELFTEST, 0);
|
||||
/* set the mag internal sampling rate up to at least i Hz */
|
||||
ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
if (ret) {
|
||||
warnx("mag self test FAILED! Check calibration.");
|
||||
struct mag_scale scale;
|
||||
ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
|
||||
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
|
||||
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
|
||||
} else {
|
||||
warnx("mag calibration and self test OK");
|
||||
}
|
||||
if (ret)
|
||||
errx(ret,"sampling rate could not be set");
|
||||
|
||||
} else if (argc == 2 && !strcmp(argv[0], "rate")) {
|
||||
|
||||
/* set the driver to poll at i Hz */
|
||||
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
if (ret)
|
||||
errx(ret,"pollrate could not be set");
|
||||
|
||||
} else if (argc == 2 && !strcmp(argv[0], "range")) {
|
||||
|
||||
/* set the range to i G */
|
||||
ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
if (ret)
|
||||
errx(ret,"range could not be set");
|
||||
|
||||
} else if(argc == 1 && !strcmp(argv[0], "check")) {
|
||||
ret = ioctl(fd, MAGIOCSELFTEST, 0);
|
||||
|
||||
if (ret) {
|
||||
warnx("mag self test FAILED! Check calibration:");
|
||||
struct mag_scale scale;
|
||||
ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
|
||||
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
|
||||
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
|
||||
} else {
|
||||
warnx("mag calibration and self test OK");
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t");
|
||||
errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t");
|
||||
}
|
||||
|
||||
int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0);
|
||||
int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
|
||||
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
|
||||
int range = -1;//ioctl(fd, MAGIOCGRANGE, 0);
|
||||
int range = ioctl(fd, MAGIOCGRANGE, 0);
|
||||
|
||||
warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
|
||||
warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
|
||||
|
||||
close(fd);
|
||||
}
|
||||
@@ -210,6 +224,7 @@ static void
|
||||
do_accel(int argc, char *argv[])
|
||||
{
|
||||
int fd;
|
||||
int ret;
|
||||
|
||||
fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
|
||||
@@ -219,50 +234,52 @@ do_accel(int argc, char *argv[])
|
||||
|
||||
} else {
|
||||
|
||||
if (argc >= 2) {
|
||||
if (argc == 2 && !strcmp(argv[0], "sampling")) {
|
||||
|
||||
char* end;
|
||||
int i = strtol(argv[1],&end,10);
|
||||
/* set the accel internal sampling rate up to at least i Hz */
|
||||
ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
if (!strcmp(argv[0], "sampling")) {
|
||||
if (ret)
|
||||
errx(ret,"sampling rate could not be set");
|
||||
|
||||
/* set the accel internal sampling rate up to at leat i Hz */
|
||||
ioctl(fd, ACCELIOCSSAMPLERATE, i);
|
||||
} else if (argc == 2 && !strcmp(argv[0], "rate")) {
|
||||
|
||||
} else if (!strcmp(argv[0], "rate")) {
|
||||
/* set the driver to poll at i Hz */
|
||||
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
/* set the driver to poll at i Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, i);
|
||||
} else if (!strcmp(argv[0], "range")) {
|
||||
if (ret)
|
||||
errx(ret,"pollrate could not be set");
|
||||
|
||||
/* set the range to i dps */
|
||||
ioctl(fd, ACCELIOCSRANGE, i);
|
||||
}
|
||||
} else if (argc > 0) {
|
||||
} else if (argc == 2 && !strcmp(argv[0], "range")) {
|
||||
|
||||
if (!strcmp(argv[0], "check")) {
|
||||
int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
|
||||
/* set the range to i G */
|
||||
ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
if (ret) {
|
||||
warnx("accel self test FAILED! Check calibration.");
|
||||
struct accel_scale scale;
|
||||
ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
|
||||
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
|
||||
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
|
||||
} else {
|
||||
warnx("accel calibration and self test OK");
|
||||
}
|
||||
if (ret)
|
||||
errx(ret,"range could not be set");
|
||||
|
||||
} else if(argc == 1 && !strcmp(argv[0], "check")) {
|
||||
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
|
||||
|
||||
if (ret) {
|
||||
warnx("accel self test FAILED! Check calibration:");
|
||||
struct accel_scale scale;
|
||||
ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
|
||||
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
|
||||
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
|
||||
} else {
|
||||
warnx("accel calibration and self test OK");
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
|
||||
errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t");
|
||||
}
|
||||
|
||||
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
|
||||
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
|
||||
int range = ioctl(fd, ACCELIOCGRANGE, 0);
|
||||
|
||||
warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range);
|
||||
warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
|
||||
|
||||
close(fd);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user