PX4 Firmware
PX4 Autopilot Software http://px4.io
output_mavlink.cpp
Go to the documentation of this file.
1 /****************************************************************************
2 *
3 * Copyright (c) 2016-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 output_mavlink.cpp
36  * @author Leon Müller (thedevleon)
37  * @author Beat Küng <beat-kueng@gmx.net>
38  *
39  */
40 
41 #include "output_mavlink.h"
42 
43 #include <math.h>
44 
46 #include <px4_platform_common/defines.h>
47 
48 
49 namespace vmount
50 {
51 
53  : OutputBase(output_config)
54 {
55 }
56 
57 int OutputMavlink::update(const ControlData *control_data)
58 {
59  vehicle_command_s vehicle_command{};
60  vehicle_command.timestamp = hrt_absolute_time();
61  vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id;
62  vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id;
63 
64  if (control_data) {
65  //got new command
66  _set_angle_setpoints(control_data);
67 
68  vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
69  vehicle_command.timestamp = hrt_absolute_time();
70 
71  if (control_data->type == ControlData::Type::Neutral) {
72  vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
73 
74  } else {
75  vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
76  }
77 
78  _vehicle_command_pub.publish(vehicle_command);
79  }
80 
82 
85 
86  vehicle_command.timestamp = t;
87  vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
88 
89  // vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively
90  // vmount uses radians, MAVLink uses degrees
91  vehicle_command.param1 = (_angle_outputs[1] + _config.pitch_offset) * M_RAD_TO_DEG_F;
92  vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F;
93  vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F;
94  vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING;
95 
96  _vehicle_command_pub.publish(vehicle_command);
97 
98  _last_update = t;
99 
100  return 0;
101 }
102 
104 {
105  PX4_INFO("Output: Mavlink");
106 }
107 
108 } /* namespace vmount */
109 
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
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
const OutputConfig & _config
Definition: output.h:103
bool publish(const T &data)
Publish the struct.
void _calculate_output_angles(const hrt_abstime &t)
calculate the _angle_outputs (with speed) and stabilize if needed
Definition: output.cpp:201
move to neutral position
float yaw_offset
Offset for yaw channel in radians.
Definition: output.h:65
Definition: common.h:45
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
uint32_t mavlink_comp_id
Definition: output.h:68
void _set_angle_setpoints(const ControlData *control_data)
set angle setpoints, speeds & stabilize flags
Definition: output.cpp:122
float _angle_outputs[3]
calculated output angles (roll, pitch, yaw) [rad]
Definition: output.h:119
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
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).