mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 18:23:33 +08:00
Implement RC and DL failsafe action handling for multirotors
Move RC and DL failsafe actions handling from navigator to commander (credits to @AndreasAntener) Separate manual kill switch handling via manual_lockdown to prevent override and release of software lockdown by RC switch Other changes: Add failsafe tune Fix LED blinking for Pixracer Return back support for rc inputs in simulator but now it is configurable via cmake
This commit is contained in:
Anton Matosov
committed by
Lorenz Meier
Lorenz Meier
parent
964dabe179
commit
3a17c07b1e
@@ -30,6 +30,23 @@
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
option(ENABLE_UART_RC_INPUT "Enable RC Input from UART mavlink connection" OFF)
|
||||
|
||||
if(ENABLE_UART_RC_INPUT)
|
||||
if (APPLE)
|
||||
set(PIXHAWK_DEVICE "/dev/cu.usbmodem1")
|
||||
elseif (UNIX AND NOT APPLE)
|
||||
set(PIXHAWK_DEVICE "/dev/ttyACM0")
|
||||
elseif (WIN32)
|
||||
set(PIXHAWK_DEVICE "COM3")
|
||||
endif()
|
||||
|
||||
set(PIXHAWK_DEVICE_BAUD 115200)
|
||||
endif()
|
||||
configure_file(simulator_config.h.in simulator_config.h @ONLY)
|
||||
include_directories(${CMAKE_CURRENT_BINARY_DIR})
|
||||
|
||||
set(SIMULATOR_SRCS simulator.cpp)
|
||||
if (NOT ${OS} STREQUAL "qurt")
|
||||
list(APPEND SIMULATOR_SRCS
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 Anton Matosov. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#cmakedefine ENABLE_UART_RC_INPUT
|
||||
|
||||
#cmakedefine PIXHAWK_DEVICE "@PIXHAWK_DEVICE@"
|
||||
#cmakedefine PIXHAWK_DEVICE_BAUD @PIXHAWK_DEVICE_BAUD@
|
||||
@@ -1,6 +1,7 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
|
||||
* Copyright (c) 2016 Anton Matosov. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -34,6 +35,7 @@
|
||||
#include <px4_log.h>
|
||||
#include <px4_time.h>
|
||||
#include "simulator.h"
|
||||
#include <simulator_config.h>
|
||||
#include "errno.h"
|
||||
#include <geo/geo.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
@@ -47,8 +49,15 @@ extern "C" __EXPORT hrt_abstime hrt_reset(void);
|
||||
|
||||
#define SEND_INTERVAL 20
|
||||
#define UDP_PORT 14560
|
||||
#define PIXHAWK_DEVICE "/dev/ttyACM0"
|
||||
|
||||
#define PRESS_GROUND 101325.0f
|
||||
#define DENSITY 1.2041f
|
||||
|
||||
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
|
||||
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
|
||||
static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
#ifndef B460800
|
||||
#define B460800 460800
|
||||
#endif
|
||||
@@ -57,12 +66,8 @@ extern "C" __EXPORT hrt_abstime hrt_reset(void);
|
||||
#define B921600 921600
|
||||
#endif
|
||||
|
||||
#define PRESS_GROUND 101325.0f
|
||||
#define DENSITY 1.2041f
|
||||
|
||||
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
|
||||
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
|
||||
static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
|
||||
static int openUart(const char *uart_name, int baud);
|
||||
#endif
|
||||
|
||||
static int _fd;
|
||||
static unsigned char _buf[1024];
|
||||
@@ -657,6 +662,25 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
|
||||
fds[0].fd = _fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
// setup serial connection to autopilot (used to get manual controls)
|
||||
int serial_fd = openUart(PIXHAWK_DEVICE, PIXHAWK_DEVICE_BAUD);
|
||||
|
||||
char serial_buf[1024];
|
||||
|
||||
if (serial_fd >= 0) {
|
||||
fds[1].fd = serial_fd;
|
||||
fds[1].events = POLLIN;
|
||||
fd_count++;
|
||||
|
||||
PX4_INFO("Start using %s for radio control input.", PIXHAWK_DEVICE);
|
||||
|
||||
} else {
|
||||
PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
int len = 0;
|
||||
|
||||
// wait for first data from simulator and respond with first controls
|
||||
@@ -782,9 +806,131 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
|
||||
// got data from PIXHAWK
|
||||
if (fd_count > 1 && fds[1].revents & POLLIN) {
|
||||
len = ::read(serial_fd, serial_buf, sizeof(serial_buf));
|
||||
|
||||
if (len > 0) {
|
||||
mavlink_message_t msg;
|
||||
|
||||
mavlink_status_t serial_status = {};
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
|
||||
// have a message, handle it
|
||||
handle_message(&msg, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
int openUart(const char *uart_name, int baud)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 0: speed = B0; break;
|
||||
|
||||
case 50: speed = B50; break;
|
||||
|
||||
case 75: speed = B75; break;
|
||||
|
||||
case 110: speed = B110; break;
|
||||
|
||||
case 134: speed = B134; break;
|
||||
|
||||
case 150: speed = B150; break;
|
||||
|
||||
case 200: speed = B200; break;
|
||||
|
||||
case 300: speed = B300; break;
|
||||
|
||||
case 600: speed = B600; break;
|
||||
|
||||
case 1200: speed = B1200; break;
|
||||
|
||||
case 1800: speed = B1800; break;
|
||||
|
||||
case 2400: speed = B2400; break;
|
||||
|
||||
case 4800: speed = B4800; break;
|
||||
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
case 460800: speed = B460800; break;
|
||||
|
||||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
|
||||
baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* open uart */
|
||||
int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (uart_fd < 0) {
|
||||
return uart_fd;
|
||||
}
|
||||
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
memset(&uart_config, 0, sizeof(uart_config));
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
|
||||
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
|
||||
::close(uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
tcgetattr(uart_fd, &uart_config);
|
||||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
|
||||
::close(uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Make raw
|
||||
cfmakeraw(&uart_config);
|
||||
|
||||
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERR SET CONF %s\n", uart_name);
|
||||
::close(uart_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return uart_fd;
|
||||
}
|
||||
#endif
|
||||
|
||||
int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
|
||||
{
|
||||
|
||||
|
||||
Reference in New Issue
Block a user