fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 12:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into pr-boards_serial_rc

Browse Source
This commit is contained in:
Ramon Roche
2025-07-21 12:32:24 -07:00
committed by GitHub
parent d9039b0cf6 8dd14d9aaa
commit 8575199bc7
6419 changed files with 609408 additions and 20370 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/common/include/px4_platform_common/defines.h
+1 -1
View File
@@ -97,7 +97,7 @@ __BEGIN_DECLS
extern long PX4_TICKS_PER_SEC;
__END_DECLS
#define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR
#define PX4_ROOTFSDIR CONFIG_BOARD_ROOT_PATH
// Qurt doesn't have an SD card for storage
#ifndef __PX4_QURT
platforms/common/include/px4_platform_common/log.h
+16 -16
View File
@@ -70,33 +70,33 @@ __END_DECLS
/****************************************************************************
* Messages that should never be filtered or compiled out
****************************************************************************/
#define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_INFO, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_INFO(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_INFO, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_INFO_RAW(FMT, ...) qurt_log_raw(FMT, ##__VA_ARGS__)
#if defined(TRACE_BUILD)
/****************************************************************************
* Extremely Verbose settings for a Trace build
****************************************************************************/
#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_PANIC(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_PANIC, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_ERROR, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_WARN, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_DEBUG, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#elif defined(DEBUG_BUILD)
/****************************************************************************
* Verbose settings for a Debug build
****************************************************************************/
#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_PANIC(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_PANIC, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_ERROR, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_WARN, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_DEBUG, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#elif defined(RELEASE_BUILD)
/****************************************************************************
* Non-verbose settings for a Release build to minimize strings in build
****************************************************************************/
#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_PANIC(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_PANIC, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_ERROR, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
@@ -104,14 +104,14 @@ __END_DECLS
/****************************************************************************
* Medium verbose settings for a default build
****************************************************************************/
#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_PANIC(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_PANIC, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_ERROR, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log_module(_PX4_LOG_LEVEL_WARN, MODULE_NAME, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
#endif
#define PX4_LOG_NAMED(name, FMT, ...) qurt_log( _PX4_LOG_LEVEL_INFO, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__)
#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_INFO, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__)
#define PX4_LOG_NAMED(name, FMT, ...) qurt_log_module( _PX4_LOG_LEVEL_INFO, MODULE_NAME, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__)
#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log_module( _PX4_LOG_LEVEL_INFO, MODULE_NAME, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__)
#else
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+1 -1
View File
@@ -89,7 +89,7 @@ static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30};
static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31};
static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32};
static constexpr wq_config_t lp_default{"wq:lp_default", 2350, -50};
static constexpr wq_config_t lp_default{"wq:lp_default", 3500, -50};
static constexpr wq_config_t test1{"wq:test1", 2000, 0};
static constexpr wq_config_t test2{"wq:test2", 2000, 0};
platforms/common/pab_manifest.c
+2 -1
View File
@@ -404,7 +404,7 @@ static const px4_hw_mft_item_t base_configuration_18[] = {
// BASE ID 0x100 Holybro Pixhawk Jetson Baseboard Alaised to ID 0
// BASE ID 0x150 ZeroOne Pixhawk Baseboard Alaised to ID 0
// BASE ID 0x200 AmovLab Pixhawk Baseboard Alaised to ID 0
static px4_hw_mft_list_entry_t mft_lists[] = {
// ver_rev
{HW_BASE_ID(0), base_configuration_0, arraySize(base_configuration_0)}, // std Base with PX4IO
@@ -421,6 +421,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
{HW_BASE_ID(18), base_configuration_18, arraySize(base_configuration_18)}, // Auterion Skynode S ver 18
{HW_BASE_ID(0x100), base_configuration_0, arraySize(base_configuration_0)}, // Holybro Pixhawk Jetson Baseboard ver 0x100 Alaised to ID 0
{HW_BASE_ID(0x150), base_configuration_0, arraySize(base_configuration_0)}, // ZeroOne Pixhawk Baseboard ver 0x150
{HW_BASE_ID(0x200), base_configuration_0, arraySize(base_configuration_0)}, // AmovLab Pixhawk Baseboard ver 0x150
};
/************************************************************************************
platforms/common/uORB/CMakeLists.txt
+3 -1
View File
@@ -112,4 +112,6 @@ if(PX4_TESTING)
add_subdirectory(uORB_tests)
endif()
px4_add_functional_gtest(SRC uORBMessageFieldsTest.cpp LINKLIBS uORB)
if(PX4_TESTING)
add_subdirectory(test)
endif()
platforms/common/uORB/Subscription.hpp
+10 -18
View File
@@ -118,16 +118,8 @@ public:
bool valid() const { return _node != nullptr; }
bool advertised()
{
if (valid()) {
return Manager::is_advertised(_node);
}
// try to initialize
if (subscribe()) {
// check again if valid
if (valid()) {
return Manager::is_advertised(_node);
}
return Manager::is_advertised(_node);
}
return false;
@@ -138,11 +130,11 @@ public:
*/
bool updated()
{
if (!valid()) {
subscribe();
if (subscribe()) {
return Manager::updates_available(_node, _last_generation);
}
return valid() ? Manager::updates_available(_node, _last_generation) : false;
return false;
}
/**
@@ -151,11 +143,11 @@ public:
*/
bool update(void *dst)
{
if (!valid()) {
subscribe();
if (subscribe()) {
return Manager::orb_data_copy(_node, dst, _last_generation, true);
}
return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false;
return false;
}
/**
@@ -164,11 +156,11 @@ public:
*/
bool copy(void *dst)
{
if (!valid()) {
subscribe();
if (subscribe()) {
return Manager::orb_data_copy(_node, dst, _last_generation, false);
}
return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, false) : false;
return false;
}
/**
platforms/common/uORB/test/CMakeLists.txt
+36
View File
@@ -0,0 +1,36 @@
############################################################################
#
# Copyright (c) 2019-2024 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.
#
############################################################################
px4_add_functional_gtest(SRC uORBMessageFieldsTest.cpp LINKLIBS uORB)
px4_add_functional_gtest(SRC uORBSubscriptionTest.cpp LINKLIBS uORB)
platforms/common/uORB/uORBMessageFieldsTest.cpp → platforms/common/uORB/test/uORBMessageFieldsTest.cpp
+1 -1
View File
@@ -31,7 +31,7 @@
*
****************************************************************************/
#include "uORBMessageFields.hpp"
#include <uORB/uORBMessageFields.hpp>
#include <gtest/gtest.h>
#include <containers/Bitset.hpp>
platforms/common/uORB/test/uORBSubscriptionTest.cpp
+114
View File
@@ -0,0 +1,114 @@
/****************************************************************************
*
* Copyright (c) 2019-2024 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.
*
****************************************************************************/
/**
* Test for Subscription
*/
#include <gtest/gtest.h>
#include <uORB/Subscription.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/orb_test.h>
namespace uORB
{
namespace test
{
class uORBSubscriptionTestable : public uORB::Subscription
{
public:
uORBSubscriptionTestable() : Subscription(ORB_ID(orb_test), 0)
{
}
void setNodeValue(void *node)
{
_node = node;
}
void *getNodeValue()
{
return _node;
}
};
class uORBSubscriptionTest : public ::testing::Test
{
protected:
uORBSubscriptionTestable testable;
static void SetUpTestSuite()
{
uORB::Manager::initialize();
orb_test_s message{};
orb_advertise(ORB_ID(orb_test), &message);
}
static void TearDownTestSuite()
{
uORB::Manager::terminate();
}
void TearDown() override
{
testable.setNodeValue(nullptr);
}
};
TEST_F(uORBSubscriptionTest, updateWhenSubscribedThenNotSubscribedTwice)
{
int anyValue = 1;
testable.setNodeValue(&anyValue);
testable.updated();
ASSERT_EQ(testable.getNodeValue(), &anyValue) << "Original node value don't have to be overrwiten";
}
TEST_F(uORBSubscriptionTest, updateWhenNotSubscribedThenSubscribed)
{
testable.setNodeValue(nullptr);
testable.updated();
ASSERT_NE(testable.getNodeValue(), nullptr) << "Node value after 'updated' have to be initialized";
}
}
}
platforms/nuttx/NuttX/nuttx
+1 -1
Submodule platforms/nuttx/NuttX/nuttx updated: 9a213327fb...886acbbdb4
platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
+15
View File
@@ -418,6 +418,21 @@ void up_bdshot_erpm(void)
}
int up_bdshot_num_erpm_ready(void)
{
int num_ready = 0;
for (unsigned i = 0; i < DSHOT_TIMERS; ++i) {
// We only check that data has been received, rather than if it's valid.
// This ensures data is published even if one channel has bit errors.
if (bdshot_recv_mask & (1 << i)) {
++num_ready;
}
}
return num_ready;
}
int up_bdshot_get_erpm(uint8_t channel, int *erpm)
{
platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h
-3
View File
@@ -151,9 +151,6 @@ __EXPORT int io_timer_free_channel(unsigned channel);
__EXPORT int io_timer_get_channel_mode(unsigned channel);
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
__EXPORT extern void io_timer_trigger(void);
__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
/**
* Returns the pin configuration for a specific channel, to be used as GPIO output.
platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
+711 -124
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2019-2021 PX4 Development Team. All rights reserved.
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
* Author: Igor Misic <igy1000mb@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
@@ -8,14 +8,14 @@
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 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.
* 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.
* 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
@@ -32,7 +32,6 @@
*
****************************************************************************/
#if (CONFIG_STM32_HAVE_IP_DMA_V1)
//Do nothing. IP DMA V1 MCUs are not supported.
#else
@@ -45,29 +44,34 @@
#include <px4_arch/io_timer.h>
#include <drivers/drv_dshot.h>
#include <px4_platform_common/log.h>
#include <stdio.h>
#include <drivers/drv_input_capture.h>
#define MOTOR_PWM_BIT_1 14u
#define MOTOR_PWM_BIT_0 7u
#define DSHOT_TIMERS MAX_IO_TIMERS
#define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS
#define ONE_MOTOR_DATA_SIZE 16u
#define ONE_MOTOR_BUFF_SIZE 17u
#define ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * ONE_MOTOR_BUFF_SIZE)
#define DSHOT_THROTTLE_POSITION 5u
#define DSHOT_TELEMETRY_POSITION 4u
#define NIBBLES_SIZE 4u
#define DSHOT_NUMBER_OF_NIBBLES 3u
#define DSHOT_END_OF_STREAM 16u
#define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4
#include <lib/perf/perf_counter.h>
// This can be overriden for a specific board.
#ifndef BOARD_DMA_NUM_DSHOT_CHANNELS
#define BOARD_DMA_NUM_DSHOT_CHANNELS 1
#endif
// DShot protocol definitions
#define ONE_MOTOR_DATA_SIZE 16u
#define MOTOR_PWM_BIT_1 14u
#define MOTOR_PWM_BIT_0 7u
#define DSHOT_THROTTLE_POSITION 5u
#define DSHOT_TELEMETRY_POSITION 4u
#define NIBBLES_SIZE 4u
#define DSHOT_NUMBER_OF_NIBBLES 3u
#define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4
// DMA stream configuration registers
#define DSHOT_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | \
DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE)
DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE)
typedef struct dshot_handler_t {
bool init;
DMA_HANDLE dma_handle;
uint32_t dma_size;
} dshot_handler_t;
// 16-bit because not all of the General Purpose Timers support 32-bit
#define DSHOT_BIDIRECTIONAL_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | DMA_SCR_MINC | \
DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE)
#if defined(CONFIG_ARMV7M_DCACHE)
# define DMA_BUFFER_MASK (ARMV7M_DCACHE_LINESIZE - 1)
@@ -75,137 +79,514 @@ typedef struct dshot_handler_t {
#else
#define DMA_ALIGN_UP(n) (n)
#endif
#define DSHOT_BURST_BUFFER_SIZE(motors_number) (DMA_ALIGN_UP(sizeof(uint32_t)*ONE_MOTOR_BUFF_SIZE*motors_number))
static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {};
static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)]
#define CHANNEL_OUTPUT_BUFF_SIZE 17u
#define CHANNEL_CAPTURE_BUFF_SIZE 32u
#define DSHOT_OUTPUT_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t) * CHANNEL_OUTPUT_BUFF_SIZE * channel_count))
#define DSHOT_CAPTURE_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint16_t)* CHANNEL_CAPTURE_BUFF_SIZE * channel_count))
static void init_timer_config(uint32_t channel_mask);
static void init_timers_dma_up(void);
static int32_t init_timer_channels(uint8_t timer_index);
static void select_next_capture_channel(uint8_t timer_index);
static uint8_t output_channel_from_timer_channel(uint8_t timer_index, uint8_t timer_channel);
static void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg);
static void capture_complete_callback(void *arg);
static void process_capture_results(uint8_t timer_index, uint8_t channel_index);
static unsigned calculate_period(uint8_t timer_index, uint8_t channel_index);
// Timer configuration struct
typedef struct timer_config_t {
DMA_HANDLE dma_handle; // DMA stream for DMA update and eRPM Capture Compare
bool enabled; // Timer enabled
bool enabled_channels[4]; // Timer Channels enabled (requested)
bool initialized; // Timer initialized
bool initialized_channels[4]; // Timer channels initialized (successfully started)
bool bidirectional; // Timer in bidir (inverted) mode
int capture_channel_index; // Timer channel currently being catured in bidirectional mode
uint8_t timer_index; // Timer index. Necessary to have memory for passing pointer to hrt callback
} timer_config_t;
static timer_config_t timer_configs[MAX_IO_TIMERS] = {};
// Output buffer of interleaved motor output bytes
static uint8_t dshot_burst_buffer_array[MAX_IO_TIMERS * DSHOT_OUTPUT_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)]
px4_cache_aligned_data() = {};
static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {};
static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {};
int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
// Buffer containing channel capture data for a single timer
static uint16_t dshot_capture_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE]
px4_cache_aligned_data() = {};
static bool _bidirectional = false;
static uint8_t _bidi_timer_index = 0; // TODO: BDSHOT_TIM param to select timer index?
static uint32_t _dshot_frequency = 0;
// eRPM data for channels on the singular timer
static int32_t _erpms[MAX_TIMER_IO_CHANNELS] = {};
static bool _erpms_ready[MAX_TIMER_IO_CHANNELS] = {};
// hrt callback handle for captcomp post dma processing
static struct hrt_call _cc_call;
// decoding status for each channel
static uint32_t read_ok[MAX_NUM_CHANNELS_PER_TIMER] = {};
static uint32_t read_fail_nibble[MAX_NUM_CHANNELS_PER_TIMER] = {};
static uint32_t read_fail_crc[MAX_NUM_CHANNELS_PER_TIMER] = {};
static uint32_t read_fail_zero[MAX_NUM_CHANNELS_PER_TIMER] = {};
static perf_counter_t hrt_callback_perf = NULL;
static void init_timer_config(uint32_t channel_mask)
{
unsigned buffer_offset = 0;
// Mark timers in use, channels in use, and timers for bidir dshot
for (unsigned output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
if (channel_mask & (1 << output_channel)) {
uint8_t timer_index = timer_io_channels[output_channel].timer_index;
for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) {
dshot_handler[timer_index].init = false;
}
uint8_t timer_channel = timer_io_channels[output_channel].timer_channel;
for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
if (io_timers[timer].base == 0) { // no more timers configured
break;
}
// we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-align"
dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset];
#pragma GCC diagnostic pop
buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps);
if (buffer_offset > sizeof(dshot_burst_buffer_array)) {
return -EINVAL; // something is wrong with the board configuration or some other logic
}
}
/* Init channels */
int ret_val = OK;
int channels_init_mask = 0;
for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) {
if (channel_mask & (1 << channel)) {
uint8_t timer = timer_io_channels[channel].timer_index;
if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer
if ((timer_channel <= 0) || (timer_channel >= 5)) {
// invalid channel, only 1 - 4
continue;
}
ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL);
channel_mask &= ~(1 << channel);
uint8_t timer_channel_index = timer_channel - 1;
if (OK == ret_val) {
dshot_handler[timer].init = true;
channels_init_mask |= 1 << channel;
if (io_timers[timer_index].dshot.dma_base == 0) {
// board does not configure dshot on this timer
continue;
}
} else if (ret_val == -EBUSY) {
/* either timer or channel already used - this is not fatal */
ret_val = 0;
// NOTE: only 1 timer can be used if Bidirectional DShot is enabled
if (_bidirectional && (timer_index != _bidi_timer_index)) {
continue;
}
// NOTE: this is necessary to pass timer_index to DMA callback
timer_configs[timer_index].timer_index = timer_index;
// Mark timer as enabled
timer_configs[timer_index].enabled = true;
// Mark channel as enabled
timer_configs[timer_index].enabled_channels[timer_channel_index] = true;
// Mark timer as bidirectional
if (_bidirectional && timer_index == _bidi_timer_index) {
timer_configs[timer_index].bidirectional = true;
}
}
}
}
// Initializes dshot on configured timers if DShot mode is enabled and DMA is available.
static void init_timers_dma_up(void)
{
for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
if (!timer_configs[timer_index].enabled) {
// timer is not enabled for dshot
continue;
}
if (io_timers[timer_index].dshot.dma_map_up == 0) {
// timer does not have DMA UP mapped
continue;
}
// NOTE: only 1 timer can be used if Bidirectional DShot is enabled
if (_bidirectional && (timer_index != _bidi_timer_index)) {
continue;
}
// Attempt to allocate DMA for Burst
timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
if (timer_configs[timer_index].dma_handle == NULL) {
PX4_WARN("Failed to allocate Timer %u DMA UP", timer_index);
continue;
}
PX4_INFO("Allocated DMA UP Timer Index %u", timer_index);
timer_configs[timer_index].initialized = true;
}
// Bidirectional DShot will free/allocate DMA stream on every update event. This is required
// in order to reconfigure the DMA stream between Timer Burst and CaptureCompare.
if (_bidirectional) {
// Free the allocated DMA channels
for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
if (timer_configs[timer_index].dma_handle != NULL) {
stm32_dmafree(timer_configs[timer_index].dma_handle);
timer_configs[timer_index].dma_handle = NULL;
PX4_INFO("Freed DMA UP Timer Index %u", timer_index);
}
}
}
}
static int32_t init_timer_channels(uint8_t timer_index)
{
int32_t channels_init_mask = 0;
if (!timer_configs[timer_index].enabled || !timer_configs[timer_index].initialized) {
// timer is not enabled or could not be initialized
return 0;
}
io_timer_channel_mode_t mode = timer_configs[timer_index].bidirectional ? IOTimerChanMode_DshotInverted :
IOTimerChanMode_Dshot;
for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
uint8_t timer_channel = timer_io_channels[output_channel].timer_channel;
if ((timer_channel <= 0) || (timer_channel >= 5)) {
// invalid channel, only 1 - 4
continue;
}
uint8_t timer_channel_index = timer_channel - 1;
bool this_timer = timer_index == timer_io_channels[output_channel].timer_index;
bool channel_enabled = timer_configs[timer_index].enabled_channels[timer_channel_index];
if (this_timer && channel_enabled) {
int ret = io_timer_channel_init(output_channel, mode, NULL, NULL);
if (ret != OK) {
PX4_WARN("io_timer_channel_init %u failed", output_channel);
continue;
}
timer_configs[timer_index].initialized_channels[timer_channel_index] = true;
channels_init_mask |= (1 << output_channel);
if (timer_configs[timer_index].bidirectional) {
PX4_DEBUG("DShot initialized OutputChannel %u (bidirectional)", output_channel);
} else {
PX4_DEBUG("DShot initialized OutputChannel %u", output_channel);
}
}
}
for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) {
return channels_init_mask;
}
if (true == dshot_handler[timer_index].init) {
dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps *
ONE_MOTOR_BUFF_SIZE;
io_timer_set_dshot_mode(timer_index, dshot_pwm_freq,
io_timers_channel_mapping.element[timer_index].channel_count_including_gaps);
int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
{
_dshot_frequency = dshot_pwm_freq;
_bidirectional = enable_bidirectional_dshot;
dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap);
if (_bidirectional) {
PX4_INFO("Bidirectional DShot enabled, only one timer will be used");
hrt_callback_perf = perf_alloc(PC_ELAPSED, "dshot: callback perf");
}
if (NULL == dshot_handler[timer_index].dma_handle) {
ret_val = ERROR;
// NOTE: if bidirectional is enabled only 1 timer can be used. This is because Burst mode uses 1 DMA channel per timer
// and CaptureCompare (for reading ESC eRPM) uses 4 DMA. Even if we only used CaptureCompare on a single timer
// we would still need 5 DMA; 1 DMA for the second timer burst, and 4 DMA for the first timer CaptureCompare. The only
// way to support more than 1 timer is to burst/captcomp sequentially for each timer enabled for dshot. The code is
// structured in a way to allow extending support for this in the future.
// Initialize timer_config data based on enabled channels
init_timer_config(channel_mask);
// Initializes dshot on each timer if DShot mode is enabled and DMA is available
init_timers_dma_up();
int32_t channels_init_mask = 0;
for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
channels_init_mask |= init_timer_channels(timer_index);
}
unsigned output_buffer_offset = 0;
for (unsigned timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
if (timer_configs[timer_index].initialized) {
if (io_timers[timer_index].base == 0) { // no more timers configured
break;
}
// we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-align"
dshot_output_buffer[timer_index] = (uint32_t *) &dshot_burst_buffer_array[output_buffer_offset];
#pragma GCC diagnostic pop
uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps;
output_buffer_offset += DSHOT_OUTPUT_BUFFER_SIZE(channel_count);
if (output_buffer_offset > sizeof(dshot_burst_buffer_array)) {
return -EINVAL; // something is wrong with the board configuration or some other logic
}
}
}
return ret_val == OK ? channels_init_mask : ret_val;
return channels_init_mask;
}
int up_bdshot_get_erpm(uint8_t channel, int *erpm)
// Kicks off a DMA transmit for each configured timer and the associated channels
void up_dshot_trigger()
{
// Not implemented
return -1;
}
// Enable DShot inverted on all channels
io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
IO_TIMER_ALL_MODES_CHANNELS);
int up_bdshot_channel_status(uint8_t channel)
{
// Not implemented
return -1;
}
// For each timer, begin DMA transmit
for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
if (timer_configs[timer_index].enabled && timer_configs[timer_index].initialized) {
void up_bdshot_status(void)
{
}
uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps;
void up_dshot_trigger(void)
{
for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) {
io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count);
if (true == dshot_handler[timer].init) {
if (_bidirectional) {
// Deallocate DMA from previous transaction
if (timer_configs[timer_index].dma_handle != NULL) {
stm32_dmastop(timer_configs[timer_index].dma_handle);
stm32_dmafree(timer_configs[timer_index].dma_handle);
timer_configs[timer_index].dma_handle = NULL;
}
// Allocate DMA
timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
if (timer_configs[timer_index].dma_handle == NULL) {
PX4_WARN("DMA allocation for timer %u failed", timer_index);
continue;
}
}
// Flush cache so DMA sees the data
up_clean_dcache((uintptr_t)dshot_burst_buffer[timer],
(uintptr_t)dshot_burst_buffer[timer] +
DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps));
up_clean_dcache((uintptr_t) dshot_output_buffer[timer_index],
(uintptr_t) dshot_output_buffer[timer_index] +
DSHOT_OUTPUT_BUFFER_SIZE(channel_count));
px4_stm32_dmasetup(dshot_handler[timer].dma_handle,
io_timers[timer].base + STM32_GTIM_DMAR_OFFSET,
(uint32_t)(dshot_burst_buffer[timer]),
dshot_handler[timer].dma_size,
px4_stm32_dmasetup(timer_configs[timer_index].dma_handle,
io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET,
(uint32_t)(dshot_output_buffer[timer_index]),
channel_count * CHANNEL_OUTPUT_BUFF_SIZE,
DSHOT_DMA_SCR);
// Clean UDE flag before DMA is started
io_timer_update_dma_req(timer, false);
// Trigger DMA (DShot Outputs)
stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false);
io_timer_update_dma_req(timer, true);
io_timer_update_dma_req(timer_index, false);
// Trigger DMA (DShot Outputs)
if (timer_configs[timer_index].bidirectional) {
stm32_dmastart(timer_configs[timer_index].dma_handle, dma_burst_finished_callback,
&timer_configs[timer_index].timer_index,
false);
} else {
stm32_dmastart(timer_configs[timer_index].dma_handle, NULL, NULL, false);
}
// Enable DMA update request
io_timer_update_dma_req(timer_index, true);
}
}
}
/**
* bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution)
* bit 12 - dshot telemetry enable/disable
* bits 13-16 - XOR checksum
**/
void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry)
static void select_next_capture_channel(uint8_t timer_index)
{
bool found = false;
int next_index = timer_configs[timer_index].capture_channel_index;
while (!found) {
next_index++;
if (next_index > 3) {
next_index = 0;
}
if (timer_configs[timer_index].initialized_channels[next_index]) {
found = true;
}
}
timer_configs[timer_index].capture_channel_index = next_index;
}
static uint8_t output_channel_from_timer_channel(uint8_t timer_index, uint8_t timer_channel_index)
{
for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index;
uint8_t channel_index = timer_io_channels[output_channel].timer_channel - 1;
if (is_this_timer && (channel_index == timer_channel_index)) {
// We found the output channel associated with this timer channel
return output_channel;
}
}
// TODO: error handling?
return 0;
}
void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
{
uint8_t timer_index = *((uint8_t *)arg);
// Clean DMA UP configuration
if (timer_configs[timer_index].dma_handle != NULL) {
stm32_dmastop(timer_configs[timer_index].dma_handle);
stm32_dmafree(timer_configs[timer_index].dma_handle);
timer_configs[timer_index].dma_handle = NULL;
}
// Disable DMA update request
io_timer_update_dma_req(timer_index, false);
// De-allocate timer
io_timer_unallocate_timer(timer_index);
// Flush cache so DMA sees the data
memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer));
up_clean_dcache((uintptr_t) dshot_capture_buffer,
(uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));
// Unallocate timer channel for currently selected capture_channel
uint8_t capture_channel = timer_configs[timer_index].capture_channel_index;
uint8_t output_channel = output_channel_from_timer_channel(timer_index, capture_channel);
// Re-initialize output for CaptureDMA for next time
io_timer_unallocate_channel(output_channel);
io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
// Select the next capture channel
select_next_capture_channel(timer_index);
// Allocate DMA for currently selected capture_channel
capture_channel = timer_configs[timer_index].capture_channel_index;
timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[capture_channel]);
// If DMA handler is valid, start DMA
if (timer_configs[timer_index].dma_handle == NULL) {
PX4_WARN("failed to allocate dma for timer %u channel %u", timer_index, capture_channel);
return;
}
// Set Capture mode for this channel
io_timer_set_dshot_capture_mode(timer_index, capture_channel, _dshot_frequency);
io_timer_capture_dma_req(timer_index, capture_channel, true);
// Choose which CC register for this DMA stream
uint32_t periph_addr = io_timers[timer_index].base + STM32_GTIM_CCR1_OFFSET + (4 * capture_channel);
// Setup DMA for this channel
px4_stm32_dmasetup(timer_configs[timer_index].dma_handle,
periph_addr,
(uint32_t) dshot_capture_buffer[capture_channel],
CHANNEL_CAPTURE_BUFF_SIZE,
DSHOT_BIDIRECTIONAL_DMA_SCR);
// NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. Instead
// we use an hrt callback to schedule the processing of the received and DMAd eRPM frames.
stm32_dmastart(timer_configs[timer_index].dma_handle, NULL, NULL, false);
// Enable CaptureDMA and on all configured channels
io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS);
// 30us to switch regardless of DShot frequency + eRPM frame time + 10us for good measure
hrt_abstime frame_us = (16 * 1000000) / _dshot_frequency; // 16 bits * us_per_s / bits_per_s
hrt_abstime delay = 30 + frame_us + 10;
hrt_call_after(&_cc_call, delay, capture_complete_callback, arg);
}
static void capture_complete_callback(void *arg)
{
perf_begin(hrt_callback_perf);
uint8_t timer_index = *((uint8_t *)arg);
// Unallocate the timer as CaptureDMA
io_timer_unallocate_timer(timer_index);
uint8_t capture_channel = timer_configs[timer_index].capture_channel_index;
// Disable capture DMA
io_timer_capture_dma_req(timer_index, capture_channel, false);
// Stop DMA (should already be finished)
stm32_dmastop(timer_configs[timer_index].dma_handle);
// Re-initialize all output channels on this timer
for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (is_this_timer && channel_initialized) {
io_timer_unallocate_channel(output_channel);
// Initialize back to DShotInverted to bring IO back to the expected idle state
io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL);
}
}
// Invalidate the dcache to ensure most recent data is available
up_invalidate_dcache((uintptr_t) dshot_capture_buffer,
(uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));
// Process eRPM frames from all channels on this timer
process_capture_results(timer_index, capture_channel);
// Enable all channels configured as DShotInverted
io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS);
perf_end(hrt_callback_perf);
}
void process_capture_results(uint8_t timer_index, uint8_t channel_index)
{
const unsigned period = calculate_period(timer_index, channel_index);
uint8_t output_channel = output_channel_from_timer_channel(timer_index, channel_index);
if (period == 0) {
// If the parsing failed, set the eRPM to 0
_erpms[output_channel] = 0;
} else if (period == 65408) {
// Special case for zero motion (e.g., stationary motor)
_erpms[output_channel] = 0;
} else {
// Convert the period to eRPM
_erpms[output_channel] = (1000000 * 60 / 100 + period / 2) / period;
}
// We set it ready anyway, not to hold up other channels when used in round robin.
_erpms_ready[output_channel] = true;
}
/**
* bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution)
* bit 12 - dshot telemetry enable/disable
* bits 13-16 - XOR checksum
**/
void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry)
{
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (!channel_initialized) {
return;
}
uint16_t packet = 0;
uint16_t checksum = 0;
packet |= throttle << DSHOT_THROTTLE_POSITION;
packet |= data << DSHOT_THROTTLE_POSITION;
packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION;
uint16_t csum_data = packet;
@@ -213,21 +594,25 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet
/* XOR checksum calculation */
csum_data >>= NIBBLES_SIZE;
for (unsigned i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) {
for (uint8_t i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) {
checksum ^= (csum_data & 0x0F); // XOR data by nibbles
csum_data >>= NIBBLES_SIZE;
}
packet |= (checksum & 0x0F);
if (_bidirectional) {
packet |= ((~checksum) & 0x0F);
unsigned timer = timer_io_channels[motor_number].timer_index;
uint32_t *buffer = dshot_burst_buffer[timer];
const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer];
unsigned num_motors = mapping->channel_count_including_gaps;
unsigned timer_channel_index = timer_io_channels[motor_number].timer_channel - mapping->lowest_timer_channel;
} else {
packet |= ((checksum) & 0x0F);
}
for (unsigned motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) {
buffer[motor_data_index * num_motors + timer_channel_index] =
const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer_index];
uint8_t num_motors = mapping->channel_count_including_gaps;
uint8_t timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel;
for (uint8_t motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) {
dshot_output_buffer[timer_index][motor_data_index * num_motors + timer_channel] =
(packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first
packet <<= 1;
}
@@ -235,7 +620,209 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet
int up_dshot_arm(bool armed)
{
return io_timer_set_enable(armed, IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS);
return io_timer_set_enable(armed, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
IO_TIMER_ALL_MODES_CHANNELS);
}
int up_bdshot_num_erpm_ready(void)
{
int num_ready = 0;
for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; ++i) {
if (_erpms_ready[i]) {
++num_ready;
}
}
return num_ready;
}
int up_bdshot_get_erpm(uint8_t output_channel, int *erpm)
{
uint8_t timer_index = timer_io_channels[output_channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (channel_initialized) {
*erpm = _erpms[output_channel];
_erpms_ready[output_channel] = false;
return PX4_OK;
}
// this channel is not configured for dshot
return PX4_ERROR;
}
int up_bdshot_channel_status(uint8_t channel)
{
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
// TODO: track that each channel is communicating using the decode stats
if (channel_initialized) {
return 1;
}
return 0;
}
void up_bdshot_status(void)
{
PX4_INFO("dshot driver stats:");
if (_bidirectional) {
PX4_INFO("Bidirectional DShot enabled");
}
uint8_t timer_index = _bidi_timer_index;
for (uint8_t timer_channel_index = 0; timer_channel_index < MAX_NUM_CHANNELS_PER_TIMER; timer_channel_index++) {
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (channel_initialized) {
PX4_INFO("Timer %u, Channel %u: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu",
timer_index, timer_channel_index,
read_ok[timer_channel_index],
read_fail_nibble[timer_channel_index],
read_fail_crc[timer_channel_index],
read_fail_zero[timer_channel_index]);
}
}
}
uint8_t nibbles_from_mapped(uint8_t mapped)
{
switch (mapped) {
case 0x19:
return 0x00;
case 0x1B:
return 0x01;
case 0x12:
return 0x02;
case 0x13:
return 0x03;
case 0x1D:
return 0x04;
case 0x15:
return 0x05;
case 0x16:
return 0x06;
case 0x17:
return 0x07;
case 0x1a:
return 0x08;
case 0x09:
return 0x09;
case 0x0A:
return 0x0A;
case 0x0B:
return 0x0B;
case 0x1E:
return 0x0C;
case 0x0D:
return 0x0D;
case 0x0E:
return 0x0E;
case 0x0F:
return 0x0F;
default:
// Unknown mapped
return 0xFF;
}
}
unsigned calculate_period(uint8_t timer_index, uint8_t channel_index)
{
uint32_t value = 0;
uint32_t high = 1; // We start off with high
unsigned shifted = 0;
// We can ignore the very first data point as it's the pulse before it starts.
unsigned previous = dshot_capture_buffer[channel_index][1];
// Loop through the capture buffer for the specified channel
for (unsigned i = 2; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) {
if (dshot_capture_buffer[channel_index][i] == 0) {
// Once we get zeros we're through
break;
}
// This seemss to work with dshot 150, 300, 600, 1200
// The values were found by trial and error to get the quantization just right.
const uint32_t bits = (dshot_capture_buffer[channel_index][i] - previous + 5) / 20;
// Convert GCR encoded pulse train into value
for (unsigned bit = 0; bit < bits; ++bit) {
value = (value << 1) | high;
++shifted;
}
// The next edge toggles.
high = !high;
previous = dshot_capture_buffer[channel_index][i];
}
if (shifted == 0) {
// no data yet, or this time
++read_fail_zero[channel_index];
return 0;
}
// We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end.
value <<= (21 - shifted);
// From GCR to eRPM according to:
// https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission
unsigned gcr = (value ^ (value >> 1));
uint32_t data = 0;
// 20bits -> 5 mapped -> 4 nibbles
for (unsigned i = 0; i < 4; ++i) {
uint32_t nibble = nibbles_from_mapped(gcr & 0x1F) << (4 * i);
if (nibble == 0xFF) {
++read_fail_nibble[channel_index];;
return 0;
}
data |= nibble;
gcr >>= 5;
}
unsigned shift = (data & 0xE000) >> 13;
unsigned period = ((data & 0x1FF0) >> 4) << shift;
unsigned crc = data & 0xF;
unsigned payload = (data & 0xFFF0) >> 4;
unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F;
if (crc != calculated_crc) {
++read_fail_crc[channel_index];;
return 0;
}
++read_ok[channel_index];;
return period;
}
#endif
platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c
+18 -7
View File
@@ -79,7 +79,11 @@
# define HRT_TIMER_BASE STM32_TIM1_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN
#if defined(CONFIG_ARCH_CHIP_STM32H7)
# define HRT_TIMER_VECTOR STM32_IRQ_TIMCC
#else
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1CC
#endif
# define HRT_TIMER_CLOCK STM32_APB2_TIM1_CLKIN
# if CONFIG_STM32_TIM1
# error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1
@@ -156,8 +160,17 @@
# if CONFIG_STM32_TIM11
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
# endif
#elif HRT_TIMER == 12
# define HRT_TIMER_BASE STM32_TIM12_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB1LENR_TIM12EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM12
# define HRT_TIMER_CLOCK STM32_APB1_TIM12_CLKIN
# if CONFIG_STM32_TIM12
# error must not set CONFIG_STM32_TIM12=y and HRT_TIMER=12
# endif
#else
# error HRT_TIMER must be a value between 1 and 11
# error HRT_TIMER must be a value between 1 and 12
#endif
/*
@@ -223,7 +236,6 @@
/*
* Specific registers and bits used by HRT sub-functions
*/
/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
#if HRT_TIMER_CHANNEL == 1
# define rCCR_HRT rCCR1 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */
@@ -297,13 +309,12 @@ int hrt_ioctl(unsigned int cmd, unsigned long arg);
# define GTIM_CCER_CC4NP 0
# define PPM_EDGE_FLIP
# endif
/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
# if HRT_PPM_CHANNEL == 1
# define rCCR_PPM rCCR1 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */
# define SR_INT_PPM GTIM_SR_CC1IF /* capture interrupt (non-DMA mode) */
# define SR_OVF_PPM GTIM_SR_CC1OF /* capture overflow (non-DMA mode) */
# define CCMR1_PPM 1 /* not on TI1/TI2 */
# define CCMR1_PPM (1 << 0) /* not on TI1/TI2 */
# define CCMR2_PPM 0 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC1P
@@ -312,7 +323,7 @@ int hrt_ioctl(unsigned int cmd, unsigned long arg);
# define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */
# define SR_INT_PPM GTIM_SR_CC2IF /* capture interrupt (non-DMA mode) */
# define SR_OVF_PPM GTIM_SR_CC2OF /* capture overflow (non-DMA mode) */
# define CCMR1_PPM 2 /* not on TI1/TI2 */
# define CCMR1_PPM (1 << 8) /* not on TI1/TI2 */
# define CCMR2_PPM 0 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC2P
@@ -322,7 +333,7 @@ int hrt_ioctl(unsigned int cmd, unsigned long arg);
# define SR_INT_PPM GTIM_SR_CC3IF /* capture interrupt (non-DMA mode) */
# define SR_OVF_PPM GTIM_SR_CC3OF /* capture overflow (non-DMA mode) */
# define CCMR1_PPM 0 /* not on TI1/TI2 */
# define CCMR2_PPM 1 /* on TI3, not on TI4 */
# define CCMR2_PPM (1 << 0) /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC3P
# elif HRT_PPM_CHANNEL == 4
@@ -331,7 +342,7 @@ int hrt_ioctl(unsigned int cmd, unsigned long arg);
# define SR_INT_PPM GTIM_SR_CC4IF /* capture interrupt (non-DMA mode) */
# define SR_OVF_PPM GTIM_SR_CC4OF /* capture overflow (non-DMA mode) */
# define CCMR1_PPM 0 /* not on TI1/TI2 */
# define CCMR2_PPM 2 /* on TI3, not on TI4 */
# define CCMR2_PPM (1 << 8) /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC4P
# else
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h
+3 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
* Author: Igor Misic <igy1000mb@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
@@ -58,5 +58,6 @@
*/
typedef struct dshot_conf_t {
uint32_t dma_base;
uint32_t dmamap;
uint32_t dma_map_up;
uint32_t dma_map_ch[4];
} dshot_conf_t;
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
+8 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
* Copyright (C) 2024 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
@@ -81,6 +81,8 @@ typedef enum io_timer_channel_mode_t {
IOTimerChanMode_PPS = 8,
IOTimerChanMode_RPM = 9,
IOTimerChanMode_Other = 10,
IOTimerChanMode_DshotInverted = 11,
IOTimerChanMode_CaptureDMA = 12,
IOTimerChanModeSize
} io_timer_channel_mode_t;
@@ -159,7 +161,12 @@ __EXPORT int io_timer_unallocate_channel(unsigned channel);
__EXPORT int io_timer_get_channel_mode(unsigned channel);
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
__EXPORT extern void io_timer_trigger(unsigned channels_mask);
__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
__EXPORT int io_timer_set_dshot_burst_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
__EXPORT void io_timer_capture_dma_req(uint8_t timer, uint8_t timer_channel_index, bool enable);
__EXPORT int io_timer_set_dshot_capture_mode(uint8_t timer, uint8_t timer_channel_index, unsigned dshot_pwm_freq);
/**
* Reserve a timer
@@ -169,7 +176,6 @@ __EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mod
__EXPORT int io_timer_unallocate_timer(unsigned timer);
__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
/**
* Returns the pin configuration for a specific channel, to be used as GPIO output.
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h
+6 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
* Copyright (C) 2024 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
@@ -103,7 +103,7 @@ static inline constexpr timer_io_channels_t initIOTimerChannelOutputClear(const
}
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dma = {})
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dma = {})
{
bool nuttx_config_timer_enabled = false;
io_timers_t ret{};
@@ -268,9 +268,10 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm
constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (STM32_TIMx)");
// DShot
if (dshot_dma.index != DMA::Invalid) {
ret.dshot.dma_base = getDMABaseRegister(dshot_dma);
ret.dshot.dmamap = getTimerUpdateDMAMap(timer, dshot_dma);
if (dma.index != DMA::Invalid) {
ret.dshot.dma_base = getDMABaseRegister(dma);
ret.dshot.dma_map_up = getTimerUpdateDMAMap(timer, dma);
getTimerChannelDMAMap(timer, dma, ret.dshot.dma_map_ch);
}
return ret;
platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c
+4 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
* Copyright (C) 2024 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
@@ -365,8 +365,10 @@ int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge)
rv = -ENXIO;
/* Any pins in capture mode */
int mode = io_timer_get_channel_mode(channel);
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
if (mode == IOTimerChanMode_Capture ||
mode == IOTimerChanMode_CaptureDMA) {
uint16_t edge_bits = 0xffff;
platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c
+145 -37
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
* Copyright (C) 2024 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
@@ -61,7 +61,6 @@
#include <stm32_gpio.h>
#include <stm32_tim.h>
static int io_timer_handler0(int irq, void *context, void *arg);
static int io_timer_handler1(int irq, void *context, void *arg);
static int io_timer_handler2(int irq, void *context, void *arg);
@@ -73,8 +72,14 @@ static int io_timer_handler7(int irq, void *context, void *arg);
#if defined(HAVE_GTIM_CCXNP)
#define HW_GTIM_CCER_CC1NP GTIM_CCER_CC1NP
#define HW_GTIM_CCER_CC2NP GTIM_CCER_CC2NP
#define HW_GTIM_CCER_CC3NP GTIM_CCER_CC3NP
#define HW_GTIM_CCER_CC4NP GTIM_CCER_CC4NP
#else
# define HW_GTIM_CCER_CC1NP 0
# define HW_GTIM_CCER_CC2NP 0
# define HW_GTIM_CCER_CC3NP 0
# define HW_GTIM_CCER_CC4NP 0
#endif
#define arraySize(a) (sizeof((a))/sizeof(((a)[0])))
@@ -96,41 +101,42 @@ static int io_timer_handler7(int irq, void *context, void *arg);
#define MAX_CHANNELS_PER_TIMER 4
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET)
#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET)
#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET)
#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET)
#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET)
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET)
#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET)
#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET)
#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET)
#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET)
#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET)
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET)
#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET)
#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET)
#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET)
#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET)
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET)
#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET)
#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET)
#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET)
#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET)
#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET)
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
#define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF)
#define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF)
#define CCMR_C1_RESET 0x00ff
#define CCMR_C1_NUM_BITS 8
#define CCER_C1_NUM_BITS 4
#define CCMR_C1_RESET 0x00ff
#define CCMR_C1_NUM_BITS 8
#define CCER_C1_NUM_BITS 4
#define CCMR_C1_CAPTURE_INIT (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT) | \
(GTIM_CCMR_ICPSC_NOPSC << GTIM_CCMR1_IC1PSC_SHIFT) | \
(GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT)
#define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
#define CCMR_C1_PWMOUT_INVERTED_INIT (GTIM_CCMR_MODE_PWM2 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
#define CCMR_C1_PWMIN_INIT 0 // TBD
@@ -141,15 +147,15 @@ static int io_timer_handler7(int irq, void *context, void *arg);
#endif
/* The transfer is done to 1 register starting from TIMx_CR1 + TIMx_DCR.DBA */
#define TIM_DMABURSTLENGTH_1TRANSFER 0x00000000U
#define TIM_DMABURSTLENGTH_1TRANSFER 0x00000000U
/* The transfer is done to 2 registers starting from TIMx_CR1 + TIMx_DCR.DBA */
#define TIM_DMABURSTLENGTH_2TRANSFERS 0x00000100U
#define TIM_DMABURSTLENGTH_2TRANSFERS 0x00000100U
/* The transfer is done to 3 registers starting from TIMx_CR1 + TIMx_DCR.DBA */
#define TIM_DMABURSTLENGTH_3TRANSFERS 0x00000200U
#define TIM_DMABURSTLENGTH_3TRANSFERS 0x00000200U
/* The transfer is done to 4 registers starting from TIMx_CR1 + TIMx_DCR.DBA */
#define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U
#define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U
// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other
// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
@@ -161,15 +167,15 @@ io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { };
/* Stats and handlers are only useful for Capture */
typedef struct channel_stat_t {
uint32_t isr_cout;
uint32_t overflows;
uint32_t isr_cout;
uint32_t overflows;
} channel_stat_t;
static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS];
static struct channel_handler_entry {
channel_handler_t callback;
void *context;
void *context;
} channel_handlers[MAX_TIMER_IO_CHANNELS];
#endif // !defined(BOARD_HAS_NO_CAPTURE)
@@ -519,7 +525,57 @@ void io_timer_update_dma_req(uint8_t timer, bool enable)
}
}
int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length)
void io_timer_capture_dma_req(uint8_t timer, uint8_t timer_channel_index, bool enable)
{
if (enable) {
switch (timer_channel_index) {
case 0:
rDIER(timer) |= ATIM_DIER_CC1DE;
rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC1G);
break;
case 1:
rDIER(timer) |= ATIM_DIER_CC2DE;
rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC2G);
break;
case 2:
rDIER(timer) |= ATIM_DIER_CC3DE;
rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC3G);
break;
case 3:
rDIER(timer) |= ATIM_DIER_CC4DE;
rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC4G);
break;
}
} else {
switch (timer_channel_index) {
case 0:
rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC1G);
rDIER(timer) &= ~ATIM_DIER_CC1DE;
break;
case 1:
rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC2G);
rDIER(timer) &= ~ATIM_DIER_CC2DE;
break;
case 2:
rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC3G);
rDIER(timer) &= ~ATIM_DIER_CC3DE;
break;
case 3:
rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC4G);
rDIER(timer) &= ~ATIM_DIER_CC4DE;
break;
}
}
}
int io_timer_set_dshot_burst_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length)
{
int ret_val = OK;
uint32_t tim_dma_burst_length;
@@ -545,7 +601,7 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_
rPSC(timer) = ((int)(io_timers[timer].clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1;
rEGR(timer) = ATIM_EGR_UG;
// find the lowest channel index for the timer (they are not necesarily in ascending order)
// find the lowest channel index for the timer (they are not necessarily in ascending order)
unsigned lowest_timer_channel = 4;
uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
@@ -574,6 +630,47 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_
return ret_val;
}
int io_timer_set_dshot_capture_mode(uint8_t timer, uint8_t timer_channel_index, unsigned dshot_pwm_freq)
{
// Timer Autor Reload Register max value
rARR(timer) = 0xFFFFFFFF;
// Timer Prescalar
rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1;
switch (timer_channel_index) {
case 0:
rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC1G;
rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | HW_GTIM_CCER_CC1NP);
rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT);
rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | HW_GTIM_CCER_CC1NP);
break;
case 1:
rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC2G;
rCCER(timer) &= ~(GTIM_CCER_CC2E | GTIM_CCER_CC2P | HW_GTIM_CCER_CC2NP);
rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT);
rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | HW_GTIM_CCER_CC2NP);
break;
case 2:
rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC3G;
rCCER(timer) &= ~(GTIM_CCER_CC3E | GTIM_CCER_CC3P | HW_GTIM_CCER_CC3NP);
rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT);
rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | HW_GTIM_CCER_CC3NP);
break;
case 3:
rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC4G;
rCCER(timer) &= ~(GTIM_CCER_CC4E | GTIM_CCER_CC4P | HW_GTIM_CCER_CC4NP);
rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT);
rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | HW_GTIM_CCER_CC4NP);
break;
}
return 0;
}
static inline void io_timer_set_PWM_mode(unsigned timer)
{
rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
@@ -773,6 +870,12 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
setbits = CCMR_C1_PWMOUT_INIT;
break;
case IOTimerChanMode_DshotInverted:
ccer_setbits = 0;
dier_setbits = 0;
setbits = CCMR_C1_PWMOUT_INVERTED_INIT;
break;
case IOTimerChanMode_PWMIn:
setbits = CCMR_C1_PWMIN_INIT;
gpio = timer_io_channels[channel].gpio_in;
@@ -781,6 +884,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
#if !defined(BOARD_HAS_NO_CAPTURE)
case IOTimerChanMode_Capture:
case IOTimerChanMode_CaptureDMA:
setbits = CCMR_C1_CAPTURE_INIT;
gpio = timer_io_channels[channel].gpio_in;
break;
@@ -897,10 +1001,12 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
break;
case IOTimerChanMode_Dshot:
case IOTimerChanMode_DshotInverted:
dier_bit = 0;
/* fallthrough */
case IOTimerChanMode_Capture:
case IOTimerChanMode_CaptureDMA:
cr1_bit = state ? GTIM_CR1_CEN : 0;
/* fallthrough */
@@ -946,6 +1052,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
(mode == IOTimerChanMode_PWMOut ||
mode == IOTimerChanMode_OneShot ||
mode == IOTimerChanMode_Dshot ||
mode == IOTimerChanMode_DshotInverted ||
mode == IOTimerChanMode_Trigger))) {
action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
}
@@ -985,7 +1092,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
/* arm requires the timer be enabled */
rCR1(actions) |= cr1_bit;
} else {
} else {
rCR1(actions) = 0;
}
@@ -1006,6 +1113,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value)
if ((mode != IOTimerChanMode_PWMOut) &&
(mode != IOTimerChanMode_OneShot) &&
(mode != IOTimerChanMode_Dshot) &&
(mode != IOTimerChanMode_DshotInverted) &&
(mode != IOTimerChanMode_Trigger)) {
rv = -EIO;
platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h
+5
View File
@@ -35,6 +35,11 @@
#include "../../../stm32_common/include/px4_arch/hw_description.h"
static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch)
{
// Not supported
}
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
{
// not used on STM32F1
platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h
+5
View File
@@ -35,6 +35,11 @@
#include "../../../stm32_common/include/px4_arch/hw_description.h"
static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch)
{
// Not supported
}
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
{
uint32_t dma_map = 0;
platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/hw_description.h
+41
View File
@@ -35,6 +35,47 @@
#include "../../../stm32_common/include/px4_arch/hw_description.h"
static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch)
{
switch (timer) {
case Timer::Timer1:
if (dma.index == DMA::Index2) {
dma_map_ch[0] = DMAMAP_TIM1_CH1;
dma_map_ch[1] = DMAMAP_TIM1_CH2_1;
dma_map_ch[2] = DMAMAP_TIM1_CH3_2;
dma_map_ch[3] = DMAMAP_TIM1_CH4;
}
break;
case Timer::Timer2:
case Timer::Timer3:
break;
case Timer::Timer4:
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_TIM4_CH1;
dma_map_ch[1] = DMAMAP_TIM4_CH2;
dma_map_ch[2] = DMAMAP_TIM4_CH3;
dma_map_ch[3] = 0;
}
break;
case Timer::Timer5:
case Timer::Timer6:
case Timer::Timer7:
case Timer::Timer8:
case Timer::Timer9:
case Timer::Timer10:
case Timer::Timer11:
case Timer::Timer12:
case Timer::Timer13:
case Timer::Timer14:
break;
}
}
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
{
uint32_t dma_map = 0;
platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h
+178 -14
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2024 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
@@ -35,47 +35,211 @@
#include "../../../stm32_common/include/px4_arch/hw_description.h"
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch)
{
uint32_t dma_map = 0;
switch (timer) {
case Timer::Timer1:
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM1UP_0 : DMAMAP_DMA12_TIM1UP_1;
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM1CH1_0;
dma_map_ch[1] = DMAMAP_DMA12_TIM1CH2_0;
dma_map_ch[2] = DMAMAP_DMA12_TIM1CH3_0;
dma_map_ch[3] = DMAMAP_DMA12_TIM1CH4_0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM1CH1_1;
dma_map_ch[1] = DMAMAP_DMA12_TIM1CH2_1;
dma_map_ch[2] = DMAMAP_DMA12_TIM1CH3_1;
dma_map_ch[3] = DMAMAP_DMA12_TIM1CH4_1;
}
break;
case Timer::Timer2:
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM2UP_0 : DMAMAP_DMA12_TIM2UP_1;
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM2CH1_0;
dma_map_ch[1] = DMAMAP_DMA12_TIM2CH2_0;
dma_map_ch[2] = DMAMAP_DMA12_TIM2CH3_0;
dma_map_ch[3] = DMAMAP_DMA12_TIM2CH4_0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM2CH1_1;
dma_map_ch[1] = DMAMAP_DMA12_TIM2CH2_1;
dma_map_ch[2] = DMAMAP_DMA12_TIM2CH3_1;
dma_map_ch[3] = DMAMAP_DMA12_TIM2CH4_1;
}
break;
case Timer::Timer3:
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM3UP_0 : DMAMAP_DMA12_TIM3UP_1;
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM3CH1_0;
dma_map_ch[1] = DMAMAP_DMA12_TIM3CH2_0;
dma_map_ch[2] = DMAMAP_DMA12_TIM3CH3_0;
dma_map_ch[3] = DMAMAP_DMA12_TIM3CH4_0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM3CH1_1;
dma_map_ch[1] = DMAMAP_DMA12_TIM3CH2_1;
dma_map_ch[2] = DMAMAP_DMA12_TIM3CH3_1;
dma_map_ch[3] = DMAMAP_DMA12_TIM3CH4_1;
}
break;
case Timer::Timer4:
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM4UP_0 : DMAMAP_DMA12_TIM4UP_1;
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM4CH1_0;
dma_map_ch[1] = DMAMAP_DMA12_TIM4CH2_0;
dma_map_ch[2] = DMAMAP_DMA12_TIM4CH3_0;
dma_map_ch[3] = 0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM4CH1_1;
dma_map_ch[1] = DMAMAP_DMA12_TIM4CH2_1;
dma_map_ch[2] = DMAMAP_DMA12_TIM4CH3_1;
dma_map_ch[3] = 0;
}
break;
case Timer::Timer5:
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM5UP_0 : DMAMAP_DMA12_TIM5UP_1;
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM5CH1_0;
dma_map_ch[1] = DMAMAP_DMA12_TIM5CH2_0;
dma_map_ch[2] = DMAMAP_DMA12_TIM5CH3_0;
dma_map_ch[3] = DMAMAP_DMA12_TIM5CH4_0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM5CH1_1;
dma_map_ch[1] = DMAMAP_DMA12_TIM5CH2_1;
dma_map_ch[2] = DMAMAP_DMA12_TIM5CH3_1;
dma_map_ch[3] = DMAMAP_DMA12_TIM5CH4_1;
}
break;
case Timer::Timer6:
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM6UP_0 : DMAMAP_DMA12_TIM6UP_1;
// No channels available
break;
case Timer::Timer7:
// No channels available
break;
case Timer::Timer8:
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM8CH1_0;
dma_map_ch[1] = DMAMAP_DMA12_TIM8CH2_0;
dma_map_ch[2] = DMAMAP_DMA12_TIM8CH3_0;
dma_map_ch[3] = DMAMAP_DMA12_TIM8CH4_0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM8CH1_1;
dma_map_ch[1] = DMAMAP_DMA12_TIM8CH2_1;
dma_map_ch[2] = DMAMAP_DMA12_TIM8CH3_1;
dma_map_ch[3] = DMAMAP_DMA12_TIM8CH4_1;
}
break;
case Timer::Timer9:
// Non-existent
break;
case Timer::Timer10:
// Non-existent
break;
case Timer::Timer11:
// Non-existent
break;
case Timer::Timer12:
// Non-existent
break;
case Timer::Timer13:
// Non-existent
break;
case Timer::Timer14:
// Non-existent
break;
case Timer::Timer15:
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM15CH1_0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM15CH1_1;
}
break;
case Timer::Timer16:
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM16CH1_0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM16CH1_1;
}
break;
case Timer::Timer17:
if (dma.index == DMA::Index1) {
dma_map_ch[0] = DMAMAP_DMA12_TIM17CH1_0;
} else {
dma_map_ch[0] = DMAMAP_DMA12_TIM17CH1_1;
}
break;
}
}
static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma)
{
uint32_t dma_map_up = 0;
switch (timer) {
case Timer::Timer1:
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM1UP_0 : DMAMAP_DMA12_TIM1UP_1;
break;
case Timer::Timer2:
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM2UP_0 : DMAMAP_DMA12_TIM2UP_1;
break;
case Timer::Timer3:
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM3UP_0 : DMAMAP_DMA12_TIM3UP_1;
break;
case Timer::Timer4:
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM4UP_0 : DMAMAP_DMA12_TIM4UP_1;
break;
case Timer::Timer5:
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM5UP_0 : DMAMAP_DMA12_TIM5UP_1;
break;
case Timer::Timer6:
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM6UP_0 : DMAMAP_DMA12_TIM6UP_1;
break;
case Timer::Timer7:
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM7UP_0 : DMAMAP_DMA12_TIM7UP_1;
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM7UP_0 : DMAMAP_DMA12_TIM7UP_1;
break;
case Timer::Timer8:
dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM8UP_0 : DMAMAP_DMA12_TIM8UP_1;
dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM8UP_0 : DMAMAP_DMA12_TIM8UP_1;
break;
@@ -91,6 +255,6 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const
break;
}
constexpr_assert(dma_map != 0, "Invalid DMA config for given timer");
return dma_map;
constexpr_assert(dma_map_up != 0, "Invalid DMA config for given timer");
return dma_map_up;
}
platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer.h
+2
View File
@@ -33,3 +33,5 @@
#pragma once
#include "../../../stm32_common/include/px4_arch/io_timer.h"
#define HAVE_GTIM_CCXNP
platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h
+6 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2024 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
@@ -135,7 +135,7 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t
return ret;
}
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dma = {})
static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dma = {})
{
bool nuttx_config_timer_enabled = false;
io_timers_t ret{};
@@ -306,9 +306,10 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm
constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (STM32_TIMx)");
// DShot
if (dshot_dma.index != DMA::Invalid) {
ret.dshot.dma_base = getDMABaseRegister(dshot_dma);
ret.dshot.dmamap = getTimerUpdateDMAMap(timer, dshot_dma);
if (dma.index != DMA::Invalid) {
ret.dshot.dma_base = getDMABaseRegister(dma);
ret.dshot.dma_map_up = getTimerUpdateDMAMap(timer, dma);
getTimerChannelDMAMap(timer, dma, ret.dshot.dma_map_ch);
}
return ret;
platforms/posix/CMakeLists.txt
+34 -15
View File
@@ -21,18 +21,42 @@ px4_posix_generate_alias(
PREFIX ${PX4_SHELL_COMMAND_PREFIX}
)
if (CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
add_executable(px4
src/px4/common/main_fuzztesting.cpp
apps.cpp
)
else()
add_definitions(-DPX4_SOURCE_DIR="${PX4_SOURCE_DIR}" -DPX4_BINARY_DIR="${PX4_BINARY_DIR}")
add_definitions(-DPX4_SOURCE_DIR="${PX4_SOURCE_DIR}" -DPX4_BINARY_DIR="${PX4_BINARY_DIR}")
add_executable(px4
src/px4/common/main.cpp
add_executable(px4
src/px4/common/main.cpp
apps.cpp
)
if (BUILD_TESTING)
# Build mavlink fuzz tests. These run other modules and thus cannot be a functional/unit test
add_executable(mavlink_fuzz_tests EXCLUDE_FROM_ALL
src/px4/common/mavlink_fuzz_tests.cpp
apps.cpp
)
)
set_target_properties(mavlink_fuzz_tests PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${PX4_BINARY_DIR}")
target_include_directories(mavlink_fuzz_tests PRIVATE SYSTEM "${CMAKE_BINARY_DIR}/mavlink")
add_dependencies(test_results mavlink_fuzz_tests)
link_fuzztest(mavlink_fuzz_tests)
target_link_libraries(mavlink_fuzz_tests
PRIVATE
${module_libraries}
m
parameters
)
target_compile_options(mavlink_fuzz_tests
PRIVATE
# There warnings come from within fuzztest
-Wno-float-equal
-Wno-sign-compare
-Wno-shadow
-Wno-extra
-Wno-non-template-friend
# From mavlink
-Wno-cast-align
-Wno-address-of-packed-member
)
endif()
target_link_libraries(px4
@@ -52,11 +76,6 @@ endif()
target_link_libraries(px4 PRIVATE uORB)
if (CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
target_include_directories(px4 PRIVATE SYSTEM "${CMAKE_BINARY_DIR}/mavlink}")
target_compile_options(px4 PRIVATE "-Wno-cast-align")
endif()
#=============================================================================
# install
#
platforms/posix/src/px4/common/gtest_runner/CMakeLists.txt
+1 -1
View File
@@ -36,4 +36,4 @@ set(SRCS
)
px4_add_library(gtest_functional_main ${SRCS})
target_link_libraries(gtest_functional_main PUBLIC gtest)
target_link_libraries(gtest_functional_main PUBLIC gtest fuzztest::init_fuzztest)
platforms/posix/src/px4/common/gtest_runner/gtest_functional_main.cpp
+4
View File
@@ -32,6 +32,7 @@
****************************************************************************/
#include <gtest/gtest.h>
#include <fuzztest/init_fuzztest.h>
#include <uORB/Subscription.hpp>
@@ -41,6 +42,9 @@ int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
fuzztest::ParseAbslFlags(argc, argv);
fuzztest::InitFuzzTest(&argc, &argv);
uORB::Manager::initialize();
param_init();
return RUN_ALL_TESTS();
platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt
+1 -1
View File
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.8.12)
cmake_minimum_required(VERSION 3.5)
# We want to test the lockstep schedule even if it is not used otherwise.
px4_add_library(lockstep_scheduler
platforms/posix/src/px4/common/lockstep_scheduler/test/CMakeLists.txt
+1 -1
View File
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.8.12)
cmake_minimum_required(VERSION 3.5)
add_executable(lockstep_scheduler_test
src/lockstep_scheduler_test.cpp
platforms/posix/src/px4/common/main_fuzztesting.cpp
-158
View File
@@ -1,158 +0,0 @@
/****************************************************************************
*
* Copyright (C) 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
* 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.
*
****************************************************************************/
/**
* This is an alternative main entrypoint for fuzz testing.
*/
#include <stdint.h>
#include "px4_platform_common/init.h"
#include "px4_platform_common/posix.h"
#include "apps.h"
#include "px4_daemon/client.h"
#include "px4_daemon/server.h"
#include "px4_daemon/pxh.h"
#include <netinet/in.h>
#include <arpa/inet.h>
#include "common/mavlink.h"
#define MODULE_NAME "px4"
#ifndef PATH_MAX
#define PATH_MAX 1024
#endif
namespace px4
{
void init_once();
}
extern "C" int LLVMFuzzerTestOneInput(const uint8_t *data, const size_t size);
void initialize_fake_px4_once();
void send_mavlink(const uint8_t *data, const size_t size);
extern "C" int LLVMFuzzerTestOneInput(const uint8_t *data, const size_t size)
{
initialize_fake_px4_once();
send_mavlink(data, size);
return 0;
}
void initialize_fake_px4_once()
{
static bool first_time = true;
if (!first_time) {
return;
}
first_time = false;
px4::init_once();
px4::init(0, nullptr, "px4");
px4_daemon::Pxh pxh;
pxh.process_line("uorb start", true);
pxh.process_line("param load", true);
pxh.process_line("dataman start", true);
pxh.process_line("load_mon start", true);
pxh.process_line("battery_simulator start", true);
pxh.process_line("tone_alarm start", true);
pxh.process_line("rc_update start", true);
pxh.process_line("sensors start", true);
pxh.process_line("commander start", true);
pxh.process_line("navigator start", true);
pxh.process_line("ekf2 start", true);
pxh.process_line("mc_att_control start", true);
pxh.process_line("mc_pos_control start", true);
pxh.process_line("land_detector start multicopter", true);
pxh.process_line("logger start", true);
pxh.process_line("mavlink start -x -o 14540 -r 4000000", true);
pxh.process_line("mavlink boot_complete", true);
}
void send_mavlink(const uint8_t *data, const size_t size)
{
int socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (socket_fd < 0) {
PX4_ERR("socket error: %s", strerror(errno));
return;
}
struct sockaddr_in addr {};
addr.sin_family = AF_INET;
inet_pton(AF_INET, "0.0.0.0", &(addr.sin_addr));
addr.sin_port = htons(14540);
if (bind(socket_fd, reinterpret_cast<sockaddr *>(&addr), sizeof(addr)) != 0) {
PX4_ERR("bind error: %s", strerror(errno));
close(socket_fd);
return;
}
mavlink_message_t message {};
uint8_t buffer[MAVLINK_MAX_PACKET_LEN] {};
for (size_t i = 0; i < size; i += sizeof(message)) {
const size_t copy_len = std::min(sizeof(message), size - i);
//printf("copy_len: %zu, %zu (%zu)\n", i, copy_len, size);
memcpy(reinterpret_cast<void *>(&message), data + i, copy_len);
const ssize_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);
struct sockaddr_in dest_addr {};
dest_addr.sin_family = AF_INET;
inet_pton(AF_INET, "127.0.0.1", &dest_addr.sin_addr.s_addr);
dest_addr.sin_port = htons(14556);
if (sendto(socket_fd, buffer, buffer_len, 0, reinterpret_cast<sockaddr *>(&dest_addr),
sizeof(dest_addr)) != buffer_len) {
PX4_ERR("sendto error: %s", strerror(errno));
}
}
close(socket_fd);
}
platforms/posix/src/px4/common/mavlink_fuzz_tests.cpp
+146
View File
@@ -0,0 +1,146 @@
/****************************************************************************
*
* Copyright (c) 2025 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.
*
****************************************************************************/
#define MODULE_NAME "mavlink_fuzz_tests"
#include <gtest/gtest.h>
#include <fuzztest/fuzztest.h>
#include <px4_platform_common/init.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/log.h>
#include "px4_daemon/pxh.h"
#include <common/mavlink.h>
#include <netinet/in.h>
#include <arpa/inet.h>
namespace px4
{
void init_once();
}
extern "C" const char *__asan_default_options() { return "detect_leaks=0"; }
class MavlinkFuzzTest
{
public:
MavlinkFuzzTest()
{
// This object will be reused for each fuzzing iteration call
px4::init_once();
px4::init(0, nullptr, "px4");
px4_daemon::Pxh pxh;
pxh.process_line("param load", true);
pxh.process_line("dataman start", true);
pxh.process_line("load_mon start", true);
pxh.process_line("battery_simulator start", true);
pxh.process_line("tone_alarm start", true);
pxh.process_line("rc_update start", true);
pxh.process_line("sensors start", true);
pxh.process_line("commander start", true);
pxh.process_line("navigator start", true);
pxh.process_line("ekf2 start", true);
pxh.process_line("mc_att_control start", true);
pxh.process_line("mc_pos_control start", true);
pxh.process_line("land_detector start multicopter", true);
pxh.process_line("logger start", true);
pxh.process_line("mavlink start -x -o 14513 -u 14514 -r 4000000", true);
pxh.process_line("mavlink boot_complete", true);
_socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (_socket_fd < 0) {
PX4_ERR("socket error: %s", strerror(errno));
return;
}
sockaddr_in addr {};
addr.sin_family = AF_INET;
inet_pton(AF_INET, "0.0.0.0", &(addr.sin_addr));
addr.sin_port = htons(14513);
if (bind(_socket_fd, reinterpret_cast<sockaddr *>(&addr), sizeof(addr)) != 0) {
PX4_ERR("bind error: %s", strerror(errno));
close(_socket_fd);
return;
}
}
~MavlinkFuzzTest()
{
close(_socket_fd);
}
void run(std::vector<uint8_t> data)
{
sendMavlink(data.data(), data.size());
}
private:
void sendMavlink(const uint8_t *data, const size_t size)
{
mavlink_message_t message {};
uint8_t buffer[MAVLINK_MAX_PACKET_LEN] {};
for (size_t i = 0; i < size; i += sizeof(message)) {
const size_t copy_len = std::min(sizeof(message), size - i);
memcpy(reinterpret_cast<void *>(&message), data + i, copy_len);
// Set a correct checksum as it is hard for the fuzzer to pass the correct checksum
mavlink_finalize_message_buffer(&message, message.sysid, message.compid, &_status, 0, message.len,
mavlink_get_crc_extra(&message));
const ssize_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);
sockaddr_in dest_addr {};
dest_addr.sin_family = AF_INET;
inet_pton(AF_INET, "127.0.0.1", &dest_addr.sin_addr.s_addr);
dest_addr.sin_port = htons(14514);
if (sendto(_socket_fd, buffer, buffer_len, 0, reinterpret_cast<sockaddr *>(&dest_addr),
sizeof(dest_addr)) != buffer_len) {
PX4_ERR("sendto error: %s", strerror(errno));
}
}
}
mavlink_status_t _status{};
int _socket_fd{-1};
};
FUZZ_TEST_F(MavlinkFuzzTest, run);
platforms/posix/src/px4/common/px4_daemon/pxh.cpp
+4 -2
View File
@@ -101,7 +101,7 @@ int Pxh::process_line(const std::string &line, bool silently_fail)
// Note that argv[argc] always needs to be a nullptr.
// Therefore add one more entry.
const char *arg[words.size() + 1];
char **arg = new char *[words.size() + 1];
for (unsigned i = 0; i < words.size(); ++i) {
arg[i] = (char *)words[i].c_str();
@@ -110,7 +110,7 @@ int Pxh::process_line(const std::string &line, bool silently_fail)
// Explicitly set this nullptr.
arg[words.size()] = nullptr;
int retval = _apps[command](words.size(), (char **)arg);
int retval = _apps[command](words.size(), arg);
if (retval) {
if (!silently_fail) {
@@ -118,6 +118,8 @@ int Pxh::process_line(const std::string &line, bool silently_fail)
}
}
delete[] arg;
return retval;
} else if (command == "help") {
platforms/qurt/include/qurt_log.h
+15 -6
View File
@@ -37,6 +37,10 @@
#include <stdint.h>
#include <px4_platform_common/defines.h>
#define BASE_BUFFER_SIZE 256
#define MAX_MODULE_NAME_SIZE 32
#define MODULE_BUFFER_SIZE (BASE_BUFFER_SIZE + MAX_MODULE_NAME_SIZE)
__BEGIN_DECLS
extern void qurt_log_to_apps(int level, const char *message);
@@ -44,22 +48,27 @@ extern void qurt_log_to_apps(int level, const char *message);
// Defining hap_debug
void HAP_debug(const char *msg, int level, const char *filename, int line);
static __inline void qurt_log(int level, const char *file, int line,
const char *format, ...)
static __inline void qurt_log_module(int level, const char *module, const char *file, int line,
const char *format, ...)
{
char buf[256];
char buf[BASE_BUFFER_SIZE];
va_list args;
va_start(args, format);
vsnprintf(buf, sizeof(buf), format, args);
va_end(args);
HAP_debug(buf, level, file, line);
qurt_log_to_apps(level, buf);
char module_buf[MODULE_BUFFER_SIZE];
snprintf(module_buf, MAX_MODULE_NAME_SIZE, "[%s] ", module);
strcat(module_buf, buf);
HAP_debug(module_buf, level, file, line);
qurt_log_to_apps(level, module_buf);
}
static __inline void qurt_log_raw(const char *format, ...)
{
char buf[256];
char buf[BASE_BUFFER_SIZE];
va_list args;
va_start(args, format);
vsnprintf(buf, sizeof(buf), format, args);
platforms/qurt/src/px4/SerialImpl.cpp
+2
View File
@@ -37,6 +37,8 @@
#include <drivers/device/qurt/uart.h>
#include <drivers/drv_hrt.h>
#define MODULE_NAME "SerialImpl"
namespace device
{
platforms/qurt/src/px4/drv_hrt.cpp
+2
View File
@@ -43,6 +43,8 @@
#include "hrt_work.h"
#define MODULE_NAME "drv_hrt"
static constexpr unsigned HRT_INTERVAL_MIN = 50;
static constexpr unsigned HRT_INTERVAL_MAX = 50000000;
platforms/qurt/src/px4/main.cpp
+2
View File
@@ -44,6 +44,8 @@
#include "apps.h"
#define MODULE_NAME "main"
#define MAX_ARGS 8 // max number of whitespace separated args after app name
__BEGIN_DECLS
platforms/qurt/src/px4/tasks.cpp
+3 -1
View File
@@ -41,6 +41,8 @@
#include <pthread.h>
#include "hrt_work.h"
#define MODULE_NAME "tasks"
#define PX4_TASK_STACK_SIZE 8192
#define PX4_TASK_MAX_NAME_LENGTH 32
#define PX4_TASK_MAX_ARGC 32
@@ -129,7 +131,7 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma
int task_index = 0;
char *p = (char *)argv;
PX4_INFO("Creating pthread %s\n", name);
PX4_INFO("Creating pthread %s", name);
if (task_mutex_initialized == false) {
task_mutex_initialized = true;
platforms/qurt/unresolved_symbols.c
+2
View File
@@ -4,6 +4,8 @@
#include <qurt_alloc.h>
#include <termios.h>
#define MODULE_NAME "unresolved_symbols"
__attribute__((visibility("default"))) void free(void *ptr)
{
qurt_free(ptr);