Compare commits

...

12 Commits

Author SHA1 Message Date
Matthias Grob b5f6699f2e mixer_module: send a last sample out after all outputs were disabled
This matters for PWM when the last output gets disabled on either FMU or IO
it would just keep on running.

Also when rebooting with a parameters reset or new airframe with no mapped outputs
it would previously keep outputting PWM with the disarmed value of the new airframe
e.g. 1000us which is a safety hazard because servos could break the physical limit of the
model or miscalibrated ESCs spinning motors.
2024-03-25 19:21:54 +01:00
Matthias Grob 1096384a38 px4iofirmware: don't switch to disarmed or failsafe value on disabled PWM outputs
If the output is set to 0 then the FMU had this channel disabled/no function mapped
to it. In that case we do not want to suddenly start outputing failsafe or disarmed
signals.
2024-03-25 19:21:54 +01:00
Matthias Grob 999a71c4dd px4io: don't output on disabled PWM pins
Same logic as on the FMU PWM updateOutputs() in PWMOut.cpp
2024-03-25 19:21:54 +01:00
Thomas Frans bcbae86b9f code: add more style options in `.editorconfig` 2024-03-25 09:48:09 -04:00
Eric Katzfey 4a553938fb
VOXL2: HRT updates for synchronization with Qurt time (#22881)
- Added offset to Posix hrt time to account for synchronization with Qurt hrt time
- Added new Kconfig to configure synchronization of HRT timestamps on VOXL2
- Moved voxl2 libfc sensor library submodule from muorb module to boards directory
- Added check to make sure hrt_elapsed_time can never be negative
2024-03-22 15:24:51 -04:00
Daniel Agar c024ea396a boards/px4/fmu-v5x: remove legacy rover_pos_control to reduce flash usage 2024-03-22 15:17:03 -04:00
Eric Katzfey 69028f37a9
New platform independent Serial interface (#21723) 2024-03-21 21:00:23 -04:00
Thomas Frans bb9f4d42f3 gps: fix incorrect task id in module startup 2024-03-21 20:58:59 -04:00
Beat Küng 2e12e14a23 boards/px4/fmu-v5x: remove sd_stress & reflect to reduce flash usage 2024-03-21 20:58:21 -04:00
Thomas Frans d0251b8688
add `.editorconfig` for consistent code style across editors (#22916)
EditorConfig is a well-known convention to share style settings across
different editors. Adding one will make it easier for new contributors
or people who like to use a different editor to contribute.
2024-03-21 20:56:20 -04:00
Eric Katzfey 82a1aa37db
uORB: fix for uORB communicator, only send most recent data for new subscription (#22893) 2024-03-21 20:54:43 -04:00
Eric Katzfey 5f6dc1c5d0
uORB: SubscriptionBlocking purged the broken attempt to set the mutex protocol in constructor 2024-03-21 20:53:34 -04:00
53 changed files with 2035 additions and 222 deletions

14
.editorconfig Normal file
View File

@ -0,0 +1,14 @@
root = true
[*]
insert_final_newline = false
[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
indent_style = tab
tab_width = 8
# Not in the official standard, but supported by many editors
max_line_length = 120
[*.yaml]
indent_style = space
indent_size = 2

2
.gitmodules vendored
View File

@ -81,5 +81,5 @@
url = https://github.com/PX4/PX4-gazebo-models.git
branch = main
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
path = src/modules/muorb/apps/libfc-sensor-api
path = boards/modalai/voxl2/libfc-sensor-api
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git

View File

@ -30,5 +30,5 @@ exec find boards msg src platforms test \
-path src/lib/cdrstream/cyclonedds -prune -o \
-path src/lib/cdrstream/rosidl -prune -o \
-path src/modules/zenoh/zenoh-pico -prune -o \
-path src/modules/muorb/apps/libfc-sensor-api -prune -o \
-path boards/modalai/voxl2/libfc-sensor-api -prune -o \
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

View File

@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y

View File

@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y

View File

@ -0,0 +1,34 @@
############################################################################
#
# Copyright (c) 2024 ModalAI, Inc. 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.
#
############################################################################
include_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)

View File

@ -1,7 +1,7 @@
# Link against the public stub version of the proprietary fc sensor library
target_link_libraries(px4 PRIVATE
${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
px4_layer
${module_libraries}
)

View File

@ -1,10 +1,9 @@
#!/bin/bash
cd src/modules/muorb/apps/libfc-sensor-api
cd boards/modalai/voxl2/libfc-sensor-api
rm -fR build
mkdir build
cd build
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
make
cd ../../../../../..
cd ../../../../..

View File

@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
CONFIG_MODULES_PAYLOAD_DELIVERER=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@ -102,7 +101,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_REBOOT=y
CONFIG_SYSTEMCMDS_REFLECT=y
CONFIG_SYSTEMCMDS_SD_BENCH=y
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
CONFIG_SYSTEMCMDS_TOP=y

View File

@ -52,6 +52,7 @@ add_library(px4_platform STATIC
shutdown.cpp
spi.cpp
pab_manifest.c
Serial.cpp
${SRCS}
)
target_link_libraries(px4_platform prebuild_targets px4_work_queue)

138
platforms/common/Serial.cpp Normal file
View File

@ -0,0 +1,138 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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.
*
****************************************************************************/
#include <px4_platform_common/Serial.hpp>
namespace device
{
Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
{
// If no baudrate was specified then set it to a reasonable default value
if (baudrate == 0) {
(void) _impl.setBaudrate(9600);
}
}
Serial::~Serial()
{
}
bool Serial::open()
{
return _impl.open();
}
bool Serial::isOpen() const
{
return _impl.isOpen();
}
bool Serial::close()
{
return _impl.close();
}
ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
{
return _impl.read(buffer, buffer_size);
}
ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
{
return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
}
ssize_t Serial::write(const void *buffer, size_t buffer_size)
{
return _impl.write(buffer, buffer_size);
}
uint32_t Serial::getBaudrate() const
{
return _impl.getBaudrate();
}
bool Serial::setBaudrate(uint32_t baudrate)
{
return _impl.setBaudrate(baudrate);
}
ByteSize Serial::getBytesize() const
{
return _impl.getBytesize();
}
bool Serial::setBytesize(ByteSize bytesize)
{
return _impl.setBytesize(bytesize);
}
Parity Serial::getParity() const
{
return _impl.getParity();
}
bool Serial::setParity(Parity parity)
{
return _impl.setParity(parity);
}
StopBits Serial::getStopbits() const
{
return _impl.getStopbits();
}
bool Serial::setStopbits(StopBits stopbits)
{
return _impl.setStopbits(stopbits);
}
FlowControl Serial::getFlowcontrol() const
{
return _impl.getFlowcontrol();
}
bool Serial::setFlowcontrol(FlowControl flowcontrol)
{
return _impl.setFlowcontrol(flowcontrol);
}
const char *Serial::getPort() const
{
return _impl.getPort();
}
} // namespace device

View File

@ -0,0 +1,97 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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
#include <SerialImpl.hpp>
#include <px4_platform_common/SerialCommon.hpp>
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
namespace device __EXPORT
{
class Serial
{
public:
Serial(const char *port, uint32_t baudrate = 57600,
ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
virtual ~Serial();
// Open sets up the port and gets it configured based on desired configuration
bool open();
bool isOpen() const;
bool close();
ssize_t read(uint8_t *buffer, size_t buffer_size);
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
ssize_t write(const void *buffer, size_t buffer_size);
// If port is already open then the following configuration functions
// will reconfigure the port. If the port is not yet open then they will
// simply store the configuration in preparation for the port to be opened.
uint32_t getBaudrate() const;
bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const;
bool setBytesize(ByteSize bytesize);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
const char *getPort() const;
private:
// Disable copy constructors
Serial(const Serial &);
Serial &operator=(const Serial &);
// platform implementation
SerialImpl _impl;
};
} // namespace device

View File

@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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
namespace device
{
namespace SerialConfig
{
// ByteSize: number of data bits
enum class ByteSize {
FiveBits = 5,
SixBits = 6,
SevenBits = 7,
EightBits = 8,
};
// Parity: enable parity checking
enum class Parity {
None = 0,
Odd = 1,
Even = 2,
};
// StopBits: number of stop bits
enum class StopBits {
One = 1,
Two = 2
};
// FlowControl: enable flow control
enum class FlowControl {
Disabled = 0,
Enabled = 1,
};
} // namespace SerialConfig
} // namespace device

View File

@ -13,11 +13,21 @@ __END_DECLS
#define px4_clock_gettime system_clock_gettime
#endif
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)
__BEGIN_DECLS
__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
__END_DECLS
#else
#define px4_clock_settime system_clock_settime
#endif
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
__BEGIN_DECLS
__EXPORT int px4_usleep(useconds_t usec);
__EXPORT unsigned int px4_sleep(unsigned int seconds);
__EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
@ -27,7 +37,6 @@ __END_DECLS
#else
#define px4_clock_settime system_clock_settime
#define px4_usleep system_usleep
#define px4_sleep system_sleep
#define px4_pthread_cond_timedwait system_pthread_cond_timedwait

View File

@ -56,31 +56,6 @@ public:
SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
SubscriptionCallback(meta, interval_us, instance)
{
// pthread_mutexattr_init
pthread_mutexattr_t attr;
int ret_attr_init = pthread_mutexattr_init(&attr);
if (ret_attr_init != 0) {
PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
}
#if defined(PTHREAD_PRIO_NONE)
// pthread_mutexattr_settype
// PTHREAD_PRIO_NONE not available on cygwin
int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
if (ret_mutexattr_settype != 0) {
PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
}
#endif // PTHREAD_PRIO_NONE
// pthread_mutex_init
int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
if (ret_mutex_init != 0) {
PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init);
}
}
virtual ~SubscriptionBlocking()

View File

@ -412,7 +412,10 @@ int16_t uORB::DeviceNode::process_add_subscription()
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
ch->send_message(_meta->o_name, _meta->o_size, _data);
// Only send the most recent data to initialize the remote end.
if (_data_valid) {
ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue)));
}
}
return PX4_OK;

