PX4 Firmware
PX4 Autopilot Software http://px4.io
input_rc.cpp
Go to the documentation of this file.
1 /****************************************************************************
2 *
3 * Copyright (c) 2016-2017 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 input_rc.cpp
36  * @author Beat Küng <beat-kueng@gmx.net>
37  *
38  */
39 
40 #include "input_rc.h"
41 
42 #include <math.h>
43 #include <errno.h>
44 #include <px4_platform_common/posix.h>
45 #include <px4_platform_common/defines.h>
46 
47 
48 namespace vmount
49 {
50 
51 
52 InputRC::InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
53  : _do_stabilization(do_stabilization)
54 {
55  _aux_channels[0] = aux_channel_roll;
56  _aux_channels[1] = aux_channel_pitch;
57  _aux_channels[2] = aux_channel_yaw;
58 }
59 
61 {
64  }
65 }
66 
68 {
69  _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
70 
72  return -errno;
73  }
74 
75  return 0;
76 }
77 
78 int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
79 {
80  // Default to no change signalled by NULL.
81  *control_data = nullptr;
82 
83  px4_pollfd_struct_t polls[1];
84  polls[0].fd = _manual_control_setpoint_sub;
85  polls[0].events = POLLIN;
86 
87  int ret = px4_poll(polls, 1, timeout_ms);
88 
89  if (ret < 0) {
90  return -errno;
91  }
92 
93  if (ret == 0) {
94  // Timeout, leave NULL
95  } else {
96  if (polls[0].revents & POLLIN) {
97  // Only if there was a change, we update the control data, otherwise leave it NULL.
99  *control_data = &_control_data;
100  }
101  }
102  }
103 
104  return 0;
105 }
106 
107 bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active)
108 {
109  manual_control_setpoint_s manual_control_setpoint;
110  orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint);
111  control_data.type = ControlData::Type::Angle;
112 
113  float new_aux_values[3];
114 
115  for (int i = 0; i < 3; ++i) {
116  new_aux_values[i] = _get_aux_value(manual_control_setpoint, i);
117  }
118 
119  // If we were already active previously, we just update normally. Otherwise, there needs to be
120  // a major stick movement to re-activate manual (or it's running for the very first time).
121  bool major_movement = false;
122 
123  // Detect a big stick movement
124  for (int i = 0; i < 3; ++i) {
125  if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
126  major_movement = true;
127  }
128  }
129 
130  if (already_active || major_movement || _first_time) {
131 
132  _first_time = false;
133 
134  for (int i = 0; i < 3; ++i) {
135  control_data.type_data.angle.is_speed[i] = false;
136  control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F;
137  control_data.stabilize_axis[i] = _do_stabilization;
138 
139  _last_set_aux_values[i] = new_aux_values[i];
140  }
141 
142  control_data.gimbal_shutter_retract = false;
143  return true;
144 
145  } else {
146  return false;
147  }
148 }
149 
150 float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx)
151 {
152  switch (_aux_channels[channel_idx]) {
153 
154  case 1:
155  return manual_control_setpoint.aux1;
156 
157  case 2:
158  return manual_control_setpoint.aux2;
159 
160  case 3:
161  return manual_control_setpoint.aux3;
162 
163  case 4:
164  return manual_control_setpoint.aux4;
165 
166  case 5:
167  return manual_control_setpoint.aux5;
168 
169  case 6:
170  return manual_control_setpoint.aux6;
171 
172  default:
173  return 0.0f;
174  }
175 }
176 
178 {
179  PX4_INFO("Input: RC (channels: roll=%i, pitch=%i, yaw=%i)", _aux_channels[0], _aux_channels[1], _aux_channels[2]);
180 }
181 
182 } /* namespace vmount */
virtual ~InputRC()
Definition: input_rc.cpp:60
int _manual_control_setpoint_sub
Definition: input_rc.h:86
virtual bool _read_control_data_from_subscription(ControlData &control_data, bool already_active)
Definition: input_rc.cpp:107
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
static int timeout_ms
ControlData _control_data
Definition: input.h:83
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
Definition: common.h:45
int _aux_channels[3]
Definition: input_rc.h:85
virtual int initialize()
Definition: input_rc.cpp:67
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
bool gimbal_shutter_retract
whether to lock the gimbal (only in RC output mode)
Definition: common.h:88
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
Definition: input_rc.cpp:78
float angles[3]
attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] if is_speed[i] == false
Definition: common.h:68
InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
Definition: input_rc.cpp:52
This defines the common API between an input and an output of the vmount driver.
Definition: common.h:55
virtual void print_status()
report status to stdout
Definition: input_rc.cpp:177
bool is_speed[3]
if true, the angle is the angular speed in rad/s
Definition: common.h:70
control the roll, pitch & yaw angle directly
const bool _do_stabilization
Definition: input_rc.h:84
struct vmount::ControlData::TypeData::TypeAngle angle
#define M_PI_F
Definition: ashtech.cpp:44
bool _first_time
Definition: input_rc.h:88
float _last_set_aux_values[3]
Definition: input_rc.h:89
bool stabilize_axis[3]
whether the vmount driver should stabilize an axis (if the output supports it, this can also be done ...
Definition: common.h:85
float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx)
Definition: input_rc.cpp:150
union vmount::ControlData::TypeData type_data