PX4 Firmware
PX4 Autopilot Software http://px4.io
output.h
Go to the documentation of this file.
1 /****************************************************************************
2 *
3 * Copyright (c) 2016 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 output.h
36  * @author Beat Küng <beat-kueng@gmx.net>
37  *
38  */
39 
40 #pragma once
41 
42 #include "common.h"
43 #include <drivers/drv_hrt.h>
44 #include <lib/ecl/geo/geo.h>
45 #include <uORB/uORB.h>
47 
48 
49 namespace vmount
50 {
51 
52 struct OutputConfig {
53  float gimbal_retracted_mode_value; /**< Mixer output value for selecting gimbal retracted mode */
54  float gimbal_normal_mode_value; /**< Mixer output value for selecting gimbal normal mode */
55 
56  /** Scale factor for pitch channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
57  float pitch_scale;
58  /** Scale factor for roll channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
59  float roll_scale;
60  /** Scale factor for yaw channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
61  float yaw_scale;
62 
63  float pitch_offset; /**< Offset for pitch channel in radians */
64  float roll_offset; /**< Offset for roll channel in radians */
65  float yaw_offset; /**< Offset for yaw channel in radians */
66 
67  uint32_t mavlink_sys_id; /**< Mavlink target system id for mavlink output */
68  uint32_t mavlink_comp_id;
69 };
70 
71 
72 /**
73  ** class OutputBase
74  * Base class for all driver output classes
75  */
77 {
78 public:
79  OutputBase(const OutputConfig &output_config);
80  virtual ~OutputBase();
81 
82  virtual int initialize();
83 
84  /**
85  * Update the output.
86  * @param data new command if non null
87  * @return 0 on success, <0 otherwise
88  */
89  virtual int update(const ControlData *control_data) = 0;
90 
91  /** report status to stdout */
92  virtual void print_status() = 0;
93 
94  /** Publish _angle_outputs as a mount_orientation message. */
95  void publish();
96 
97 protected:
98  float _calculate_pitch(double lon, double lat, float altitude,
99  const vehicle_global_position_s &global_position);
100 
101  map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
102 
104 
105  /** set angle setpoints, speeds & stabilize flags */
106  void _set_angle_setpoints(const ControlData *control_data);
107 
108  /** check if vehicle position changed and update the setpoint angles if in gps mode */
109  void _handle_position_update(bool force_update = false);
110 
111  const ControlData *_cur_control_data = nullptr;
112  float _angle_setpoints[3] = { 0.f, 0.f, 0.f }; ///< [rad]
113  float _angle_speeds[3] = { 0.f, 0.f, 0.f };
114  bool _stabilize[3] = { false, false, false };
115 
116  /** calculate the _angle_outputs (with speed) and stabilize if needed */
117  void _calculate_output_angles(const hrt_abstime &t);
118 
119  float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad]
121 
122  int _get_vehicle_attitude_sub() const { return _vehicle_attitude_sub; }
123 
124 private:
125  int _vehicle_attitude_sub = -1;
126  int _vehicle_global_position_sub = -1;
127 
128  orb_advert_t _mount_orientation_pub = nullptr;
129 };
130 
131 
132 } /* namespace vmount */
uint32_t mavlink_sys_id
Mavlink target system id for mavlink output.
Definition: output.h:67
float pitch_offset
Offset for pitch channel in radians.
Definition: output.h:63
float yaw_scale
Scale factor for yaw channel (maps from angle in radians to actuator output in [-1,1]).
Definition: output.h:61
API for the uORB lightweight object broker.
Definition of geo / math functions to perform geodesic calculations.
const OutputConfig & _config
Definition: output.h:103
int _get_vehicle_attitude_sub() const
Definition: output.h:122
void print_status()
Definition: Commander.cpp:517
float yaw_offset
Offset for yaw channel in radians.
Definition: output.h:65
Definition: common.h:45
High-resolution timer with callouts and timekeeping.
float gimbal_normal_mode_value
Mixer output value for selecting gimbal normal mode.
Definition: output.h:54
float gimbal_retracted_mode_value
Mixer output value for selecting gimbal retracted mode.
Definition: output.h:53
hrt_abstime _last_update
Definition: output.h:120
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
This defines the common API between an input and an output of the vmount driver.
Definition: common.h:55
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
uint32_t mavlink_comp_id
Definition: output.h:68
float pitch_scale
Scale factor for pitch channel (maps from angle in radians to actuator output in [-1,1]).
Definition: output.h:57
float roll_scale
Scale factor for roll channel (maps from angle in radians to actuator output in [-1,1]).
Definition: output.h:59
class OutputBase Base class for all driver output classes
Definition: output.h:76
float roll_offset
Offset for roll channel in radians.
Definition: output.h:64