View File

@ -0,0 +1,394 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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.
*
****************************************************************************/
#include <SerialImpl.hpp>
#include <string.h> // strncpy
#include <termios.h>
#include <px4_log.h>
#include <fcntl.h>
#include <errno.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#define MODULE_NAME "SerialImpl"
namespace device
{
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
{
if (port) {
strncpy(_port, port, sizeof(_port) - 1);
_port[sizeof(_port) - 1] = '\0';
} else {
_port[0] = 0;
}
}
SerialImpl::~SerialImpl()
{
if (isOpen()) {
close();
}
}
bool SerialImpl::validateBaudrate(uint32_t baudrate)
{
return ((baudrate == 9600) ||
(baudrate == 19200) ||
(baudrate == 38400) ||
(baudrate == 57600) ||
(baudrate == 115200) ||
(baudrate == 230400) ||
(baudrate == 460800) ||
(baudrate == 921600));
}
bool SerialImpl::configure()
{
/* process baud rate */
int speed;
if (! validateBaudrate(_baudrate)) {
PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
return false;
}
switch (_baudrate) {
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;
#ifndef B460800
#define B460800 460800
#endif
case 460800: speed = B460800; break;
#ifndef B921600
#define B921600 921600
#endif
case 921600: speed = B921600; break;
default:
PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
return false;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcgetattr)", termios_state);
return false;
}
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
/* no parity, one stop bit, disable flow control */
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
return false;
}
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
return false;
}
return true;
}
bool SerialImpl::open()
{
if (isOpen()) {
return true;
}
// Open the serial port
int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (serial_fd < 0) {
PX4_ERR("failed to open %s err: %d", _port, errno);
return false;
}
_serial_fd = serial_fd;
// Configure the serial port
if (! configure()) {
PX4_ERR("failed to configure %s err: %d", _port, errno);
return false;
}
_open = true;
return _open;
}
bool SerialImpl::isOpen() const
{
return _open;
}
bool SerialImpl::close()
{
if (_serial_fd >= 0) {
::close(_serial_fd);
}
_serial_fd = -1;
_open = false;
return true;
}
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
{
if (!_open) {
PX4_ERR("Cannot read from serial device until it has been opened");
return -1;
}
int ret = ::read(_serial_fd, buffer, buffer_size);
if (ret < 0) {
PX4_DEBUG("%s read error %d", _port, ret);
}
return ret;
}
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
{
if (!_open) {
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
return -1;
}
if (buffer_size < character_count) {
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
return -1;
}
const hrt_abstime start_time_us = hrt_absolute_time();
int total_bytes_read = 0;
while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
// Poll for incoming UART data.
pollfd fds[1];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
if (remaining_time <= 0) { break; }
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
if (ret > 0) {
if (fds[0].revents & POLLIN) {
const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
int err = 0;
int bytes_available = 0;
err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
if (err != 0 || bytes_available < (int)character_count) {
px4_usleep(sleeptime);
}
ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
if (ret > 0) {
total_bytes_read += ret;
}
} else {
PX4_ERR("Got a poll error");
return -1;
}
}
}
return total_bytes_read;
}
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
{
if (!_open) {
PX4_ERR("Cannot write to serial device until it has been opened");
return -1;
}
int written = ::write(_serial_fd, buffer, buffer_size);
::fsync(_serial_fd);
if (written < 0) {
PX4_ERR("%s write error %d", _port, written);
}
return written;
}
const char *SerialImpl::getPort() const
{
return _port;
}
uint32_t SerialImpl::getBaudrate() const
{
return _baudrate;
}
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
if (! validateBaudrate(baudrate)) {
PX4_ERR("ERR: invalid baudrate: %lu", baudrate);
return false;
}
// check if already configured
if ((baudrate == _baudrate) && _open) {
return true;
}
_baudrate = baudrate;
// process baud rate change now if port is already open
if (_open) {
return configure();
}
return true;
}
ByteSize SerialImpl::getBytesize() const
{
return _bytesize;
}
bool SerialImpl::setBytesize(ByteSize bytesize)
{
return bytesize == ByteSize::EightBits;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
bool SerialImpl::setParity(Parity parity)
{
return parity == Parity::None;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
return stopbits == StopBits::One;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
return flowcontrol == FlowControl::Disabled;
}
} // namespace device

