From 127ae329978940bd5e1944bc08bc5d7a65dc3361 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 11:43:05 +0100 Subject: [PATCH 01/22] Completed sensors test, WIP on ADC test --- apps/px4/tests/test_adc.c | 169 +++++++++++----------------------- apps/px4/tests/test_sensors.c | 141 +++++++++++++++++++++++++--- 2 files changed, 183 insertions(+), 127 deletions(-) diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c index 7795f07acf..4ffa9374f1 100644 --- a/apps/px4/tests/test_adc.c +++ b/apps/px4/tests/test_adc.c @@ -1,7 +1,6 @@ /**************************************************************************** - * px4/sensors/test_gpio.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 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 @@ -13,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * @@ -32,9 +31,10 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file test_adc.c + * Test for the analog to digital converter. + */ #include #include @@ -54,91 +54,46 @@ #include - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_gpio - ****************************************************************************/ - int test_adc(int argc, char *argv[]) { - int fd0; + int fd0 = 0; int ret = 0; - //struct adc_msg_s sample[4],sample2[4],sample3[4],sample4[4],sample5[4],sample6[4],sample7[4],sample8[4],sample9[4]; + #pragma pack(push,1) struct adc_msg4_s { - uint8_t am_channel1; /* The 8-bit ADC Channel */ - int32_t am_data1; /* ADC convert result (4 bytes) */ - uint8_t am_channel2; /* The 8-bit ADC Channel */ - int32_t am_data2; /* ADC convert result (4 bytes) */ - uint8_t am_channel3; /* The 8-bit ADC Channel */ - int32_t am_data3; /* ADC convert result (4 bytes) */ - uint8_t am_channel4; /* The 8-bit ADC Channel */ - int32_t am_data4; /* ADC convert result (4 bytes) */ - } __attribute__((__packed__));; + uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ + int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ + uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ + int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ + uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ + int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ + uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ + int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ + }; + #pragma pack(pop) - struct adc_msg4_s sample1[4], sample2[4]; + struct adc_msg4_s sample1; - size_t readsize; - ssize_t nbytes, nbytes2; - int i; + ssize_t nbytes; int j; int errval; - for (j = 0; j < 1; j++) { - char name[11]; - sprintf(name, "/dev/adc%d", j); - fd0 = open(name, O_RDONLY | O_NONBLOCK); + fd0 = open("/dev/adc0", O_RDONLY | O_NONBLOCK); - if (fd0 < 0) { - printf("ADC: %s open fail\n", name); - return ERROR; + if (fd0 <= 0) { + message("/dev/adc0 open fail: %d", errno); + return ERROR; - } else { - printf("Opened %s successfully\n", name); - } + } else { + message("opened /dev/adc0 successfully"); + } + usleep(10000); - /* first adc read round */ - readsize = 4 * sizeof(struct adc_msg_s); - up_udelay(10000);//microseconds - nbytes = read(fd0, sample1, readsize); - up_udelay(10000);//microseconds - nbytes2 = read(fd0, sample2, readsize); -// nbytes2 = read(fd0, sample3, readsize); -// nbytes2 = read(fd0, sample4, readsize); -// nbytes2 = read(fd0, sample5, readsize); -// nbytes2 = read(fd0, sample6, readsize); -// nbytes2 = read(fd0, sample7, readsize); -// nbytes2 = read(fd0, sample8, readsize); - //nbytes2 = read(fd0, sample9, readsize); + for (j = 0; j < 10; j++) { + + /* sleep 1 millisecond */ + usleep(1000); + nbytes = read(fd0, &sample1, sizeof(sample1)); /* Handle unexpected return values */ @@ -146,62 +101,44 @@ int test_adc(int argc, char *argv[]) errval = errno; if (errval != EINTR) { - message("read %s failed: %d\n", - name, errval); + message("reading /dev/adc0 failed: %d", errval); errval = 3; goto errout_with_dev; } - message("\tInterrupted read...\n"); + message("\tinterrupted read.."); } else if (nbytes == 0) { - message("\tNo data read, Ignoring\n"); + message("\tno data read, ignoring."); + ret = ERROR; } /* Print the sample data on successful return */ else { - int nsamples = nbytes / sizeof(struct adc_msg_s); - - if (nsamples * sizeof(struct adc_msg_s) != nbytes) { - message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n", - nbytes, sizeof(struct adc_msg_s)); + if (nbytes != sizeof(sample1)) { + message("\tsample 1 size %d is not matching struct size %d, ignoring", + nbytes, sizeof(sample1)); + ret = ERROR; } else { - message("Sample:"); - for (i = 0; i < 1 ; i++) { - message("%d: channel: %d value: %d\n", - i, sample1[i].am_channel1, sample1[i].am_data1); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i, sample1[i].am_channel2, sample1[i].am_data2); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i, sample1[i].am_channel3, sample1[i].am_data3); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i, sample1[i].am_channel4, sample1[i].am_data4); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i + 1, sample2[i].am_channel1, sample2[i].am_data1); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i + 1, sample2[i].am_channel2, sample2[i].am_data2); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i + 1, sample2[i].am_channel3, sample2[i].am_data3); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i + 1, sample2[i].am_channel4, sample2[i].am_data4); -// message("%d: channel: %d value: %d\n", -// i, sample9[i].am_channel, sample9[i].am_data); - } + message("CYCLE %d:", j); + + message("channel: %d value: %d", + sample1.am_channel1, sample1.am_data1); + message("channel: %d value: %d", + sample1.am_channel2, sample1.am_data2); + message("channel: %d value: %d", + sample1.am_channel3, sample1.am_data3); + message("channel: %d value: %d", + sample1.am_channel4, sample1.am_data4); } } + fflush(stdout); } - printf("\t ADC test successful.\n"); + message("\t ADC test successful."); errout_with_dev: diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c index c801869ab6..ad7c740648 100644 --- a/apps/px4/tests/test_sensors.c +++ b/apps/px4/tests/test_sensors.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +36,7 @@ * @file test_sensors.c * Tests the onboard sensors. * - * The sensors app must not be running when performing this test. + * @author Lorenz Meier */ #include @@ -56,7 +56,10 @@ #include "tests.h" +#include #include +#include +#include /**************************************************************************** * Pre-processor Definitions @@ -70,8 +73,10 @@ * Private Function Prototypes ****************************************************************************/ -//static int lis331(int argc, char *argv[]); -static int mpu6000(int argc, char *argv[]); +static int accel(int argc, char *argv[]); +static int gyro(int argc, char *argv[]); +static int mag(int argc, char *argv[]); +static int baro(int argc, char *argv[]); /**************************************************************************** * Private Data @@ -82,7 +87,10 @@ struct { const char *path; int (* test)(int argc, char *argv[]); } sensors[] = { - {"mpu6000", "/dev/accel", mpu6000}, + {"accel", "/dev/accel", accel}, + {"gyro", "/dev/gyro", gyro}, + {"mag", "/dev/mag", mag}, + {"baro", "/dev/baro", baro}, {NULL, NULL, NULL} }; @@ -95,9 +103,9 @@ struct { ****************************************************************************/ static int -mpu6000(int argc, char *argv[]) +accel(int argc, char *argv[]) { - printf("\tMPU-6000: test start\n"); + printf("\tACCEL: test start\n"); fflush(stdout); int fd; @@ -107,7 +115,7 @@ mpu6000(int argc, char *argv[]) fd = open("/dev/accel", O_RDONLY); if (fd < 0) { - printf("\tMPU-6000: open fail, run first.\n"); + printf("\tACCEL: open fail, run or or first.\n"); return ERROR; } @@ -118,11 +126,11 @@ mpu6000(int argc, char *argv[]) ret = read(fd, &buf, sizeof(buf)); if (ret < 3) { - printf("\tMPU-6000: read1 fail (%d)\n", ret); + printf("\tACCEL: read1 fail (%d)\n", ret); return ERROR; } else { - printf("\tMPU-6000 values: acc: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);//\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); + printf("\tACCEL values: acc: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); } // /* wait at least 10ms, sensor should have data after no more than 2ms */ @@ -141,7 +149,118 @@ mpu6000(int argc, char *argv[]) /* XXX more tests here */ /* Let user know everything is ok */ - printf("\tOK: MPU-6000 passed all tests successfully\n"); + printf("\tOK: ACCEL passed all tests successfully\n"); + + return OK; +} + +static int +gyro(int argc, char *argv[]) +{ + printf("\tGYRO: test start\n"); + fflush(stdout); + + int fd; + struct gyro_report buf; + int ret; + + fd = open("/dev/gyro", O_RDONLY); + + if (fd < 0) { + printf("\tGYRO: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret < 3) { + printf("\tGYRO: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tGYRO values: rates: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); + } + + /* Let user know everything is ok */ + printf("\tOK: GYRO passed all tests successfully\n"); + + return OK; +} + +static int +mag(int argc, char *argv[]) +{ + printf("\tMAG: test start\n"); + fflush(stdout); + + int fd; + struct mag_report buf; + int ret; + + fd = open("/dev/mag", O_RDONLY); + + if (fd < 0) { + printf("\tMAG: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret < 3) { + printf("\tMAG: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tMAG values: mag. field: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); + } + + /* Let user know everything is ok */ + printf("\tOK: MAG passed all tests successfully\n"); + + return OK; +} + +static int +baro(int argc, char *argv[]) +{ + printf("\tBARO: test start\n"); + fflush(stdout); + + int fd; + struct baro_report buf; + int ret; + + fd = open("/dev/baro", O_RDONLY); + + if (fd < 0) { + printf("\tBARO: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret < 3) { + printf("\tBARO: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tBARO values: pressure: %8.4f mbar\taltitude: %8.4f m\ttemperature: %8.4f deg Celsius\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature); + } + + /* Let user know everything is ok */ + printf("\tOK: BARO passed all tests successfully\n"); return OK; } From 15f43b1acc456ae47bac0575e785ed87d95db93c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 12:13:15 +0100 Subject: [PATCH 02/22] improved ADC test, not yet stable --- apps/px4/tests/test_adc.c | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c index 4ffa9374f1..c33af1311d 100644 --- a/apps/px4/tests/test_adc.c +++ b/apps/px4/tests/test_adc.c @@ -81,18 +81,18 @@ int test_adc(int argc, char *argv[]) fd0 = open("/dev/adc0", O_RDONLY | O_NONBLOCK); if (fd0 <= 0) { - message("/dev/adc0 open fail: %d", errno); + message("/dev/adc0 open fail: %d\n", errno); return ERROR; } else { - message("opened /dev/adc0 successfully"); + message("opened /dev/adc0 successfully\n"); } usleep(10000); for (j = 0; j < 10; j++) { - /* sleep 1 millisecond */ - usleep(1000); + /* sleep 20 milliseconds */ + usleep(20000); nbytes = read(fd0, &sample1, sizeof(sample1)); /* Handle unexpected return values */ @@ -101,15 +101,15 @@ int test_adc(int argc, char *argv[]) errval = errno; if (errval != EINTR) { - message("reading /dev/adc0 failed: %d", errval); + message("reading /dev/adc0 failed: %d\n", errval); errval = 3; goto errout_with_dev; } - message("\tinterrupted read.."); + message("\tinterrupted read..\n"); } else if (nbytes == 0) { - message("\tno data read, ignoring."); + message("\tno data read, ignoring.\n"); ret = ERROR; } @@ -117,22 +117,22 @@ int test_adc(int argc, char *argv[]) else { if (nbytes != sizeof(sample1)) { - message("\tsample 1 size %d is not matching struct size %d, ignoring", + message("\tsample 1 size %d is not matching struct size %d, ignoring\n", nbytes, sizeof(sample1)); ret = ERROR; } else { - message("CYCLE %d:", j); + message("CYCLE %d:\n", j); + message("channel: %d value: %d\n", + (int)sample1.am_channel1, sample1.am_data1); message("channel: %d value: %d", - sample1.am_channel1, sample1.am_data1); + (int)sample1.am_channel2, sample1.am_data2); message("channel: %d value: %d", - sample1.am_channel2, sample1.am_data2); + (int)sample1.am_channel3, sample1.am_data3); message("channel: %d value: %d", - sample1.am_channel3, sample1.am_data3); - message("channel: %d value: %d", - sample1.am_channel4, sample1.am_data4); + (int)sample1.am_channel4, sample1.am_data4); } } fflush(stdout); From 90466afe05359cd5fc1d5b44b13d73912fb6571b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 12:13:40 +0100 Subject: [PATCH 03/22] Comments cleanup / polishing --- apps/mavlink/mavlink_receiver.c | 4 +++- apps/mavlink/orb_listener.c | 4 +++- apps/multirotor_pos_control/multirotor_pos_control.c | 6 ++++-- apps/sensors/sensors.cpp | 4 ++-- apps/uORB/topics/subsystem_info.h | 12 ++++++++---- 5 files changed, 20 insertions(+), 10 deletions(-) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 39e574c047..e1a4e8e8af 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file mavlink_receiver.c * MAVLink protocol message receive and dispatch + * + * @author Lorenz Meier */ /* XXX trim includes */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 683964a0d9..fafc8f79c7 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file orb_listener.c * Monitors ORB topics and sends update messages as appropriate. + * + * @author Lorenz Meier */ // XXX trim includes diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 9821fc7e5f..7b8d83aa8a 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -32,8 +32,10 @@ * ****************************************************************************/ -/* - * @file Implementation of AR.Drone 1.0 / 2.0 control interface +/** + * @file multirotor_pos_control.c + * + * Skeleton for multirotor position controller */ #include diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 3e9f35eaf4..675a8602a4 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -34,9 +34,9 @@ /** * @file sensors.cpp - * @author Lorenz Meier - * * Sensor readout process. + * + * @author Lorenz Meier */ #include diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h index 89b3974783..c3e039d66b 100644 --- a/apps/uORB/topics/subsystem_info.h +++ b/apps/uORB/topics/subsystem_info.h @@ -1,9 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * Thomas Gubler + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +37,10 @@ /** * @file subsystem_info.h * Definition of the subsystem info topic. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes */ #ifndef TOPIC_SUBSYSTEM_INFO_H_ From e99a684fd8e574c51779adc8b5b062ad680c570f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 16:51:17 +0100 Subject: [PATCH 04/22] Removed debug ioctl output --- apps/drivers/px4fmu/fmu.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index fff713bb5e..ae888323d8 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -419,7 +419,8 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - debug("ioctl 0x%04x 0x%08x", cmd, arg); + // XXX disabled, confusing users + //debug("ioctl 0x%04x 0x%08x", cmd, arg); /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); From f08e99311f154879933321c5ac6873ec47a590a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 16:51:38 +0100 Subject: [PATCH 05/22] Better printing of baro test values --- apps/px4/tests/test_sensors.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c index ad7c740648..dc1f39046b 100644 --- a/apps/px4/tests/test_sensors.c +++ b/apps/px4/tests/test_sensors.c @@ -130,7 +130,7 @@ accel(int argc, char *argv[]) return ERROR; } else { - printf("\tACCEL values: acc: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); + printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); } // /* wait at least 10ms, sensor should have data after no more than 2ms */ @@ -182,7 +182,7 @@ gyro(int argc, char *argv[]) return ERROR; } else { - printf("\tGYRO values: rates: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); + printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } /* Let user know everything is ok */ @@ -219,7 +219,7 @@ mag(int argc, char *argv[]) return ERROR; } else { - printf("\tMAG values: mag. field: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); + printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); } /* Let user know everything is ok */ @@ -256,7 +256,7 @@ baro(int argc, char *argv[]) return ERROR; } else { - printf("\tBARO values: pressure: %8.4f mbar\taltitude: %8.4f m\ttemperature: %8.4f deg Celsius\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature); + printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature); } /* Let user know everything is ok */ From e8eb887515def14775c70289dcba67e789a96659 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 16:52:06 +0100 Subject: [PATCH 06/22] Added hil command to emulate PWM outputs --- apps/drivers/hil/Makefile | 42 ++ apps/drivers/hil/hil.cpp | 829 +++++++++++++++++++++++++++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 3 files changed, 872 insertions(+) create mode 100644 apps/drivers/hil/Makefile create mode 100644 apps/drivers/hil/hil.cpp diff --git a/apps/drivers/hil/Makefile b/apps/drivers/hil/Makefile new file mode 100644 index 0000000000..1fb6e37bce --- /dev/null +++ b/apps/drivers/hil/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Interface driver for the PX4FMU board +# + +APPNAME = hil +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp new file mode 100644 index 0000000000..eb080ffc4c --- /dev/null +++ b/apps/drivers/hil/hil.cpp @@ -0,0 +1,829 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 hil.cpp + * + * Driver/configurator for the virtual HIL port. + * + * This virtual driver emulates PWM / servo outputs for setups where + * the connected hardware does not provide enough or no PWM outputs. + * + * Its only function is to take actuator_control uORB messages, + * mix them with any loaded mixer and output the result to the + * actuator_output uORB topic. HIL can also be performed with normal + * PWM outputs, a special flag prevents the outputs to be operated + * during HIL mode. If HIL is not performed with a standalone FMU, + * but in a real system, it is NOT recommended to use this virtual + * driver. Use instead the normal FMU or IO driver. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +// #include + +#include +#include +#include + +#include +#include + +#include + +class HIL : public device::CDev +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_8PWM, + MODE_12PWM, + MODE_16PWM, + MODE_NONE + }; + HIL(); + ~HIL(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + + virtual int init(); + + int set_mode(Mode mode); + int set_pwm_rate(unsigned rate); + +private: + static const unsigned _max_actuators = 4; + + Mode _mode; + int _update_rate; + int _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + orb_advert_t _t_outputs; + unsigned _num_outputs; + bool _primary_pwm_device; + + volatile bool _task_should_exit; + bool _armed; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(file *filp, int cmd, unsigned long arg); + +}; + +namespace +{ + +HIL *g_hil; + +} // namespace + +HIL::HIL() : + CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/), + _mode(MODE_NONE), + _update_rate(50), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _num_outputs(0), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +HIL::~HIL() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_hil = nullptr; +} + +int +HIL::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* reset GPIOs */ + // gpio_reset(); + + /* start the IO interface task */ + _task = task_spawn("fmuservo", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&HIL::task_main_trampoline, + nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +HIL::task_main_trampoline(int argc, char *argv[]) +{ + g_hil->task_main(); +} + +int +HIL::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: + debug("MODE_2PWM"); + /* multi-port with flow control lines as PWM */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_4PWM: + debug("MODE_4PWM"); + /* multi-port as 4 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_8PWM: + debug("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_12PWM: + debug("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_16PWM: + debug("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_NONE: + debug("MODE_NONE"); + /* disable servo outputs and set a very low update rate */ + _update_rate = 10; + break; + + default: + return -EINVAL; + } + _mode = mode; + return OK; +} + +int +HIL::set_pwm_rate(unsigned rate) +{ + if ((rate > 500) || (rate < 10)) + return -EINVAL; + + _update_rate = rate; + return OK; +} + +void +HIL::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &outputs); + + pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + + log("starting"); + + /* loop until killed */ + while (!_task_should_exit) { + + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + if (update_rate_in_ms < 2) + update_rate_in_ms = 2; + orb_set_interval(_t_actuators, update_rate_in_ms); + // up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* sleep waiting for data, but no more than a second */ + int ret = ::poll(&fds[0], 2, 1000); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + /* do mixing */ + _mixers->mix(&outputs.output[0], num_outputs); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + + /* output to the servo */ + // up_pwm_servo_set(i, outputs.output[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PWM servo armed status */ + // up_pwm_servo_arm(aa.armed); + } + } + + ::close(_t_actuators); + ::close(_t_armed); + + /* make sure servos are off */ + // up_pwm_servo_deinit(); + + log("stopping"); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +HIL::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls->control[control_index]; + return 0; +} + +int +HIL::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + debug("ioctl 0x%04x 0x%08x", cmd, arg); + + // /* try it as a GPIO ioctl first */ + // ret = HIL::gpio_ioctl(filp, cmd, arg); + // if (ret != -ENOTTY) + // return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch(_mode) { + case MODE_2PWM: + case MODE_4PWM: + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + ret = HIL::pwm_ioctl(filp, cmd, arg); + break; + default: + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + int channel; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + // up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + // up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + // channel = cmd - PWM_SERVO_SET(0); +// up_pwm_servo_set(channel, arg); XXX + + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(0): + case PWM_SERVO_GET(1): { + // channel = cmd - PWM_SERVO_SET(0); + // *(servo_position_t *)arg = up_pwm_servo_get(channel); + break; + } + + case MIXERIOCGETOUTPUTCOUNT: + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + + } else { + *(unsigned *)arg = 2; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCADDMULTIROTOR: + /* XXX not yet supported */ + ret = -ENOTTY; + break; + + case MIXERIOCLOADFILE: { + const char *path = (const char *)arg; + + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) { + ret = -ENOMEM; + } else { + + debug("loading mixers from %s", path); + ret = _mixers->load_from_file(path); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +namespace +{ + +enum PortMode { + PORT_MODE_UNDEFINED = 0, + PORT1_MODE_UNSET, + PORT1_FULL_PWM, + PORT1_PWM_AND_SERIAL, + PORT1_PWM_AND_GPIO, + PORT2_MODE_UNSET, + PORT2_8PWM, + PORT2_12PWM, + PORT2_16PWM, +}; + +PortMode g_port_mode; + +int +hil_new_mode(PortMode new_mode, int update_rate) +{ + // uint32_t gpio_bits; + + +// /* reset to all-inputs */ +// g_hil->ioctl(0, GPIO_RESET, 0); + + // gpio_bits = 0; + + HIL::Mode servo_mode = HIL::MODE_NONE; + + switch (new_mode) { + case PORT_MODE_UNDEFINED: + case PORT1_MODE_UNSET: + case PORT2_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT1_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = HIL::MODE_4PWM; + break; + + case PORT1_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = HIL::MODE_2PWM; +// /* set RX/TX multi-GPIOs to serial mode */ +// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT1_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = HIL::MODE_2PWM; + break; + + case PORT2_8PWM: + /* select 8-pin PWM mode */ + servo_mode = HIL::MODE_8PWM; + break; + + case PORT2_12PWM: + /* select 12-pin PWM mode */ + servo_mode = HIL::MODE_12PWM; + break; + + case PORT2_16PWM: + /* select 16-pin PWM mode */ + servo_mode = HIL::MODE_16PWM; + break; + } + +// /* adjust GPIO config for serial mode(s) */ +// if (gpio_bits != 0) +// g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* (re)set the PWM output mode */ + g_hil->set_mode(servo_mode); + if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0)) + g_hil->set_pwm_rate(update_rate); + + return OK; +} + +int +hil_start(void) +{ + int ret = OK; + + if (g_hil == nullptr) { + + g_hil = new HIL; + + if (g_hil == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_hil->init(); + + if (ret != OK) { + delete g_hil; + g_hil = nullptr; + } + } + } + + return ret; +} + +void +test(void) +{ + int fd; + + fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + + if (fd < 0) { + puts("open fail"); + exit(1); + } + + ioctl(fd, PWM_SERVO_ARM, 0); + ioctl(fd, PWM_SERVO_SET(0), 1000); + + close(fd); + + exit(0); +} + +void +fake(int argc, char *argv[]) +{ + if (argc < 5) { + puts("hil fake (values -100 .. 100)"); + exit(1); + } + + actuator_controls_s ac; + + ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; + + ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; + + ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; + + ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; + + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + + if (handle < 0) { + puts("advertise failed"); + exit(1); + } + + exit(0); +} + +} // namespace + +extern "C" __EXPORT int hil_main(int argc, char *argv[]); + +int +hil_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNDEFINED; + int pwm_update_rate_in_hz = 0; + + if (!strcmp(argv[1], "test")) + test(); + + if (!strcmp(argv[1], "fake")) + fake(argc - 1, argv + 1); + + if (hil_start() != OK) + errx(1, "failed to start the FMU driver"); + + /* + * Mode switches. + * + * XXX use getopt? + */ + for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */ + + if (!strcmp(argv[i], "mode_pwm")) { + new_mode = PORT1_FULL_PWM; + + } else if (!strcmp(argv[i], "mode_pwm_serial")) { + new_mode = PORT1_PWM_AND_SERIAL; + + } else if (!strcmp(argv[i], "mode_pwm_gpio")) { + new_mode = PORT1_PWM_AND_GPIO; + + } else if (!strcmp(argv[i], "mode_port2_pwm8")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(argv[i], "mode_port2_pwm12")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(argv[i], "mode_port2_pwm16")) { + new_mode = PORT2_8PWM; + } + + /* look for the optional pwm update rate for the supported modes */ + if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { + // if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) { + // XXX all modes have PWM settings + if (argc > i + 1) { + pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } + // } else { + // fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); + // } + } + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNDEFINED) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return OK; + + /* switch modes */ + return hil_new_mode(new_mode, pwm_update_rate_in_hz); + } + + /* test, etc. here */ + + fprintf(stderr, "HIL: unrecognized command, try:\n"); + fprintf(stderr, " mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); + return -EINVAL; +} diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 5b2546911a..2e9a989ad7 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -96,6 +96,7 @@ CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/px4fmu +CONFIGURED_APPS += drivers/hil # Testing stuff CONFIGURED_APPS += px4/sensors_bringup From 84a0261278d4015d2638043d116a6e591d1867f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 16:54:28 +0100 Subject: [PATCH 07/22] Outputting mixed actuators instead of raw control output --- apps/mavlink/orb_listener.c | 49 +++++++++++++++++++------------------ 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index fafc8f79c7..8c38692b7b 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -421,7 +421,7 @@ l_actuator_outputs(struct listener *l) /* copy actuator data into local buffer */ orb_copy(ids[l->arg], *l->subp, &act_outputs); - if (gcs_link) + if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, l->arg /* port number */, act_outputs.output[0], @@ -432,6 +432,30 @@ l_actuator_outputs(struct listener *l) act_outputs.output[5], act_outputs.output[6], act_outputs.output[7]); + + /* only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7], + mavlink_mode, + 0); + } + } } void @@ -484,29 +508,6 @@ l_vehicle_attitude_controls(struct listener *l) "ctrl3 ", actuators.control[3]); } - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - actuators.control[0], - actuators.control[1], - actuators.control[2], - actuators.control[3], - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } } void From b43f692d3a5e999895f6781a4df79f7179ac273c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 17:07:17 +0100 Subject: [PATCH 08/22] Correctly handling 8+ outputs, currently only first 8 supported --- apps/drivers/hil/hil.cpp | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index eb080ffc4c..dd5463d4e4 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -335,7 +335,30 @@ HIL::task_main() fds[1].fd = _t_armed; fds[1].events = POLLIN; - unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + unsigned num_outputs; + + /* select the number of virtual outputs */ + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + // XXX only support the lower 8 - trivial to extend + num_outputs = 8; + break; + + case MODE_NONE: + default: + num_outputs = 0; + break; + } log("starting"); From ee5abb0745908c1dca78d8468b3fba34d5eab42c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 17:21:13 +0100 Subject: [PATCH 09/22] Fixed the number of control inputs for simple mixer. Contributed by Thomas Gubler --- apps/systemlib/mixer/mixer_group.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 0041512358..19ce25553a 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -168,6 +168,7 @@ mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd /* let's assume we're going to read a simple mixer */ mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + mixinfo->control_count = inputs; /* first, get the output scaler */ ret = mixer_getline(fd, buf, sizeof(buf)); From 41629e0ddb44a52ce3eee676d113cf41cfee699d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 18:14:25 +0100 Subject: [PATCH 10/22] Operational mixing and outputs in hil --- apps/drivers/hil/hil.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index dd5463d4e4..a8cb31f0a7 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -211,8 +211,9 @@ HIL::init() if (ret != OK) return ret; - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + // XXX already claimed with CDEV + ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; @@ -821,12 +822,12 @@ hil_main(int argc, char *argv[]) if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { // if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) { // XXX all modes have PWM settings - if (argc > i + 1) { - pwm_update_rate_in_hz = atoi(argv[i + 1]); - } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); - return 1; - } + if (argc > i + 1) { + pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } // } else { // fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); // } From d29c66b028cfc2552bb24e2f2477b306906f91cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 18:31:50 +0100 Subject: [PATCH 11/22] Code cleanup in mavlink app --- apps/mavlink/mavlink.c | 5 --- apps/mavlink/mavlink_receiver.c | 73 --------------------------------- 2 files changed, 78 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 9c39b2714f..460faf4462 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -143,15 +143,10 @@ set_hil_on_off(bool hil_enabled) /* Enable HIL */ if (hil_enabled && !mavlink_hil_enabled) { - //printf("\n HIL ON \n"); - /* Advertise topics */ pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - printf("\n pub_hil_attitude :%i\n", pub_hil_attitude); - printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos); - mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index e1a4e8e8af..3e485274eb 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -218,8 +218,6 @@ handle_message(mavlink_message_t *msg) if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - //printf("got message\n"); - //printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode); if (mavlink_system.sysid < 4) { @@ -276,61 +274,6 @@ handle_message(mavlink_message_t *msg) /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } - - // /* change armed status if required */ - // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); - - // bool cmd_generated = false; - - // if (v_status.flag_control_offboard_enabled != cmd_armed) { - // vcmd.param1 = cmd_armed; - // vcmd.param2 = 0; - // vcmd.param3 = 0; - // vcmd.param4 = 0; - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // /* check if input has to be enabled */ - // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || - // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || - // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || - // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { - // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); - // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); - // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); - // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // if (cmd_generated) { - // /* check if topic is advertised */ - // if (cmd_pub <= 0) { - // cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - // } else { - // /* create command */ - // orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - // } - // } } } @@ -342,8 +285,6 @@ handle_message(mavlink_message_t *msg) * COMMAND_LONG message or a SET_MODE message */ - // printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false"); - if (mavlink_hil_enabled) { if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { @@ -351,20 +292,6 @@ handle_message(mavlink_message_t *msg) mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); - // printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n " - // "ROLL %i \n PITCH %i \n YAW %i \n" - // "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n", - // hil_state.lat/1000000, // 1e7 - // hil_state.lon/1000000, // 1e7 - // hil_state.alt/1000, // mm - // hil_state.roll, // float rad - // hil_state.pitch, // float rad - // hil_state.yaw, // float rad - // hil_state.rollspeed, // float rad/s - // hil_state.pitchspeed, // float rad/s - // hil_state.yawspeed); // float rad/s - - hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; From 596d20e2a3869af6f497d31bfcb1d11622ef5236 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 18:32:20 +0100 Subject: [PATCH 12/22] Increased stack sizes, 1K is not enough when calling printf() from within app --- apps/drivers/hil/hil.cpp | 6 +++--- apps/drivers/px4fmu/fmu.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index a8cb31f0a7..bef21848b5 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -222,11 +222,11 @@ HIL::init() /* reset GPIOs */ // gpio_reset(); - /* start the IO interface task */ - _task = task_spawn("fmuservo", + /* start the HIL interface task */ + _task = task_spawn("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1024, + 2048, (main_t)&HIL::task_main_trampoline, nullptr); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index ae888323d8..3eb4a9ef22 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -224,7 +224,7 @@ PX4FMU::init() _task = task_spawn("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1024, + 2048, (main_t)&PX4FMU::task_main_trampoline, nullptr); From 13443e43bf6e4fb0ae58bd6de01ec4c0cd0b3338 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 18:32:42 +0100 Subject: [PATCH 13/22] Silenced attitude ekf if not getting sensor data in HIL mode --- .../attitude_estimator_ekf_main.c | 27 ++++++++++++------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 05a6292a29..b25e612297 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -204,6 +205,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -216,6 +219,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -226,13 +232,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) thread_running = true; /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - orb_advert_t pub_dbg = -1; + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs /* keep track of sensor updates */ uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; struct attitude_estimator_ekf_params ekf_params; @@ -259,8 +267,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n"); + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + } } else { /* only update parameters if they changed */ @@ -347,9 +359,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - int32_t z_k_sizes = 9; - // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; - static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ @@ -392,7 +401,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) continue; } - uint64_t timing_diff = hrt_absolute_time() - timing_start; + // uint64_t timing_diff = hrt_absolute_time() - timing_start; // /* print rotation matrix every 200th time */ if (printcounter % 200 == 0) { From 92fe049f6524b88861e7f3ef4f6c2344095f39ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 18:35:53 +0100 Subject: [PATCH 14/22] Sending correct actuator scaling --- apps/mavlink/orb_listener.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 8c38692b7b..df6b1a4758 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -444,14 +444,14 @@ l_actuator_outputs(struct listener *l) /* HIL message as per MAVLink spec */ mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), - act_outputs.output[0], - act_outputs.output[1], - act_outputs.output[2], - act_outputs.output[3], - act_outputs.output[4], - act_outputs.output[5], - act_outputs.output[6], - act_outputs.output[7], + (act_outputs.output[0] - 1000.0f) / 1000.0f, + (act_outputs.output[1] - 1000.0f) / 1000.0f, + (act_outputs.output[2] - 1000.0f) / 1000.0f, + (act_outputs.output[3] - 1000.0f) / 1000.0f, + (act_outputs.output[4] - 1000.0f) / 1000.0f, + (act_outputs.output[5] - 1000.0f) / 1000.0f, + (act_outputs.output[6] - 1000.0f) / 1000.0f, + (act_outputs.output[7] - 1000.0f) / 1000.0f, mavlink_mode, 0); } From 6adab3b31930074ad9336ac596c1b4c2d6a4bd74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 19:04:04 +0100 Subject: [PATCH 15/22] Hack to detect the number of control inputs --- apps/mavlink/orb_listener.c | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index df6b1a4758..e214f435ea 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -441,13 +441,27 @@ l_actuator_outputs(struct listener *l) uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + float rudder, throttle; + + // XXX very ugly, needs rework + if (isfinite(act_outputs.output[3]) + && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { + /* throttle is fourth output */ + rudder = (act_outputs.output[2] - 1000.0f) / 1000.0f; + throttle = (act_outputs.output[3] - 1000.0f) / 1000.0f; + } else { + /* only three outputs, put throttle on position 4 / index 3 */ + rudder = 0; + throttle = (act_outputs.output[2] - 1000.0f) / 1000.0f; + } + /* HIL message as per MAVLink spec */ mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1000.0f) / 1000.0f, (act_outputs.output[1] - 1000.0f) / 1000.0f, - (act_outputs.output[2] - 1000.0f) / 1000.0f, - (act_outputs.output[3] - 1000.0f) / 1000.0f, + rudder, + throttle, (act_outputs.output[4] - 1000.0f) / 1000.0f, (act_outputs.output[5] - 1000.0f) / 1000.0f, (act_outputs.output[6] - 1000.0f) / 1000.0f, From fb022f40e50e34c5e37356247fc98bc3a5c88f4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 19:06:58 +0100 Subject: [PATCH 16/22] Fixed zero offset in HIL --- apps/mavlink/orb_listener.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index e214f435ea..f7bc32935d 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -447,25 +447,25 @@ l_actuator_outputs(struct listener *l) if (isfinite(act_outputs.output[3]) && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1000.0f) / 1000.0f; - throttle = (act_outputs.output[3] - 1000.0f) / 1000.0f; + rudder = (act_outputs.output[2] - 1500.0f) / 1000.0f; + throttle = (act_outputs.output[3] - 1500.0f) / 1000.0f; } else { /* only three outputs, put throttle on position 4 / index 3 */ rudder = 0; - throttle = (act_outputs.output[2] - 1000.0f) / 1000.0f; + throttle = (act_outputs.output[2] - 1500.0f) / 1000.0f; } /* HIL message as per MAVLink spec */ mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), - (act_outputs.output[0] - 1000.0f) / 1000.0f, - (act_outputs.output[1] - 1000.0f) / 1000.0f, + (act_outputs.output[0] - 1500.0f) / 1000.0f, + (act_outputs.output[1] - 1500.0f) / 1000.0f, rudder, throttle, - (act_outputs.output[4] - 1000.0f) / 1000.0f, - (act_outputs.output[5] - 1000.0f) / 1000.0f, - (act_outputs.output[6] - 1000.0f) / 1000.0f, - (act_outputs.output[7] - 1000.0f) / 1000.0f, + (act_outputs.output[4] - 1500.0f) / 1000.0f, + (act_outputs.output[5] - 1500.0f) / 1000.0f, + (act_outputs.output[6] - 1500.0f) / 1000.0f, + (act_outputs.output[7] - 1500.0f) / 1000.0f, mavlink_mode, 0); } From f8291711d3daf0e4af12b018f7cc711414e3bf95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Nov 2012 19:10:57 +0100 Subject: [PATCH 17/22] Correct scaling for throttle --- apps/mavlink/orb_listener.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index f7bc32935d..f17c78c7a5 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -448,11 +448,11 @@ l_actuator_outputs(struct listener *l) && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { /* throttle is fourth output */ rudder = (act_outputs.output[2] - 1500.0f) / 1000.0f; - throttle = (act_outputs.output[3] - 1500.0f) / 1000.0f; + throttle = (act_outputs.output[3] - 1000.0f) / 1000.0f; } else { /* only three outputs, put throttle on position 4 / index 3 */ rudder = 0; - throttle = (act_outputs.output[2] - 1500.0f) / 1000.0f; + throttle = (act_outputs.output[2] - 1000.0f) / 1000.0f; } /* HIL message as per MAVLink spec */ From 7d8d7a76b986e7acefb4a61f3da3625db1f6dd11 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Nov 2012 12:55:57 +0100 Subject: [PATCH 18/22] Fixed scalings for fixed wing and multirotors --- apps/mavlink/mavlink.c | 15 +++--- apps/mavlink/orb_listener.c | 98 +++++++++++++++++++++++++++---------- 2 files changed, 79 insertions(+), 34 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 460faf4462..2527e0b0ff 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -132,6 +132,7 @@ static struct mavlink_logbuffer lb; static void mavlink_update_system(void); static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); static void usage(void); +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); @@ -161,15 +162,13 @@ set_hil_on_off(bool hil_enabled) } else if (baudrate < 115200) { /* 20 Hz */ hil_rate_interval = 50; - } else if (baudrate < 460800) { - /* 50 Hz */ - hil_rate_interval = 20; } else { /* 100 Hz */ hil_rate_interval = 10; } orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { @@ -263,7 +262,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } -static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { int ret = OK; @@ -453,19 +452,19 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) /* * Internal function to give access to the channel status for each channel */ -mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +mavlink_status_t* mavlink_get_channel_status(uint8_t channel) { static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[chan]; + return &m_mavlink_status[channel]; } /* * Internal function to give access to the channel buffer for each channel */ -mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) +mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) { static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_buffer[chan]; + return &m_mavlink_buffer[channel]; } void mavlink_update_system(void) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index f17c78c7a5..3ac229d734 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -441,33 +441,79 @@ l_actuator_outputs(struct listener *l) uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); - float rudder, throttle; - - // XXX very ugly, needs rework - if (isfinite(act_outputs.output[3]) - && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { - /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1500.0f) / 1000.0f; - throttle = (act_outputs.output[3] - 1000.0f) / 1000.0f; - } else { - /* only three outputs, put throttle on position 4 / index 3 */ - rudder = 0; - throttle = (act_outputs.output[2] - 1000.0f) / 1000.0f; - } - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 1000.0f, - (act_outputs.output[1] - 1500.0f) / 1000.0f, - rudder, - throttle, - (act_outputs.output[4] - 1500.0f) / 1000.0f, - (act_outputs.output[5] - 1500.0f) / 1000.0f, - (act_outputs.output[6] - 1500.0f) / 1000.0f, - (act_outputs.output[7] - 1500.0f) / 1000.0f, - mavlink_mode, - 0); + + /* scale / assign outputs depending on system type */ + + if (mavlink_system.type == MAV_TYPE_QUADROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_mode, + 0); + } else { + float rudder, throttle; + + /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ + + // XXX very ugly, needs rework + if (isfinite(act_outputs.output[3]) + && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { + /* throttle is fourth output */ + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); + } else { + /* only three outputs, put throttle on position 4 / index 3 */ + rudder = 0; + throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); + } + + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + (act_outputs.output[0] - 1500.0f) / 600.0f, + (act_outputs.output[1] - 1500.0f) / 600.0f, + rudder, + throttle, + (act_outputs.output[4] - 1500.0f) / 600.0f, + (act_outputs.output[5] - 1500.0f) / 600.0f, + (act_outputs.output[6] - 1500.0f) / 600.0f, + (act_outputs.output[7] - 1500.0f) / 600.0f, + mavlink_mode, + 0); + } } } } From dca3bce1ca89595f5df3788da34afe3b30bfb35a Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 12 Nov 2012 01:35:51 -0800 Subject: [PATCH 19/22] Add a new performance counter for measuring periodic/interval events. --- apps/systemlib/perf_counter.c | 70 ++++++++++++++++++++++++++++++++--- apps/systemlib/perf_counter.h | 3 +- 2 files changed, 67 insertions(+), 6 deletions(-) diff --git a/apps/systemlib/perf_counter.c b/apps/systemlib/perf_counter.c index e6c49d4326..ff15ef4793 100644 --- a/apps/systemlib/perf_counter.c +++ b/apps/systemlib/perf_counter.c @@ -73,6 +73,20 @@ struct perf_ctr_elapsed { uint64_t time_most; }; +/** + * PC_INTERVAL counter. + */ +struct perf_ctr_interval { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_event; + uint64_t time_first; + uint64_t time_last; + uint64_t time_least; + uint64_t time_most; + +}; + /** * List of all known counters. */ @@ -93,6 +107,10 @@ perf_alloc(enum perf_counter_type type, const char *name) ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1); break; + case PC_INTERVAL: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1); + break; + default: break; } @@ -127,6 +145,32 @@ perf_count(perf_counter_t handle) ((struct perf_ctr_count *)handle)->event_count++; break; + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + hrt_abstime now = hrt_absolute_time(); + + switch (pci->event_count) { + case 0: + pci->time_first = now; + break; + case 1: + pci->time_least = now - pci->time_last; + pci->time_most = now - pci->time_last; + break; + default: { + hrt_abstime interval = now - pci->time_last; + if (interval < pci->time_least) + pci->time_least = interval; + if (interval > pci->time_most) + pci->time_most = interval; + break; + } + } + pci->time_last = now; + pci->event_count++; + break; + } + default: break; } @@ -187,13 +231,29 @@ perf_print_counter(perf_counter_t handle) ((struct perf_ctr_count *)handle)->event_count); break; - case PC_ELAPSED: + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", handle->name, - ((struct perf_ctr_elapsed *)handle)->event_count, - ((struct perf_ctr_elapsed *)handle)->time_total, - ((struct perf_ctr_elapsed *)handle)->time_least, - ((struct perf_ctr_elapsed *)handle)->time_most); + pce->event_count, + pce->time_total, + pce->time_least, + pce->time_most); + break; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + + printf("%s: %llu events, %llu avg, min %lluus max %lluus\n", + handle->name, + pci->event_count, + (pci->time_last - pci->time_first) / pci->event_count, + pci->time_least, + pci->time_most); + break; + } default: break; diff --git a/apps/systemlib/perf_counter.h b/apps/systemlib/perf_counter.h index 5dc4410562..6e6c80d5bc 100644 --- a/apps/systemlib/perf_counter.h +++ b/apps/systemlib/perf_counter.h @@ -44,7 +44,8 @@ */ enum perf_counter_type { PC_COUNT, /**< count the number of times an event occurs */ - PC_ELAPSED /**< measure the time elapsed performing an event */ + PC_ELAPSED, /**< measure the time elapsed performing an event */ + PC_INTERVAL /**< measure the interval between instances of an event */ }; struct perf_ctr_header; From 2c12802f25cef99da345493d01c54c7c39602ccc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Nov 2012 10:59:35 +0100 Subject: [PATCH 20/22] Fixed HIL compile warnings --- apps/drivers/hil/hil.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index bef21848b5..abb44886e7 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -474,6 +474,7 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg) ret = HIL::pwm_ioctl(filp, cmd, arg); break; default: + ret = -ENOTTY; debug("not in a PWM mode"); break; } @@ -489,7 +490,7 @@ int HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; - int channel; + // int channel; lock(); From 66e806754ba17c127cd9fa657b0bad5d0b2015a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Nov 2012 17:36:17 +0100 Subject: [PATCH 21/22] Fixed GPS status detection --- apps/gps/mtk.c | 7 ++++--- apps/gps/ubx.c | 11 ++++++----- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index 374280d28f..604dba05c7 100644 --- a/apps/gps/mtk.c +++ b/apps/gps/mtk.c @@ -418,10 +418,11 @@ void *mtk_watchdog_loop(void *args) mtk_gps->satellite_info_available = 0; // global_data_send_subsystem_info(&mtk_present_enabled_healthy); mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n"); - mtk_healthy = true; - mtk_fail_count = 0; - once_ok = true; } + + mtk_healthy = true; + mtk_fail_count = 0; + once_ok = true; } usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS); diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 03ae622a12..21e917bf88 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -272,7 +272,8 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) ubx_gps->timestamp = hrt_absolute_time(); ubx_gps->counter++; - + ubx_gps->s_variance = packet->sAcc; + ubx_gps->p_variance = packet->pAcc; //pthread_mutex_lock(ubx_mutex); ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time(); @@ -785,10 +786,6 @@ void *ubx_watchdog_loop(void *args) sleep(1); } else { - /* gps healthy */ - ubx_success_count++; - ubx_healthy = true; - ubx_fail_count = 0; if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) { //printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed); @@ -799,6 +796,10 @@ void *ubx_watchdog_loop(void *args) once_ok = true; } + /* gps healthy */ + ubx_success_count++; + ubx_healthy = true; + ubx_fail_count = 0; } usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS); From e9942e4af7f5b35e7d3efe4381ed6127e5c6a91c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Nov 2012 21:08:51 +0100 Subject: [PATCH 22/22] minor HIL related tweaks --- apps/commander/state_machine_helper.c | 2 ++ apps/drivers/hil/hil.cpp | 1 + apps/mavlink/mavlink.c | 4 ++-- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a64d99cd49..891efe9d79 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -579,6 +579,8 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ state_machine_publish(status_pub, current_status, mavlink_fd); publish_armed_status(current_status); printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.") } /* NEVER actually switch off HIL without reboot */ diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index bef21848b5..ecccfb5b0f 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -824,6 +824,7 @@ hil_main(int argc, char *argv[]) // XXX all modes have PWM settings if (argc > i + 1) { pwm_update_rate_in_hz = atoi(argv[i + 1]); + printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz); } else { fprintf(stderr, "missing argument for pwm update rate (-u)\n"); return 1; diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 2527e0b0ff..991bbfbab6 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -163,8 +163,8 @@ set_hil_on_off(bool hil_enabled) /* 20 Hz */ hil_rate_interval = 50; } else { - /* 100 Hz */ - hil_rate_interval = 10; + /* 200 Hz */ + hil_rate_interval = 5; } orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);