PX4 Firmware
PX4 Autopilot Software http://px4.io
commander_helper.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013, 2014 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 commander_helper.cpp
36  * Commander helper functions implementations
37  *
38  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
39  * @author Julian Oes <julian@oes.ch>
40  * @author Anton Babushkin <anton.babushkin@me.com>
41  *
42  */
43 
44 #include <px4_platform_common/defines.h>
45 #include <px4_platform_common/posix.h>
46 #include <stdio.h>
47 #include <unistd.h>
48 #include <stdint.h>
49 #include <stdbool.h>
50 #include <fcntl.h>
51 #include <math.h>
52 #include <string.h>
53 
54 #include <uORB/uORB.h>
60 #include <systemlib/err.h>
61 #include <parameters/param.h>
62 #include <drivers/drv_hrt.h>
63 #include <drivers/drv_tone_alarm.h>
64 
65 #include "commander_helper.h"
66 #include "DevMgr.hpp"
67 
68 using namespace DriverFramework;
69 
70 #define VEHICLE_TYPE_FIXED_WING 1
71 #define VEHICLE_TYPE_QUADROTOR 2
72 #define VEHICLE_TYPE_COAXIAL 3
73 #define VEHICLE_TYPE_HELICOPTER 4
74 #define VEHICLE_TYPE_GROUND_ROVER 10
75 #define VEHICLE_TYPE_HEXAROTOR 13
76 #define VEHICLE_TYPE_OCTOROTOR 14
77 #define VEHICLE_TYPE_TRICOPTER 15
78 #define VEHICLE_TYPE_VTOL_DUOROTOR 19
79 #define VEHICLE_TYPE_VTOL_QUADROTOR 20
80 #define VEHICLE_TYPE_VTOL_TILTROTOR 21
81 #define VEHICLE_TYPE_VTOL_RESERVED2 22
82 #define VEHICLE_TYPE_VTOL_RESERVED3 23
83 #define VEHICLE_TYPE_VTOL_RESERVED4 24
84 #define VEHICLE_TYPE_VTOL_RESERVED5 25
85 
86 #define BLINK_MSG_TIME 700000 // 3 fast blinks (in us)
87 
88 bool is_multirotor(const struct vehicle_status_s *current_status)
89 {
90  return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
91  (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
92  (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
93  (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
94 }
95 
96 bool is_rotary_wing(const struct vehicle_status_s *current_status)
97 {
98  return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
99  || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
100 }
101 
102 bool is_vtol(const struct vehicle_status_s *current_status)
103 {
104  return (current_status->system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
105  current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR ||
106  current_status->system_type == VEHICLE_TYPE_VTOL_TILTROTOR ||
107  current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED2 ||
108  current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED3 ||
109  current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED4 ||
110  current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED5);
111 }
112 
113 bool is_vtol_tailsitter(const struct vehicle_status_s *current_status)
114 {
115  return (current_status->system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
116  current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
117 }
118 
119 bool is_fixed_wing(const struct vehicle_status_s *current_status)
120 {
121  return current_status->system_type == VEHICLE_TYPE_FIXED_WING;
122 }
123 
124 bool is_ground_rover(const struct vehicle_status_s *current_status)
125 {
126  return current_status->system_type == VEHICLE_TYPE_GROUND_ROVER;
127 }
128 
129 static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message
130 static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence
131 static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end
132 static unsigned int tune_durations[TONE_NUMBER_OF_TUNES] {};
133 
134 static DevHandle h_leds;
135 static DevHandle h_buzzer;
137 static orb_advert_t led_control_pub = nullptr;
140 
142 {
147  tune_durations[TONE_HOME_SET] = 800000;
151  tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH);
152  return PX4_OK;
153 }
154 
156 {
158 }
159 
160 void set_tune_override(int tune)
161 {
162  tune_control.tune_id = tune;
163  tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
167 }
168 
169 void set_tune(int tune)
170 {
171  unsigned int new_tune_duration = tune_durations[tune];
172 
173  /* don't interrupt currently playing non-repeating tune by repeating */
174  if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
175  /* allow interrupting current non-repeating tune by the same tune */
176  if (tune != tune_current || new_tune_duration != 0) {
177  tune_control.tune_id = tune;
178  tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
182  }
183 
184  tune_current = tune;
185 
186  if (new_tune_duration != 0) {
187  tune_end = hrt_absolute_time() + new_tune_duration;
188 
189  } else {
190  tune_end = 0;
191  }
192  }
193 }
194 
195 void tune_home_set(bool use_buzzer)
196 {
198  rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
199 
200  if (use_buzzer) {
202  }
203 }
204 
205 void tune_mission_ok(bool use_buzzer)
206 {
208  rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
209 
210  if (use_buzzer) {
212  }
213 }
214 
215 void tune_mission_fail(bool use_buzzer)
216 {
218  rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
219 
220  if (use_buzzer) {
222  }
223 }
224 
225 /**
226  * Blink green LED and play positive tune (if use_buzzer == true).
227  */
228 void tune_positive(bool use_buzzer)
229 {
231  rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
232 
233  if (use_buzzer) {
235  }
236 }
237 
238 /**
239  * Blink white LED and play neutral tune (if use_buzzer == true).
240  */
241 void tune_neutral(bool use_buzzer)
242 {
244  rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST);
245 
246  if (use_buzzer) {
248  }
249 }
250 
251 /**
252  * Blink red LED and play negative tune (if use_buzzer == true).
253  */
254 void tune_negative(bool use_buzzer)
255 {
257  rgbled_set_color_and_mode(led_control_s::COLOR_RED, led_control_s::MODE_BLINK_FAST);
258 
259  if (use_buzzer) {
261  }
262 }
263 
264 void tune_failsafe(bool use_buzzer)
265 {
267  rgbled_set_color_and_mode(led_control_s::COLOR_PURPLE, led_control_s::MODE_BLINK_FAST);
268 
269  if (use_buzzer) {
271  }
272 }
273 
275 {
276  if (blink_msg_end == 0) {
277  return 0;
278 
279  } else if (hrt_absolute_time() > blink_msg_end) {
280  blink_msg_end = 0;
281  return 2;
282 
283  } else {
284  return 1;
285  }
286 }
287 
288 int led_init()
289 {
290  blink_msg_end = 0;
291 
292  led_control.led_mask = 0xff;
293  led_control.mode = led_control_s::MODE_OFF;
294  led_control.priority = 0;
296  led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
297 
298  /* first open normal LEDs */
299  DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);
300 
301  if (!h_leds.isValid()) {
302  PX4_WARN("LED: getHandle fail\n");
303  return PX4_ERROR;
304  }
305 
306  /* the green LED is only available on FMUv5 */
307  (void)h_leds.ioctl(LED_ON, LED_GREEN);
308 
309  /* the blue LED is only available on AeroCore but not FMUv2 */
310  (void)h_leds.ioctl(LED_ON, LED_BLUE);
311 
312  /* switch blue off */
313  led_off(LED_BLUE);
314 
315  /* we consider the amber led mandatory */
316  if (h_leds.ioctl(LED_ON, LED_AMBER)) {
317  PX4_WARN("Amber LED: ioctl fail\n");
318  return PX4_ERROR;
319  }
320 
321  /* switch amber off */
323 
324  return 0;
325 }
326 
328 {
330  DevMgr::releaseHandle(h_leds);
331 }
332 
333 int led_toggle(int led)
334 {
335  return h_leds.ioctl(LED_TOGGLE, led);
336 }
337 
338 int led_on(int led)
339 {
340  return h_leds.ioctl(LED_ON, led);
341 }
342 
343 int led_off(int led)
344 {
345  return h_leds.ioctl(LED_OFF, led);
346 }
347 
348 void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)
349 {
351  led_control.color = color;
352  led_control.num_blinks = blinks;
353  led_control.priority = prio;
356 }
357 
358 void rgbled_set_color_and_mode(uint8_t color, uint8_t mode)
359 {
360  rgbled_set_color_and_mode(color, mode, 0, 0);
361 }
#define VEHICLE_TYPE_FIXED_WING
void tune_neutral(bool use_buzzer)
Blink white LED and play neutral tune (if use_buzzer == true).
void tune_mission_fail(bool use_buzzer)
API for the uORB lightweight object broker.
uint8_t priority
Definition: led_control.h:77
bool is_fixed_wing(const struct vehicle_status_s *current_status)
static led_control_s led_control
bool is_multirotor(const struct vehicle_status_s *current_status)
static DevHandle h_leds
static orb_advert_t led_control_pub
#define VEHICLE_TYPE_VTOL_RESERVED5
void set_tune_override(int tune)
static orb_advert_t tune_control_pub
int led_off(int led)
int led_init()
#define VEHICLE_TYPE_VTOL_RESERVED2
int led_toggle(int led)
uint8_t led_mask
Definition: led_control.h:73
void tune_positive(bool use_buzzer)
Blink green LED and play positive tune (if use_buzzer == true).
uint8_t color
Definition: led_control.h:74
void led_deinit()
High-resolution timer with callouts and timekeeping.
uint64_t timestamp
Definition: tune_control.h:57
orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
Definition: uORB.cpp:48
Global flash based parameter store.
uint8_t tune_override
Definition: tune_control.h:62
#define LED_AMBER
Definition: drv_board_led.h:52
static hrt_abstime blink_msg_end
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
#define LED_BLUE
Definition: drv_board_led.h:54
static DevHandle h_buzzer
Commander helper functions definitions.
bool is_vtol_tailsitter(const struct vehicle_status_s *current_status)
#define VEHICLE_TYPE_TRICOPTER
uint8_t mode
Definition: led_control.h:75
#define LED_ON
Definition: drv_board_led.h:59
#define LED0_DEVICE_PATH
Definition: drv_board_led.h:47
#define LED_GREEN
Definition: drv_board_led.h:56
#define VEHICLE_TYPE_VTOL_TILTROTOR
#define LED_OFF
Definition: drv_board_led.h:60
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
#define BLINK_MSG_TIME
#define VEHICLE_TYPE_HEXAROTOR
bool is_rotary_wing(const struct vehicle_status_s *current_status)
void tune_mission_ok(bool use_buzzer)
bool is_vtol(const struct vehicle_status_s *current_status)
uint64_t timestamp
Definition: led_control.h:72
bool is_ground_rover(const struct vehicle_status_s *current_status)
int blink_msg_state()
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
Driver for the PX4 audio alarm port, /dev/tone_alarm.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
#define VEHICLE_TYPE_VTOL_RESERVED3
#define VEHICLE_TYPE_GROUND_ROVER
uint8_t num_blinks
Definition: led_control.h:76
#define VEHICLE_TYPE_VTOL_RESERVED4
#define VEHICLE_TYPE_QUADROTOR
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)
#define LED_TOGGLE
Definition: drv_board_led.h:61
#define VEHICLE_TYPE_VTOL_QUADROTOR
static hrt_abstime tune_end
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
#define VEHICLE_TYPE_VTOL_DUOROTOR
int buzzer_init()
#define VEHICLE_TYPE_COAXIAL
static tune_control_s tune_control
#define VEHICLE_TYPE_HELICOPTER
void set_tune(int tune)
mode
Definition: vtol_type.h:76
void tune_failsafe(bool use_buzzer)
static int tune_current
#define VEHICLE_TYPE_OCTOROTOR
void buzzer_deinit()
static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]
int led_on(int led)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void tune_negative(bool use_buzzer)
Blink red LED and play negative tune (if use_buzzer == true).
uint8_t tune_id
Definition: tune_control.h:61
void tune_home_set(bool use_buzzer)
uint8_t volume
Definition: tune_control.h:63