View File

@ -0,0 +1,104 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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
#include <stdint.h>
#include <unistd.h>
#include <px4_platform_common/SerialCommon.hpp>
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
namespace device
{
class SerialImpl
{
public:
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol);
virtual ~SerialImpl();
bool open();
bool isOpen() const;
bool close();
ssize_t read(uint8_t *buffer, size_t buffer_size);
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
ssize_t write(const void *buffer, size_t buffer_size);
const char *getPort() const;
uint32_t getBaudrate() const;
bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const;
bool setBytesize(ByteSize bytesize);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
private:
int _serial_fd{-1};
bool _open{false};
char _port[32] {};
uint32_t _baudrate{0};
ByteSize _bytesize{ByteSize::EightBits};
Parity _parity{Parity::None};
StopBits _stopbits{StopBits::One};
FlowControl _flowcontrol{FlowControl::Disabled};
bool validateBaudrate(uint32_t baudrate);
bool configure();
};
} // namespace device

View File

@ -3,6 +3,8 @@
add_library(px4_layer
${KERNEL_SRCS}
cdc_acm_check.cpp
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
SerialImpl.cpp
)
target_link_libraries(px4_layer

View File

@ -15,6 +15,8 @@ add_library(px4_layer
usr_board_ctrl.c
usr_hrt.cpp
usr_mcu_version.cpp
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
SerialImpl.cpp
)
target_link_libraries(px4_layer

View File

@ -0,0 +1,103 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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
#include <stdint.h>
#include <unistd.h>
#include <px4_platform_common/SerialCommon.hpp>
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
namespace device
{
class SerialImpl
{
public:
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol);
virtual ~SerialImpl();
bool open();
bool isOpen() const;
bool close();
ssize_t read(uint8_t *buffer, size_t buffer_size);
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
ssize_t write(const void *buffer, size_t buffer_size);
const char *getPort() const;
uint32_t getBaudrate() const;
bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const;
bool setBytesize(ByteSize bytesize);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
private:
int _serial_fd{-1};
bool _open{false};
char _port[32] {};
uint32_t _baudrate{0};
ByteSize _bytesize{ByteSize::EightBits};
Parity _parity{Parity::None};
StopBits _stopbits{StopBits::One};
FlowControl _flowcontrol{FlowControl::Disabled};
bool validateBaudrate(uint32_t baudrate);
bool configure();
};
} // namespace device

View File

@ -46,6 +46,8 @@ add_library(px4_layer
drv_hrt.cpp
cpuload.cpp
print_load.cpp
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
SerialImpl.cpp
)
target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable

View File

@ -0,0 +1,387 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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.
*
****************************************************************************/
#include <SerialImpl.hpp>
#include <string.h> // strncpy
#include <termios.h>
#include <px4_log.h>
#include <fcntl.h>
#include <errno.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
namespace device
{
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
{
if (port) {
strncpy(_port, port, sizeof(_port) - 1);
_port[sizeof(_port) - 1] = '\0';
} else {
_port[0] = 0;
}
}
SerialImpl::~SerialImpl()
{
if (isOpen()) {
close();
}
}
bool SerialImpl::validateBaudrate(uint32_t baudrate)
{
return ((baudrate == 9600) ||
(baudrate == 19200) ||
(baudrate == 38400) ||
(baudrate == 57600) ||
(baudrate == 115200) ||
(baudrate == 230400) ||
(baudrate == 460800) ||
(baudrate == 921600));
}
bool SerialImpl::configure()
{
/* process baud rate */
int speed;
if (! validateBaudrate(_baudrate)) {
PX4_ERR("ERR: unknown baudrate: %u", _baudrate);
return false;
}
switch (_baudrate) {
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;
#ifndef B460800
#define B460800 460800
#endif
case 460800: speed = B460800; break;
#ifndef B921600
#define B921600 921600
#endif
case 921600: speed = B921600; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", _baudrate);
return false;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcgetattr)", termios_state);
return false;
}
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
/* no parity, one stop bit, disable flow control */
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
return false;
}
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
return false;
}
return true;
}
bool SerialImpl::open()
{
if (isOpen()) {
return true;
}
// Open the serial port
int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (serial_fd < 0) {
PX4_ERR("failed to open %s err: %d", _port, errno);
return false;
}
_serial_fd = serial_fd;
// Configure the serial port
if (! configure()) {
PX4_ERR("failed to configure %s err: %d", _port, errno);
return false;
}
_open = true;
return _open;
}
bool SerialImpl::isOpen() const
{
return _open;
}
bool SerialImpl::close()
{
if (_serial_fd >= 0) {
::close(_serial_fd);
}
_serial_fd = -1;
_open = false;
return true;
}
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
{
if (!_open) {
PX4_ERR("Cannot read from serial device until it has been opened");
return -1;
}
int ret = ::read(_serial_fd, buffer, buffer_size);
if (ret < 0) {
PX4_DEBUG("%s read error %d", _port, ret);
}
return ret;
}
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
{
if (!_open) {
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
return -1;
}
if (buffer_size < character_count) {
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
return -1;
}
const hrt_abstime start_time_us = hrt_absolute_time();
int total_bytes_read = 0;
while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
// Poll for incoming UART data.
pollfd fds[1];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
if (remaining_time <= 0) { break; }
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
if (ret > 0) {
if (fds[0].revents & POLLIN) {
const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
px4_usleep(sleeptime);
ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
if (ret > 0) {
total_bytes_read += ret;
}
} else {
PX4_ERR("Got a poll error");
return -1;
}
}
}
return total_bytes_read;
}
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
{
if (!_open) {
PX4_ERR("Cannot write to serial device until it has been opened");
return -1;
}
int written = ::write(_serial_fd, buffer, buffer_size);
::fsync(_serial_fd);
if (written < 0) {
PX4_ERR("%s write error %d", _port, written);
}
return written;
}
const char *SerialImpl::getPort() const
{
return _port;
}
uint32_t SerialImpl::getBaudrate() const
{
return _baudrate;
}
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
if (! validateBaudrate(baudrate)) {
PX4_ERR("ERR: invalid baudrate: %u", baudrate);
return false;
}
// check if already configured
if ((baudrate == _baudrate) && _open) {
return true;
}
_baudrate = baudrate;
// process baud rate change now if port is already open
if (_open) {
return configure();
}
return true;
}
ByteSize SerialImpl::getBytesize() const
{
return _bytesize;
}
bool SerialImpl::setBytesize(ByteSize bytesize)
{
return bytesize == ByteSize::EightBits;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
bool SerialImpl::setParity(Parity parity)
{
return parity == Parity::None;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
return stopbits == StopBits::One;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
return flowcontrol == FlowControl::Disabled;
}
} // namespace device

