forked from Archive/PX4-Autopilot
Compare commits
1 Commits
main
...
pr-hrt_clo
Author | SHA1 | Date |
---|---|---|
Daniel Agar | 0d3daf3efd |
|
@ -1,14 +0,0 @@
|
|||
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
|
|
@ -81,5 +81,5 @@
|
|||
url = https://github.com/PX4/PX4-gazebo-models.git
|
||||
branch = main
|
||||
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
|
||||
path = boards/modalai/voxl2/libfc-sensor-api
|
||||
path = src/modules/muorb/apps/libfc-sensor-api
|
||||
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git
|
||||
|
|
|
@ -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 boards/modalai/voxl2/libfc-sensor-api -prune -o \
|
||||
-path src/modules/muorb/apps/libfc-sensor-api -prune -o \
|
||||
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
|
||||
|
|
|
@ -33,7 +33,6 @@ CONFIG_DRIVERS_RC_INPUT=y
|
|||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
|
|
Binary file not shown.
|
@ -20,7 +20,6 @@ CONFIG_UAVCANNODE_RTK_DATA=y
|
|||
CONFIG_UAVCANNODE_SAFETY_BUTTON=y
|
||||
CONFIG_UAVCANNODE_STATIC_PRESSURE=y
|
||||
CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
|
|
|
@ -11,5 +11,3 @@ param set-default SENS_IMU_CLPNOTI 0
|
|||
|
||||
safety_button start
|
||||
tone_alarm start
|
||||
|
||||
ekf2 start
|
||||
|
|
|
@ -50,7 +50,6 @@ CONFIG_STACK_COLORATION=y
|
|||
CONFIG_START_DAY=30
|
||||
CONFIG_START_MONTH=11
|
||||
CONFIG_STDIO_DISABLE_BUFFERING=y
|
||||
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||
CONFIG_STM32_NOEXT_VECTORS=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
CONFIG_TASK_NAME_SIZE=0
|
||||
|
|
|
@ -123,7 +123,6 @@ CONFIG_STM32_ADC1=y
|
|||
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
|
||||
CONFIG_STM32_DMA1=y
|
||||
CONFIG_STM32_DMA2=y
|
||||
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||
CONFIG_STM32_FLASH_PREFETCH=y
|
||||
CONFIG_STM32_FLOWCONTROL_BROKEN=y
|
||||
CONFIG_STM32_I2C1=y
|
||||
|
|
|
@ -33,7 +33,7 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||
/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
|
|
|
@ -33,7 +33,7 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||
/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
|
|
|
@ -35,7 +35,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
|||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
|
|
|
@ -36,7 +36,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
|||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_DRIVERS_ADC_ADS1115=n
|
||||
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
|
||||
CONFIG_DRIVERS_IRLOCK=n
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -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
|
||||
|
|
|
@ -4,7 +4,6 @@ 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
|
||||
|
|
|
@ -1,34 +0,0 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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)
|
|
@ -1,7 +1,7 @@
|
|||
|
||||
# Link against the public stub version of the proprietary fc sensor library
|
||||
target_link_libraries(px4 PRIVATE
|
||||
${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
|
||||
${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
|
||||
px4_layer
|
||||
${module_libraries}
|
||||
)
|
||||
|
|
|
@ -11,6 +11,7 @@ CONFIG_DRIVERS_VOXL2_IO=y
|
|||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
CONFIG_MODULES_LOGGER=y
|
||||
CONFIG_MODULES_MAVLINK=y
|
||||
CONFIG_MODULES_MUORB_APPS=y
|
||||
|
|
|
@ -1,9 +1,10 @@
|
|||
#!/bin/bash
|
||||
|
||||
cd boards/modalai/voxl2/libfc-sensor-api
|
||||
cd src/modules/muorb/apps/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 ../../../../../..
|
||||
|
||||
|
|
|
@ -76,6 +76,7 @@ qshell flight_mode_manager start
|
|||
# Start all of the processing modules on the applications processor
|
||||
dataman start
|
||||
navigator start
|
||||
load_mon start
|
||||
|
||||
# Start microdds_client for ros2 offboard messages from agent over localhost
|
||||
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
||||
|
|
|
@ -207,6 +207,7 @@ qshell flight_mode_manager start
|
|||
# Start all of the processing modules on the applications processor
|
||||
dataman start
|
||||
navigator start
|
||||
load_mon start
|
||||
|
||||
# This bridge allows raw data packets to be sent over UART to the ESC
|
||||
voxl2_io_bridge start
|
||||
|
|
|
@ -49,6 +49,7 @@ CONFIG_MODULES_FW_POS_CONTROL=y
|
|||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_GYRO_FFT=y
|
||||
CONFIG_MODULES_LAND_DETECTOR=y
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_DRIVERS_HEATER=n
|
||||
CONFIG_DRIVERS_UAVCAN=n
|
||||
CONFIG_CYPHAL_BMS_SUBSCRIBER=y
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_BAROMETERS=n
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
|
||||
CONFIG_COMMON_MAGNETOMETER=n
|
||||
|
|
|
@ -40,7 +40,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
|||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
|
|
Binary file not shown.
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_BAROMETERS=n
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=n
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_BAROMETERS=n
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=n
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_HYGROMETERS=n
|
||||
CONFIG_COMMON_OSD=n
|
||||
CONFIG_DRIVERS_ADC_ADS1115=n
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_BAROMETERS=n
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
|
||||
CONFIG_COMMON_HYGROMETERS=n
|
||||
|
|
|
@ -81,6 +81,7 @@ 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
|
||||
|
@ -101,7 +102,9 @@ 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_SD_STRESS=y
|
||||
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||
|
|
Binary file not shown.
|
@ -30,7 +30,6 @@ CONFIG_DRIVERS_PX4IO=y
|
|||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
|
|
Binary file not shown.
|
@ -42,7 +42,6 @@ CONFIG_DRIVERS_RC_INPUT=y
|
|||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
|
|
Binary file not shown.
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_DRIVERS_UAVCAN=n
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=n
|
||||
CONFIG_MODULES_ZENOH=y
|
||||
|
|
|
@ -333,10 +333,6 @@ if(EXISTS ${BOARD_DEFCONFIG})
|
|||
set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE)
|
||||
endif()
|
||||
|
||||
if(UAVCAN_TIMER_OVERRIDE)
|
||||
set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE)
|
||||
endif()
|
||||
|
||||
# OPTIONS
|
||||
|
||||
if(CONSTRAINED_FLASH)
|
||||
|
|
|
@ -58,7 +58,6 @@ set(msg_files
|
|||
CameraCapture.msg
|
||||
CameraStatus.msg
|
||||
CameraTrigger.msg
|
||||
CanInterfaceStatus.msg
|
||||
CellularStatus.msg
|
||||
CollisionConstraints.msg
|
||||
CollisionReport.msg
|
||||
|
|
|
@ -1,6 +0,0 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 interface
|
||||
|
||||
uint64 io_errors
|
||||
uint64 frames_tx
|
||||
uint64 frames_rx
|
|
@ -52,7 +52,6 @@ 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)
|
||||
|
|
|
@ -1,138 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -1,97 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -1,70 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -13,21 +13,11 @@ __END_DECLS
|
|||
#define px4_clock_gettime system_clock_gettime
|
||||
#endif
|
||||
|
||||
#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_clock_settime(clockid_t clk_id, const struct timespec *tp);
|
||||
|
||||
__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,
|
||||
|
@ -37,6 +27,7 @@ __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
|
||||
|
|
|
@ -56,6 +56,31 @@ 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()
|
||||
|
|
|
@ -412,10 +412,7 @@ 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.
|
||||
// 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)));
|
||||
}
|
||||
ch->send_message(_meta->o_name, _meta->o_size, _data);
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
|
|
|
@ -1,394 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -1,104 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -3,8 +3,6 @@
|
|||
add_library(px4_layer
|
||||
${KERNEL_SRCS}
|
||||
cdc_acm_check.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(px4_layer
|
||||
|
|
|
@ -15,8 +15,6 @@ 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
|
||||
|
|
|
@ -244,6 +244,8 @@
|
|||
# error HRT_TIMER_CHANNEL must be a value between 1 and 4
|
||||
#endif
|
||||
|
||||
static volatile hrt_abstime base_time;
|
||||
|
||||
/*
|
||||
* Queue of callout entries.
|
||||
*/
|
||||
|
@ -276,6 +278,112 @@ static void hrt_call_reschedule(void);
|
|||
static void hrt_call_invoke(void);
|
||||
|
||||
|
||||
#define HRT_TIME_SYNC
|
||||
|
||||
#if defined(HRT_TIME_SYNC)
|
||||
|
||||
#include <math.h>
|
||||
|
||||
static bool sync_set = false;
|
||||
static bool sync_locked = false;
|
||||
static uint32_t sync_jump_cnt = 0;
|
||||
|
||||
static float sync_prev_adj = 0;
|
||||
static float sync_rel_rate_ppm = 0;
|
||||
static float sync_rel_rate_error_integral = 0;
|
||||
static int32_t sync_accumulated_correction_nsec = 0;
|
||||
static int32_t sync_correction_nsec_per_overflow = 0;
|
||||
static hrt_abstime prev_sync_adj_at;
|
||||
|
||||
static const float sync_offset_p = 0.01f; ///< PPM per one usec error
|
||||
static const float sync_rate_i = 0.02f; ///< PPM per one PPM error for second
|
||||
static const float sync_rate_error_corner_freq = 0.01f;
|
||||
static const float sync_max_rate_correction_ppm = 300.0f;
|
||||
static const float sync_lock_thres_rate_ppm = 2.f;
|
||||
static const hrt_abstime sync_lock_thres_offset = 4000; ///< usec
|
||||
static const hrt_abstime sync_min_jump = 10000; ///< Min error to jump rather than change rate
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(float adj_usec)
|
||||
{
|
||||
const hrt_abstime ts = hrt_absolute_time();
|
||||
const float dt = (ts - prev_sync_adj_at) / 1e6f;
|
||||
prev_sync_adj_at = ts;
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * sync_offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (sync_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
sync_prev_adj = adj_usec;
|
||||
sync_rel_rate_ppm = lowpass(sync_rel_rate_ppm, new_rel_rate_ppm, sync_rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - sync_rel_rate_ppm;
|
||||
|
||||
if (dt > 10) {
|
||||
sync_rel_rate_error_integral = 0;
|
||||
|
||||
} else {
|
||||
sync_rel_rate_error_integral += rel_rate_error * dt * sync_rate_i;
|
||||
sync_rel_rate_error_integral = fmaxf(sync_rel_rate_error_integral, -sync_max_rate_correction_ppm);
|
||||
sync_rel_rate_error_integral = fminf(sync_rel_rate_error_integral, sync_max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/* Rate controller */
|
||||
float total_rate_correction_ppm = rel_rate_error + sync_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = fmaxf(total_rate_correction_ppm, -sync_max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = fminf(total_rate_correction_ppm, sync_max_rate_correction_ppm);
|
||||
|
||||
sync_correction_nsec_per_overflow = (HRT_COUNTER_PERIOD * 1000) * (total_rate_correction_ppm / 1e6f);
|
||||
|
||||
// syslog(LOG_INFO, "$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// (double)adj_usec, (double)sync_rel_rate_ppm, (double)sync_rel_rate_error_integral, (double)target_rel_rate_ppm,
|
||||
// (double)total_rate_correction_ppm);
|
||||
}
|
||||
|
||||
void hrt_absolute_time_adjust(int64_t adjustment)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
if (fabsf((float)adjustment) > (float)sync_min_jump || !sync_set) {
|
||||
|
||||
const hrt_abstime time_prev = base_time;
|
||||
base_time += adjustment;
|
||||
syslog(LOG_NOTICE, "HRT: resetting %llu -> %llu\n", time_prev, base_time);
|
||||
|
||||
sync_set = true;
|
||||
sync_locked = false;
|
||||
sync_jump_cnt++;
|
||||
sync_prev_adj = 0;
|
||||
sync_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!sync_locked) {
|
||||
sync_locked =
|
||||
(fabsf(sync_rel_rate_ppm) < sync_lock_thres_rate_ppm) &&
|
||||
(fabsf(sync_prev_adj) < sync_lock_thres_offset);
|
||||
}
|
||||
}
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
#endif // HRT_TIME_SYNC
|
||||
|
||||
|
||||
|
||||
int hrt_ioctl(unsigned int cmd, unsigned long arg);
|
||||
/*
|
||||
* Specific registers and bits used by PPM sub-functions
|
||||
|
@ -665,7 +773,6 @@ hrt_absolute_time(void)
|
|||
* pair. Discourage the compiler from moving loads/stores
|
||||
* to these outside of the protected range.
|
||||
*/
|
||||
static volatile hrt_abstime base_time;
|
||||
static volatile uint32_t last_count;
|
||||
|
||||
/* prevent re-entry */
|
||||
|
@ -683,12 +790,34 @@ hrt_absolute_time(void)
|
|||
*/
|
||||
if (count < last_count) {
|
||||
base_time += HRT_COUNTER_PERIOD;
|
||||
|
||||
#if defined(HRT_TIME_SYNC)
|
||||
|
||||
if (sync_set) {
|
||||
sync_accumulated_correction_nsec += sync_correction_nsec_per_overflow;
|
||||
|
||||
if (abs(sync_accumulated_correction_nsec) >= 1000) {
|
||||
base_time += sync_accumulated_correction_nsec / 1000;
|
||||
sync_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (sync_correction_nsec_per_overflow > 0) {
|
||||
sync_correction_nsec_per_overflow--;
|
||||
|
||||
} else if (sync_correction_nsec_per_overflow < 0) {
|
||||
sync_correction_nsec_per_overflow++;
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* HRT_TIME_SYNC */
|
||||
}
|
||||
|
||||
/* save the count for next time */
|
||||
last_count = count;
|
||||
|
||||
/* compute the current time */
|
||||
|
||||
abstime = HRT_COUNTER_SCALE(base_time + count);
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
|
|
@ -1,103 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -46,8 +46,6 @@ 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
|
||||
|
|
|
@ -1,387 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -51,11 +51,6 @@
|
|||
#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};
|
||||
|
@ -112,29 +107,6 @@ 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)
|
||||
}
|
||||
|
@ -477,7 +449,6 @@ 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)
|
||||
|
|
|
@ -50,4 +50,5 @@ add_library(px4 SHARED
|
|||
target_link_libraries(px4
|
||||
modules__muorb__slpi
|
||||
${module_libraries}
|
||||
px4_layer
|
||||
)
|
||||
|
|
|
@ -1,108 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -38,7 +38,6 @@ set(QURT_LAYER_SRCS
|
|||
px4_qurt_impl.cpp
|
||||
main.cpp
|
||||
qurt_log.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
add_library(px4_layer
|
||||
|
|
|
@ -1,326 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
|
@ -81,7 +81,7 @@ static void hrt_unlock()
|
|||
px4_sem_post(&_hrt_lock);
|
||||
}
|
||||
|
||||
int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
|
||||
int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
|
|
@ -131,6 +131,11 @@ typedef struct latency_boardctl {
|
|||
*/
|
||||
__EXPORT extern hrt_abstime hrt_absolute_time(void);
|
||||
|
||||
/**
|
||||
* Adjust the synchronized clock.
|
||||
*/
|
||||
__EXPORT extern void hrt_absolute_time_adjust(int64_t adjustment);
|
||||
|
||||
/**
|
||||
* Convert a timespec to absolute time.
|
||||
*/
|
||||
|
@ -162,16 +167,7 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
|
|||
*/
|
||||
static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *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;
|
||||
return hrt_absolute_time() - *then;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -45,6 +45,7 @@
|
|||
#include <poll.h>
|
||||
#endif
|
||||
|
||||
#include <termios.h>
|
||||
#include <cstring>
|
||||
|
||||
#include <drivers/drv_sensor.h>
|
||||
|
@ -56,8 +57,6 @@
|
|||
#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>
|
||||
|
@ -82,7 +81,6 @@
|
|||
#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
|
||||
|
@ -171,10 +169,7 @@ public:
|
|||
void reset_if_scheduled();
|
||||
|
||||
private:
|
||||
#ifdef __PX4_LINUX
|
||||
int _spi_fd {-1}; ///< SPI interface to GPS
|
||||
#endif
|
||||
Serial *_uart = nullptr; ///< UART interface to GPS
|
||||
int _serial_fd{-1}; ///< serial 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
|
||||
|
@ -334,11 +329,8 @@ 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) {
|
||||
|
@ -411,23 +403,10 @@ 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);
|
||||
|
||||
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;
|
||||
}
|
||||
return ::write(gps->_serial_fd, data1, (size_t)data2);
|
||||
|
||||
case GPSCallbackType::setBaudrate:
|
||||
return gps->setBaudrate(data2);
|
||||
|
@ -470,64 +449,72 @@ 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 ((_interface == GPSHelper::Interface::UART) && (_uart)) {
|
||||
ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
|
||||
#if !defined(__PX4_QURT)
|
||||
|
||||
// SPI is only supported on LInux
|
||||
#if defined(__PX4_LINUX)
|
||||
/* For non QURT, use the usual polling. */
|
||||
|
||||
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
|
||||
//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;
|
||||
|
||||
//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
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _spi_fd;
|
||||
fds[0].events = POLLIN;
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
|
||||
|
||||
ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
|
||||
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);
|
||||
|
||||
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);
|
||||
#ifdef __PX4_NUTTX
|
||||
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(_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;
|
||||
}
|
||||
}
|
||||
|
||||
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()
|
||||
|
@ -596,38 +583,105 @@ bool GPS::injectData(uint8_t *data, size_t len)
|
|||
{
|
||||
dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
size_t written = ::write(_serial_fd, data, len);
|
||||
::fsync(_serial_fd);
|
||||
return written == len;
|
||||
}
|
||||
|
||||
int GPS::setBaudrate(unsigned baud)
|
||||
{
|
||||
if (_interface == GPSHelper::Interface::UART) {
|
||||
if ((_uart) && (_uart->setBaudrate(baud))) {
|
||||
return 0;
|
||||
}
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
switch (baud) {
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
} else if (_interface == GPSHelper::Interface::SPI) {
|
||||
// Can't set the baudrate on a SPI port but just return a success
|
||||
return 0;
|
||||
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", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
return -1;
|
||||
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;
|
||||
}
|
||||
|
||||
void GPS::initializeCommunicationDump()
|
||||
|
@ -786,58 +840,31 @@ GPS::run()
|
|||
_helper = nullptr;
|
||||
}
|
||||
|
||||
if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
|
||||
if (_serial_fd < 0) {
|
||||
/* open the serial port */
|
||||
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
|
||||
// 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);
|
||||
if (_serial_fd < 0) {
|
||||
PX4_ERR("failed to open %s err: %d", _port, errno);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
#ifdef __PX4_LINUX
|
||||
|
||||
} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
|
||||
_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
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);
|
||||
|
||||
if (_spi_fd < 0) {
|
||||
PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
|
||||
px4_sleep(1);
|
||||
continue;
|
||||
}
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
|
||||
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);
|
||||
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
|
||||
|
||||
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);
|
||||
if (status_value < 0) {
|
||||
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* __PX4_LINUX */
|
||||
|
@ -1029,17 +1056,9 @@ GPS::run()
|
|||
}
|
||||
}
|
||||
|
||||
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 (_serial_fd >= 0) {
|
||||
::close(_serial_fd);
|
||||
_serial_fd = -1;
|
||||
}
|
||||
|
||||
if (_mode_auto) {
|
||||
|
@ -1387,7 +1406,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;
|
||||
}
|
||||
|
||||
|
@ -1458,12 +1477,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
|||
break;
|
||||
|
||||
case 'i':
|
||||
if (!strcmp(myoptarg, "uart")) {
|
||||
interface = GPSHelper::Interface::UART;
|
||||
#ifdef __PX4_LINUX
|
||||
} else if (!strcmp(myoptarg, "spi")) {
|
||||
if (!strcmp(myoptarg, "spi")) {
|
||||
interface = GPSHelper::Interface::SPI;
|
||||
#endif
|
||||
|
||||
} else if (!strcmp(myoptarg, "uart")) {
|
||||
interface = GPSHelper::Interface::UART;
|
||||
|
||||
} else {
|
||||
PX4_ERR("unknown interface: %s", myoptarg);
|
||||
error_flag = true;
|
||||
|
@ -1471,12 +1490,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
|||
break;
|
||||
|
||||
case 'j':
|
||||
if (!strcmp(myoptarg, "uart")) {
|
||||
interface_secondary = GPSHelper::Interface::UART;
|
||||
#ifdef __PX4_LINUX
|
||||
} else if (!strcmp(myoptarg, "spi")) {
|
||||
if (!strcmp(myoptarg, "spi")) {
|
||||
interface_secondary = GPSHelper::Interface::SPI;
|
||||
#endif
|
||||
|
||||
} else if (!strcmp(myoptarg, "uart")) {
|
||||
interface_secondary = GPSHelper::Interface::UART;
|
||||
|
||||
} else {
|
||||
PX4_ERR("unknown interface for secondary: %s", myoptarg);
|
||||
error_flag = true;
|
||||
|
|
|
@ -364,13 +364,6 @@ 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);
|
||||
|
|
|
@ -42,22 +42,12 @@ set(UAVCAN_PLATFORM "generic")
|
|||
if(CONFIG_ARCH_CHIP)
|
||||
if(${CONFIG_NET_CAN} MATCHES "y")
|
||||
set(UAVCAN_DRIVER "socketcan")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
set(UAVCAN_DRIVER "kinetis")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
|
||||
set(UAVCAN_DRIVER "stm32h7")
|
||||
set(UAVCAN_TIMER 5) # The default timer is TIM5
|
||||
if (DEFINED config_uavcan_timer_override)
|
||||
set (UAVCAN_TIMER ${config_uavcan_timer_override})
|
||||
endif()
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
|
||||
set(UAVCAN_DRIVER "stm32")
|
||||
set(UAVCAN_TIMER 6) # The default timer is TIM6
|
||||
if (DEFINED config_uavcan_timer_override)
|
||||
set (UAVCAN_TIMER ${config_uavcan_timer_override})
|
||||
endif()
|
||||
endif()
|
||||
endif()
|
||||
|
||||
|
@ -74,8 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
|
|||
add_definitions(
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
-DUAVCAN_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
|
||||
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
|
||||
|
|
|
@ -96,8 +96,3 @@ depends on DRIVERS_UAVCAN
|
|||
string "UAVCAN peripheral firmware"
|
||||
help
|
||||
list of UAVCAN peripheral firmware to build and embed
|
||||
|
||||
menuconfig BOARD_UAVCAN_TIMER_OVERRIDE
|
||||
depends on DRIVERS_UAVCAN
|
||||
int "UAVCAN timer override"
|
||||
default 0
|
||||
|
|
|
@ -15,7 +15,6 @@ and the following commandline defines:
|
|||
| Setting | Description |
|
||||
|-------------------|-----------------------------------------------------------------------------------|
|
||||
|UAVCAN_KINETIS_NUM_IFACES | - {1..2} Sets the number of CAN interfaces the SW will support |
|
||||
|UAVCAN_KINETIS_TIMER_NUMBER | - {0..3} Sets the Periodic Interrupt Timer (PITn) channel |
|
||||
|
||||
Things that could be improved:
|
||||
1. Build time command line configuration of Mailbox/FIFO and filters
|
||||
|
|
|
@ -18,12 +18,3 @@
|
|||
#if !defined(UAVCAN_KINETIS_NUM_IFACES) || (UAVCAN_KINETIS_NUM_IFACES != 1 && UAVCAN_KINETIS_NUM_IFACES != 2)
|
||||
# error "UAVCAN_KINETIS_NUM_IFACES must be set to either 1 or 2"
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Any PIT timer channel (PIT0-PIT3)
|
||||
* e.g. -DUAVCAN_KINETIS_TIMER_NUMBER=2
|
||||
*/
|
||||
#ifndef UAVCAN_KINETIS_TIMER_NUMBER
|
||||
// In this case the clock driver should be implemented by the application
|
||||
# define UAVCAN_KINETIS_TIMER_NUMBER 0
|
||||
#endif
|
||||
|
|
|
@ -46,57 +46,6 @@ uavcan::UtcTime getUtc();
|
|||
*/
|
||||
void adjustUtc(uavcan::UtcDuration adjustment);
|
||||
|
||||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F),
|
||||
rate_i(0.02F),
|
||||
rate_error_corner_freq(0.01F),
|
||||
max_rate_correction_ppm(300.0F),
|
||||
lock_thres_rate_ppm(2.0F),
|
||||
lock_thres_offset(uavcan::UtcDuration::fromMSec(4)),
|
||||
min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Clock rate error.
|
||||
* Positive if the hardware timer is slower than reference time.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
float getUtcRateCorrectionPPM();
|
||||
|
||||
/**
|
||||
* Number of non-gradual adjustments performed so far.
|
||||
* Ideally should be zero.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
uavcan::uint32_t getUtcJumpCount();
|
||||
|
||||
/**
|
||||
* Whether UTC is synchronized and locked.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
bool isUtcLocked();
|
||||
|
||||
/**
|
||||
* UTC sync params get/set.
|
||||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -7,31 +7,7 @@
|
|||
#include <uavcan_kinetis/thread.hpp>
|
||||
#include "internal.hpp"
|
||||
|
||||
#if UAVCAN_KINETIS_TIMER_NUMBER
|
||||
# include <cassert>
|
||||
# include <math.h>
|
||||
|
||||
/*
|
||||
* Timer instance
|
||||
* todo:Consider using Lifetime Timer support
|
||||
*/
|
||||
# define TIMX_IRQHandler UAVCAN_KINETIS_GLUE3(PIT, UAVCAN_KINETIS_TIMER_NUMBER, _IRQHandler)
|
||||
# define TIMX (KINETIS_PIT_BASE + (UAVCAN_KINETIS_TIMER_NUMBER << 4))
|
||||
# define TMR_REG(o) (TIMX + (o))
|
||||
# define TIMX_INPUT_CLOCK BOARD_BUS_FREQ
|
||||
# define TIMX_INTERRUPT_FREQ 16
|
||||
# define TIMX_IRQn UAVCAN_KINETIS_GLUE2(KINETIS_IRQ_PITCH, UAVCAN_KINETIS_TIMER_NUMBER)
|
||||
|
||||
# if UAVCAN_KINETIS_TIMER_NUMBER >= 0 && UAVCAN_KINETIS_TIMER_NUMBER <= 3
|
||||
# define KINETIS_PIT_LDVAL_OFFSET KINETIS_PIT_LDVAL0_OFFSET
|
||||
# define KINETIS_PIT_CVAL_OFFSET KINETIS_PIT_CVAL0_OFFSET
|
||||
# define KINETIS_PIT_TCTRL_OFFSET KINETIS_PIT_TCTRL0_OFFSET
|
||||
# define KINETIS_PIT_TFLG_OFFSET KINETIS_PIT_TFLG0_OFFSET
|
||||
# else
|
||||
# error "This UAVCAN_KINETIS_TIMER_NUMBER is not supported yet"
|
||||
# endif
|
||||
|
||||
extern "C" UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler);
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace uavcan_kinetis
|
||||
{
|
||||
|
@ -40,30 +16,12 @@ namespace clock
|
|||
namespace
|
||||
{
|
||||
|
||||
const uavcan::uint32_t CountsPerPeriod = (TIMX_INPUT_CLOCK / TIMX_INTERRUPT_FREQ);
|
||||
const uavcan::uint32_t CountsPerUs = (TIMX_INPUT_CLOCK / 1000000);
|
||||
const uavcan::uint32_t USecPerOverflow = (1000000 / TIMX_INTERRUPT_FREQ);
|
||||
|
||||
Mutex mutex;
|
||||
|
||||
bool initialized = false;
|
||||
|
||||
bool utc_set = false;
|
||||
bool utc_locked = false;
|
||||
uavcan::uint32_t utc_jump_cnt = 0;
|
||||
UtcSyncParams utc_sync_params;
|
||||
float utc_prev_adj = 0;
|
||||
float utc_rel_rate_ppm = 0;
|
||||
float utc_rel_rate_error_integral = 0;
|
||||
uavcan::int32_t utc_accumulated_correction_nsec = 0;
|
||||
uavcan::int32_t utc_correction_nsec_per_overflow = 0;
|
||||
uavcan::MonotonicTime prev_utc_adj_at;
|
||||
|
||||
uavcan::uint64_t time_mono = 0;
|
||||
uavcan::uint64_t time_utc = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void init()
|
||||
{
|
||||
CriticalSectionLocker lock;
|
||||
|
@ -73,231 +31,34 @@ void init()
|
|||
}
|
||||
|
||||
initialized = true;
|
||||
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
|
||||
// Power-on Clock
|
||||
modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
|
||||
|
||||
// Enable module
|
||||
putreg32(0, KINETIS_PIT_MCR);
|
||||
|
||||
// Start the timer
|
||||
|
||||
putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET));
|
||||
putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
|
||||
#if 0
|
||||
// This has to be off or uses the default priority
|
||||
// Without the ability to point the vector
|
||||
// Directly to this ISR this will reenter the
|
||||
// exception_common and cause the interrupt
|
||||
// Stack pointer to be reset
|
||||
up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
#endif
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
}
|
||||
|
||||
void setUtc(uavcan::UtcTime time)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
|
||||
return time + (cnt / CountsPerUs);
|
||||
// DO NOTHING
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
|
||||
usec = time + (cnt / CountsPerUs);
|
||||
|
||||
} // End Scope Critical section
|
||||
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
// total_rate_correction_ppm);
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
hrt_absolute_time_adjust(adj_usec);
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
@ -321,45 +82,5 @@ SystemClock &SystemClock::instance()
|
|||
return *ptr;
|
||||
}
|
||||
|
||||
} // namespace uavcan_kinetis
|
||||
} // namespace uavcan_stm32
|
||||
|
||||
|
||||
/**
|
||||
* Timer interrupt handler
|
||||
*/
|
||||
|
||||
extern "C"
|
||||
UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler)
|
||||
{
|
||||
putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET));
|
||||
|
||||
using namespace uavcan_kinetis::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
time_mono += USecPerOverflow;
|
||||
|
||||
if (utc_set) {
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0) {
|
||||
utc_correction_nsec_per_overflow--;
|
||||
|
||||
} else if (utc_correction_nsec_per_overflow < 0) {
|
||||
utc_correction_nsec_per_overflow++;
|
||||
|
||||
} else {
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -17,12 +17,3 @@
|
|||
#if !defined(UAVCAN_STM32_NUM_IFACES) || (UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2)
|
||||
# error "UAVCAN_STM32_NUM_IFACES must be set to either 1 or 2"
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5)
|
||||
* e.g. -DUAVCAN_STM32_TIMER_NUMBER=2
|
||||
*/
|
||||
#ifndef UAVCAN_STM32_TIMER_NUMBER
|
||||
// In this case the clock driver should be implemented by the application
|
||||
# define UAVCAN_STM32_TIMER_NUMBER 0
|
||||
#endif
|
||||
|
|
|
@ -45,56 +45,6 @@ uavcan::UtcTime getUtc();
|
|||
*/
|
||||
void adjustUtc(uavcan::UtcDuration adjustment);
|
||||
|
||||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F)
|
||||
, rate_i(0.02F)
|
||||
, rate_error_corner_freq(0.01F)
|
||||
, max_rate_correction_ppm(300.0F)
|
||||
, lock_thres_rate_ppm(2.0F)
|
||||
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
|
||||
, min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{ }
|
||||
};
|
||||
|
||||
/**
|
||||
* Clock rate error.
|
||||
* Positive if the hardware timer is slower than reference time.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
float getUtcRateCorrectionPPM();
|
||||
|
||||
/**
|
||||
* Number of non-gradual adjustments performed so far.
|
||||
* Ideally should be zero.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
uavcan::uint32_t getUtcJumpCount();
|
||||
|
||||
/**
|
||||
* Whether UTC is synchronized and locked.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
bool isUtcLocked();
|
||||
|
||||
/**
|
||||
* UTC sync params get/set.
|
||||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -6,49 +6,7 @@
|
|||
#include <uavcan_stm32/thread.hpp>
|
||||
#include "internal.hpp"
|
||||
|
||||
#if UAVCAN_STM32_TIMER_NUMBER
|
||||
|
||||
#include <cassert>
|
||||
#include <math.h>
|
||||
|
||||
/*
|
||||
* Timer instance
|
||||
*/
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
# define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE)
|
||||
# define TMR_REG(o) (TIMX + (o))
|
||||
# define TIMX_INPUT_CLOCK UAVCAN_STM32_GLUE3(STM32_APB1_TIM, UAVCAN_STM32_TIMER_NUMBER, _CLKIN)
|
||||
|
||||
# define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER)
|
||||
# endif
|
||||
|
||||
# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
|
||||
# define TIMX_RCC_ENR RCC->APB1ENR
|
||||
# define TIMX_RCC_RSTR RCC->APB1RSTR
|
||||
# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
|
||||
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
|
||||
# else
|
||||
# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
|
||||
# endif
|
||||
|
||||
/**
|
||||
* UAVCAN_STM32_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed.
|
||||
* This is useful at least with certain versions of ChibiOS which do not support the bit
|
||||
* RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active,
|
||||
* the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed.
|
||||
* Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870
|
||||
* A normal way to use the override feature is to provide an alternative macro, e.g.:
|
||||
*
|
||||
* -DUAVCAN_STM32_TIMX_INPUT_CLOCK=STM32_HCLK
|
||||
*
|
||||
* Alternatively, the new clock rate can be specified directly.
|
||||
*/
|
||||
# ifdef UAVCAN_STM32_TIMX_INPUT_CLOCK
|
||||
# undef TIMX_INPUT_CLOCK
|
||||
# define TIMX_INPUT_CLOCK UAVCAN_STM32_TIMX_INPUT_CLOCK
|
||||
# endif
|
||||
|
||||
extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace uavcan_stm32
|
||||
{
|
||||
|
@ -57,26 +15,9 @@ namespace clock
|
|||
namespace
|
||||
{
|
||||
|
||||
const uavcan::uint32_t USecPerOverflow = 65536;
|
||||
|
||||
Mutex mutex;
|
||||
|
||||
bool initialized = false;
|
||||
|
||||
bool utc_set = false;
|
||||
bool utc_locked = false;
|
||||
uavcan::uint32_t utc_jump_cnt = 0;
|
||||
UtcSyncParams utc_sync_params;
|
||||
float utc_prev_adj = 0;
|
||||
float utc_rel_rate_ppm = 0;
|
||||
float utc_rel_rate_error_integral = 0;
|
||||
uavcan::int32_t utc_accumulated_correction_nsec = 0;
|
||||
uavcan::int32_t utc_correction_nsec_per_overflow = 0;
|
||||
uavcan::MonotonicTime prev_utc_adj_at;
|
||||
|
||||
uavcan::uint64_t time_mono = 0;
|
||||
uavcan::uint64_t time_utc = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
@ -89,246 +30,34 @@ void init()
|
|||
}
|
||||
|
||||
initialized = true;
|
||||
|
||||
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
|
||||
// Power-on and reset
|
||||
modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
|
||||
modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
|
||||
modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
|
||||
|
||||
|
||||
// Start the timer
|
||||
putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
|
||||
putreg16(((TIMX_INPUT_CLOCK / 1000000) - 1), TMR_REG(STM32_BTIM_PSC_OFFSET));
|
||||
putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
|
||||
putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
|
||||
putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
|
||||
// need to investigate
|
||||
// up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
|
||||
# endif
|
||||
}
|
||||
|
||||
void setUtc(uavcan::UtcTime time)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
|
||||
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
|
||||
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
|
||||
return time + cnt;
|
||||
# endif
|
||||
// DO NOTHING
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
|
||||
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
|
||||
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
|
||||
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
# endif
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
|
||||
usec = time + cnt;
|
||||
|
||||
# ifndef NDEBUG
|
||||
static uavcan::uint64_t prev_usec = 0; // Self-test
|
||||
UAVCAN_ASSERT(prev_usec <= usec);
|
||||
(void)prev_usec;
|
||||
prev_usec = usec;
|
||||
# endif
|
||||
} // End Scope Critical section
|
||||
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
// total_rate_correction_ppm);
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
hrt_absolute_time_adjust(adj_usec);
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
|
|||
|
||||
} // namespace uavcan_stm32
|
||||
|
||||
|
||||
/**
|
||||
* Timer interrupt handler
|
||||
*/
|
||||
|
||||
extern "C"
|
||||
UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
|
||||
{
|
||||
UAVCAN_STM32_IRQ_PROLOGUE();
|
||||
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
# endif
|
||||
|
||||
using namespace uavcan_stm32::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
time_mono += USecPerOverflow;
|
||||
|
||||
if (utc_set) {
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0) {
|
||||
utc_correction_nsec_per_overflow--;
|
||||
|
||||
} else if (utc_correction_nsec_per_overflow < 0) {
|
||||
utc_correction_nsec_per_overflow++;
|
||||
|
||||
} else {
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
|
||||
UAVCAN_STM32_IRQ_EPILOGUE();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -17,12 +17,3 @@
|
|||
#if !defined(UAVCAN_STM32H7_NUM_IFACES) || (UAVCAN_STM32H7_NUM_IFACES != 1 && UAVCAN_STM32H7_NUM_IFACES != 2)
|
||||
# error "UAVCAN_STM32H7_NUM_IFACES must be set to either 1 or 2"
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5)
|
||||
* e.g. -DUAVCAN_STM32H7_TIMER_NUMBER=2
|
||||
*/
|
||||
#ifndef UAVCAN_STM32H7_TIMER_NUMBER
|
||||
// In this case the clock driver should be implemented by the application
|
||||
# define UAVCAN_STM32H7_TIMER_NUMBER 0
|
||||
#endif
|
||||
|
|
|
@ -45,56 +45,6 @@ uavcan::UtcTime getUtc();
|
|||
*/
|
||||
void adjustUtc(uavcan::UtcDuration adjustment);
|
||||
|
||||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F)
|
||||
, rate_i(0.02F)
|
||||
, rate_error_corner_freq(0.01F)
|
||||
, max_rate_correction_ppm(300.0F)
|
||||
, lock_thres_rate_ppm(2.0F)
|
||||
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
|
||||
, min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{ }
|
||||
};
|
||||
|
||||
/**
|
||||
* Clock rate error.
|
||||
* Positive if the hardware timer is slower than reference time.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
float getUtcRateCorrectionPPM();
|
||||
|
||||
/**
|
||||
* Number of non-gradual adjustments performed so far.
|
||||
* Ideally should be zero.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
uavcan::uint32_t getUtcJumpCount();
|
||||
|
||||
/**
|
||||
* Whether UTC is synchronized and locked.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
bool isUtcLocked();
|
||||
|
||||
/**
|
||||
* UTC sync params get/set.
|
||||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -6,49 +6,7 @@
|
|||
#include <uavcan_stm32h7/thread.hpp>
|
||||
#include "internal.hpp"
|
||||
|
||||
#if UAVCAN_STM32H7_TIMER_NUMBER
|
||||
|
||||
#include <cassert>
|
||||
#include <math.h>
|
||||
|
||||
/*
|
||||
* Timer instance
|
||||
*/
|
||||
# if UAVCAN_STM32H7_NUTTX
|
||||
# define TIMX UAVCAN_STM32H7_GLUE3(STM32_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _BASE)
|
||||
# define TMR_REG(o) (TIMX + (o))
|
||||
# define TIMX_INPUT_CLOCK UAVCAN_STM32H7_GLUE3(STM32_APB1_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _CLKIN)
|
||||
|
||||
# define TIMX_IRQn UAVCAN_STM32H7_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32H7_TIMER_NUMBER)
|
||||
# endif
|
||||
|
||||
# if UAVCAN_STM32H7_TIMER_NUMBER >= 2 && UAVCAN_STM32H7_TIMER_NUMBER <= 7
|
||||
# define TIMX_RCC_ENR RCC->APB1ENR
|
||||
# define TIMX_RCC_RSTR RCC->APB1RSTR
|
||||
# define TIMX_RCC_ENR_MASK UAVCAN_STM32H7_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, EN)
|
||||
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32H7_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, RST)
|
||||
# else
|
||||
# error "This UAVCAN_STM32H7_TIMER_NUMBER is not supported yet"
|
||||
# endif
|
||||
|
||||
/**
|
||||
* UAVCAN_STM32H7_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed.
|
||||
* This is useful at least with certain versions of ChibiOS which do not support the bit
|
||||
* RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active,
|
||||
* the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed.
|
||||
* Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870
|
||||
* A normal way to use the override feature is to provide an alternative macro, e.g.:
|
||||
*
|
||||
* -DUAVCAN_STM32H7_TIMX_INPUT_CLOCK=STM32_HCLK
|
||||
*
|
||||
* Alternatively, the new clock rate can be specified directly.
|
||||
*/
|
||||
# ifdef UAVCAN_STM32H7_TIMX_INPUT_CLOCK
|
||||
# undef TIMX_INPUT_CLOCK
|
||||
# define TIMX_INPUT_CLOCK UAVCAN_STM32H7_TIMX_INPUT_CLOCK
|
||||
# endif
|
||||
|
||||
extern "C" UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler);
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace uavcan_stm32h7
|
||||
{
|
||||
|
@ -57,26 +15,9 @@ namespace clock
|
|||
namespace
|
||||
{
|
||||
|
||||
const uavcan::uint32_t USecPerOverflow = 65536;
|
||||
|
||||
Mutex mutex;
|
||||
|
||||
bool initialized = false;
|
||||
|
||||
bool utc_set = false;
|
||||
bool utc_locked = false;
|
||||
uavcan::uint32_t utc_jump_cnt = 0;
|
||||
UtcSyncParams utc_sync_params;
|
||||
float utc_prev_adj = 0;
|
||||
float utc_rel_rate_ppm = 0;
|
||||
float utc_rel_rate_error_integral = 0;
|
||||
uavcan::int32_t utc_accumulated_correction_nsec = 0;
|
||||
uavcan::int32_t utc_correction_nsec_per_overflow = 0;
|
||||
uavcan::MonotonicTime prev_utc_adj_at;
|
||||
|
||||
uavcan::uint64_t time_mono = 0;
|
||||
uavcan::uint64_t time_utc = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
@ -89,246 +30,34 @@ void init()
|
|||
}
|
||||
|
||||
initialized = true;
|
||||
|
||||
|
||||
# if UAVCAN_STM32H7_NUTTX
|
||||
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
|
||||
// Power-on and reset
|
||||
modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
|
||||
modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
|
||||
modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
|
||||
|
||||
|
||||
// Start the timer
|
||||
putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
|
||||
putreg16(((TIMX_INPUT_CLOCK / 1000000) - 1), TMR_REG(STM32_BTIM_PSC_OFFSET));
|
||||
putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
|
||||
putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
|
||||
putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
|
||||
// need to investigate
|
||||
// up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
|
||||
# endif
|
||||
}
|
||||
|
||||
void setUtc(uavcan::UtcTime time)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
# if UAVCAN_STM32H7_NUTTX
|
||||
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
|
||||
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
|
||||
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
|
||||
return time + cnt;
|
||||
# endif
|
||||
// DO NOTHING
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
|
||||
# if UAVCAN_STM32H7_NUTTX
|
||||
|
||||
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
|
||||
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
|
||||
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
# endif
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
|
||||
usec = time + cnt;
|
||||
|
||||
# ifndef NDEBUG
|
||||
static uavcan::uint64_t prev_usec = 0; // Self-test
|
||||
UAVCAN_ASSERT(prev_usec <= usec);
|
||||
(void)prev_usec;
|
||||
prev_usec = usec;
|
||||
# endif
|
||||
} // End Scope Critical section
|
||||
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
// total_rate_correction_ppm);
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
hrt_absolute_time_adjust(adj_usec);
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
|
|||
|
||||
} // namespace uavcan_stm32h7
|
||||
|
||||
|
||||
/**
|
||||
* Timer interrupt handler
|
||||
*/
|
||||
|
||||
extern "C"
|
||||
UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler)
|
||||
{
|
||||
UAVCAN_STM32H7_IRQ_PROLOGUE();
|
||||
|
||||
# if UAVCAN_STM32H7_NUTTX
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
# endif
|
||||
|
||||
using namespace uavcan_stm32h7::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
time_mono += USecPerOverflow;
|
||||
|
||||
if (utc_set) {
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0) {
|
||||
utc_correction_nsec_per_overflow--;
|
||||
|
||||
} else if (utc_correction_nsec_per_overflow < 0) {
|
||||
utc_correction_nsec_per_overflow++;
|
||||
|
||||
} else {
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
|
||||
UAVCAN_STM32H7_IRQ_EPILOGUE();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -744,41 +744,6 @@ UavcanNode::Run()
|
|||
|
||||
_node.spinOnce(); // expected to be non-blocking
|
||||
|
||||
// Publish status
|
||||
constexpr hrt_abstime status_pub_interval = 100_ms;
|
||||
|
||||
if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) {
|
||||
_last_can_status_pub = hrt_absolute_time();
|
||||
|
||||
for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
|
||||
if (i > UAVCAN_NUM_IFACES) {
|
||||
break;
|
||||
}
|
||||
|
||||
auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
|
||||
|
||||
if (!iface) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
|
||||
can_interface_status_s status{
|
||||
.timestamp = hrt_absolute_time(),
|
||||
.io_errors = iface_perf_cnt.errors,
|
||||
.frames_tx = iface_perf_cnt.frames_tx,
|
||||
.frames_rx = iface_perf_cnt.frames_rx,
|
||||
.interface = static_cast<uint8_t>(i),
|
||||
};
|
||||
|
||||
if (_can_status_pub_handles[i] == nullptr) {
|
||||
int instance{0};
|
||||
_can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance);
|
||||
}
|
||||
|
||||
(void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status);
|
||||
}
|
||||
}
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
|
|
|
@ -96,7 +96,6 @@
|
|||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/can_interface_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/uavcan_parameter_request.h>
|
||||
#include <uORB/topics/uavcan_parameter_value.h>
|
||||
|
@ -310,10 +309,6 @@ private:
|
|||
|
||||
uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
|
||||
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::PublicationMulti<can_interface_status_s> _can_status_pub{ORB_ID(can_interface_status)};
|
||||
|
||||
hrt_abstime _last_can_status_pub{0};
|
||||
orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr};
|
||||
|
||||
/*
|
||||
* The MAVLink parameter bridge needs to know the maximum parameter index
|
||||
|
|
|
@ -42,22 +42,12 @@ set(UAVCAN_PLATFORM "generic")
|
|||
if(CONFIG_ARCH_CHIP)
|
||||
if(${CONFIG_NET_CAN} MATCHES "y")
|
||||
set(UAVCAN_DRIVER "socketcan")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
set(UAVCAN_DRIVER "kinetis")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
|
||||
set(UAVCAN_DRIVER "stm32h7")
|
||||
set(UAVCAN_TIMER 5) # The default timer is TIM5
|
||||
if (DEFINED config_uavcan_timer_override)
|
||||
set (UAVCAN_TIMER ${config_uavcan_timer_override})
|
||||
endif()
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
|
||||
set(UAVCAN_DRIVER "stm32")
|
||||
set(UAVCAN_TIMER 5) # The default timer is TIM5
|
||||
if (DEFINED config_uavcan_timer_override)
|
||||
set (UAVCAN_TIMER ${config_uavcan_timer_override})
|
||||
endif()
|
||||
endif()
|
||||
endif()
|
||||
|
||||
|
@ -74,7 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
|
|||
add_definitions(
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
|
||||
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
|
||||
|
|
|
@ -364,7 +364,7 @@
|
|||
},
|
||||
"1": {
|
||||
"name": "manual_control_loss",
|
||||
"description": "Manual control loss"
|
||||
"description": "manual control loss"
|
||||
},
|
||||
"2": {
|
||||
"name": "gcs_connection_loss",
|
||||
|
@ -372,19 +372,19 @@
|
|||
},
|
||||
"3": {
|
||||
"name": "low_battery_level",
|
||||
"description": "Low battery level"
|
||||
"description": "low battery level"
|
||||
},
|
||||
"4": {
|
||||
"name": "critical_battery_level",
|
||||
"description": "Critical battery level"
|
||||
"description": "critical battery level"
|
||||
},
|
||||
"5": {
|
||||
"name": "emergency_battery_level",
|
||||
"description": "Emergency battery level"
|
||||
"description": "emergency battery level"
|
||||
},
|
||||
"6": {
|
||||
"name": "low_remaining_flight_time",
|
||||
"description": "Remaining flight time low"
|
||||
"description": "low remaining flight time"
|
||||
}
|
||||
}
|
||||
},
|
||||
|
@ -402,19 +402,19 @@
|
|||
},
|
||||
"2": {
|
||||
"name": "fallback_posctrl",
|
||||
"description": "Position mode"
|
||||
"description": "fallback to position control"
|
||||
},
|
||||
"3": {
|
||||
"name": "fallback_altctrl",
|
||||
"description": "Altitude mode"
|
||||
"description": "fallback to altitude control"
|
||||
},
|
||||
"4": {
|
||||
"name": "fallback_stabilized",
|
||||
"description": "Stabilized mode"
|
||||
"description": "fallback to stabilized"
|
||||
},
|
||||
"5": {
|
||||
"name": "hold",
|
||||
"description": "Hold"
|
||||
"description": "hold"
|
||||
},
|
||||
"6": {
|
||||
"name": "rtl",
|
||||
|
@ -422,11 +422,11 @@
|
|||
},
|
||||
"7": {
|
||||
"name": "land",
|
||||
"description": "Land"
|
||||
"description": "land"
|
||||
},
|
||||
"8": {
|
||||
"name": "descend",
|
||||
"description": "Descend"
|
||||
"description": "descend"
|
||||
},
|
||||
"9": {
|
||||
"name": "disarm",
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2022 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
|
||||
|
@ -51,18 +51,17 @@ static constexpr float sq(float var) { return var * var; }
|
|||
// rot312(1) - Second rotation is a RH rotation about the X axis (rad)
|
||||
// rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
|
||||
// See http://www.atacolorado.com/eulersequences.doc
|
||||
template<typename T = float>
|
||||
inline matrix::Dcm<T> taitBryan312ToRotMat(const matrix::Vector3<T> &rot312)
|
||||
inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
|
||||
{
|
||||
// Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence
|
||||
const T c2 = cosf(rot312(2)); // third rotation is pitch
|
||||
const T s2 = sinf(rot312(2));
|
||||
const T s1 = sinf(rot312(1)); // second rotation is roll
|
||||
const T c1 = cosf(rot312(1));
|
||||
const T s0 = sinf(rot312(0)); // first rotation is yaw
|
||||
const T c0 = cosf(rot312(0));
|
||||
const float c2 = cosf(rot312(2)); // third rotation is pitch
|
||||
const float s2 = sinf(rot312(2));
|
||||
const float s1 = sinf(rot312(1)); // second rotation is roll
|
||||
const float c1 = cosf(rot312(1));
|
||||
const float s0 = sinf(rot312(0)); // first rotation is yaw
|
||||
const float c0 = cosf(rot312(0));
|
||||
|
||||
matrix::Dcm<T> R;
|
||||
matrix::Dcmf R;
|
||||
R(0, 0) = c0 * c2 - s0 * s1 * s2;
|
||||
R(1, 1) = c0 * c1;
|
||||
R(2, 2) = c2 * c1;
|
||||
|
@ -76,21 +75,20 @@ inline matrix::Dcm<T> taitBryan312ToRotMat(const matrix::Vector3<T> &rot312)
|
|||
return R;
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline matrix::Dcm<T> quatToInverseRotMat(const matrix::Quaternion<T> &quat)
|
||||
inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
|
||||
{
|
||||
const T q00 = quat(0) * quat(0);
|
||||
const T q11 = quat(1) * quat(1);
|
||||
const T q22 = quat(2) * quat(2);
|
||||
const T q33 = quat(3) * quat(3);
|
||||
const T q01 = quat(0) * quat(1);
|
||||
const T q02 = quat(0) * quat(2);
|
||||
const T q03 = quat(0) * quat(3);
|
||||
const T q12 = quat(1) * quat(2);
|
||||
const T q13 = quat(1) * quat(3);
|
||||
const T q23 = quat(2) * quat(3);
|
||||
const float q00 = quat(0) * quat(0);
|
||||
const float q11 = quat(1) * quat(1);
|
||||
const float q22 = quat(2) * quat(2);
|
||||
const float q33 = quat(3) * quat(3);
|
||||
const float q01 = quat(0) * quat(1);
|
||||
const float q02 = quat(0) * quat(2);
|
||||
const float q03 = quat(0) * quat(3);
|
||||
const float q12 = quat(1) * quat(2);
|
||||
const float q13 = quat(1) * quat(3);
|
||||
const float q23 = quat(2) * quat(3);
|
||||
|
||||
matrix::Dcm<T> dcm;
|
||||
matrix::Dcmf dcm;
|
||||
dcm(0, 0) = q00 + q11 - q22 - q33;
|
||||
dcm(1, 1) = q00 - q11 + q22 - q33;
|
||||
dcm(2, 2) = q00 - q11 - q22 + q33;
|
||||
|
@ -107,46 +105,40 @@ inline matrix::Dcm<T> quatToInverseRotMat(const matrix::Quaternion<T> &quat)
|
|||
// We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
|
||||
// when there is more roll than pitch tilt and a 3-1-2 rotation sequence
|
||||
// when there is more pitch than roll tilt to avoid gimbal lock.
|
||||
template<typename T = float>
|
||||
inline bool shouldUse321RotationSequence(const matrix::Dcm<T> &R)
|
||||
inline bool shouldUse321RotationSequence(const matrix::Dcmf &R)
|
||||
{
|
||||
return fabsf(R(2, 0)) < fabsf(R(2, 1));
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline float getEuler321Yaw(const matrix::Dcm<T> &R)
|
||||
inline float getEuler321Yaw(const matrix::Dcmf &R)
|
||||
{
|
||||
return atan2f(R(1, 0), R(0, 0));
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline float getEuler312Yaw(const matrix::Dcm<T> &R)
|
||||
inline float getEuler312Yaw(const matrix::Dcmf &R)
|
||||
{
|
||||
return atan2f(-R(0, 1), R(1, 1));
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline T getEuler321Yaw(const matrix::Quaternion<T> &q)
|
||||
inline float getEuler321Yaw(const matrix::Quatf &q)
|
||||
{
|
||||
// Values from yaw_input_321.c file produced by
|
||||
// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
|
||||
const T a = static_cast<T>(2.) * (q(0) * q(3) + q(1) * q(2));
|
||||
const T b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3));
|
||||
const float a = 2.f * (q(0) * q(3) + q(1) * q(2));
|
||||
const float b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3));
|
||||
return atan2f(a, b);
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline T getEuler312Yaw(const matrix::Quaternion<T> &q)
|
||||
inline float getEuler312Yaw(const matrix::Quatf &q)
|
||||
{
|
||||
// Values from yaw_input_312.c file produced by
|
||||
// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
|
||||
const T a = static_cast<T>(2.) * (q(0) * q(3) - q(1) * q(2));
|
||||
const T b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3));
|
||||
const float a = 2.f * (q(0) * q(3) - q(1) * q(2));
|
||||
const float b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3));
|
||||
return atan2f(a, b);
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline T getEulerYaw(const matrix::Dcm<T> &R)
|
||||
inline float getEulerYaw(const matrix::Dcmf &R)
|
||||
{
|
||||
if (shouldUse321RotationSequence(R)) {
|
||||
return getEuler321Yaw(R);
|
||||
|
@ -156,32 +148,23 @@ inline T getEulerYaw(const matrix::Dcm<T> &R)
|
|||
}
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline T getEulerYaw(const matrix::Quaternion<T> &q)
|
||||
inline matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
|
||||
{
|
||||
return getEulerYaw(matrix::Dcm<T>(q));
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline matrix::Dcm<T> updateEuler321YawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
|
||||
{
|
||||
matrix::Euler<T> euler321(rot_in);
|
||||
matrix::Eulerf euler321(rot_in);
|
||||
euler321(2) = yaw;
|
||||
return matrix::Dcm<T>(euler321);
|
||||
return matrix::Dcmf(euler321);
|
||||
}
|
||||
|
||||
template<typename T = float>
|
||||
inline matrix::Dcm<T> updateEuler312YawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
|
||||
inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
|
||||
{
|
||||
const matrix::Vector3<T> rotVec312(yaw, // yaw
|
||||
asinf(rot_in(2, 1)), // roll
|
||||
atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch
|
||||
const matrix::Vector3f rotVec312(yaw, // yaw
|
||||
asinf(rot_in(2, 1)), // roll
|
||||
atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch
|
||||
return taitBryan312ToRotMat(rotVec312);
|
||||
}
|
||||
|
||||
// Checks which euler rotation sequence to use and update yaw in rotation matrix
|
||||
template<typename T = float>
|
||||
inline matrix::Dcm<T> updateYawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
|
||||
inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in)
|
||||
{
|
||||
if (shouldUse321RotationSequence(rot_in)) {
|
||||
return updateEuler321YawInRotMat(yaw, rot_in);
|
||||
|
|
|
@ -455,8 +455,7 @@ bool MixingOutput::update()
|
|||
}
|
||||
}
|
||||
|
||||
// Send output if any function mapped or one last disabling sample
|
||||
if (!all_disabled || !_was_all_disabled) {
|
||||
if (!all_disabled) {
|
||||
if (!_armed.armed && !_armed.manual_lockdown) {
|
||||
_actuator_test.overrideValues(outputs, _max_num_outputs);
|
||||
}
|
||||
|
@ -464,8 +463,6 @@ bool MixingOutput::update()
|
|||
limitAndUpdateOutputs(outputs, has_updates);
|
||||
}
|
||||
|
||||
_was_all_disabled = all_disabled;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -288,7 +288,6 @@ 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
|
||||
|
||||
|
|
|
@ -191,15 +191,11 @@ TEST_F(MixerModuleTest, basic)
|
|||
mixing_output.setAllMaxValues(MAX_VALUE);
|
||||
EXPECT_EQ(test_module.num_updates, 0);
|
||||
|
||||
// all functions disabled: expect to get one single update to process disabling the output signal
|
||||
// all functions disabled: not expected to get an update
|
||||
mixing_output.update();
|
||||
mixing_output.updateSubscriptions(false);
|
||||
mixing_output.update();
|
||||
EXPECT_EQ(test_module.num_updates, 1);
|
||||
mixing_output.update();
|
||||
mixing_output.updateSubscriptions(false);
|
||||
mixing_output.update();
|
||||
EXPECT_EQ(test_module.num_updates, 1);
|
||||
EXPECT_EQ(test_module.num_updates, 0);
|
||||
test_module.reset();
|
||||
|
||||
// configure motor, ensure all still disarmed
|
||||
|
|
|
@ -598,9 +598,6 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
|||
|
||||
if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) {
|
||||
tune_negative(true);
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Resolve system health failures first\t");
|
||||
events::send(events::ID("commander_arm_denied_resolve_failures"), {events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: Resolve system health failures first");
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
}
|
||||
|
@ -2422,14 +2419,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
|||
if (_cpuload_sub.copy(&cpuload)) {
|
||||
const float cpuload_percent = cpuload.load * 100.f;
|
||||
|
||||
bool overload = false;
|
||||
|
||||
// Only check CPU load if it hasn't been disabled
|
||||
if (!(_param_com_cpu_max.get() < FLT_EPSILON)) {
|
||||
overload = (cpuload_percent > _param_com_cpu_max.get());
|
||||
}
|
||||
|
||||
overload = overload || (cpuload.ram_usage > 0.99f);
|
||||
bool overload = (cpuload_percent > _param_com_cpu_max.get()) || (cpuload.ram_usage > 0.99f);
|
||||
|
||||
if (_overload_start == 0 && overload) {
|
||||
_overload_start = time_now_us;
|
||||
|
|
|
@ -293,9 +293,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
|||
mavlink_log_warning(reporter.mavlink_log_pub(), "GNSS data fusion stopped\t");
|
||||
}
|
||||
|
||||
// only report this failure as critical if not already in a local position invalid state
|
||||
events::Log log_level = reporter.failsafeFlags().local_position_invalid ? events::Log::Info : events::Log::Error;
|
||||
events::send(events::ID("check_estimator_gnss_fusion_stopped"), {log_level, events::LogInternal::Info},
|
||||
events::send(events::ID("check_estimator_gnss_fusion_stopped"), {events::Log::Error, events::LogInternal::Info},
|
||||
"GNSS data fusion stopped");
|
||||
|
||||
} else if (!_gps_was_fused && ekf_gps_fusion) {
|
||||
|
|
|
@ -369,7 +369,7 @@ FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int par
|
|||
{
|
||||
ActionOptions options{};
|
||||
|
||||
options.allow_user_takeover = UserTakeoverAllowed::Auto;
|
||||
options.allow_user_takeover = UserTakeoverAllowed::Always;
|
||||
options.cause = Cause::RemainingFlightTimeLow;
|
||||
|
||||
switch (command_after_remaining_flight_time_low(param_value)) {
|
||||
|
|
|
@ -195,7 +195,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
|
|||
events::send<uint32_t, events::px4::enums::failsafe_action_t, uint16_t>(
|
||||
events::ID("commander_failsafe_enter_generic_hold"),
|
||||
{events::Log::Critical, events::LogInternal::Warning},
|
||||
"Failsafe, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
|
||||
"Failsafe activated, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
|
||||
(uint16_t) delay_s);
|
||||
|
||||
} else {
|
||||
|
@ -204,7 +204,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
|
|||
events::send<uint32_t, events::px4::enums::failsafe_action_t, uint16_t, events::px4::enums::failsafe_cause_t>(
|
||||
events::ID("commander_failsafe_enter_hold"),
|
||||
{events::Log::Critical, events::LogInternal::Warning},
|
||||
"{4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
|
||||
"Failsafe activated due to {4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
|
||||
(uint16_t) delay_s, failsafe_cause);
|
||||
}
|
||||
|
||||
|
@ -231,7 +231,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
|
|||
events::send<uint32_t, events::px4::enums::failsafe_action_t>(
|
||||
events::ID("commander_failsafe_enter_generic"),
|
||||
{events::Log::Critical, events::LogInternal::Warning},
|
||||
"Failsafe, triggering {2}", mavlink_mode, failsafe_action);
|
||||
"Failsafe activated, triggering {2}", mavlink_mode, failsafe_action);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
@ -272,7 +272,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
|
|||
events::send<uint32_t, events::px4::enums::failsafe_action_t, events::px4::enums::failsafe_cause_t>(
|
||||
events::ID("commander_failsafe_enter"),
|
||||
{events::Log::Critical, events::LogInternal::Warning},
|
||||
"{3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause);
|
||||
"Failsafe activated due to {3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -517,8 +517,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
|
|||
break;
|
||||
}
|
||||
|
||||
returned_state.cause = Cause::Generic;
|
||||
|
||||
// fallthrough
|
||||
case Action::FallbackAltCtrl:
|
||||
if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_ALTCTL)) {
|
||||
|
@ -526,8 +524,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
|
|||
break;
|
||||
}
|
||||
|
||||
returned_state.cause = Cause::Generic;
|
||||
|
||||
// fallthrough
|
||||
case Action::FallbackStab:
|
||||
if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_STAB)) {
|
||||
|
@ -535,8 +531,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
|
|||
break;
|
||||
} // else: fall through here as well. If stabilized isn't available, we most certainly end up in Terminate
|
||||
|
||||
returned_state.cause = Cause::Generic;
|
||||
|
||||
// fallthrough
|
||||
case Action::Hold:
|
||||
if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
|
||||
|
@ -544,8 +538,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
|
|||
break;
|
||||
}
|
||||
|
||||
returned_state.cause = Cause::Generic;
|
||||
|
||||
// fallthrough
|
||||
case Action::RTL:
|
||||
if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
|
||||
|
@ -553,8 +545,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
|
|||
break;
|
||||
}
|
||||
|
||||
returned_state.cause = Cause::Generic;
|
||||
|
||||
// fallthrough
|
||||
case Action::Land:
|
||||
if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
|
||||
|
@ -562,8 +552,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
|
|||
break;
|
||||
}
|
||||
|
||||
returned_state.cause = Cause::Generic;
|
||||
|
||||
// fallthrough
|
||||
case Action::Descend:
|
||||
if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_DESCEND)) {
|
||||
|
@ -571,8 +559,6 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
|
|||
break;
|
||||
}
|
||||
|
||||
returned_state.cause = Cause::Generic;
|
||||
|
||||
// fallthrough
|
||||
case Action::Terminate:
|
||||
selected_action = Action::Terminate;
|
||||
|
|
|
@ -147,7 +147,7 @@ public:
|
|||
bool rc_sticks_takeover_request,
|
||||
const failsafe_flags_s &status_flags);
|
||||
|
||||
bool inFailsafe() const { return (_selected_action != Action::None && _selected_action != Action::Warn); }
|
||||
bool inFailsafe() const { return _selected_action != Action::None; }
|
||||
|
||||
Action selectedAction() const { return _selected_action; }
|
||||
|
||||
|
|
|
@ -485,14 +485,9 @@ public:
|
|||
#else
|
||||
// Efficient implementation of the Joseph stabilized covariance update
|
||||
// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
|
||||
// P = (I - K * H) * P * (I - K * H).T + K * R * K.T
|
||||
// = P_temp * (I - H.T * K.T) + K * R * K.T
|
||||
// = P_temp - P_temp * H.T * K.T + K * R * K.T
|
||||
|
||||
// Step 1: conventional update
|
||||
// Compute P_temp and store it in P to avoid allocating more memory
|
||||
// P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major
|
||||
VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T
|
||||
VectorState PH = P * H;
|
||||
|
||||
for (unsigned i = 0; i < State::size; i++) {
|
||||
for (unsigned j = 0; j < State::size; j++) {
|
||||
|
@ -501,7 +496,7 @@ public:
|
|||
}
|
||||
|
||||
// Step 2: stabilized update
|
||||
PH = P * H; // H is stored as a column vector. H is in fact H.T
|
||||
PH = P * H;
|
||||
|
||||
for (unsigned i = 0; i < State::size; i++) {
|
||||
for (unsigned j = 0; j <= i; j++) {
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue