PX4 Firmware
PX4 Autopilot Software http://px4.io
test_ppm_loopback.c
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file test_ppm_loopback.c
36  * Tests the PWM outputs and PPM input
37  */
38 
39 #include <px4_platform_common/time.h>
40 #include <px4_platform_common/px4_config.h>
41 
42 #include <sys/types.h>
43 
44 #include <stdio.h>
45 #include <stdlib.h>
46 #include <unistd.h>
47 #include <fcntl.h>
48 #include <errno.h>
49 
50 #include <arch/board/board.h>
51 #include <drivers/drv_pwm_output.h>
52 #include <drivers/drv_rc_input.h>
54 #include <systemlib/err.h>
55 
56 #include "tests_main.h"
57 
58 #include <math.h>
59 #include <float.h>
60 
61 int test_ppm_loopback(int argc, char *argv[])
62 {
63 
64  int _rc_sub = orb_subscribe(ORB_ID(input_rc));
65 
66  int servo_fd, result;
67  servo_position_t pos;
68 
69  servo_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR);
70 
71  if (servo_fd < 0) {
72  printf("failed opening /dev/pwm_servo\n");
73  }
74 
75  printf("Servo readback, pairs of values should match defaults\n");
76 
77  unsigned servo_count;
78  result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
79 
80  if (result != OK) {
81  warnx("PWM_SERVO_GET_COUNT");
82 
83  (void)close(servo_fd);
84  return ERROR;
85  }
86 
87  for (unsigned i = 0; i < servo_count; i++) {
88  result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos);
89 
90  if (result < 0) {
91  printf("failed reading channel %u\n", i);
92  }
93 
94  //printf("%u: %u %u\n", i, pos, data[i]);
95 
96  }
97 
98  // /* tell safety that its ok to disable it with the switch */
99  // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0);
100  // if (result != OK)
101  // warnx("FAIL: PWM_SERVO_SET_ARM_OK");
102  // tell output device that the system is armed (it will output values if safety is off)
103  // result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
104  // if (result != OK)
105  // warnx("FAIL: PWM_SERVO_ARM");
106 
107  int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
108 
109 
110  for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
111  result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
112 
113  if (result) {
114  (void)close(servo_fd);
115  return ERROR;
116 
117  } else {
118  warnx("channel %d set to %d", i, pwm_values[i]);
119  }
120  }
121 
122  warnx("servo count: %d", servo_count);
123 
124  struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0};
125 
126  for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
127  pwm_out.values[i] = pwm_values[i];
128  //warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]);
129  pwm_out.channel_count++;
130  }
131 
132  result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out);
133 
134  /* give driver 10 ms to propagate */
135 
136  /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
137  struct input_rc_s rc_input;
138  orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
139  px4_usleep(100000);
140 
141  /* open PPM input and expect values close to the output values */
142 
143  bool rc_updated;
144  orb_check(_rc_sub, &rc_updated);
145 
146  if (rc_updated) {
147 
148  orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
149 
150  // int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);
151 
152 
153 
154  // struct input_rc_s rc;
155  // result = read(ppm_fd, &rc, sizeof(rc));
156 
157  // if (result != sizeof(rc)) {
158  // warnx("Error reading RC output");
159  // (void)close(servo_fd);
160  // (void)close(ppm_fd);
161  // return ERROR;
162  // }
163 
164  /* go and check values */
165  for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
166  if (abs(rc_input.values[i] - pwm_values[i]) > 10) {
167  warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]);
168  (void)close(servo_fd);
169  return ERROR;
170  }
171  }
172 
173  } else {
174  warnx("failed reading RC input data");
175  (void)close(servo_fd);
176  return ERROR;
177  }
178 
179  close(servo_fd);
180 
181  warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!");
182 
183  return 0;
184 }
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
R/C input interface.
uint16_t servo_position_t
Servo output signal type, value is actual servo output pulse width in microseconds.
#define PWM_SERVO_GET_COUNT
get the number of servos in *(unsigned *)arg
#define PWM_SERVO_SET(_servo)
set a single servo to a specific value
int test_ppm_loopback(int argc, char *argv[])
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
#define PWM_OUTPUT0_DEVICE_PATH
uint16_t values[PWM_OUTPUT_MAX_CHANNELS]
uint16_t values[18]
Definition: input_rc.h:76
#define PWM_SERVO_SET_DISARMED_PWM
set the PWM value when disarmed - should be no PWM (zero) by default
uint32_t channel_count
#define warnx(...)
Definition: err.h:95
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
Tests declaration file.
#define PWM_SERVO_GET(_servo)
get a single specific servo value
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
#define OK
Definition: uavcan_main.cpp:71
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196