View File

@ -51,6 +51,11 @@
#include <errno.h>
#include "hrt_work.h"
// Voxl2 board specific API definitions to get time offset
#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
#include "fc_sensor.h"
#endif
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
#include <lockstep_scheduler/lockstep_scheduler.h>
static LockstepScheduler lockstep_scheduler {true};
@ -107,6 +112,29 @@ hrt_abstime hrt_absolute_time()
#else // defined(ENABLE_LOCKSTEP_SCHEDULER)
struct timespec ts;
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
hrt_abstime temp_abstime = ts_to_abstime(&ts);
int apps_time_offset = fc_sensor_get_time_offset();
if (apps_time_offset < 0) {
hrt_abstime temp_offset = -apps_time_offset;
if (temp_offset >= temp_abstime) {
temp_abstime = 0;
} else {
temp_abstime -= temp_offset;
}
} else {
temp_abstime += (hrt_abstime) apps_time_offset;
}
ts.tv_sec = temp_abstime / 1000000;
ts.tv_nsec = (temp_abstime % 1000000) * 1000;
# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
return ts_to_abstime(&ts);
#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
}
@ -449,6 +477,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
return system_clock_gettime(clk_id, tp);
}
#if defined(ENABLE_LOCKSTEP_SCHEDULER)

View File

