PX4 Firmware
PX4 Autopilot Software http://px4.io
motor_test.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2014 PX4 Development Team. All rights reserved.
4  * Author: Holger Steinhaus <hsteinhaus@gmx.de>
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 
35 /**
36  * @file motor_test.c
37  *
38  * Tool for drive testing
39  */
40 
41 #include <drivers/drv_hrt.h>
42 #include <px4_platform_common/px4_config.h>
43 #include <px4_platform_common/getopt.h>
44 #include <px4_platform_common/log.h>
45 #include <px4_platform_common/module.h>
47 #include <uORB/topics/test_motor.h>
48 
49 extern "C" __EXPORT int motor_test_main(int argc, char *argv[]);
50 
51 static void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms);
52 static void usage(const char *reason);
53 
54 void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms)
55 {
56  test_motor_s test_motor{};
57  test_motor.timestamp = hrt_absolute_time();
58  test_motor.motor_number = channel;
59  test_motor.value = value;
60  test_motor.action = value >= 0.f ? test_motor_s::ACTION_RUN : test_motor_s::ACTION_STOP;
61  test_motor.driver_instance = driver_instance;
62  test_motor.timeout_ms = timeout_ms;
63 
64  uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
65  test_motor_pub.publish(test_motor);
66 
67  if (test_motor.action == test_motor_s::ACTION_STOP) {
68  PX4_INFO("motors stop command sent");
69 
70  } else {
71  PX4_INFO("motor %d set to %.2f", channel, (double)value);
72  }
73 }
74 
75 static void usage(const char *reason)
76 {
77  if (reason != nullptr) {
78  PX4_WARN("%s", reason);
79  }
80 
81  PRINT_MODULE_DESCRIPTION(
82  R"DESCR_STR(
83 Utility to test motors.
84 
85 WARNING: remove all props before using this command.
86 )DESCR_STR");
87 
88  PRINT_MODULE_USAGE_NAME("motor_test", "command");
89  PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Set motor(s) to a specific output value");
90  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 7, "Motor to test (0...7, all if not specified)", true);
91  PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 100, "Power (0...100)", true);
92  PRINT_MODULE_USAGE_PARAM_INT('t', 0, 0, 100, "Timeout in seconds (default=no timeout)", true);
93  PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 4, "driver instance", true);
94  PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop all motors");
95  PRINT_MODULE_USAGE_COMMAND_DESCR("iterate", "Iterate all motors starting and stopping one after the other");
96 
97 }
98 
99 int motor_test_main(int argc, char *argv[])
100 {
101  int channel = -1; //default to all channels
102  unsigned long lval;
103  float value = 0.0f;
104  uint8_t driver_instance = 0;
105  int ch;
106  int timeout_ms = 0;
107 
108  int myoptind = 1;
109  const char *myoptarg = nullptr;
110 
111  while ((ch = px4_getopt(argc, argv, "i:m:p:t:", &myoptind, &myoptarg)) != EOF) {
112  switch (ch) {
113 
114  case 'i':
115  driver_instance = (uint8_t)strtol(myoptarg, nullptr, 0);
116  break;
117 
118  case 'm':
119  /* Read in motor number */
120  channel = (int)strtol(myoptarg, nullptr, 0);
121  break;
122 
123  case 'p':
124  /* Read in power value */
125  lval = strtoul(myoptarg, nullptr, 0);
126 
127  if (lval > 100) {
128  usage("value invalid");
129  return 1;
130  }
131 
132  value = ((float)lval) / 100.f;
133  break;
134 
135  case 't':
136  timeout_ms = strtol(myoptarg, nullptr, 0) * 1000;
137  break;
138 
139  default:
140  usage(nullptr);
141  return 1;
142  }
143  }
144 
145  bool run_test = true;
146 
147  if (myoptind >= 0 && myoptind < argc) {
148  if (strcmp("stop", argv[myoptind]) == 0) {
149  channel = 0;
150  value = -1.f;
151 
152  } else if (strcmp("iterate", argv[myoptind]) == 0) {
153  value = 0.15f;
154 
155  for (int i = 0; i < 8; ++i) {
156  motor_test(i, value, driver_instance, 0);
157  px4_usleep(500000);
158  motor_test(i, -1.f, driver_instance, 0);
159  px4_usleep(10000);
160  }
161 
162  run_test = false;
163 
164  } else if (strcmp("test", argv[myoptind]) == 0) {
165  // nothing to do
166  } else {
167  usage(nullptr);
168  return 0;
169  }
170 
171  } else {
172  usage(nullptr);
173  return 0;
174  }
175 
176  if (run_test) {
177  if (channel < 0) {
178  for (int i = 0; i < 8; ++i) {
179  motor_test(i, value, driver_instance, timeout_ms);
180  px4_usleep(10000);
181  }
182 
183  } else {
184  motor_test(channel, value, driver_instance, timeout_ms);
185  }
186  }
187 
188  return 0;
189 }
Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH) ...
static int timeout_ms
__EXPORT int motor_test_main(int argc, char *argv[])
Definition: motor_test.cpp:99
Definition: I2C.hpp:51
static void usage(const char *reason)
Definition: motor_test.cpp:75
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uint64_t timestamp
Definition: test_motor.h:57
static void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms)
Definition: motor_test.cpp:54
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).