PX4 Firmware
PX4 Autopilot Software http://px4.io
output.cpp
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.cpp
36  * @author Leon Müller (thedevleon)
37  * @author Beat Küng <beat-kueng@gmx.net>
38  *
39  */
40 
41 #include "output.h"
42 #include <errno.h>
43 
48 #include <px4_platform_common/defines.h>
49 #include <lib/ecl/geo/geo.h>
50 #include <math.h>
51 #include <mathlib/mathlib.h>
52 #include <matrix/math.hpp>
53 
54 using matrix::wrap_pi;
55 
56 namespace vmount
57 {
58 
59 OutputBase::OutputBase(const OutputConfig &output_config)
60  : _config(output_config)
61 {
63 }
64 
66 {
67  if (_vehicle_attitude_sub >= 0) {
69  }
70 
73  }
74 
77  }
78 }
79 
81 {
82  if ((_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude))) < 0) {
83  return -errno;
84  }
85 
86  if ((_vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position))) < 0) {
87  return -errno;
88  }
89 
90  return 0;
91 }
92 
94 {
95  int instance;
96  mount_orientation_s mount_orientation;
97 
98  for (unsigned i = 0; i < 3; ++i) {
99  mount_orientation.attitude_euler_angle[i] = _angle_outputs[i];
100  }
101 
102  orb_publish_auto(ORB_ID(mount_orientation), &_mount_orientation_pub, &mount_orientation, &instance, ORB_PRIO_DEFAULT);
103 }
104 
105 float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
106  const vehicle_global_position_s &global_position)
107 {
109  map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
110  }
111 
112  float x1, y1, x2, y2;
113  map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
114  map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
115  float dx = x1 - x2, dy = y1 - y2;
116  float target_distance = sqrtf(dx * dx + dy * dy);
117  float z = altitude - global_position.alt;
118 
119  return atan2f(z, target_distance);
120 }
121 
123 {
124  _cur_control_data = control_data;
125 
126  for (int i = 0; i < 3; ++i) {
127  _stabilize[i] = control_data->stabilize_axis[i];
128  _angle_speeds[i] = 0.f;
129  }
130 
131  switch (control_data->type) {
133  for (int i = 0; i < 3; ++i) {
134  if (control_data->type_data.angle.is_speed[i]) {
135  _angle_speeds[i] = control_data->type_data.angle.angles[i];
136 
137  } else {
138  _angle_setpoints[i] = control_data->type_data.angle.angles[i];
139  }
140  }
141 
142  break;
143 
146  break;
147 
149  _angle_setpoints[0] = 0.f;
150  _angle_setpoints[1] = 0.f;
151  _angle_setpoints[2] = 0.f;
152  break;
153  }
154 }
155 
156 void OutputBase::_handle_position_update(bool force_update)
157 {
158  bool need_update = force_update;
159 
161  return;
162  }
163 
164  if (!force_update) {
166  }
167 
168  if (!need_update) {
169  return;
170  }
171 
172  vehicle_global_position_s vehicle_global_position;
173  orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position);
174  const double &vlat = vehicle_global_position.lat;
175  const double &vlon = vehicle_global_position.lon;
176 
177  const double &lat = _cur_control_data->type_data.lonlat.lat;
178  const double &lon = _cur_control_data->type_data.lonlat.lon;
179  const float &alt = _cur_control_data->type_data.lonlat.altitude;
180 
182 
183  // interface: use fixed pitch value > -pi otherwise consider ROI altitude
186 
187  } else {
188  _angle_setpoints[1] = _calculate_pitch(lon, lat, alt, vehicle_global_position);
189  }
190 
191  _angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_global_position.yaw;
192 
193  // add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
196 
197  // make sure yaw is wrapped correctly for the output
199 }
200 
202 {
203  //take speed into account
204  float dt = (t - _last_update) / 1.e6f;
205 
206  for (int i = 0; i < 3; ++i) {
207  _angle_setpoints[i] += dt * _angle_speeds[i];
208  }
209 
210  //get the output angles and stabilize if necessary
211  vehicle_attitude_s vehicle_attitude;
212  matrix::Eulerf euler;
213 
214  if (_stabilize[0] || _stabilize[1] || _stabilize[2]) {
215  orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
216  euler = matrix::Quatf(vehicle_attitude.q);
217  }
218 
219  for (int i = 0; i < 3; ++i) {
220  if (_stabilize[i]) {
221  _angle_outputs[i] = _angle_setpoints[i] - euler(i);
222 
223  } else {
225  }
226 
227  //bring angles into proper range [-pi, pi]
228  while (_angle_outputs[i] > M_PI_F) { _angle_outputs[i] -= 2.f * M_PI_F; }
229 
230  while (_angle_outputs[i] < -M_PI_F) { _angle_outputs[i] += 2.f * M_PI_F; }
231  }
232 }
233 
234 } /* namespace vmount */
235 
struct vmount::ControlData::TypeData::TypeLonLat lonlat
float yaw_angle_offset
angular offset for yaw [rad]
Definition: common.h:79
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
double lon
longitude in [deg]
Definition: common.h:74
const ControlData * _cur_control_data
Definition: output.h:111
float _calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position)
Definition: output.cpp:105
int _vehicle_attitude_sub
Definition: output.h:125
bool _stabilize[3]
Definition: output.h:114
int _vehicle_global_position_sub
Definition: output.h:126
Definition of geo / math functions to perform geodesic calculations.
virtual int initialize()
Definition: output.cpp:80
bool map_projection_initialized(const struct map_projection_reference_s *ref)
Checks if projection given as argument was initialized.
Definition: geo.cpp:68
void _handle_position_update(bool force_update=false)
check if vehicle position changed and update the setpoint angles if in gps mode
Definition: output.cpp:156
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
void _calculate_output_angles(const hrt_abstime &t)
calculate the _angle_outputs (with speed) and stabilize if needed
Definition: output.cpp:201
float altitude
altitude in [m]
Definition: common.h:76
move to neutral position
LidarLite * instance
Definition: ll40ls.cpp:65
orb_advert_t _mount_orientation_pub
Definition: output.h:128
Definition: common.h:45
float roll_angle
roll is set to a fixed angle.
Definition: common.h:77
float _angle_speeds[3]
Definition: output.h:113
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
double lat
latitude in [deg]
Definition: common.h:75
OutputBase(const OutputConfig &output_config)
Definition: output.cpp:59
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
float angles[3]
attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] if is_speed[i] == false
Definition: common.h:68
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
Returns the bearing to the next waypoint in radians.
Definition: geo.cpp:320
Euler angles class.
Definition: AxisAngle.hpp:18
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
virtual ~OutputBase()
Definition: output.cpp:65
Type wrap_pi(Type x)
Wrap value in range [-π, π)
void _set_angle_setpoints(const ControlData *control_data)
set angle setpoints, speeds & stabilize flags
Definition: output.cpp:122
float _angle_setpoints[3]
[rad]
Definition: output.h:112
float _angle_outputs[3]
calculated output angles (roll, pitch, yaw) [rad]
Definition: output.h:119
bool is_speed[3]
if true, the angle is the angular speed in rad/s
Definition: common.h:70
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
void publish()
Publish _angle_outputs as a mount_orientation message.
Definition: output.cpp:93
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
Definition: geo.cpp:132
float dt
Definition: px4io.c:73
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
control the roll, pitch & yaw angle directly
struct vmount::ControlData::TypeData::TypeAngle angle
#define M_PI_F
Definition: ashtech.cpp:44
map_projection_reference_s _projection_reference
reference to convert (lon, lat) to local [m]
Definition: output.h:101
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0)
Initializes the map transformation given by the argument and sets the timestamp to now...
Definition: geo.cpp:105
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
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
Definition: uORB.h:177
union vmount::ControlData::TypeData type_data