@ -50,5 +50,4 @@ add_library(px4 SHARED
target_link_libraries(px4
modules__muorb__slpi
${module_libraries}
px4_layer
)

View File

@ -0,0 +1,108 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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
#include <unistd.h>
#include <px4_platform_common/SerialCommon.hpp>
using device::SerialConfig::ByteSize;
using device::SerialConfig::Parity;
using device::SerialConfig::StopBits;
using device::SerialConfig::FlowControl;
namespace device
{
class SerialImpl
{
public:
SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol);
virtual ~SerialImpl();
bool open();
bool isOpen() const;
bool close();
ssize_t read(uint8_t *buffer, size_t buffer_size);
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
ssize_t write(const void *buffer, size_t buffer_size);
const char *getPort() const;
bool setPort(const char *port);
uint32_t getBaudrate() const;
bool setBaudrate(uint32_t baudrate);
ByteSize getBytesize() const;
bool setBytesize(ByteSize bytesize);
Parity getParity() const;
bool setParity(Parity parity);
StopBits getStopbits() const;
bool setStopbits(StopBits stopbits);
FlowControl getFlowcontrol() const;
bool setFlowcontrol(FlowControl flowcontrol);
private:
int _serial_fd{-1};
bool _open{false};
char _port[32] {};
uint32_t _baudrate{0};
ByteSize _bytesize{ByteSize::EightBits};
Parity _parity{Parity::None};
StopBits _stopbits{StopBits::One};
FlowControl _flowcontrol{FlowControl::Disabled};
bool validateBaudrate(uint32_t baudrate);
// Mutex used to lock the read functions
//pthread_mutex_t read_mutex;
// Mutex used to lock the write functions
//pthread_mutex_t write_mutex;
};
} // namespace device

View File

