aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2014-10-30 15:10:54 +0100
committerLorenz Meier <lm@inf.ethz.ch>2014-10-30 15:10:54 +0100
commit54a5f7a138863e05935765b74369e42f69bb9495 (patch)
tree84c5f8f18b7f5e38f10b31fa7e8e0614e7e060ed
parentd0d2c7f9c69c5cfdd3aabf757796d2d6ada32875 (diff)
parent9a92241a1a37b8cdfcc0ce4c2d2e44cba7b94dc5 (diff)
downloadpx4-firmware-54a5f7a138863e05935765b74369e42f69bb9495.tar.gz
px4-firmware-54a5f7a138863e05935765b74369e42f69bb9495.tar.bz2
px4-firmware-54a5f7a138863e05935765b74369e42f69bb9495.zip
Merge branch 'master' of github.com:PX4/Firmware
-rw-r--r--src/lib/external_lgpl/tecs/tecs.cpp8
-rw-r--r--src/lib/external_lgpl/tecs/tecs.h3
-rw-r--r--src/modules/uORB/objects_common.cpp3
-rw-r--r--src/modules/uORB/topics/test_motor.h70
-rw-r--r--src/systemcmds/motor_test/module.mk41
-rw-r--r--src/systemcmds/motor_test/motor_test.c128
6 files changed, 246 insertions, 7 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 6a2a61b04..0ed210cf4 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_underspeed) {
- _throttle_dem_unc = 1.0f;
+ _throttle_dem = 1.0f;
} else {
// Calculate gain scaler from specific energy error to throttle
@@ -363,10 +363,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
} else {
_throttle_dem = ff_throttle;
}
- }
- // Constrain throttle demand
- _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+ // Constrain throttle demand
+ _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+ }
}
void TECS::_detect_bad_descent(void)
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 8ac31de6f..eb45237b7 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -345,9 +345,6 @@ private:
// climbout mode
bool _climbOutDem;
- // throttle demand before limiting
- float _throttle_dem_unc;
-
// pitch demand before limiting
float _pitch_dem_unc;
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index b921865eb..b91a00c1e 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -201,6 +201,9 @@ ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
+#include "topics/test_motor.h"
+ORB_DEFINE(test_motor, struct test_motor_s);
+
#include "topics/debug_key_value.h"
ORB_DEFINE(debug_key_value, struct debug_key_value_s);
diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h
new file mode 100644
index 000000000..491096934
--- /dev/null
+++ b/src/modules/uORB/topics/test_motor.h
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 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 test_motor.h
+ *
+ * Test a single drive motor while the vehicle is disarmed
+ *
+ * Publishing values to this topic causes a single motor to spin, e.g. for
+ * ground test purposes. This topic will be ignored while the vehicle is armed.
+ *
+ */
+
+#ifndef TOPIC_TEST_MOTOR_H
+#define TOPIC_TEST_MOTOR_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_MOTOR_OUTPUTS 8
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct test_motor_s {
+ uint64_t timestamp; /**< output timestamp in us since system boot */
+ unsigned motor_number; /**< number of motor to spin */
+ float value; /**< output data, in natural output units */
+};
+
+/**
+ * @}
+ */
+
+/* actuator output sets; this list can be expanded as more drivers emerge */
+ORB_DECLARE(test_motor);
+
+#endif
diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk
new file mode 100644
index 000000000..eb36d2ded
--- /dev/null
+++ b/src/systemcmds/motor_test/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+# Copyright (c) 2014 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.
+#
+############################################################################
+
+#
+# Build the motor_test tool.
+#
+
+MODULE_COMMAND = motor_test
+SRCS = motor_test.c
+
+MODULE_STACKSIZE = 4096
diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c
new file mode 100644
index 000000000..079f99674
--- /dev/null
+++ b/src/systemcmds/motor_test/motor_test.c
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ * Author: Holger Steinhaus <hsteinhaus@gmx.de>
+ *
+ * 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 motor_test.c
+ *
+ * Tool for drive testing
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/test_motor.h>
+
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+
+__EXPORT int motor_test_main(int argc, char *argv[]);
+static void motor_test(unsigned channel, float value);
+static void usage(const char *reason);
+
+void motor_test(unsigned channel, float value)
+{
+ orb_advert_t _test_motor_pub;
+ struct test_motor_s _test_motor;
+
+ _test_motor.motor_number = channel;
+ _test_motor.timestamp = hrt_absolute_time();
+ _test_motor.value = value;
+
+ if (_test_motor_pub > 0) {
+ /* publish armed state */
+ orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
+ } else {
+ /* advertise and publish */
+ _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
+ }
+}
+
+static void usage(const char *reason)
+{
+ if (reason != NULL)
+ warnx("%s", reason);
+
+ errx(1,
+ "usage:\n"
+ "motor_test\n"
+ " -m <channel> Motor to test (0..7)\n"
+ " -p <power> Power (0..100)\n");
+}
+
+int motor_test_main(int argc, char *argv[])
+{
+ unsigned long channel, lval;
+ float value;
+ int ch;
+
+ if (argc != 5) {
+ usage("please specify motor and power");
+ }
+
+ while ((ch = getopt(argc, argv, "m:p:")) != EOF) {
+ switch (ch) {
+
+ case 'm':
+ /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+ channel = strtoul(optarg, NULL, 0);
+ break;
+
+ case 'p':
+ /* Read in custom low value */
+ lval = strtoul(optarg, NULL, 0);
+
+ if (lval > 100)
+ usage("value invalid");
+
+ value = (float)lval/100.f;
+ break;
+ default:
+ usage(NULL);
+ }
+ }
+
+ motor_test(channel, value);
+
+ printf("motor %d set to %.2f\n", channel, value);
+
+ exit(0);
+}