aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/px4/tests/test_float.c29
-rw-r--r--apps/px4/tests/test_sensors.c40
-rw-r--r--nuttx/configs/px4fmu/src/drv_l3gd20.c2
-rw-r--r--nuttx/configs/px4fmu/src/drv_mpu6000.c42
4 files changed, 72 insertions, 41 deletions
diff --git a/apps/px4/tests/test_float.c b/apps/px4/tests/test_float.c
index 0439476e6..4922d06b1 100644
--- a/apps/px4/tests/test_float.c
+++ b/apps/px4/tests/test_float.c
@@ -62,7 +62,7 @@ int test_float(int argc, char *argv[])
int ret = 0;
printf("\n--- SINGLE PRECISION TESTS ---\n");
- printf("The single precision test involves calls to fabs(),\nif test fails check this function as well.\n\n");
+ printf("The single precision test involves calls to fabsf(),\nif test fails check this function as well.\n\n");
float f1 = 1.55f;
@@ -80,7 +80,7 @@ int test_float(int argc, char *argv[])
fflush(stdout);
- if (fabs((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
+ if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
printf("\t success: sinf(1.0f) == 0.84147f\n");
} else {
@@ -92,7 +92,7 @@ int test_float(int argc, char *argv[])
float asinf_one = asinf(1.0f);
- if (fabs((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
+ if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
printf("\t success: asinf(1.0f) == 1.57079f\n");
} else {
@@ -104,7 +104,7 @@ int test_float(int argc, char *argv[])
float cosf_one = cosf(1.0f);
- if (fabs((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
+ if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
printf("\t success: cosf(1.0f) == 0.54030f\n");
} else {
@@ -117,7 +117,7 @@ int test_float(int argc, char *argv[])
float acosf_one = acosf(1.0f);
- if (fabs((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
+ if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
printf("\t success: acosf(1.0f) == 0.0f\n");
} else {
@@ -162,9 +162,9 @@ int test_float(int argc, char *argv[])
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
- printf("\t success: printf(\"%8.4f\", 0.553415f) == %8.4f\n", 0.553415f);
+ printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", 0.553415f);
} else {
- printf("\t FAIL: printf(\"%8.4f\", 0.553415f) != \" 0.5534\", result: %s\n", sbuf);
+ printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \" 0.5534\", result: %s\n", sbuf);
ret = -5;
}
@@ -230,15 +230,24 @@ int test_float(int argc, char *argv[])
ret = -7;
}
- printf("\t testing printing: printf(0.553415): %8.4f\n", 0.553415);
-
printf("\t testing pow() with magic value\n");
- printf("\t (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
+ printf("\t (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
fflush(stdout);
usleep(20000);
double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
printf("\t success: result: %8.4f\n", (double)powres);
+ sprintf(sbuf, "%8.4f", 0.553415);
+
+ if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
+ sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
+ && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
+ printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
+ } else {
+ printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \" 0.5534\", result: %s\n", sbuf);
+ ret = -8;
+ }
+
if (ret == 0) {
printf("\n SUCCESS: All float and double tests passed.\n");
diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c
index 1a7ebcc7a..af1213711 100644
--- a/apps/px4/tests/test_sensors.c
+++ b/apps/px4/tests/test_sensors.c
@@ -331,7 +331,7 @@ mpu6000(int argc, char *argv[])
fflush(stdout);
int fd;
- int16_t buf[5] = { -1, 0, -1, 0, -1, 0};
+ int16_t buf[6] = { -1, 0, -1, 0, -1, 0};
int ret;
fd = open("/dev/mpu6000", O_RDONLY);
@@ -341,32 +341,32 @@ mpu6000(int argc, char *argv[])
return ERROR;
}
- // /* wait at least 100ms, sensor should have data after no more than 20ms */
- // usleep(100000);
+ /* wait at least 100ms, sensor should have data after no more than 20ms */
+ usleep(100000);
- // /* read data - expect samples */
- // ret = read(fd, buf, sizeof(buf));
+ /* read data - expect samples */
+ ret = read(fd, buf, sizeof(buf));
- // if (ret != sizeof(buf)) {
- // printf("\tMPU-6000: read1 fail (%d)\n", ret);
- // return ERROR;
+ if (ret != sizeof(buf)) {
+ printf("\tMPU-6000: read1 fail (%d)\n", ret);
+ return ERROR;
- // } else {
- // printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
- // }
+ } else {
+ printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+ }
- // /* wait at least 10ms, sensor should have data after no more than 2ms */
- // usleep(100000);
+ /* wait at least 10ms, sensor should have data after no more than 2ms */
+ usleep(100000);
- // ret = read(fd, buf, sizeof(buf));
+ ret = read(fd, buf, sizeof(buf));
- // if (ret != sizeof(buf)) {
- // printf("\tMPU-6000: read2 fail (%d)\n", ret);
- // return ERROR;
+ if (ret != sizeof(buf)) {
+ printf("\tMPU-6000: read2 fail (%d)\n", ret);
+ return ERROR;
- // } else {
- // printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
- // }
+ } else {
+ printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+ }
/* XXX more tests here */
diff --git a/nuttx/configs/px4fmu/src/drv_l3gd20.c b/nuttx/configs/px4fmu/src/drv_l3gd20.c
index 1c6c05449..19f2d032f 100644
--- a/nuttx/configs/px4fmu/src/drv_l3gd20.c
+++ b/nuttx/configs/px4fmu/src/drv_l3gd20.c
@@ -148,7 +148,7 @@ l3gd20_write_reg(uint8_t address, uint8_t data)
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, true);
- SPI_SNDBLOCK(l3gd20_dev.spi, &cmd, sizeof(cmd));
+ SPI_SNDBLOCK(l3gd20_dev.spi, &cmd, sizeof(cmd));
SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, false);
}
diff --git a/nuttx/configs/px4fmu/src/drv_mpu6000.c b/nuttx/configs/px4fmu/src/drv_mpu6000.c
index 47f655563..ec7426e20 100644
--- a/nuttx/configs/px4fmu/src/drv_mpu6000.c
+++ b/nuttx/configs/px4fmu/src/drv_mpu6000.c
@@ -166,7 +166,7 @@ mpu6000_write_reg(uint8_t address, uint8_t data)
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
- SPI_SNDBLOCK(mpu6000_dev.spi, &cmd, sizeof(cmd));
+ SPI_SNDBLOCK(mpu6000_dev.spi, &cmd, sizeof(cmd));
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
}
@@ -183,6 +183,19 @@ mpu6000_read_reg(uint8_t address)
return data[1];
}
+static int16_t
+mpu6000_read_int16(uint8_t address)
+{
+ uint8_t cmd[3] = {address | DIR_READ, 0, 0};
+ uint8_t data[3];
+
+ SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
+ SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd));
+ SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
+
+ return (((int16_t)data[1])<<8) | data[2];
+}
+
static int
mpu6000_set_range(uint8_t range)
{
@@ -247,39 +260,48 @@ mpu6000_read_fifo(int16_t *data)
struct { /* status register and data as read back from the device */
- uint8_t cmd;
- uint8_t int_status;
+ // uint8_t cmd;
+ // uint8_t int_status;
int16_t xacc;
int16_t yacc;
int16_t zacc;
- int8_t temp;
+ int16_t temp;
int16_t rollspeed;
int16_t pitchspeed;
int16_t yawspeed;
- } __attribute__((packed)) report;
+ } report;
- report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
+ uint8_t cmd[sizeof(report)];
+ cmd[0] = MPUREG_ACCEL_XOUT_H | DIR_READ; // was addr_incr
SPI_LOCK(mpu6000_dev.spi, true);
SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, true);
- SPI_EXCHANGE(mpu6000_dev.spi, &report, &report, sizeof(report));
+ report.xacc = mpu6000_read_int16(MPUREG_ACCEL_XOUT_H);
+ report.yacc = mpu6000_read_int16(MPUREG_ACCEL_YOUT_H);
+ report.zacc = mpu6000_read_int16(MPUREG_ACCEL_ZOUT_H);
+ report.rollspeed = mpu6000_read_int16(MPUREG_GYRO_XOUT_H);
+ report.pitchspeed = mpu6000_read_int16(MPUREG_GYRO_YOUT_H);
+ report.yawspeed = mpu6000_read_int16(MPUREG_GYRO_ZOUT_H);
SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, false);
SPI_LOCK(mpu6000_dev.spi, false);
data[0] = report.xacc;
data[1] = report.yacc;
data[2] = report.zacc;
+ data[3] = report.rollspeed;
+ data[4] = report.pitchspeed;
+ data[5] = report.yawspeed;
- return (report.int_status & 0x01);
+ return 1;//(report.int_status & 0x01);
}
static ssize_t
mpu6000_read(struct file *filp, char *buffer, size_t buflen)
{
/* if the buffer is large enough, and data are available, return success */
- if (buflen >= 6) {
+ if (buflen >= 12) {
if (mpu6000_read_fifo((int16_t *)buffer))
- return 6;
+ return 12;
/* no data */
return 0;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-14 05:49:26 +0000