@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS
px4_qurt_impl.cpp
main.cpp
qurt_log.cpp
SerialImpl.cpp
)
add_library(px4_layer

View File

@ -0,0 +1,326 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. 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.
*
****************************************************************************/
#include <SerialImpl.hpp>
#include <string.h> // strncpy
#include <px4_log.h>
#include <drivers/device/qurt/uart.h>
#include <drivers/drv_hrt.h>
namespace device
{
SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
FlowControl flowcontrol) :
_baudrate(baudrate),
_bytesize(bytesize),
_parity(parity),
_stopbits(stopbits),
_flowcontrol(flowcontrol)
{
if (port) {
strncpy(_port, port, sizeof(_port) - 1);
_port[sizeof(_port) - 1] = '\0';
} else {
_port[0] = 0;
}
}
SerialImpl::~SerialImpl()
{
if (isOpen()) {
close();
}
}
bool SerialImpl::validateBaudrate(uint32_t baudrate)
{
if ((baudrate != 9600) &&
(baudrate != 38400) &&
(baudrate != 57600) &&
(baudrate != 115200) &&
(baudrate != 230400) &&
(baudrate != 250000) &&
(baudrate != 420000) &&
(baudrate != 460800) &&
(baudrate != 921600) &&
(baudrate != 1000000) &&
(baudrate != 1843200) &&
(baudrate != 2000000)) {
return false;
}
return true;
}
bool SerialImpl::open()
{
// There's no harm in calling open multiple times on the same port.
// In fact, that's the only way to change the baudrate
_open = false;
_serial_fd = -1;
if (! validateBaudrate(_baudrate)) {
PX4_ERR("Invalid baudrate: %u", _baudrate);
return false;
}
if (_bytesize != ByteSize::EightBits) {
PX4_ERR("Qurt platform only supports ByteSize::EightBits");
return false;
}
if (_parity != Parity::None) {
PX4_ERR("Qurt platform only supports Parity::None");
return false;
}
if (_stopbits != StopBits::One) {
PX4_ERR("Qurt platform only supports StopBits::One");
return false;
}
if (_flowcontrol != FlowControl::Disabled) {
PX4_ERR("Qurt platform only supports FlowControl::Disabled");
return false;
}
// qurt_uart_open will check validity of port and baudrate
int serial_fd = qurt_uart_open(_port, _baudrate);
if (serial_fd < 0) {
PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd);
return false;
} else {
PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate);
}
_serial_fd = serial_fd;
_open = true;
return _open;
}
bool SerialImpl::isOpen() const
{
return _open;
}
bool SerialImpl::close()
{
// No close defined for qurt uart yet
// if (_serial_fd >= 0) {
// qurt_uart_close(_serial_fd);
// }
_serial_fd = -1;
_open = false;
return true;
}
ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
{
if (!_open) {
PX4_ERR("Cannot read from serial device until it has been opened");
return -1;
}
int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500);
if (ret_read < 0) {
PX4_DEBUG("%s read error %d", _port, ret_read);
}
return ret_read;
}
ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
{
if (!_open) {
PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
return -1;
}
if (buffer_size < character_count) {
PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
return -1;
}
const hrt_abstime start_time_us = hrt_absolute_time();
int total_bytes_read = 0;
while (total_bytes_read < (int) character_count) {
if (timeout_us > 0) {
const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
if (elapsed_us >= timeout_us) {
// If there was a partial read but not enough to satisfy the minimum then they will be lost
// but this really should never happen when everything is working normally.
// PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us);
// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
return total_bytes_read;
}
}
int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
if (current_bytes_read < 0) {
// Again, if there was a partial read but not enough to satisfy the minimum then they will be lost
// but this really should never happen when everything is working normally.
PX4_ERR("%s failed to read uart", __FUNCTION__);
// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
return -1;
}
// Current bytes read could be zero
total_bytes_read += current_bytes_read;
// If we have at least reached our desired minimum number of characters
// then we can return now
if (total_bytes_read >= (int) character_count) {
return total_bytes_read;
}
// Wait a set amount of time before trying again or the remaining time
// until the timeout if we are getting close
const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
int64_t time_until_timeout = timeout_us - elapsed_us;
uint64_t time_to_sleep = 5000;
if ((time_until_timeout >= 0) &&
(time_until_timeout < (int64_t) time_to_sleep)) {
time_to_sleep = time_until_timeout;
}
px4_usleep(time_to_sleep);
}
return -1;
}
ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
{
if (!_open) {
PX4_ERR("Cannot write to serial device until it has been opened");
return -1;
}
int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size);
if (ret_write < 0) {
PX4_ERR("%s write error %d", _port, ret_write);
}
return ret_write;
}
const char *SerialImpl::getPort() const
{
return _port;
}
uint32_t SerialImpl::getBaudrate() const
{
return _baudrate;
}
bool SerialImpl::setBaudrate(uint32_t baudrate)
{
if (! validateBaudrate(baudrate)) {
PX4_ERR("Invalid baudrate: %u", baudrate);
return false;
}
// check if already configured
if (baudrate == _baudrate) {
return true;
}
_baudrate = baudrate;
// process baud rate change now if port is already open
if (_open) {
return open();
}
return true;
}
ByteSize SerialImpl::getBytesize() const
{
return _bytesize;
}
bool SerialImpl::setBytesize(ByteSize bytesize)
{
return bytesize == ByteSize::EightBits;
}
Parity SerialImpl::getParity() const
{
return _parity;
}
bool SerialImpl::setParity(Parity parity)
{
return parity == Parity::None;
}
StopBits SerialImpl::getStopbits() const
{
return _stopbits;
}
bool SerialImpl::setStopbits(StopBits stopbits)
{
return stopbits == StopBits::One;
}
FlowControl SerialImpl::getFlowcontrol() const
{
return _flowcontrol;
}
bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
{
return flowcontrol == FlowControl::Disabled;
}
} // namespace device

View File

@ -81,7 +81,7 @@ static void hrt_unlock()
px4_sem_post(&_hrt_lock);
}
int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
{
return 0;
}

View File

