PX4 Firmware
PX4 Autopilot Software http://px4.io
input.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.cpp
36  * @author Beat Küng <beat-kueng@gmx.net>
37  *
38  */
39 
40 #include "input.h"
41 
42 
43 namespace vmount
44 {
45 
46 int InputBase::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
47 {
48  if (!_initialized) {
49  int ret = initialize();
50 
51  if (ret) {
52  return ret;
53  }
54 
55  //on startup, set the mount to a neutral position
58  *control_data = &_control_data;
59  _initialized = true;
60  return 0;
61  }
62 
63  return update_impl(timeout_ms, control_data, already_active);
64 }
65 
66 void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle,
67  float pitch_fixed_angle)
68 {
74  _control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle;
77 }
78 
79 } /* namespace vmount */
80 
struct vmount::ControlData::TypeData::TypeLonLat lonlat
float yaw_angle_offset
angular offset for yaw [rad]
Definition: common.h:79
double lon
longitude in [deg]
Definition: common.h:74
virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
Wait for an input update, with a timeout.
Definition: input.cpp:46
static int timeout_ms
float pitch_angle_offset
angular offset for pitch [rad]
Definition: common.h:78
float pitch_fixed_angle
ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude
Definition: common.h:80
ControlData _control_data
Definition: input.h:83
float altitude
altitude in [m]
Definition: common.h:76
move to neutral position
Definition: common.h:45
float roll_angle
roll is set to a fixed angle.
Definition: common.h:77
bool _initialized
Definition: input.h:86
bool gimbal_shutter_retract
whether to lock the gimbal (only in RC output mode)
Definition: common.h:88
virtual int initialize()
Definition: input.h:78
double lat
latitude in [deg]
Definition: common.h:75
This defines the common API between an input and an output of the vmount driver.
Definition: common.h:55
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)=0
void control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle=0.f, float pitch_fixed_angle=-10.f)
Definition: input.cpp:66
union vmount::ControlData::TypeData type_data