@ -162,7 +162,16 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
*/
static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
{
return hrt_absolute_time() - *then;
hrt_abstime now = hrt_absolute_time();
// Cannot allow a negative elapsed time as this would appear
// to be a huge positive elapsed time when represented as an
// unsigned value!
if (*then > now) {
return 0;
}
return now - *then;
}
/**

View File

@ -45,7 +45,6 @@
#include <poll.h>
#endif
#include <termios.h>
#include <cstring>
#include <drivers/drv_sensor.h>
@ -57,6 +56,8 @@
#include <px4_platform_common/cli.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/time.h>
#include <px4_platform_common/Serial.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
@ -81,6 +82,7 @@
#include <linux/spi/spidev.h>
#endif /* __PX4_LINUX */
using namespace device;
using namespace time_literals;
#define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
@ -169,7 +171,10 @@ public:
void reset_if_scheduled();
private:
int _serial_fd{-1}; ///< serial interface to GPS
#ifdef __PX4_LINUX
int _spi_fd {-1}; ///< SPI interface to GPS
#endif
Serial *_uart = nullptr; ///< UART interface to GPS
unsigned _baudrate{0}; ///< current baudrate
const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect)
char _port[20] {}; ///< device / serial port path
@ -329,8 +334,11 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
set_device_bus(c - 48); // sub 48 to convert char to integer
#ifdef __PX4_LINUX
} else if (_interface == GPSHelper::Interface::SPI) {
set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
#endif
}
if (_mode == gps_driver_mode_t::None) {
@ -403,10 +411,23 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
return num_read;
}
case GPSCallbackType::writeDeviceData:
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
case GPSCallbackType::writeDeviceData: {
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
return ::write(gps->_serial_fd, data1, (size_t)data2);
int ret = 0;
if (gps->_uart) {
ret = gps->_uart->write((void *) data1, (size_t) data2);
#ifdef __PX4_LINUX
} else if (gps->_spi_fd >= 0) {
ret = ::write(gps->_spi_fd, data1, (size_t)data2);
#endif
}
return ret;
}
case GPSCallbackType::setBaudrate:
return gps->setBaudrate(data2);
@ -449,72 +470,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
{
int ret = 0;
const unsigned character_count = 32; // minimum bytes that we want to read
const int max_timeout = 50;
int timeout_adjusted = math::min(max_timeout, timeout);
handleInjectDataTopic();
#if !defined(__PX4_QURT)
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
/* For non QURT, use the usual polling. */
// SPI is only supported on LInux
#if defined(__PX4_LINUX)
//Poll only for the serial data. In the same thread we also need to handle orb messages,
//so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
//two pollings use different underlying mechanisms (at least under posix), which makes this
//impossible. Instead we limit the maximum polling interval and regularly check for new orb
//messages.
//FIXME: add a unified poll() API
const int max_timeout = 50;
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
pollfd fds[1];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
//Poll only for the SPI data. In the same thread we also need to handle orb messages,
//so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the
//two pollings use different underlying mechanisms (at least under posix), which makes this
//impossible. Instead we limit the maximum polling interval and regularly check for new orb
//messages.
//FIXME: add a unified poll() API
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
pollfd fds[1];
fds[0].fd = _spi_fd;
fds[0].events = POLLIN;
if (ret > 0) {
/* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) {
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. But don't read immediately
* by 1-2 bytes, wait for some more data to save expensive read() calls.
* If we have all requested data available, read it without waiting.
* If more bytes are available, we'll go back to poll() again.
*/
const unsigned character_count = 32; // minimum bytes that we want to read
unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
#ifdef __PX4_NUTTX
int err = 0;
int bytes_available = 0;
err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
if (ret > 0) {
/* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) {
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. But don't read immediately
* by 1-2 bytes, wait for some more data to save expensive read() calls.
* If we have all requested data available, read it without waiting.
* If more bytes are available, we'll go back to poll() again.
*/
unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
if (err != 0 || bytes_available < (int)character_count) {
px4_usleep(sleeptime);
ret = ::read(_spi_fd, buf, buf_length);
if (ret > 0) {
_num_bytes_read += ret;
}
} else {
ret = -1;
}
#else
px4_usleep(sleeptime);
#endif
ret = ::read(_serial_fd, buf, buf_length);
if (ret > 0) {
_num_bytes_read += ret;
}
} else {
ret = -1;
}
#endif
}
return ret;
#else
/* For QURT, just use read for now, since this doesn't block, we need to slow it down
* just a bit. */
px4_usleep(10000);
return ::read(_serial_fd, buf, buf_length);
#endif
}
void GPS::handleInjectDataTopic()
@ -583,105 +596,38 @@ bool GPS::injectData(uint8_t *data, size_t len)
{
dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
size_t written = ::write(_serial_fd, data, len);
::fsync(_serial_fd);
size_t written = 0;
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
written = _uart->write((const void *) data, len);
#ifdef __PX4_LINUX
} else if (_interface == GPSHelper::Interface::SPI) {
written = ::write(_spi_fd, data, len);
::fsync(_spi_fd);
#endif
}
return written == len;
}
int GPS::setBaudrate(unsigned baud)
{
/* process baud rate */
int speed;
if (_interface == GPSHelper::Interface::UART) {
if ((_uart) && (_uart->setBaudrate(baud))) {
return 0;
}
switch (baud) {
case 9600: speed = B9600; break;
#ifdef __PX4_LINUX
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;
#ifndef B460800
#define B460800 460800
} else if (_interface == GPSHelper::Interface::SPI) {
// Can't set the baudrate on a SPI port but just return a success
return 0;
#endif
case 460800: speed = B460800; break;
#ifndef B921600
#define B921600 921600
#endif
case 921600: speed = B921600; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(_serial_fd, &uart_config);
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
/* no parity, one stop bit, disable flow control */
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
return -1;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
return -1;
}
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
GPS_ERR("ERR: %d (tcsetattr)", termios_state);
return -1;
}
return 0;
return -1;
}
void GPS::initializeCommunicationDump()
@ -840,31 +786,58 @@ GPS::run()
_helper = nullptr;
}
if (_serial_fd < 0) {
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
if (_serial_fd < 0) {
PX4_ERR("failed to open %s err: %d", _port, errno);
// Create the UART port instance
_uart = new Serial(_port);
if (_uart == nullptr) {
PX4_ERR("Error creating serial device %s", _port);
px4_sleep(1);
continue;
}
}
if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) {
// Configure the desired baudrate if one was specified by the user.
// Otherwise the default baudrate will be used.
if (_configured_baudrate) {
if (! _uart->setBaudrate(_configured_baudrate)) {
PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
px4_sleep(1);
continue;
}
}
// Open the UART. If this is successful then the UART is ready to use.
if (! _uart->open()) {
PX4_ERR("Error opening serial device %s", _port);
px4_sleep(1);
continue;
}
#ifdef __PX4_LINUX
if (_interface == GPSHelper::Interface::SPI) {
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (status_value < 0) {
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
}
if (_spi_fd < 0) {
PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
px4_sleep(1);
continue;
}
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
}
if (status_value < 0) {
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
}
status_value = ::ioctl(_spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
}
#endif /* __PX4_LINUX */
@ -1056,9 +1029,17 @@ GPS::run()
}
}
if (_serial_fd >= 0) {
::close(_serial_fd);
_serial_fd = -1;
if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
(void) _uart->close();
delete _uart;
_uart = nullptr;
#ifdef __PX4_LINUX
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
::close(_spi_fd);
_spi_fd = -1;
#endif
}
if (_mode_auto) {
@ -1406,7 +1387,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance)
entry_point, (char *const *)argv);
if (task_id < 0) {
task_id = -1;
_task_id = -1;
return -errno;
}
@ -1477,12 +1458,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
break;
case 'i':
if (!strcmp(myoptarg, "spi")) {
interface = GPSHelper::Interface::SPI;
} else if (!strcmp(myoptarg, "uart")) {
if (!strcmp(myoptarg, "uart")) {
interface = GPSHelper::Interface::UART;
#ifdef __PX4_LINUX
} else if (!strcmp(myoptarg, "spi")) {
interface = GPSHelper::Interface::SPI;
#endif
} else {
PX4_ERR("unknown interface: %s", myoptarg);
error_flag = true;
@ -1490,12 +1471,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
break;
case 'j':
if (!strcmp(myoptarg, "spi")) {
interface_secondary = GPSHelper::Interface::SPI;
} else if (!strcmp(myoptarg, "uart")) {
if (!strcmp(myoptarg, "uart")) {
interface_secondary = GPSHelper::Interface::UART;
#ifdef __PX4_LINUX
} else if (!strcmp(myoptarg, "spi")) {
interface_secondary = GPSHelper::Interface::SPI;
#endif
} else {
PX4_ERR("unknown interface for secondary: %s", myoptarg);
error_flag = true;

View File

@ -364,6 +364,13 @@ PX4IO::~PX4IO()
bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
for (size_t i = 0; i < num_outputs; i++) {
if (!_mixing_output.isFunctionSet(i)) {
// do not run any signal on disabled channels
outputs[i] = 0;
}
}
if (!_test_fmu_fail) {
/* output to the servos */
io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);

View File

@ -455,7 +455,8 @@ bool MixingOutput::update()
}
}
if (!all_disabled) {
// Send output if any function mapped or one last disabling sample
if (!all_disabled || !_was_all_disabled) {
if (!_armed.armed && !_armed.manual_lockdown) {
_actuator_test.overrideValues(outputs, _max_num_outputs);
}
@ -463,6 +464,8 @@ bool MixingOutput::update()
limitAndUpdateOutputs(outputs, has_updates);
}
_was_all_disabled = all_disabled;
return true;
}

View File

@ -288,6 +288,7 @@ private:
hrt_abstime _lowrate_schedule_interval{300_ms};
ActuatorTest _actuator_test{_function_assignment};
uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only)
bool _was_all_disabled{false};
uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback

View File

@ -191,11 +191,15 @@ TEST_F(MixerModuleTest, basic)
mixing_output.setAllMaxValues(MAX_VALUE);
EXPECT_EQ(test_module.num_updates, 0);
// all functions disabled: not expected to get an update
// all functions disabled: expect to get one single update to process disabling the output signal
mixing_output.update();
mixing_output.updateSubscriptions(false);
mixing_output.update();
EXPECT_EQ(test_module.num_updates, 0);
EXPECT_EQ(test_module.num_updates, 1);
mixing_output.update();
mixing_output.updateSubscriptions(false);
mixing_output.update();
EXPECT_EQ(test_module.num_updates, 1);
test_module.reset();
// configure motor, ensure all still disarmed

View File

@ -39,7 +39,7 @@ px4_add_module(
INCLUDES
../test
../aggregator
libfc-sensor-api/inc
${PX4_BOARD_DIR}/libfc-sensor-api/inc
SRCS
uORBAppsProtobufChannel.cpp
muorb_main.cpp

View File

@ -4,3 +4,11 @@ menuconfig MODULES_MUORB_APPS
depends on PLATFORM_POSIX
---help---
Enable support for muorb apps
config MUORB_APPS_SYNC_TIMESTAMP
bool "Sync timestamp with external processor"
depends on MODULES_MUORB_APPS
default y
help
causes HRT timestamp to use an externally calculated offset for synchronization

View File

@ -180,15 +180,19 @@ mixer_tick()
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
// Set failsafe value if the PWM output isn't disabled
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
if (r_page_servos[i] != 0) {
r_page_servos[i] = r_page_servo_failsafe[i];
}
}
} else if (source == MIX_DISARMED) {
/* copy disarmed values to the servo outputs */
// Set disarmed value if the PWM output isn't disabled
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_disarmed[i];
if (r_page_servos[i] != 0) {
r_page_servos[i] = r_page_servo_disarmed[i];
}
}
}