From 40c9789e7d7b3273d215b7d97551f0783cd9b190 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Fri, 27 Oct 2023 12:24:47 -0600 Subject: [PATCH 01/55] boards: arkv6x fix wrong pwm output values --- boards/ark/fmu-v6x/src/board_config.h | 1 - boards/ark/fmu-v6x/src/spix_sync.c | 8 +++----- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 001a31ae73c5..4782fc7efdd8 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -247,7 +247,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define BOARD_PWM_FREQ 1024000 #define GPIO_FMU_CH1 /* PI0 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0) #define GPIO_FMU_CH2 /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12) diff --git a/boards/ark/fmu-v6x/src/spix_sync.c b/boards/ark/fmu-v6x/src/spix_sync.c index 2bda38696267..056e38e75f74 100644 --- a/boards/ark/fmu-v6x/src/spix_sync.c +++ b/boards/ark/fmu-v6x/src/spix_sync.c @@ -82,9 +82,7 @@ #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) #define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) -#if !defined(BOARD_PWM_FREQ) -#define BOARD_PWM_FREQ 1000000 -#endif +#define BOARD_SPIX_SYNC_PWM_FREQ 1024000 unsigned spix_sync_timer_get_period(unsigned timer) @@ -129,11 +127,11 @@ static void spix_sync_timer_init_timer(unsigned timer, unsigned rate) * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly. */ - rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; + rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_SPIX_SYNC_PWM_FREQ) - 1; /* configure the timer to update at the desired rate */ - rARR(timer) = (BOARD_PWM_FREQ / rate) - 1; + rARR(timer) = (BOARD_SPIX_SYNC_PWM_FREQ / rate) - 1; /* generate an update event; reloads the counter and all registers */ rEGR(timer) = GTIM_EGR_UG; From e9a142ac7dd7cb71130e8311d7e960a2edd3d652 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 20 Oct 2023 10:29:26 +1300 Subject: [PATCH 02/55] mavlink: properly set mission_type This was defaulted to 0 before which messed with transmitting geofence and rally items. Signed-off-by: Julian Oes --- src/modules/mavlink/mavlink_mission.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index e89f0f1f42fd..ec6bdea979c4 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1587,6 +1587,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->frame = mission_item->frame; mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->autocontinue = mission_item->autocontinue; + mavlink_mission_item->mission_type = _mission_type; /* default mappings for generic commands */ if (mission_item->frame == MAV_FRAME_MISSION) { From 813494bc3d8eb29cace4bb235100db8dd95142de Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Oct 2023 08:09:58 +1300 Subject: [PATCH 03/55] lib: add FIFO ringbuffer class This adds a reusable class for a simple FIFO ringbuffer. Signed-off-by: Julian Oes --- src/lib/CMakeLists.txt | 3 +- src/lib/ringbuffer/CMakeLists.txt | 40 +++++ src/lib/ringbuffer/Ringbuffer.cpp | 180 +++++++++++++++++++ src/lib/ringbuffer/Ringbuffer.hpp | 120 +++++++++++++ src/lib/ringbuffer/RingbufferTest.cpp | 247 ++++++++++++++++++++++++++ 5 files changed, 589 insertions(+), 1 deletion(-) create mode 100644 src/lib/ringbuffer/CMakeLists.txt create mode 100644 src/lib/ringbuffer/Ringbuffer.cpp create mode 100644 src/lib/ringbuffer/Ringbuffer.hpp create mode 100644 src/lib/ringbuffer/RingbufferTest.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f6bcc9260b6a..43694f5e9a77 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-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 @@ -61,6 +61,7 @@ add_subdirectory(pid EXCLUDE_FROM_ALL) add_subdirectory(pid_design EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) +add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) add_subdirectory(systemlib EXCLUDE_FROM_ALL) diff --git a/src/lib/ringbuffer/CMakeLists.txt b/src/lib/ringbuffer/CMakeLists.txt new file mode 100644 index 000000000000..78a26de16664 --- /dev/null +++ b/src/lib/ringbuffer/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(ringbuffer + Ringbuffer.cpp +) + +target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer) diff --git a/src/lib/ringbuffer/Ringbuffer.cpp b/src/lib/ringbuffer/Ringbuffer.cpp new file mode 100644 index 000000000000..64fc33d8361d --- /dev/null +++ b/src/lib/ringbuffer/Ringbuffer.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * 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 "Ringbuffer.hpp" + +#include +#include +#include + + +Ringbuffer::~Ringbuffer() +{ + deallocate(); +} + +bool Ringbuffer::allocate(size_t buffer_size) +{ + assert(_ringbuffer == nullptr); + + _size = buffer_size; + _ringbuffer = new uint8_t[_size]; + return _ringbuffer != nullptr; +} + +void Ringbuffer::deallocate() +{ + delete[] _ringbuffer; + _ringbuffer = nullptr; + _size = 0; +} + +size_t Ringbuffer::space_available() const +{ + if (_start > _end) { + return _start - _end - 1; + + } else { + return _start - _end - 1 + _size; + } +} + +size_t Ringbuffer::space_used() const +{ + if (_start <= _end) { + return _end - _start; + + } else { + // Potential wrap around. + return _end - _start + _size; + } +} + + +bool Ringbuffer::push_back(const uint8_t *buf, size_t buf_len) +{ + if (buf_len == 0 || buf == nullptr) { + // Nothing to add, we better don't try. + return false; + } + + if (_start > _end) { + // Add after end up to start, no wrap around. + + // Leave one byte free so that start don't end up the same + // which signals empty. + const size_t available = _start - _end - 1; + + if (available < buf_len) { + return false; + } + + memcpy(&_ringbuffer[_end], buf, buf_len); + _end += buf_len; + + } else { + // Add after end, maybe wrap around. + const size_t available = _start - _end - 1 + _size; + + if (available < buf_len) { + return false; + } + + const size_t remaining_packet_len = _size - _end; + + if (buf_len > remaining_packet_len) { + memcpy(&_ringbuffer[_end], buf, remaining_packet_len); + _end = 0; + + memcpy(&_ringbuffer[_end], buf + remaining_packet_len, buf_len - remaining_packet_len); + _end += buf_len - remaining_packet_len; + + } else { + memcpy(&_ringbuffer[_end], buf, buf_len); + _end += buf_len; + } + } + + return true; +} + +size_t Ringbuffer::pop_front(uint8_t *buf, size_t buf_max_len) +{ + if (buf == nullptr) { + // User needs to supply a valid pointer. + return 0; + } + + if (_start == _end) { + // Empty + return 0; + } + + if (_start < _end) { + + // No wrap around. + size_t to_copy_len = math::min(_end - _start, buf_max_len); + + memcpy(buf, &_ringbuffer[_start], to_copy_len); + _start += to_copy_len; + + return to_copy_len; + + } else { + // Potential wrap around. + size_t to_copy_len = _end - _start + _size; + + if (to_copy_len > buf_max_len) { + to_copy_len = buf_max_len; + } + + const size_t remaining_buf_len = _size - _start; + + if (to_copy_len > remaining_buf_len) { + + memcpy(buf, &_ringbuffer[_start], remaining_buf_len); + _start = 0; + memcpy(buf + remaining_buf_len, &_ringbuffer[_start], to_copy_len - remaining_buf_len); + _start += to_copy_len - remaining_buf_len; + + } else { + memcpy(buf, &_ringbuffer[_start], to_copy_len); + _start += to_copy_len; + } + + return to_copy_len; + } +} diff --git a/src/lib/ringbuffer/Ringbuffer.hpp b/src/lib/ringbuffer/Ringbuffer.hpp new file mode 100644 index 000000000000..541f322db947 --- /dev/null +++ b/src/lib/ringbuffer/Ringbuffer.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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 +#include + + +// FIFO ringbuffer implementation. +// +// The ringbuffer can store up 1 byte less than allocated as +// start and end marker need to be one byte apart when the buffer +// is full, otherwise it would suddenly be empty. +// +// The buffer is not thread-safe. + +class Ringbuffer +{ +public: + /* @brief Constructor + * + * @note Does not allocate automatically. + */ + Ringbuffer() = default; + + /* + * @brief Destructor + * + * Automatically calls deallocate. + */ + ~Ringbuffer(); + + /* @brief Allocate ringbuffer + * + * @param buffer_size Number of bytes to allocate on heap. + * + * @returns false if allocation fails. + */ + bool allocate(size_t buffer_size); + + /* + * @brief Deallocate ringbuffer + * + * @note only required to deallocate and reallocate again. + */ + void deallocate(); + + /* + * @brief Space available to copy bytes into + * + * @returns number of free bytes. + */ + size_t space_available() const; + + /* + * @brief Space used to copy data from + * + * @returns number of used bytes. + */ + size_t space_used() const; + + /* + * @brief Copy data into ringbuffer + * + * @param buf Pointer to buffer to copy from. + * @param buf_len Number of bytes to copy. + * + * @returns true if packet could be copied into buffer. + */ + bool push_back(const uint8_t *buf, size_t buf_len); + + /* + * @brief Get data from ringbuffer + * + * @param buf Pointer to buffer where data can be copied into. + * @param max_buf_len Max number of bytes to copy. + * + * @returns 0 if buffer is empty. + */ + size_t pop_front(uint8_t *buf, size_t max_buf_len); + +private: + size_t _size {0}; + uint8_t *_ringbuffer {nullptr}; + size_t _start{0}; + size_t _end{0}; +}; diff --git a/src/lib/ringbuffer/RingbufferTest.cpp b/src/lib/ringbuffer/RingbufferTest.cpp new file mode 100644 index 000000000000..e5e0a7a57f8a --- /dev/null +++ b/src/lib/ringbuffer/RingbufferTest.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * 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 +#include +#include + + +#include "Ringbuffer.hpp" + +class TempData +{ +public: + TempData(size_t len) + { + _size = len; + _buf = new uint8_t[_size]; + } + + ~TempData() + { + delete[] _buf; + _buf = nullptr; + } + + uint8_t *buf() const + { + return _buf; + } + + size_t size() const + { + return _size; + } + + void paint(unsigned offset = 0) + { + for (size_t i = 0; i < _size; ++i) { + _buf[i] = (uint8_t)((i + offset) % UINT8_MAX); + } + } + +private: + uint8_t *_buf {nullptr}; + size_t _size{0}; + +}; + +bool operator==(const TempData &lhs, const TempData &rhs) +{ + if (lhs.size() != rhs.size()) { + return false; + } + + return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0; +} + + +TEST(Ringbuffer, AllocateAndDeallocate) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + buf.deallocate(); + + ASSERT_TRUE(buf.allocate(1000)); + // The second time we forget to clean up, but we expect no leak. +} + +TEST(Ringbuffer, PushATooBigMessage) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{200}; + + // A message that doesn't fit should get rejected. + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); +} + +TEST(Ringbuffer, PushAndPopOne) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + EXPECT_EQ(buf.space_used(), 20); + EXPECT_EQ(buf.space_available(), 79); + + // Get everything + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20); + EXPECT_EQ(data, out); + + // Nothing remaining + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0); +} + +TEST(Ringbuffer, PushAndPopSeveral) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{90}; + data.paint(); + + // 9 little chunks in + for (unsigned i = 0; i < 9; ++i) { + EXPECT_TRUE(buf.push_back(data.buf() + i * 10, 10)); + } + + // 10 won't because of overhead inside the buffer + EXPECT_FALSE(buf.push_back(data.buf(), 10)); + + TempData out{90}; + // Take it back out in 2 big steps + EXPECT_EQ(buf.pop_front(out.buf(), 50), 50); + EXPECT_EQ(buf.pop_front(out.buf() + 50, 40), 40); + EXPECT_EQ(data, out); +} + +TEST(Ringbuffer, PushAndTryToPopMore) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData out1{80}; + EXPECT_EQ(buf.pop_front(out1.buf(), out1.size()), data1.size()); +} + +TEST(Ringbuffer, PushAndPopSeveralInterleaved) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData data2{30}; + data2.paint(50); + EXPECT_TRUE(buf.push_back(data2.buf(), data2.size())); + + TempData out12{80}; + EXPECT_EQ(buf.pop_front(out12.buf(), out12.size()), out12.size()); + + TempData out12_ref{80}; + out12_ref.paint(); + EXPECT_EQ(out12_ref, out12); + + TempData data3{50}; + data3.paint(33); + EXPECT_TRUE(buf.push_back(data3.buf(), data3.size())); + + TempData out3{50}; + EXPECT_EQ(buf.pop_front(out3.buf(), out3.size()), data3.size()); + EXPECT_EQ(data3, out3); +} + +TEST(Ringbuffer, PushEmpty) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); +} + +TEST(Ringbuffer, PopWithoutBuffer) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); + + TempData data{50}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + EXPECT_EQ(buf.pop_front(nullptr, 50), 0); +} + +TEST(Ringbuffer, EmptyAndNoSpaceForHeader) +{ + // Addressing a corner case where start and end are at the end + // and the same. + + Ringbuffer buf; + // Allocate 1 bytes more than the packet, 1 for the start/end logic. + ASSERT_TRUE(buf.allocate(21)); + + { + TempData data{20}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } + + { + TempData data{10}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{10}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } +} From 2edc3cf8451b0c13dc198b5b2d8d88fe281925df Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Oct 2023 08:10:54 +1300 Subject: [PATCH 04/55] lib: add variable length ringbuffer This adds a reusable class for a FIFO ringbuffer that accepts variable length packets. It is using the Ringbuffer class internally. Signed-off-by: Julian Oes --- src/lib/CMakeLists.txt | 1 + .../variable_length_ringbuffer/CMakeLists.txt | 42 +++ .../VariableLengthRingbuffer.cpp | 106 ++++++++ .../VariableLengthRingbuffer.hpp | 111 ++++++++ .../VariableLengthRingbufferTest.cpp | 257 ++++++++++++++++++ 5 files changed, 517 insertions(+) create mode 100644 src/lib/variable_length_ringbuffer/CMakeLists.txt create mode 100644 src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp create mode 100644 src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp create mode 100644 src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 43694f5e9a77..27f926acc295 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -71,6 +71,7 @@ add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL) add_subdirectory(timesync EXCLUDE_FROM_ALL) add_subdirectory(tinybson EXCLUDE_FROM_ALL) add_subdirectory(tunes EXCLUDE_FROM_ALL) +add_subdirectory(variable_length_ringbuffer EXCLUDE_FROM_ALL) add_subdirectory(version EXCLUDE_FROM_ALL) add_subdirectory(weather_vane EXCLUDE_FROM_ALL) add_subdirectory(wind_estimator EXCLUDE_FROM_ALL) diff --git a/src/lib/variable_length_ringbuffer/CMakeLists.txt b/src/lib/variable_length_ringbuffer/CMakeLists.txt new file mode 100644 index 000000000000..2d2bf4706731 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(variable_length_ringbuffer + VariableLengthRingbuffer.cpp +) + +target_link_libraries(variable_length_ringbuffer PRIVATE ringbuffer) + +target_include_directories(variable_length_ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC VariableLengthRingbufferTest.cpp LINKLIBS variable_length_ringbuffer) diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp new file mode 100644 index 000000000000..9b607e208b07 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 "VariableLengthRingbuffer.hpp" + +#include +#include + + +VariableLengthRingbuffer::~VariableLengthRingbuffer() +{ + deallocate(); +} + +bool VariableLengthRingbuffer::allocate(size_t buffer_size) +{ + return _ringbuffer.allocate(buffer_size); +} + +void VariableLengthRingbuffer::deallocate() +{ + _ringbuffer.deallocate(); +} + +bool VariableLengthRingbuffer::push_back(const uint8_t *packet, size_t packet_len) +{ + if (packet_len == 0 || packet == nullptr) { + // Nothing to add, we better don't try. + return false; + } + + size_t space_required = packet_len + sizeof(Header); + + if (space_required > _ringbuffer.space_available()) { + return false; + } + + Header header{static_cast(packet_len)}; + bool result = _ringbuffer.push_back(reinterpret_cast(&header), sizeof(header)); + assert(result); + + result = _ringbuffer.push_back(packet, packet_len); + assert(result); + + // In case asserts are commented out to prevent unused warnings. + (void)result; + + return true; +} + +size_t VariableLengthRingbuffer::pop_front(uint8_t *buf, size_t buf_max_len) +{ + if (buf == nullptr) { + // User needs to supply a valid pointer. + return 0; + } + + // Check next header + Header header; + + if (_ringbuffer.pop_front(reinterpret_cast(&header), sizeof(header)) < sizeof(header)) { + return 0; + } + + // We can't fit the packet into the user supplied buffer. + // This should never happen as the user has to supply a big // enough buffer. + assert(static_cast(header.len) <= buf_max_len); + + size_t bytes_read = _ringbuffer.pop_front(buf, header.len); + assert(bytes_read == header.len); + + return bytes_read; +} diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp new file mode 100644 index 000000000000..89f92eb24670 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 +#include + + +// FIFO ringbuffer implementation for packets of variable length. +// +// The variable length is implemented using a 4 byte header +// containing a the length. +// +// The buffer is not thread-safe. + +class VariableLengthRingbuffer +{ +public: + /* @brief Constructor + * + * @note Does not allocate automatically. + */ + VariableLengthRingbuffer() = default; + + /* + * @brief Destructor + * + * Automatically calls deallocate. + */ + ~VariableLengthRingbuffer(); + + /* @brief Allocate ringbuffer + * + * @note The variable length requires 4 bytes + * of overhead per packet. + * + * @param buffer_size Number of bytes to allocate on heap. + * + * @returns false if allocation fails. + */ + bool allocate(size_t buffer_size); + + /* + * @brief Deallocate ringbuffer + * + * @note only required to deallocate and reallocate again. + */ + void deallocate(); + + /* + * @brief Copy packet into ringbuffer + * + * @param packet Pointer to packet to copy from. + * @param packet_len Length of packet. + * + * @returns true if packet could be copied into buffer. + */ + bool push_back(const uint8_t *packet, size_t packet_len); + + /* + * @brief Get packet from ringbuffer + * + * @note max_buf_len needs to be bigger equal to any pushed packet. + * + * @param buf Pointer to where next packet can be copied into. + * @param max_buf_len Max size of buf + * + * @returns 0 if packet is bigger than max_len or buffer is empty. + */ + size_t pop_front(uint8_t *buf, size_t max_buf_len); + +private: + struct Header { + uint32_t len; + }; + + Ringbuffer _ringbuffer {}; +}; diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp new file mode 100644 index 000000000000..958f36947eda --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * 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 +#include +#include + + +#include "VariableLengthRingbuffer.hpp" + +class TempData +{ +public: + TempData(size_t len) + { + _size = len; + _buf = new uint8_t[_size]; + } + + ~TempData() + { + delete[] _buf; + _buf = nullptr; + } + + uint8_t *buf() const + { + return _buf; + } + + size_t size() const + { + return _size; + } + + void paint(unsigned offset = 0) + { + for (size_t i = 0; i < _size; ++i) { + _buf[i] = (uint8_t)((i + offset) % UINT8_MAX); + } + } + +private: + uint8_t *_buf {nullptr}; + size_t _size{0}; + +}; + +bool operator==(const TempData &lhs, const TempData &rhs) +{ + if (lhs.size() != rhs.size()) { + return false; + } + + return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0; +} + + +TEST(VariableLengthRingbuffer, AllocateAndDeallocate) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + buf.deallocate(); + + ASSERT_TRUE(buf.allocate(1000)); + // The second time we forget to clean up, but we expect no leak. +} + +TEST(VariableLengthRingbuffer, PushATooBigMessage) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{200}; + + // A message that doesn't fit should get rejected. + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); +} + +TEST(VariableLengthRingbuffer, PushAndPopOne) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + // Out buffer is the same size + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20); + EXPECT_EQ(data, out); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + // Out buffer is supposedly bigger + TempData out2{20}; + EXPECT_EQ(buf.pop_front(out2.buf(), 21), 20); + EXPECT_EQ(data, out2); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + // Disabled because it doesn't work reliably. + // For some reason the abort works when tests are filtered using TESTFILTER + // but not when all tests are run. + // + // Out buffer is too small + // Asserts are disabled in release build + //TempData out3{19}; + //EXPECT_DEATH(buf.pop_front(out3.buf(), out3.size()), ".*"); +} + +TEST(VariableLengthRingbuffer, PushAndPopSeveral) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + // 4 should fit + for (unsigned i = 0; i < 4; ++i) { + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + } + + // 5 won't because of overhead + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); + + // Take 4 back out + for (unsigned i = 0; i < 4; ++i) { + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), data.size()); + EXPECT_EQ(data, out); + } + + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0); +} + +TEST(VariableLengthRingbuffer, PushAndPopSeveralVariableSize) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData data2{30}; + data2.paint(42); + EXPECT_TRUE(buf.push_back(data2.buf(), data2.size())); + + // Supposedly more space + TempData out1{50}; + EXPECT_EQ(buf.pop_front(out1.buf(), 100), data1.size()); + EXPECT_EQ(data1, out1); + + TempData data3{50}; + data3.paint(33); + EXPECT_TRUE(buf.push_back(data3.buf(), data3.size())); + + // Supposedly more space + TempData out2{30}; + EXPECT_EQ(buf.pop_front(out2.buf(), 100), data2.size()); + EXPECT_EQ(data2, out2); + + // Supposedly more space + TempData out3{50}; + EXPECT_EQ(buf.pop_front(out3.buf(), 100), data3.size()); + EXPECT_EQ(data3, out3); + + TempData out4{100}; + EXPECT_EQ(buf.pop_front(out4.buf(), out4.size()), 0); +} + +TEST(VariableLengthRingbuffer, PushEmpty) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); +} + +TEST(VariableLengthRingbuffer, PopWithoutBuffer) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); + + TempData data{50}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + EXPECT_EQ(buf.pop_front(nullptr, 50), 0); +} + +TEST(VariableLengthRingbuffer, EmptyAndNoSpaceForHeader) +{ + // Addressing a corner case where start and end are at the end + // and the same. + + VariableLengthRingbuffer buf; + // Allocate 4+1 bytes more than the packet, 4 for the header, 1 for the start/end logic. + ASSERT_TRUE(buf.allocate(25)); + + { + TempData data{20}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } + + { + TempData data{10}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{10}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } +} From e5d92c5195b32485146cee1da334e1685617f2fb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Oct 2023 08:12:26 +1300 Subject: [PATCH 05/55] mavlink: fix MAVLink message forwarding This switches from the horribly intertwined ringbuffer implementation to the new VariableLengthRingbuffer implementation. By ditching the previous implementation, we fix MAVLink message forwarding, which didn't work reliably. The reason it didn't work is that multiple mavlink messages could be added but only one of them was sent out because the buffer didn't keep track of the messages properly and only read the first one. Signed-off-by: Julian Oes --- src/modules/mavlink/CMakeLists.txt | 3 +- src/modules/mavlink/mavlink_main.cpp | 184 ++++----------------------- src/modules/mavlink/mavlink_main.h | 32 +---- 3 files changed, 32 insertions(+), 187 deletions(-) diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 45f7737e6e03..8f5f6231af30 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-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 @@ -140,6 +140,7 @@ px4_add_module( mavlink_c timesync tunes + variable_length_ringbuffer version UNITY_BUILD ) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 40f6cbf8191b..36aec9ec1dbd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -178,6 +178,7 @@ Mavlink::~Mavlink() perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); + perf_free(_forwarding_error_perf); } void @@ -1218,117 +1219,16 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } } -int -Mavlink::message_buffer_init(int size) -{ - _message_buffer.size = size; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - _message_buffer.data = (char *)malloc(_message_buffer.size); - - int ret; - - if (_message_buffer.data == nullptr) { - ret = PX4_ERROR; - _message_buffer.size = 0; - - } else { - ret = OK; - } - - return ret; -} - -void -Mavlink::message_buffer_destroy() -{ - _message_buffer.size = 0; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - free(_message_buffer.data); -} - -int -Mavlink::message_buffer_count() -{ - int n = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (n < 0) { - n += _message_buffer.size; - } - - return n; -} - -bool -Mavlink::message_buffer_write(const void *ptr, int size) -{ - // bytes available to write - int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; - - if (available < 0) { - available += _message_buffer.size; - } - - if (size > available) { - // buffer overflow - return false; - } - - char *c = (char *) ptr; - int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer - - if (n < size) { - // message goes over end of the buffer - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); - _message_buffer.write_ptr = 0; - - } else { - n = 0; - } - - // now: n = bytes already written - int p = size - n; // number of bytes to write - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); - _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; - return true; -} - -int -Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) -{ - // bytes available to read - int available = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (available == 0) { - return 0; // buffer is empty - } - - int n = 0; - - if (available > 0) { - // read pointer is before write pointer, all available bytes can be read - n = available; - *is_part = false; - - } else { - // read pointer is after write pointer, read bytes from read_ptr to end of the buffer - n = _message_buffer.size - _message_buffer.read_ptr; - *is_part = _message_buffer.write_ptr > 0; - } - - *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); - return n; -} - void Mavlink::pass_message(const mavlink_message_t *msg) { - /* size is 8 bytes plus variable payload */ + /* size is 12 bytes plus variable payload */ int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; - pthread_mutex_lock(&_message_buffer_mutex); - message_buffer_write(msg, size); - pthread_mutex_unlock(&_message_buffer_mutex); + LockGuard lg{_message_buffer_mutex}; + + if (!_message_buffer.push_back(reinterpret_cast(msg), size)) { + perf_count(_forwarding_error_perf); + } } MavlinkShell * @@ -2211,7 +2111,7 @@ Mavlink::task_main(int argc, char *argv[]) return PX4_ERROR; } - /* initialize send mutex */ + pthread_mutex_init(&_message_buffer_mutex, nullptr); pthread_mutex_init(&_send_mutex, nullptr); pthread_mutex_init(&_radio_status_mutex, nullptr); @@ -2221,13 +2121,12 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + LockGuard lg{_message_buffer_mutex}; + + if (!_message_buffer.allocate(2 * sizeof(mavlink_message_t) + 1)) { PX4_ERR("msg buf alloc fail"); return PX4_ERROR; } - - /* initialize message buffer mutex */ - pthread_mutex_init(&_message_buffer_mutex, nullptr); } /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ @@ -2569,50 +2468,21 @@ Mavlink::task_main(int argc, char *argv[]) _events.update(t); - /* pass messages from other UARTs */ + /* pass messages from other instances */ if (get_forwarding_on()) { - bool is_part; - uint8_t *read_ptr; - uint8_t *write_ptr; - - pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - if (available > 0) { - // Reconstruct message from buffer - - mavlink_message_t msg; - write_ptr = (uint8_t *)&msg; - - // Pull a single message from the buffer - size_t read_count = available; - - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - - message_buffer_mark_read(read_count); - - /* write second part of buffer if there is some */ - if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); - message_buffer_mark_read(available); - } - - pthread_mutex_unlock(&_message_buffer_mutex); + mavlink_message_t msg; + size_t available_bytes; + { + // We only send one message at a time, not to put too much strain on a + // link from forwarded messages. + LockGuard lg{_message_buffer_mutex}; + available_bytes = _message_buffer.pop_front(reinterpret_cast(&msg), sizeof(msg)); + // We need to make sure to release the lock here before sending the + // bytes out via IP or UART which could potentially take longer. + } + if (available_bytes > 0) { resend_message(&msg); } } @@ -2662,11 +2532,6 @@ Mavlink::task_main(int argc, char *argv[]) _socket_fd = -1; } - if (get_forwarding_on()) { - message_buffer_destroy(); - pthread_mutex_destroy(&_message_buffer_mutex); - } - if (_mavlink_ulog) { _mavlink_ulog->stop(); _mavlink_ulog = nullptr; @@ -2674,6 +2539,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_destroy(&_send_mutex); pthread_mutex_destroy(&_radio_status_mutex); + pthread_mutex_destroy(&_message_buffer_mutex); PX4_INFO("exiting channel %i", (int)_channel); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85c0361de3ad..1ba71c6a2ba1 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -419,11 +420,6 @@ class Mavlink final : public ModuleParams bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (_transmitting_enabled && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); } - bool message_buffer_write(const void *ptr, int size); - - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } - /** * Count transmitted bytes */ @@ -651,16 +647,9 @@ class Mavlink final : public ModuleParams ping_statistics_s _ping_stats {}; - struct mavlink_message_buffer { - int write_ptr; - int read_ptr; - int size; - char *data; - }; + pthread_mutex_t _message_buffer_mutex{}; + VariableLengthRingbuffer _message_buffer{}; - mavlink_message_buffer _message_buffer {}; - - pthread_mutex_t _message_buffer_mutex {}; pthread_mutex_t _send_mutex {}; pthread_mutex_t _radio_status_mutex {}; @@ -682,6 +671,7 @@ class Mavlink final : public ModuleParams perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */ perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": tx run interval")}; /**< loop interval performance counter */ perf_counter_t _send_byte_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": send_bytes error")}; /**< send bytes error count */ + perf_counter_t _forwarding_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": forwarding error")}; /**< forwarding messages error count */ void mavlink_update_parameters(); @@ -711,18 +701,6 @@ class Mavlink final : public ModuleParams */ int configure_streams_to_default(const char *configure_single_stream = nullptr); - int message_buffer_init(int size); - - void message_buffer_destroy(); - - int message_buffer_count(); - - int message_buffer_is_empty() const { return (_message_buffer.read_ptr == _message_buffer.write_ptr); } - - int message_buffer_get_ptr(void **ptr, bool *is_part); - - void message_buffer_mark_read(int n) { _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; } - void pass_message(const mavlink_message_t *msg); void publish_telemetry_status(); From 4138ab04367c7791bb33120f5f95845823794987 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 5 Nov 2023 14:58:37 +1300 Subject: [PATCH 06/55] gps: update to latest release/1.14 branch This sets the src/drivers/gps/devices submodule to the latest release/1.14 branch. This fixes a potential issue with the Unicore M10 GPS driver, making sure the AGRICA message is requested. Signed-off-by: Julian Oes --- src/drivers/gps/devices | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 99f5960eca66..3fc56c9dc069 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 99f5960eca66150e33dc277e71a4c99187c27ddc +Subproject commit 3fc56c9dc069807b3ec8ad947dfd0d11aea9944a From 6cdf09644ea195de5cb0bb829e35ffeedbdbc86d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 7 Nov 2023 16:48:41 +0100 Subject: [PATCH 07/55] PositionSmoothing: fix corner altitude bug During a round corner the L1 distance calculation was only done in 2D and the z-axis coordinate was directly coming from the next waypoint. This lead to an unpredictable altitude profile between two waypoints. Sometimes almost all altitude difference was already covered during the turn instead of going diagonally. --- src/lib/motion_planning/PositionSmoothing.cpp | 23 ++++++++----------- src/lib/motion_planning/PositionSmoothing.hpp | 2 +- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index a5c68f04d173..4b3a427a217b 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -134,21 +134,20 @@ const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, co } // Get the crossing point using L1-style guidance - auto l1_point = _getL1Point(position, waypoints); - return {l1_point(0), l1_point(1), target(2)}; + return _getL1Point(position, waypoints); } -const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const +const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const { - const Vector2f pos_traj(_trajectory[0].getCurrentPosition(), - _trajectory[1].getCurrentPosition()); - const Vector2f u_prev_to_target = Vector2f(waypoints[1] - waypoints[0]).unit_or_zero(); - const Vector2f prev_to_pos(pos_traj - Vector2f(waypoints[0])); - const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target)); - const Vector2f closest_pt = Vector2f(waypoints[0]) + prev_to_closest; + const Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + const Vector3f u_prev_to_target = (waypoints[1] - waypoints[0]).unit_or_zero(); + const Vector3f prev_to_pos(pos_traj - waypoints[0]); + const Vector3f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target)); + const Vector3f closest_pt = waypoints[0] + prev_to_closest; // Compute along-track error using L1 distance and cross-track error - const float crosstrack_error = Vector2f(closest_pt - pos_traj).length(); + const float crosstrack_error = (closest_pt - pos_traj).length(); const float l1 = math::max(_target_acceptance_radius, 5.f); float alongtrack_error = 0.f; @@ -159,9 +158,7 @@ const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve } // Position of the point on the line where L1 intersect the line between the two waypoints - const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target; - - return l1_point; + return closest_pt + alongtrack_error * u_prev_to_target; } const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3], diff --git a/src/lib/motion_planning/PositionSmoothing.hpp b/src/lib/motion_planning/PositionSmoothing.hpp index 73886d0124ed..61070d9dabd4 100644 --- a/src/lib/motion_planning/PositionSmoothing.hpp +++ b/src/lib/motion_planning/PositionSmoothing.hpp @@ -438,7 +438,7 @@ class PositionSmoothing const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3], bool is_single_waypoint, const Vector3f &feedforward_velocity_setpoint); - const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const; + const Vector3f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const; const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const; float _getMaxXYSpeed(const Vector3f(&waypoints)[3]) const; float _getMaxZSpeed(const Vector3f(&waypoints)[3]) const; From c3ed50488f4e96e4bb8394419a0e6ce9f13970a4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 19 Oct 2023 09:06:52 +1300 Subject: [PATCH 08/55] Remove SYS_USE_IO param The param is not really required anymore with the actuator configuration. Also, when it is set to 0, RC doesn't work for some boards which would be nice to avoid. Signed-off-by: Julian Oes --- ROMFS/px4fmu_common/init.d/rcS | 35 ++++++++----------- boards/ark/fmu-v6x/init/rc.board_defaults | 7 ---- .../cubeorange/init/rc.board_defaults | 2 -- .../cubeorangeplus/init/rc.board_defaults | 2 -- .../cubeyellow/init/rc.board_defaults | 2 -- .../durandal-v1/init/rc.board_defaults | 2 -- boards/holybro/pix32v5/init/rc.board_defaults | 2 -- boards/mro/x21-777/init/rc.board_defaults | 2 -- boards/mro/x21/init/rc.board_defaults | 2 -- boards/px4/fmu-v2/init/rc.board_defaults | 2 -- boards/px4/fmu-v3/init/rc.board_defaults | 2 -- boards/px4/fmu-v4pro/init/rc.board_defaults | 2 -- boards/px4/fmu-v5/init/rc.board_defaults | 7 ---- boards/px4/fmu-v5x/init/rc.board_defaults | 2 -- boards/px4/fmu-v6c/init/rc.board_defaults | 2 -- boards/px4/fmu-v6x/init/rc.board_defaults | 1 - .../smartap-airlink/init/rc.board_defaults | 2 -- boards/thepeach/k1/init/rc.board_defaults | 2 -- boards/thepeach/r1/init/rc.board_defaults | 2 -- src/drivers/px4io/px4io.cpp | 5 ++- src/drivers/px4io/px4io_params.c | 12 ------- 21 files changed, 17 insertions(+), 80 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4a391d39f9a9..8a216d8c8a5b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -266,32 +266,26 @@ else . $FCONFIG fi - # - # Start IO for PWM output or RC input if enabled - # - if param compare -s SYS_USE_IO 1 + # Check if PX4IO present and update firmware if needed. + if [ -f $IOFW ] then - # Check if PX4IO present and update firmware if needed. - if [ -f $IOFW ] + if ! px4io checkcrc ${IOFW} then - if ! px4io checkcrc ${IOFW} - then - # tune Program PX4IO - tune_control play -t 16 # tune 16 = PROG_PX4IO + # tune Program PX4IO + tune_control play -t 16 # tune 16 = PROG_PX4IO - if px4io update ${IOFW} + if px4io update ${IOFW} + then + usleep 10000 + tune_control stop + if px4io checkcrc ${IOFW} then - usleep 10000 - tune_control stop - if px4io checkcrc ${IOFW} - then - tune_control play -t 17 # tune 17 = PROG_PX4IO_OK - else - tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR - fi + tune_control play -t 17 # tune 17 = PROG_PX4IO_OK else - tune_control stop + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR fi + else + tune_control stop fi fi @@ -302,6 +296,7 @@ else fi fi + # # RC update (map raw RC input to calibrate manual control) # start before commander diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 7a26ee01c8ee..87451b0c52ea 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -32,11 +32,4 @@ then param set-default SENS_TEMP_ID 3014666 fi -if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007 -then - param set-default SYS_USE_IO 0 -else - param set-default SYS_USE_IO 1 -fi - safety_button start diff --git a/boards/cubepilot/cubeorange/init/rc.board_defaults b/boards/cubepilot/cubeorange/init/rc.board_defaults index 8ae2ed458b0e..7f961cd33e71 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_defaults +++ b/boards/cubepilot/cubeorange/init/rc.board_defaults @@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0 param set-default -s SENS_TEMP_ID 2621474 -param set-default SYS_USE_IO 1 - set IOFW "/etc/extras/cubepilot_io-v2_default.bin" diff --git a/boards/cubepilot/cubeorangeplus/init/rc.board_defaults b/boards/cubepilot/cubeorangeplus/init/rc.board_defaults index 8ae2ed458b0e..7f961cd33e71 100644 --- a/boards/cubepilot/cubeorangeplus/init/rc.board_defaults +++ b/boards/cubepilot/cubeorangeplus/init/rc.board_defaults @@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0 param set-default -s SENS_TEMP_ID 2621474 -param set-default SYS_USE_IO 1 - set IOFW "/etc/extras/cubepilot_io-v2_default.bin" diff --git a/boards/cubepilot/cubeyellow/init/rc.board_defaults b/boards/cubepilot/cubeyellow/init/rc.board_defaults index 54705e650d4f..6f39beb0c8ec 100644 --- a/boards/cubepilot/cubeyellow/init/rc.board_defaults +++ b/boards/cubepilot/cubeyellow/init/rc.board_defaults @@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 17 # Disable IMU thermal control param set-default SENS_EN_THERMAL 0 -param set-default SYS_USE_IO 1 - set IOFW "/etc/extras/cubepilot_io-v2_default.bin" diff --git a/boards/holybro/durandal-v1/init/rc.board_defaults b/boards/holybro/durandal-v1/init/rc.board_defaults index 2a3b2c0a7b38..49254efb15a4 100644 --- a/boards/holybro/durandal-v1/init/rc.board_defaults +++ b/boards/holybro/durandal-v1/init/rc.board_defaults @@ -11,5 +11,3 @@ param set-default BAT2_A_PER_V 36.367515152 # Enable IMU thermal control param set-default SENS_EN_THERMAL 1 - -param set-default SYS_USE_IO 1 diff --git a/boards/holybro/pix32v5/init/rc.board_defaults b/boards/holybro/pix32v5/init/rc.board_defaults index 94c582821480..c2300a50a770 100644 --- a/boards/holybro/pix32v5/init/rc.board_defaults +++ b/boards/holybro/pix32v5/init/rc.board_defaults @@ -9,7 +9,5 @@ param set-default BAT2_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 param set-default BAT2_A_PER_V 36.367515152 -param set-default SYS_USE_IO 1 - rgbled_pwm start safety_button start diff --git a/boards/mro/x21-777/init/rc.board_defaults b/boards/mro/x21-777/init/rc.board_defaults index e9fecd78a833..5d576dd74711 100644 --- a/boards/mro/x21-777/init/rc.board_defaults +++ b/boards/mro/x21-777/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - -param set-default SYS_USE_IO 1 diff --git a/boards/mro/x21/init/rc.board_defaults b/boards/mro/x21/init/rc.board_defaults index e9fecd78a833..5d576dd74711 100644 --- a/boards/mro/x21/init/rc.board_defaults +++ b/boards/mro/x21/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - -param set-default SYS_USE_IO 1 diff --git a/boards/px4/fmu-v2/init/rc.board_defaults b/boards/px4/fmu-v2/init/rc.board_defaults index e9fecd78a833..5d576dd74711 100644 --- a/boards/px4/fmu-v2/init/rc.board_defaults +++ b/boards/px4/fmu-v2/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - -param set-default SYS_USE_IO 1 diff --git a/boards/px4/fmu-v3/init/rc.board_defaults b/boards/px4/fmu-v3/init/rc.board_defaults index e9fecd78a833..5d576dd74711 100644 --- a/boards/px4/fmu-v3/init/rc.board_defaults +++ b/boards/px4/fmu-v3/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - -param set-default SYS_USE_IO 1 diff --git a/boards/px4/fmu-v4pro/init/rc.board_defaults b/boards/px4/fmu-v4pro/init/rc.board_defaults index 42378a19d727..8dd9df28d5a2 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_defaults +++ b/boards/px4/fmu-v4pro/init/rc.board_defaults @@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 26.4 param set-default EKF2_MULTI_IMU 2 param set-default SENS_IMU_MODE 0 -param set-default SYS_USE_IO 1 - set LOGGER_BUF 64 diff --git a/boards/px4/fmu-v5/init/rc.board_defaults b/boards/px4/fmu-v5/init/rc.board_defaults index 83c1976b74be..d7f8b05bd811 100644 --- a/boards/px4/fmu-v5/init/rc.board_defaults +++ b/boards/px4/fmu-v5/init/rc.board_defaults @@ -9,13 +9,6 @@ param set-default BAT2_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 param set-default BAT2_A_PER_V 36.367515152 -if ver hwtypecmp V5004000 V5006000 -then - param set-default SYS_USE_IO 0 -else - param set-default SYS_USE_IO 1 -fi - if ver hwtypecmp V5005000 V5005002 V5006000 V5006002 then # CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index ba812e4b7bfd..a7d844a36632 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -16,6 +16,4 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 -param set-default SYS_USE_IO 1 - safety_button start diff --git a/boards/px4/fmu-v6c/init/rc.board_defaults b/boards/px4/fmu-v6c/init/rc.board_defaults index 6418e5836d8b..f7dd26972135 100644 --- a/boards/px4/fmu-v6c/init/rc.board_defaults +++ b/boards/px4/fmu-v6c/init/rc.board_defaults @@ -9,5 +9,3 @@ param set-default BAT2_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 param set-default BAT2_A_PER_V 36.367515152 - -param set-default SYS_USE_IO 1 diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index ba812e4b7bfd..f6e71a14caee 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -16,6 +16,5 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 -param set-default SYS_USE_IO 1 safety_button start diff --git a/boards/sky-drones/smartap-airlink/init/rc.board_defaults b/boards/sky-drones/smartap-airlink/init/rc.board_defaults index c581f8c741f9..212c72657f44 100644 --- a/boards/sky-drones/smartap-airlink/init/rc.board_defaults +++ b/boards/sky-drones/smartap-airlink/init/rc.board_defaults @@ -25,8 +25,6 @@ param set-default BAT_V_OFFS_CURR 0.413 # Disable safety switch param set-default CBRK_IO_SAFETY 22027 -param set-default SYS_USE_IO 1 - safety_button start set LOGGER_BUF 32 diff --git a/boards/thepeach/k1/init/rc.board_defaults b/boards/thepeach/k1/init/rc.board_defaults index 5d8a3c182ba1..f22fa838f49e 100644 --- a/boards/thepeach/k1/init/rc.board_defaults +++ b/boards/thepeach/k1/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 - -param set-default SYS_USE_IO 1 diff --git a/boards/thepeach/r1/init/rc.board_defaults b/boards/thepeach/r1/init/rc.board_defaults index 5d8a3c182ba1..f22fa838f49e 100644 --- a/boards/thepeach/r1/init/rc.board_defaults +++ b/boards/thepeach/r1/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 - -param set-default SYS_USE_IO 1 diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 21c0a53b2bfc..3f8d0f89a125 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -336,8 +336,7 @@ class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleIn (ParamInt) _param_rc_rssi_pwm_max, (ParamInt) _param_rc_rssi_pwm_min, (ParamInt) _param_sens_en_themal, - (ParamInt) _param_sys_hitl, - (ParamInt) _param_sys_use_io + (ParamInt) _param_sys_hitl ) }; @@ -468,7 +467,7 @@ int PX4IO::init() } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) { + if (_param_sys_hitl.get() <= 0) { _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); } diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index ef1c1ee6ab92..b390bdb18a29 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -42,18 +42,6 @@ #include #include -/** - * Set usage of IO board - * - * Can be used to use a configure the use of the IO board. - * - * @value 0 IO PWM disabled (RC only) - * @value 1 IO enabled (RC & PWM) - * @reboot_required true - * @group System - */ -PARAM_DEFINE_INT32(SYS_USE_IO, 0); - /** * S.BUS out * From 9ab89702068d0e6c057ad432ba4bba3cef87ad2f Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Thu, 16 Nov 2023 17:31:33 +0100 Subject: [PATCH 09/55] atune: initialize filter if not already initialized --- .../fw_autotune_attitude_control.cpp | 2 +- .../mc_autotune_attitude_control.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index ec274f28dac2..641a90465cba 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -225,7 +225,7 @@ void FwAutotuneAttitudeControl::checkFilters() reset_filters = true; } - if (reset_filters) { + if (reset_filters || !_are_filters_initialized) { _are_filters_initialized = true; _filter_sample_rate = update_rate_hz; _sys_id.setLpfCutoffFrequency(_filter_sample_rate, _param_imu_gyro_cutoff.get()); diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp index 749186ee8d04..c99ccad653ab 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -236,7 +236,7 @@ void McAutotuneAttitudeControl::checkFilters() reset_filters = true; } - if (reset_filters && !_are_filters_initialized) { + if (reset_filters || !_are_filters_initialized) { _filter_dt = _sample_interval_avg; const float filter_rate_hz = 1.f / _filter_dt; From b15e57dd4f878d7598ff4a6601e11656da4606a4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 20 Nov 2023 18:30:57 +1300 Subject: [PATCH 10/55] icm45686: fix dt (and usage command) With the wrong dt, the flight behaviour was really poor and noisy, so this fix is absolutely required. Signed-off-by: Julian Oes --- src/drivers/imu/invensense/icm45686/ICM45686.cpp | 4 ++-- src/drivers/imu/invensense/icm45686/ICM45686.hpp | 2 +- src/drivers/imu/invensense/icm45686/icm45686_main.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/imu/invensense/icm45686/ICM45686.cpp b/src/drivers/imu/invensense/icm45686/ICM45686.cpp index 07fdf6633a86..1bd2a1283450 100644 --- a/src/drivers/imu/invensense/icm45686/ICM45686.cpp +++ b/src/drivers/imu/invensense/icm45686/ICM45686.cpp @@ -522,7 +522,7 @@ void ICM45686::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); } else { - accel.dt = FIFO_TIMESTAMP_SCALING; + accel.dt = FIFO_SAMPLE_DT; } // 20 bit hires mode @@ -634,7 +634,7 @@ void ICM45686::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); } else { - gyro.dt = FIFO_TIMESTAMP_SCALING; + gyro.dt = FIFO_SAMPLE_DT; } // 20 bit hires mode diff --git a/src/drivers/imu/invensense/icm45686/ICM45686.hpp b/src/drivers/imu/invensense/icm45686/ICM45686.hpp index f370808772e1..efcc485b7152 100644 --- a/src/drivers/imu/invensense/icm45686/ICM45686.hpp +++ b/src/drivers/imu/invensense/icm45686/ICM45686.hpp @@ -70,7 +70,7 @@ class ICM45686 : public device::SPI, public I2CSPIDriver void exit_and_cleanup() override; // Sensor Configuration - static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured + static constexpr float FIFO_SAMPLE_DT{1e6f / 6400.f}; // 6400 Hz accel & gyro ODR configured static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; diff --git a/src/drivers/imu/invensense/icm45686/icm45686_main.cpp b/src/drivers/imu/invensense/icm45686/icm45686_main.cpp index 8cf88e059754..1951acd9895e 100644 --- a/src/drivers/imu/invensense/icm45686/icm45686_main.cpp +++ b/src/drivers/imu/invensense/icm45686/icm45686_main.cpp @@ -38,7 +38,7 @@ void ICM45686::print_usage() { - PRINT_MODULE_USAGE_NAME("icm42688p", "driver"); + PRINT_MODULE_USAGE_NAME("icm45686", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); From ff2441d73a01ae9695bafcac19ceeb0fea39e917 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Tue, 21 Nov 2023 13:37:04 +0100 Subject: [PATCH 11/55] ekf2-terrain: fix validity switching Bug not present after 1.14 --- src/modules/ekf2/EKF/terrain_estimator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 657b989f4882..f368e1f615c9 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -199,7 +199,6 @@ void Ekf::resetHaglRng() _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; _time_last_hagl_fuse = _time_delayed_us; - _time_last_healthy_rng_data = 0; } void Ekf::stopHaglRngFusion() From 26109a2fe6aa0620a9143c314873b3867c6773f7 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Thu, 21 Sep 2023 17:40:36 +0200 Subject: [PATCH 12/55] ekf2 rng kin: allow check to become true during horizontal motion Even if there is some horizontal motion, a passing check should be accepted as the terrain can be flat. However, the vehicle must not be moving horizontally to invalidate the consistency as a change in terrain can make the kinematic check temporarily fail. --- .../EKF/range_finder_consistency_check.cpp | 20 +++++++++++++++---- .../EKF/range_finder_consistency_check.hpp | 3 ++- src/modules/ekf2/EKF/range_height_control.cpp | 8 ++++---- 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp index 0d8daee7dea9..453da560b421 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp @@ -37,8 +37,12 @@ #include "range_finder_consistency_check.hpp" -void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us) +void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us) { + if (horizontal_motion) { + _time_last_horizontal_motion = time_us; + } + const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; if ((_time_last_update_us == 0) @@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) { + if (fabsf(vz) < _min_vz_for_valid_consistency) { + // We can only check consistency if there is vertical motion + return; + } + if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { - _is_kinematically_consistent = false; - _time_last_inconsistent_us = time_us; + if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { + _is_kinematically_consistent = false; + _time_last_inconsistent_us = time_us; + } } else { - if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { + if (_test_ratio < 1.f + && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { _is_kinematically_consistent = true; } } diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp index 1498a4aa6cce..fa185f662955 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -47,7 +47,7 @@ class RangeFinderConsistencyCheck final RangeFinderConsistencyCheck() = default; ~RangeFinderConsistencyCheck() = default; - void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us); + void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us); void setGate(float gate) { _gate = gate; } @@ -71,6 +71,7 @@ class RangeFinderConsistencyCheck final bool _is_kinematically_consistent{true}; uint64_t _time_last_inconsistent_us{}; + uint64_t _time_last_horizontal_motion{}; static constexpr float _signed_test_ratio_tau = 2.f; diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 88a9cb719711..fa2a365f356a 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion() const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - // Run the kinematic consistency check when not moving horizontally - if (_control_status.flags.in_air && !_control_status.flags.fixed_wing - && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { + if (_control_status.flags.in_air) { + const bool horizontal_motion = _control_status.flags.fixed_wing + || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P(4, 4) + P(5, 5), 0.1f)); const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); const float var = sq(_params.range_noise) + dist_dependant_var; _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us); + _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), horizontal_motion, _time_delayed_us); } } else { From 634ad3893e1eb5b47ac6a2252e3471c42f89a1bb Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Wed, 1 Nov 2023 16:42:53 +0000 Subject: [PATCH 13/55] flight mode manager: fix terrain hold --- .../ManualAltitude/FlightTaskManualAltitude.cpp | 4 +--- .../ManualAltitude/FlightTaskManualAltitude.hpp | 3 +-- .../FlightTaskManualAltitudeSmoothVel.cpp | 12 +++++++++++- .../FlightTaskManualAltitudeSmoothVel.hpp | 3 +++ .../FlightTaskManualPositionSmoothVel.cpp | 12 +++++++++++- .../FlightTaskManualPositionSmoothVel.hpp | 2 ++ 6 files changed, 29 insertions(+), 7 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 90d5dccb1feb..3508d6b7701b 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -127,7 +127,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) { // Stop using distance to ground _terrain_hold = false; - _terrain_follow = false; // Adjust the setpoint to maintain the same height error to reduce control transients if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) { @@ -144,7 +143,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) { // Start using distance to ground _terrain_hold = true; - _terrain_follow = true; // Adjust the setpoint to maintain the same height error to reduce control transients if (PX4_ISFINITE(_position_setpoint(2))) { @@ -155,7 +153,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() } - if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) { + if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) { // terrain following _terrainFollowing(apply_brake, stopped); // respect maximum altitude diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 0b9fbe72a3fb..be9eb278d38e 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -75,6 +75,7 @@ class FlightTaskManualAltitude : public FlightTask StickYaw _stick_yaw{this}; bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data + bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_hold_max_z, @@ -121,8 +122,6 @@ class FlightTaskManualAltitude : public FlightTask void setGearAccordingToSwitch(); uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ - bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ - bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */ float _max_distance_to_ground{(float)INFINITY}; /**< max distance to ground constraint */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index dd7ee8225ef6..2f6128508e2d 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -106,5 +106,15 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState() _jerk_setpoint(2) = _smoothing.getCurrentJerk(); _acceleration_setpoint(2) = _smoothing.getCurrentAcceleration(); _velocity_setpoint(2) = _smoothing.getCurrentVelocity(); - _position_setpoint(2) = _smoothing.getCurrentPosition(); + + if (!_terrain_hold) { + if (_terrain_hold_previous) { + // Reset position setpoint to current position when switching from terrain hold to non-terrain hold + _smoothing.setCurrentPosition(_position(2)); + } + + _position_setpoint(2) = _smoothing.getCurrentPosition(); + } + + _terrain_hold_previous = _terrain_hold; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 45e013fb7c82..468388c031b9 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -67,4 +67,7 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude (ParamFloat) _param_mpc_acc_up_max, (ParamFloat) _param_mpc_acc_down_max ) + +private: + bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ }; diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 14527ba712d2..a89283e14619 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -171,5 +171,15 @@ void FlightTaskManualPositionSmoothVel::_setOutputStateZ() _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); - _position_setpoint(2) = _smoothing_z.getCurrentPosition(); + + if (!_terrain_hold) { + if (_terrain_hold_previous) { + // Reset position setpoint to current position when switching from terrain hold to non-terrain hold + _smoothing_z.setCurrentPosition(_position(2)); + } + + _position_setpoint(2) = _smoothing_z.getCurrentPosition(); + } + + _terrain_hold_previous = _terrain_hold; } diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index aa82261e9d30..c8fbdf42c95a 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -87,4 +87,6 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction + + bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ }; From 740bf63fa7d19f97ea40bce620e8d32a12a878e1 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 7 Dec 2023 10:44:50 +0100 Subject: [PATCH 14/55] TECS: set _ratio_underspeed to 0 if airspeed disabled Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 38ed565a055d..ae5715d86c89 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -353,7 +353,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag) { - if (!flag.detect_underspeed_enabled) { + if (!flag.detect_underspeed_enabled || !flag.airspeed_enabled) { _ratio_undersped = 0.0f; return; } From 5928d7f067f03ad9da1d5f4c2c919a6e9c462aaa Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 7 Dec 2023 10:45:08 +0100 Subject: [PATCH 15/55] TECS: init to airspeed filter to trim airspeed if airspeed-less Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 16 ++++++++++++---- src/lib/tecs/TECS.hpp | 5 ++++- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index ae5715d86c89..b42a6c2d95e2 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -52,10 +52,17 @@ using namespace time_literals; static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);} -void TECSAirspeedFilter::initialize(const float equivalent_airspeed) +void TECSAirspeedFilter::initialize(const float equivalent_airspeed, const float equivalent_airspeed_trim, + const bool airspeed_sensor_available) { - _airspeed_state.speed = equivalent_airspeed; - _airspeed_state.speed_rate = 0.0f; + if (airspeed_sensor_available && PX4_ISFINITE(equivalent_airspeed)) { + _airspeed_state.speed = equivalent_airspeed; + + } else { + _airspeed_state.speed = equivalent_airspeed_trim; + } + + _airspeed_state.speed_rate = 0.f; } void TECSAirspeedFilter::update(const float dt, const Input &input, const Param ¶m, @@ -654,7 +661,8 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude, .alt_rate = altitude_rate}; _altitude_reference_model.initialize(current_state); - _airspeed_filter.initialize(equivalent_airspeed); + _airspeed_filter.initialize(equivalent_airspeed, _airspeed_filter_param.equivalent_airspeed_trim, + _control_flag.airspeed_enabled); TECSControl::Setpoint control_setpoint; control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference(); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 5673342772e1..1848a8765bc4 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -88,8 +88,11 @@ class TECSAirspeedFilter * @brief Initialize filter * * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s]. + * @param[in] equivalent_airspeed_trim is the equivalent airspeed trim (vehicle setting) in [m/s]. + * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available. */ - void initialize(float equivalent_airspeed); + void initialize(float equivalent_airspeed, const float equivalent_airspeed_trim, + const bool airspeed_sensor_available); /** * @brief Update filter From 5d433ddef729ce47a87823c6964a8b2b02c214d0 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 7 Dec 2023 10:39:10 +0100 Subject: [PATCH 16/55] TECS: make sure to constrain pitch to current min/max pitch Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index b42a6c2d95e2..8353d60fdd8b 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -419,6 +419,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON); _pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment, _pitch_setpoint + pitch_increment); + _pitch_setpoint = constrain(_pitch_setpoint, param.pitch_min, param.pitch_max); //Debug Output _debug_output.energy_balance_rate_estimate = seb_rate.estimate; From a536e3dfe2fba7795f543c2c82e1da10880d904c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 6 Dec 2023 17:03:47 +0100 Subject: [PATCH 17/55] TECS: init control params to reasonable values MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The control params (eg min/max pitch) are used before they are correctly set by TECS::update(). While this is an issue we should fix, it also doesn't hurt to set them to more reasobale values (eg 30° limit). Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 1848a8765bc4..0d15b8a6c41f 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -699,9 +699,9 @@ class TECS .max_climb_rate = 5.0f, .vert_accel_limit = 0.0f, .equivalent_airspeed_trim = 15.0f, - .tas_min = 3.0f, - .pitch_max = 5.0f, - .pitch_min = -5.0f, + .tas_min = 10.0f, + .pitch_max = 0.5f, + .pitch_min = -0.5f, .throttle_trim = 0.0f, .throttle_trim_adjusted = 0.f, .throttle_max = 1.0f, From 43ba199c370b8d3a866670958452d04ad06abeec Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 11 Dec 2023 14:35:56 +0100 Subject: [PATCH 18/55] Navigator: same logic for VTOL_TAKEOFF as for TAKEOFF (#22519) Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission_block.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 565b2f2150ce..3f923fce3f88 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -694,6 +694,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi break; case NAV_CMD_TAKEOFF: + case NAV_CMD_VTOL_TAKEOFF: // if already flying (armed and !landed) treat TAKEOFF like regular POSITION if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) @@ -707,10 +708,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi break; - case NAV_CMD_VTOL_TAKEOFF: - sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - break; - case NAV_CMD_LAND: case NAV_CMD_VTOL_LAND: sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; From 968089bae4250c04fffdcc32bc0beebca9380451 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Sep 2023 17:18:09 +0200 Subject: [PATCH 19/55] ActuatorEffectivenessHelicopter: explicitly handle unsaturated case This became necessary otherwise the allocation reports saturation all the time and the rate integrator doesn't work. --- .../ActuatorEffectivenessHelicopter.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 9faae22ca434..27fcf4c57507 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -229,6 +229,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.roll_neg) { status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; } if (_saturation_flags.pitch_pos) { @@ -236,6 +239,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.pitch_neg) { status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; } if (_saturation_flags.yaw_pos) { @@ -243,6 +249,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.yaw_neg) { status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; } if (_saturation_flags.thrust_pos) { @@ -250,5 +259,8 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.thrust_neg) { status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_thrust[2] = 0.f; } } From 73fa6e0c52bcd7183fae0c4d36a6bd5c0027c28d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 15 Nov 2023 09:53:40 -0800 Subject: [PATCH 20/55] px4io:Add 'supported' command and uses it in rcS --- ROMFS/px4fmu_common/init.d/rcS | 42 ++++++++++++++++++---------------- src/drivers/px4io/px4io.cpp | 4 ++++ 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8a216d8c8a5b..19654d9adf54 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -266,37 +266,39 @@ else . $FCONFIG fi - # Check if PX4IO present and update firmware if needed. - if [ -f $IOFW ] + if px4io supported then - if ! px4io checkcrc ${IOFW} + # Check if PX4IO present and update firmware if needed. + if [ -f $IOFW ] then - # tune Program PX4IO - tune_control play -t 16 # tune 16 = PROG_PX4IO - - if px4io update ${IOFW} + if ! px4io checkcrc ${IOFW} then - usleep 10000 - tune_control stop - if px4io checkcrc ${IOFW} + # tune Program PX4IO + tune_control play -t 16 # tune 16 = PROG_PX4IO + + if px4io update ${IOFW} then - tune_control play -t 17 # tune 17 = PROG_PX4IO_OK + usleep 10000 + tune_control stop + if px4io checkcrc ${IOFW} + then + tune_control play -t 17 # tune 17 = PROG_PX4IO_OK + else + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR + fi else - tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR + tune_control stop fi - else - tune_control stop fi - fi - if ! px4io start - then - echo "PX4IO start failed" - set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + if ! px4io start + then + echo "PX4IO start failed" + set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + fi fi fi - # # RC update (map raw RC input to calibrate manual control) # start before commander diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3f8d0f89a125..08fee0726ea3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1556,6 +1556,10 @@ int PX4IO::custom_command(int argc, char *argv[]) { const char *verb = argv[0]; + if (!strcmp(verb, "supported")) { + return 0; + } + if (!strcmp(verb, "checkcrc")) { if (is_running()) { PX4_ERR("io must be stopped"); From b50a9dac84ff6df2e19a3883e7739fe39c5d8883 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Fri, 17 Nov 2023 09:13:39 -0700 Subject: [PATCH 21/55] px4io: change not supported message to INFO instead of ERR --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08fee0726ea3..9673acd96d16 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1765,7 +1765,7 @@ Output driver communicating with the IO co-processor. extern "C" __EXPORT int px4io_main(int argc, char *argv[]) { if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - PX4_ERR("PX4IO Not Supported"); + PX4_INFO("PX4IO Not Supported"); return -1; } return PX4IO::main(argc, argv); From d09aa8ade534b78f8b947b91068a53dea0dd2d55 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Dec 2023 11:58:13 +0100 Subject: [PATCH 22/55] matrix: fix slice to slice assignment to do deep copy To fix usage of a.xy() = b.xy() which should copy the first two elements over into a and not act on a copy of a. --- src/lib/matrix/matrix/Slice.hpp | 8 ++++++++ src/lib/matrix/test/MatrixSliceTest.cpp | 9 +++++++++ 2 files changed, 17 insertions(+) diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp index 207d21fdd9ea..da995a420b2c 100644 --- a/src/lib/matrix/matrix/Slice.hpp +++ b/src/lib/matrix/matrix/Slice.hpp @@ -35,6 +35,8 @@ class Slice assert(y0 + Q <= N); } + Slice(const Slice &other) = default; + const Type &operator()(size_t i, size_t j) const { assert(i < P); @@ -52,6 +54,12 @@ class Slice return (*_data)(_x0 + i, _y0 + j); } + // Separate function needed otherwise the default copy constructor matches before the deep copy implementation + Slice &operator=(const Slice &other) + { + return this->operator=(other); + } + template Slice &operator=(const Slice &other) { diff --git a/src/lib/matrix/test/MatrixSliceTest.cpp b/src/lib/matrix/test/MatrixSliceTest.cpp index 7849259c65e5..9240e6b5a8c2 100644 --- a/src/lib/matrix/test/MatrixSliceTest.cpp +++ b/src/lib/matrix/test/MatrixSliceTest.cpp @@ -262,3 +262,12 @@ TEST(MatrixSliceTest, Slice) float O_check_data_12 [4] = {2.5, 3, 4, 5}; EXPECT_EQ(res_12, (SquareMatrix(O_check_data_12))); } + +TEST(MatrixSliceTest, XYAssignmentTest) +{ + Vector3f a(1, 2, 3); + Vector3f b(4, 5, 6); + // Assign first two elements from b to first two slot of a + a.xy() = b.xy(); + EXPECT_EQ(a, Vector3f(4, 5, 3)); +} From 6eb8d042e1a8e499e416f08d4ca5340fd9e0efb7 Mon Sep 17 00:00:00 2001 From: MaEtUgR Date: Thu, 21 Dec 2023 12:18:44 +0000 Subject: [PATCH 23/55] [AUTO COMMIT] update change indication --- src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv | 4 ++-- src/modules/ekf2/test/change_indication/iris_gps.csv | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv index 4a47965592a3..243058d0ab44 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -1,5 +1,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 90000,1,-0.0096,-0.01,-0.02,-0.0004,0.0026,-0.026,-1.4e-05,0.00011,-0.0023,0,0,0,0,0,0,0.19,-4.7e-10,0.4,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 190000,1,-0.0096,-0.011,-0.02,0.0002,0.0039,-0.041,6e-06,0.00043,-0.0044,-3e-13,-2.3e-14,5.8e-15,0,0,-2e-11,0.19,-4.7e-10,0.4,0,0,0,0,0,5.8e-07,0.0027,0.0027,0.0008,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 290000,1,-0.0097,-0.011,-0.02,0.0012,0.0063,-0.053,5e-05,0.00036,-0.007,9.1e-12,9.1e-13,-1.7e-13,0,0,-4.8e-09,0.19,-4.7e-10,0.4,0,0,0,0,0,6.8e-07,0.0029,0.0029,0.00067,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 @@ -132,7 +132,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 12990000,0.7,0.0012,-0.014,0.71,-0.0078,0.0067,-0.03,-0.0011,0.0013,-3.7e+02,-1.2e-05,-5.9e-05,-2.3e-06,6.8e-06,9.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.062,0.062,0.025,0.049,0.049,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0 13090000,0.7,0.0012,-0.014,0.71,-0.0084,0.0069,-0.03,-0.0019,0.002,-3.7e+02,-1.2e-05,-5.8e-05,-2.8e-06,6.4e-06,9.8e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00021,0.00021,9.3e-05,0.071,0.071,0.025,0.057,0.057,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0 13190000,0.7,0.00095,-0.014,0.71,0.00044,0.0063,-0.027,0.0043,0.0013,-3.7e+02,-1.1e-05,-5.9e-05,-2.8e-06,5.1e-06,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.058,0.058,0.025,0.049,0.049,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0 -13290000,0.7,0.00095,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0 +13290000,0.7,0.00096,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0 13390000,0.7,0.00081,-0.014,0.71,0.001,0.0062,-0.02,0.0032,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-2e-06,2.6e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.055,0.055,0.026,0.048,0.048,0.05,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,8.8e-07,0,0,0,0,0,0,0,0 13490000,0.7,0.00084,-0.014,0.71,0.0006,0.0062,-0.019,0.0033,0.0018,-3.7e+02,-1.1e-05,-5.9e-05,-1.7e-06,1.8e-06,1.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.063,0.063,0.028,0.056,0.056,0.05,3.5e-09,3.5e-09,1.8e-09,3.5e-06,3.5e-06,8.7e-07,0,0,0,0,0,0,0,0 13590000,0.7,0.00078,-0.014,0.71,0.0008,0.0064,-0.021,0.0023,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-1.9e-06,1.7e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.052,0.052,0.028,0.048,0.048,0.05,3.3e-09,3.4e-09,1.8e-09,3.5e-06,3.5e-06,8.4e-07,0,0,0,0,0,0,0,0 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index 88b4777030ae..cddd19097efb 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -1,5 +1,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 +10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 90000,0.98,-0.0095,-0.012,0.18,-5.5e-05,-0.0032,-0.024,-3.6e-06,-0.00014,-0.0021,0,0,0,0,0,0,0.2,-3.3e-09,0.43,0,0,0,0,0,8.9e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 190000,0.98,-0.0092,-0.013,0.21,-0.0013,-0.0036,-0.037,-4.4e-05,-0.00046,-0.017,5.2e-12,-4.3e-12,-1.5e-13,0,0,-6.8e-10,0.2,0.011,0.43,0,0,0,0,0,2.8e-06,0.0027,0.0027,0.00081,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 290000,0.98,-0.0092,-0.013,0.21,-0.0016,-0.0053,-0.046,-0.00015,-0.00032,-0.018,4.4e-11,-5.4e-11,-2.6e-12,0,0,-2.9e-08,0.2,0.011,0.43,0,0,0,0,0,6.6e-06,0.0029,0.0029,0.00068,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 @@ -347,5 +347,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 34490000,0.98,-0.0069,-0.013,0.18,-0.016,-0.0096,-0.09,0.071,-0.011,-0.11,-1.4e-05,-5.6e-05,2.3e-06,0.00017,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.3e-05,3.3e-05,4.2e-05,0.053,0.053,0.0059,0.051,0.051,0.032,2.6e-11,2.6e-11,8.1e-11,2.3e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0 34590000,0.98,-0.0069,-0.013,0.18,-0.014,-0.0053,0.71,0.073,-0.0089,-0.081,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3e-05,4.2e-05,0.044,0.044,0.0059,0.045,0.045,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 34690000,0.98,-0.0068,-0.012,0.18,-0.017,-0.0032,1.7,0.071,-0.0093,0.037,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3.1e-05,4.2e-05,0.047,0.047,0.006,0.052,0.052,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -34790000,0.98,-0.0068,-0.012,0.18,-0.018,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 +34790000,0.98,-0.0068,-0.012,0.18,-0.019,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 34890000,0.98,-0.0068,-0.012,0.18,-0.022,0.0039,3.6,0.07,-0.0065,0.5,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.043,0.043,0.0061,0.052,0.052,0.032,2.6e-11,2.6e-11,7.8e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 From beb834af2b489941409f39599a98970339f8917c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 10 Jan 2024 10:27:32 +0100 Subject: [PATCH 24/55] fmu-v6x: add crossfire UART driver --- boards/px4/fmu-v6x/default.px4board | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 484b844f1910..f6198ca51fa7 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -36,6 +36,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y From 454a9875682a3450cda173a130c77289a55a39f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 24 Jan 2024 12:10:35 +1300 Subject: [PATCH 25/55] fmu-6x: fix Telem2 without flow control When flow control is used together with DMA, we need to add a pulldown to CTS. Without it, it assumes flow control and gets stuck when CTS is not connected. Signed-off-by: Julian Oes --- boards/px4/fmu-v6x/nuttx-config/include/board.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/boards/px4/fmu-v6x/nuttx-config/include/board.h b/boards/px4/fmu-v6x/nuttx-config/include/board.h index eb3004183b02..cd1f7120bec3 100644 --- a/boards/px4/fmu-v6x/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6x/nuttx-config/include/board.h @@ -380,7 +380,9 @@ #define GPIO_UART5_RX GPIO_UART5_RX_3 /* PD2 */ #define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ // GPIO_UART5_RTS no remap /* PC8 */ -// GPIO_UART5_CTS No remap /* PC9 */ +#undef GPIO_UART5_CTS +#define GPIO_UART5_CTS ((GPIO_ALT|GPIO_AF8|GPIO_PORTC|GPIO_PIN9) | GPIO_PULLDOWN) /* PC9 */ + #define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ #define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ From b0e86ba3642a160f3196947f929e8b2390c26b46 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 1 Feb 2024 16:01:05 +0100 Subject: [PATCH 26/55] fix FunctionActuatorSet: if a param is set to NaN, it should be ignored MAVLink spec: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ACTUATOR Previously, a command was overwriting all other indexes. --- .../functions/FunctionActuatorSet.hpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp index c9369addd526..7301776113b6 100644 --- a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp +++ b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp @@ -61,12 +61,17 @@ class FunctionActuatorSet : public FunctionProviderBase int index = (int)(vehicle_command.param7 + 0.5f); if (index == 0) { - _data[0] = vehicle_command.param1; - _data[1] = vehicle_command.param2; - _data[2] = vehicle_command.param3; - _data[3] = vehicle_command.param4; - _data[4] = vehicle_command.param5; - _data[5] = vehicle_command.param6; + if (PX4_ISFINITE(vehicle_command.param1)) { _data[0] = vehicle_command.param1; } + + if (PX4_ISFINITE(vehicle_command.param2)) {_data[1] = vehicle_command.param2; } + + if (PX4_ISFINITE(vehicle_command.param3)) {_data[2] = vehicle_command.param3; } + + if (PX4_ISFINITE(vehicle_command.param4)) {_data[3] = vehicle_command.param4; } + + if (PX4_ISFINITE(vehicle_command.param5)) {_data[4] = vehicle_command.param5; } + + if (PX4_ISFINITE(vehicle_command.param6)) {_data[5] = vehicle_command.param6; } } } } From 8df02de9fd846264c57ef14a1a79382c592efc5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 1 Feb 2024 16:25:02 +0100 Subject: [PATCH 27/55] commander: send ack for VEHICLE_CMD_DO_SET_ACTUATOR --- src/modules/commander/Commander.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a9061ad33ba1..8256986a6c21 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1367,6 +1367,10 @@ Commander::handle_command(const vehicle_command_s &cmd) answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); break; + case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + break; + case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: @@ -1403,7 +1407,6 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: - case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE: case vehicle_command_s::VEHICLE_CMD_DO_WINCH: case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER: From b303e9517ff8ee0cdc2c1f3209394af2e980fc89 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 7 Feb 2024 12:41:23 +0100 Subject: [PATCH 28/55] param translation: fix too early return false (#22729) Signed-off-by: Silvan Fuhrer --- src/lib/parameters/param_translation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 5a5603d4cf3d..c201e0df5a5d 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -210,8 +210,6 @@ bool param_modify_on_import(bson_node_t node) } } - return false; - //2023-02-08: translate L1 parameters after removing l1 control { if (strcmp("RWTO_L1_PERIOD", node->name) == 0) { @@ -232,4 +230,6 @@ bool param_modify_on_import(bson_node_t node) return true; } } + + return false; } From 968732477803be8a73edd13a29c8ab71458eb354 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 15 Feb 2024 09:34:52 +0100 Subject: [PATCH 29/55] gps: update submodule --- src/drivers/gps/devices | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 3fc56c9dc069..b3ffec3f173a 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 3fc56c9dc069807b3ec8ad947dfd0d11aea9944a +Subproject commit b3ffec3f173a4dcb4d9e604222a30215023e9880 From 61eb53dc75013ed890db1e683750e8ef6842685a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 14 Aug 2023 15:30:14 +1200 Subject: [PATCH 30/55] kakuteh7: use EKF2 by default Signed-off-by: Julian Oes --- boards/holybro/kakuteh7/init/rc.board_defaults | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/boards/holybro/kakuteh7/init/rc.board_defaults b/boards/holybro/kakuteh7/init/rc.board_defaults index fa014c6b485b..47dea71be617 100644 --- a/boards/holybro/kakuteh7/init/rc.board_defaults +++ b/boards/holybro/kakuteh7/init/rc.board_defaults @@ -22,13 +22,10 @@ param set-default CBRK_SUPPLY_CHK 894281 # Select the Generic 250 Racer by default param set-default SYS_AUTOSTART 4050 -# use the Q attitude estimator, it works w/o mag or GPS. -param set-default SYS_MC_EST_GROUP 3 -param set-default ATT_ACC_COMP 0 -param set-default ATT_W_ACC 0.4000 -param set-default ATT_W_GYRO_BIAS 0.0000 - +# use EKF2 without mag param set-default SYS_HAS_MAG 0 +# and enable gravity fusion +param set-default EKF2_IMU_CONTROL 7 # the startup tune is not great on a binary output buzzer, so disable it param set-default CBRK_BUZZER 782090 From 50b85ca8312b1eebcfc9d14f5194ef92ec2a68fc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Aug 2023 09:47:43 +1200 Subject: [PATCH 31/55] kakute: disable some EKF2 features To save flash. Signed-off-by: Julian Oes --- boards/holybro/kakuteh7/default.px4board | 6 ++++++ boards/holybro/kakuteh7mini/default.px4board | 6 ++++++ boards/holybro/kakuteh7v2/default.px4board | 6 ++++++ 3 files changed, 18 insertions(+) diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 713c736d5066..a9f38ba78c98 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index b019b753f90f..3310e79fbb6a 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -32,6 +32,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index d326a787978c..9c060d844224 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y From c5e2d9d871d0faf2d737b5c9df89ea78aefad7ec Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Oct 2023 03:05:54 -0700 Subject: [PATCH 32/55] px4_fmu-v6x:Rev 6 Sensors omit starting icm42688p, icm42670p, icm20649, icm20602 --- boards/px4/fmu-v6x/init/rc.board_sensors | 38 ++++++++++++------------ 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index e59fb0c938f5..97d62d58c301 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -68,32 +68,32 @@ else bmi088 -G -R 4 -s start fi fi -fi -# Internal SPI bus ICM42688p -if ver hwtypecmp V6X009010 V6X010010 -then - icm42688p -R 12 -s start -else - if ver hwtypecmp V6X000010 + # Internal SPI bus ICM42688p + if ver hwtypecmp V6X009010 V6X010010 then - icm42688p -R 14 -s start + icm42688p -R 12 -s start else - icm42688p -R 6 -s start + if ver hwtypecmp V6X000010 + then + icm42688p -R 14 -s start + else + icm42688p -R 6 -s start + fi fi -fi -if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 -then - # Internal SPI bus ICM-42670-P (hard-mounted) - icm42670p -R 10 -s start -else - if ver hwtypecmp V6X009010 V6X010010 + if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 then - icm20602 -R 6 -s start + # Internal SPI bus ICM-42670-P (hard-mounted) + icm42670p -R 10 -s start else - # Internal SPI bus ICM-20649 (hard-mounted) - icm20649 -R 14 -s start + if ver hwtypecmp V6X009010 V6X010010 + then + icm20602 -R 6 -s start + else + # Internal SPI bus ICM-20649 (hard-mounted) + icm20649 -R 14 -s start + fi fi fi From 4901edb5a42f8015a5232dce3e0c42d0ef29bf66 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 21 Sep 2023 07:44:32 -0700 Subject: [PATCH 33/55] px4_fmu-v6:Add Sensor Set Rev 6 --- boards/px4/fmu-v6x/default.px4board | 3 + boards/px4/fmu-v6x/init/rc.board_sensors | 34 +++++---- boards/px4/fmu-v6x/src/board_config.h | 6 +- boards/px4/fmu-v6x/src/manifest.c | 5 +- boards/px4/fmu-v6x/src/spi.cpp | 96 +++++++++++++++--------- 5 files changed, 94 insertions(+), 50 deletions(-) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index f6198ca51fa7..a8d1f33b8336 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -20,12 +20,15 @@ CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_COMMON_INS=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 97d62d58c301..1ee095519d8b 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -48,24 +48,32 @@ then fi -if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 +if ver hwtypecmp V6X000006 V6X004006 V6X005006 then - # Internal SPI bus ICM20649 - icm20649 -s -R 6 start + # Internal SPI bus ICM45686 + icm45686 -s -R 10 start + iim42652 -s -R 6 start + adis16470 -s -R 0 start else - # Internal SPI BMI088 - if ver hwtypecmp V6X009010 V6X010010 + if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 then - bmi088 -A -R 6 -s start - bmi088 -G -R 6 -s start + # Internal SPI bus ICM20649 + icm20649 -s -R 6 start else - if ver hwtypecmp V6X000010 + # Internal SPI BMI088 + if ver hwtypecmp V6X009010 V6X010010 then - bmi088 -A -R 0 -s start - bmi088 -G -R 0 -s start + bmi088 -A -R 6 -s start + bmi088 -G -R 6 -s start else - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start + if ver hwtypecmp V6X000010 + then + bmi088 -A -R 0 -s start + bmi088 -G -R 0 -s start + else + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start + fi fi fi @@ -112,7 +120,7 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X002001 + if ver hwtypecmp V6X002001 V6X000006 V6X004006 V6X005006 then icp201xx -I -a 0x64 start else diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index fecd7efc9c96..ff0676fab7ee 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -215,24 +215,28 @@ #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "V6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 11 // Rev 0 and Rev 3,4 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 13 // Rev 0 and Rev 3,4,6 Sensor sets // Base/FMUM #define V6X00 HW_VER_REV(0x0,0x0) // FMUV6X, Rev 0 #define V6X01 HW_VER_REV(0x0,0x1) // FMUV6X, BMI388 I2C2 Rev 1 #define V6X03 HW_VER_REV(0x0,0x3) // FMUV6X, Sensor Set Rev 3 #define V6X04 HW_VER_REV(0x0,0x4) // FMUV6X, Sensor Set Rev 4 +#define V6X06 HW_VER_REV(0x0,0x6) // FMUV6X, Sensor Set Rev 6 #define V6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 #define V6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 #define V6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 +#define V6X16 HW_VER_REV(0x1,0x6) // NO PX4IO, Sensor Set Rev 6 #define V6X21 HW_VER_REV(0x2,0x1) // FMUV6X, CUAV Sensor Set #define V6X40 HW_VER_REV(0x4,0x0) // FMUV6X, HB CM4 base Rev 0 #define V6X41 HW_VER_REV(0x4,0x1) // FMUV6X, BMI388 I2C2 HB CM4 base Rev 1 #define V6X43 HW_VER_REV(0x4,0x3) // FMUV6X, Sensor Set HB CM4 base Rev 3 #define V6X44 HW_VER_REV(0x4,0x4) // FMUV6X, Sensor Set HB CM4 base Rev 4 +#define V6X46 HW_VER_REV(0x4,0x6) // FMUV6X, Sensor Set HB CM4 base Rev 6 #define V6X50 HW_VER_REV(0x5,0x0) // FMUV6X, HB Mini Rev 0 #define V6X51 HW_VER_REV(0x5,0x1) // FMUV6X, BMI388 I2C2 HB Mini Rev 1 #define V6X53 HW_VER_REV(0x5,0x3) // FMUV6X, Sensor Set HB Mini Rev 3 #define V6X54 HW_VER_REV(0x5,0x4) // FMUV6X, Sensor Set HB Mini Rev 4 +#define V6X56 HW_VER_REV(0x5,0x6) // FMUV6X, Sensor Set HB Mini Rev 6 #define V6X90 HW_VER_REV(0x9,0x0) // Rev 0 #define V6X0910 HW_VER_REV(0x9,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver9 #define V6X1010 HW_VER_REV(0x10,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver10 diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c index 313d49f9bf24..bda800b74919 100644 --- a/boards/px4/fmu-v6x/src/manifest.c +++ b/boards/px4/fmu-v6x/src/manifest.c @@ -142,17 +142,20 @@ static px4_hw_mft_list_entry_t mft_lists[] = { {V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, {V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 {V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 + {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 + {V6X06, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 {V6X40, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // HB CM4 base {V6X41, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 HB CM4 base {V6X43, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 {V6X44, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4 + {V6X46, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 {V6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // HB Mini {V6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini {V6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 {V6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 + {V6X56, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 6 {V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO {V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 - {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 {V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, {V6X0910, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X, rev from EEPROM Auterion Skynode ver9 diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index d3bbdb9315f8..8b002e157efa 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -84,6 +84,29 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), + initSPIHWVersion(V6X06, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X21, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -132,16 +155,15 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X50, { + initSPIHWVersion(V6X44, { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), }, {GPIO::PortF, GPIO::Pin4}), initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), }, {GPIO::PortE, GPIO::Pin7}), // initSPIBus(SPI::Bus::SPI4, { // // no devices @@ -156,15 +178,15 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X44, { + initSPIHWVersion(V6X46, { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), }, {GPIO::PortF, GPIO::Pin4}), initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), }, {GPIO::PortE, GPIO::Pin7}), // initSPIBus(SPI::Bus::SPI4, { // // no devices @@ -179,39 +201,16 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - // never shipped - //initSPIHWVersion(V6X50, { - // initSPIBus(SPI::Bus::SPI1, { - // initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - // }, {GPIO::PortI, GPIO::Pin11}), - // initSPIBus(SPI::Bus::SPI2, { - // initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - // }, {GPIO::PortF, GPIO::Pin4}), - // initSPIBus(SPI::Bus::SPI3, { - // initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - // initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - // }, {GPIO::PortE, GPIO::Pin7}), - // // initSPIBus(SPI::Bus::SPI4, { - // // // no devices - // // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // // }, {GPIO::PortG, GPIO::Pin8}), - // initSPIBus(SPI::Bus::SPI5, { - // initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - // }), - // initSPIBusExternal(SPI::Bus::SPI6, { - // initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - // initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - // }), - //}), - initSPIHWVersion(V6X04, { + initSPIHWVersion(V6X50, { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), }, {GPIO::PortF, GPIO::Pin4}), initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), }, {GPIO::PortE, GPIO::Pin7}), // initSPIBus(SPI::Bus::SPI4, { // // no devices @@ -225,6 +224,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + initSPIHWVersion(V6X53, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -248,6 +248,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + initSPIHWVersion(V6X54, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -270,6 +271,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + + initSPIHWVersion(V6X56, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X0910, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -293,6 +318,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + initSPIHWVersion(V6X1010, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), From 3691b132b2d13a5c09f008fa6fa81a4f89976ffd Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 21 Sep 2023 08:33:41 -0700 Subject: [PATCH 34/55] px4_fmuv6x:Fit Rev6 Sensors --- boards/px4/fmu-v6x/default.px4board | 1 - 1 file changed, 1 deletion(-) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index a8d1f33b8336..045ec9eb16d4 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -32,7 +32,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_COMMON_INS=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y -CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_COMMON_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y From b70381584c8067769209795c7ce7dba6991db1bd Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 11 Jan 2024 09:53:12 -0800 Subject: [PATCH 35/55] boards:Needing Migration to BOARD_HAS_HW_SPLIT_VERSIONING --- boards/ark/fmu-v6x/src/board_config.h | 2 +- boards/px4/fmu-v5x/src/board_config.h | 2 +- boards/sky-drones/smartap-airlink/src/board_config.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 4782fc7efdd8..9d940d59d955 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -204,7 +204,7 @@ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_VERSIONING // migrate to Split #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 281c769f2646..090497720a20 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -179,7 +179,7 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_VERSIONING // migrate to Split #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PF5 */ ADC3_GPIO(15) diff --git a/boards/sky-drones/smartap-airlink/src/board_config.h b/boards/sky-drones/smartap-airlink/src/board_config.h index ac21a1686cb2..5b957e0694cc 100644 --- a/boards/sky-drones/smartap-airlink/src/board_config.h +++ b/boards/sky-drones/smartap-airlink/src/board_config.h @@ -168,7 +168,7 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_VERSIONING // migrate to Split #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PF5 */ ADC3_GPIO(15) From f916aeddea05a5905b8ffc7e42a0f5ecc7dd2e5b Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 11 Jan 2024 09:57:06 -0800 Subject: [PATCH 36/55] PX4:comon Support BOARD_HAS_HW_SPLIT_VERSIONING --- .../px4_platform_common/board_common.h | 32 +++++++++++++++++++ .../common/include/px4_platform_common/spi.h | 6 +++- platforms/common/spi.cpp | 24 +++++++++++--- .../px4_platform/board_determine_hw_info.h | 4 +++ src/lib/version/version.h | 10 ++++++ 5 files changed, 71 insertions(+), 5 deletions(-) diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 7a6af777c495..90bc82b3f2fd 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -265,6 +265,18 @@ # define HW_VER_REV(v,r) ((uint32_t)((v) & 0xffff) << 16) | ((uint32_t)(r) & 0xffff) #endif +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +typedef uint16_t hw_fmun_id_t; +typedef uint16_t hw_base_id_t; +// Original Signals GPIO_HW_REV_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the FMUM +// Original Signals GPIO_HW_VER_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the BASE +# define BOARD_HAS_VERSIONING 1 +# define HW_FMUM_ID(rev) ((hw_fmun_id_t)(rev) & 0xffff) +# define HW_BASE_ID(ver) ((hw_base_id_t)(ver) & 0xffff) +# define GET_HW_FMUM_ID() (HW_FMUM_ID(board_get_hw_revision())) +# define GET_HW_BASE_ID() (HW_BASE_ID(board_get_hw_version())) +#endif + #define HW_INFO_REV_DIGITS 3 #define HW_INFO_VER_DIGITS 3 @@ -752,6 +764,26 @@ __EXPORT const char *board_get_hw_type_name(void); #define board_get_hw_type_name() "" #endif +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * Description: + * Optional returns a 0 terminated string defining the HW type. + * + * Input Parameters: + * None + * + * Returned Value: + * a 0 terminated string defining the HW type. This may be a 0 length string "" + * + ************************************************************************************/ + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +__EXPORT const char *board_get_hw_base_type_name(void); +#else +#define board_get_hw_base_type_name() "" +#endif + /************************************************************************************ * Name: board_get_hw_version * diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index 66644098228f..a2e4682b98cd 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -73,7 +73,11 @@ struct px4_spi_bus_t { struct px4_spi_bus_all_hw_t { px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS]; - int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + hw_fmun_id_t board_hw_fmun_id {USHRT_MAX}; +#else + int board_hw_version_revision {-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused +#endif }; #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp index 62713b0798fb..6a29a100b275 100644 --- a/platforms/common/spi.cpp +++ b/platforms/common/spi.cpp @@ -44,12 +44,26 @@ #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 void px4_set_spi_buses_from_hw_version() { -#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) +# if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + int hw_fmun_id = GET_HW_FMUM_ID(); + + for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { + if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_fmun_id == 0) { + px4_spi_buses = px4_spi_buses_all_hw[i].buses; + } + + if (px4_spi_buses_all_hw[i].board_hw_fmun_id == hw_fmun_id) { + px4_spi_buses = px4_spi_buses_all_hw[i].buses; + } + } + +# else + +# if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) int hw_version_revision = board_get_hw_version(); -#else +# else int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision()); -#endif - +# endif for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version_revision == 0) { @@ -61,6 +75,8 @@ void px4_set_spi_buses_from_hw_version() } } +# endif + if (!px4_spi_buses) { // fallback px4_spi_buses = px4_spi_buses_all_hw[0].buses; } diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h index 95e99ffd46d5..b0864d45c0ec 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h @@ -36,6 +36,10 @@ __BEGIN_DECLS #define HW_INFO_SUFFIX "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x%0" STRINGIFY(HW_INFO_REV_DIGITS) "x" +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +# define HW_INFO_FMUM_SUFFIX "%0" STRINGIFY(HW_INFO_REV_DIGITS) "x" +# define HW_INFO_BASE_SUFFIX "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x" +#endif /************************************************************************************ * Name: board_determine_hw_info * diff --git a/src/lib/version/version.h b/src/lib/version/version.h index ef2d9b5f0915..dc11e7cb02d9 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -88,6 +88,16 @@ static inline int px4_board_hw_revision(void) return board_get_hw_revision(); } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +/** + * get the base board type + */ +static inline const char *px4_board_base_type(void) +{ + return board_get_hw_base_type_name(); +} +#endif + /** * get the build URI (used for crash logging) */ From 4409f82efc58e9238d4e235a2cd2f1d83c8dabc0 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 11 Jan 2024 09:58:01 -0800 Subject: [PATCH 37/55] stm Support BOARD_HAS_HW_SPLIT_VERSIONING --- .../board_hw_info/board_hw_rev_ver.c | 32 +++++++++++++++++-- .../include/px4_arch/spi_hw_description.h | 16 ++++++++++ 2 files changed, 46 insertions(+), 2 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c index 54c8a836c916..6d484c0a826f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c @@ -51,7 +51,7 @@ #include #include -#if defined(BOARD_HAS_HW_VERSIONING) +#if defined(BOARD_HAS_HW_VERSIONING) || defined(BOARD_HAS_HW_SPLIT_VERSIONING) # if defined(GPIO_HW_VER_REV_DRIVE) # define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE @@ -67,7 +67,9 @@ static int hw_version = 0; static int hw_revision = 0; static char hw_info[HW_INFO_SIZE] = {0}; - +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static char hw_base_info[HW_INFO_SIZE] = {0}; +#endif /**************************************************************************** * Protected Functions ****************************************************************************/ @@ -366,6 +368,27 @@ __EXPORT const char *board_get_hw_type_name() return (const char *) hw_info; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * Description: + * Optional returns a 0 terminated string defining the base type. + * + * Input Parameters: + * None + * + * Returned Value: + * a 0 terminated string defining the HW type. This my be a 0 length string "" + * + ************************************************************************************/ + +__EXPORT const char *board_get_hw_base_type_name() +{ + return (const char *) hw_base_info; +} +#endif + /************************************************************************************ * Name: board_get_hw_version * @@ -467,7 +490,12 @@ int board_determine_hw_info() } if (rv == OK) { +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_FMUM_SUFFIX, GET_HW_FMUM_ID()); + snprintf(hw_base_info, sizeof(hw_info), HW_INFO_BASE_SUFFIX, GET_HW_BASE_ID()); +#else snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); +#endif } return rv; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h index 40cc40854b7c..3594d1d82011 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h @@ -136,6 +136,21 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI struct px4_spi_bus_array_t { px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS]; }; + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static inline constexpr px4_spi_bus_all_hw_t initSPIFmumID(hw_fmun_id_t hw_fmun_id, + const px4_spi_bus_array_t &bus_items) +{ + px4_spi_bus_all_hw_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + ret.buses[i] = bus_items.item[i]; + } + + ret.board_hw_fmun_id = hw_fmun_id; + return ret; +} +#else static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision, const px4_spi_bus_array_t &bus_items) { @@ -148,6 +163,7 @@ static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_rev ret.board_hw_version_revision = hw_version_revision; return ret; } +#endif constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]); constexpr bool validateSPIConfig(const px4_spi_bus_all_hw_t spi_buses_conf[BOARD_NUM_SPI_CFG_HW_VERSIONS]) From 433983369632cff57d169fc569c407997d16ac10 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 11 Jan 2024 09:58:58 -0800 Subject: [PATCH 38/55] px4_fmu-v6x:Use BOARD_HAS_HW_SPLIT_VERSIONING --- boards/px4/fmu-v6x/src/board_config.h | 36 ++---- boards/px4/fmu-v6x/src/manifest.c | 58 +++++---- boards/px4/fmu-v6x/src/spi.cpp | 175 +------------------------- 3 files changed, 48 insertions(+), 221 deletions(-) diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index ff0676fab7ee..6d79c1b1518e 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -208,40 +208,22 @@ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "V6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 13 // Rev 0 and Rev 3,4,6 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 6 // Base/FMUM -#define V6X00 HW_VER_REV(0x0,0x0) // FMUV6X, Rev 0 -#define V6X01 HW_VER_REV(0x0,0x1) // FMUV6X, BMI388 I2C2 Rev 1 -#define V6X03 HW_VER_REV(0x0,0x3) // FMUV6X, Sensor Set Rev 3 -#define V6X04 HW_VER_REV(0x0,0x4) // FMUV6X, Sensor Set Rev 4 -#define V6X06 HW_VER_REV(0x0,0x6) // FMUV6X, Sensor Set Rev 6 -#define V6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 -#define V6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 -#define V6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 -#define V6X16 HW_VER_REV(0x1,0x6) // NO PX4IO, Sensor Set Rev 6 -#define V6X21 HW_VER_REV(0x2,0x1) // FMUV6X, CUAV Sensor Set -#define V6X40 HW_VER_REV(0x4,0x0) // FMUV6X, HB CM4 base Rev 0 -#define V6X41 HW_VER_REV(0x4,0x1) // FMUV6X, BMI388 I2C2 HB CM4 base Rev 1 -#define V6X43 HW_VER_REV(0x4,0x3) // FMUV6X, Sensor Set HB CM4 base Rev 3 -#define V6X44 HW_VER_REV(0x4,0x4) // FMUV6X, Sensor Set HB CM4 base Rev 4 -#define V6X46 HW_VER_REV(0x4,0x6) // FMUV6X, Sensor Set HB CM4 base Rev 6 -#define V6X50 HW_VER_REV(0x5,0x0) // FMUV6X, HB Mini Rev 0 -#define V6X51 HW_VER_REV(0x5,0x1) // FMUV6X, BMI388 I2C2 HB Mini Rev 1 -#define V6X53 HW_VER_REV(0x5,0x3) // FMUV6X, Sensor Set HB Mini Rev 3 -#define V6X54 HW_VER_REV(0x5,0x4) // FMUV6X, Sensor Set HB Mini Rev 4 -#define V6X56 HW_VER_REV(0x5,0x6) // FMUV6X, Sensor Set HB Mini Rev 6 -#define V6X90 HW_VER_REV(0x9,0x0) // Rev 0 -#define V6X0910 HW_VER_REV(0x9,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver9 -#define V6X1010 HW_VER_REV(0x10,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver10 - - +#define V6X_0 HW_FMUM_ID(0x0) // FMUV6X, Auterion,HB Sensor Set Rev 0 +#define V6X_1 HW_FMUM_ID(0x1) // FMUV6X, CUAV Sensor Set Rev 1 +#define V6X_3 HW_FMUM_ID(0x3) // FMUV6X, HB Sensor Set Rev 3 +#define V6X_4 HW_FMUM_ID(0x4) // FMUV6X, HB Sensor Set Rev 4 +#define V6X_6 HW_FMUM_ID(0x6) // FMUV6X, HB Sensor Set Rev 6 +#define V6X_8 HW_FMUM_ID(0x8) // FMUV6X, HB Sensor Set Rev 8 +#define V6X_16 HW_FMUM_ID(0x10) // FMUV6X, Auterion Sensor Set Rev 16 from EEPROM #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c index bda800b74919..d62f6eaba10a 100644 --- a/boards/px4/fmu-v6x/src/manifest.c +++ b/boards/px4/fmu-v6x/src/manifest.c @@ -73,7 +73,9 @@ static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; // List of components on a specific board configuration // The index of those components is given by the enum (px4_hw_mft_item_id_t) // declared in board_common.h -static const px4_hw_mft_item_t hw_mft_list_v0600[] = { + +// BASE ID 0 +static const px4_hw_mft_item_t base_configuration_0[] = { { // PX4_MFT_PX4IO .present = 1, @@ -92,9 +94,16 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = { .mandatory = 1, .connection = px4_hw_con_onboard, }, + { + // PX4_MFT_CAN3 + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, }; -static const px4_hw_mft_item_t hw_mft_list_v0610[] = { +// BASE ID 1 +static const px4_hw_mft_item_t base_configuration_1[] = { { // PX4_MFT_PX4IO .present = 0, @@ -113,14 +122,21 @@ static const px4_hw_mft_item_t hw_mft_list_v0610[] = { .mandatory = 1, .connection = px4_hw_con_onboard, }, + { + // PX4_MFT_CAN3 + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, }; -static const px4_hw_mft_item_t hw_mft_list_v0650[] = { +// BASE ID 5 +static const px4_hw_mft_item_t base_configuration_5[] = { { // PX4_MFT_PX4IO .present = 1, .mandatory = 1, - .connection = px4_hw_con_unknown, + .connection = px4_hw_con_onboard, }, { // PX4_MFT_USB @@ -134,32 +150,24 @@ static const px4_hw_mft_item_t hw_mft_list_v0650[] = { .mandatory = 0, .connection = px4_hw_con_unknown, }, + { + // PX4_MFT_CAN3 + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, }; static px4_hw_mft_list_entry_t mft_lists[] = { // ver_rev - {V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, - {V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 - {V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 - {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 - {V6X06, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 - {V6X40, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // HB CM4 base - {V6X41, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 HB CM4 base - {V6X43, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 - {V6X44, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4 - {V6X46, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 - {V6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // HB Mini - {V6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini - {V6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 - {V6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 - {V6X56, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 6 - {V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO - {V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 - {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 - {V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, - {V6X0910, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X, rev from EEPROM Auterion Skynode ver9 - {V6X1010, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X, rev from EEPROM Auterion Skynode ver10 + {HW_BASE_ID(0), base_configuration_0, arraySize(base_configuration_0)}, // std Base with PX4IO + {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO + {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base + {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base + {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini + {HW_BASE_ID(9), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 9 + {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 }; /************************************************************************************ diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index 8b002e157efa..c0150720838f 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -36,7 +36,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(V6X00, { + initSPIFmumID(V6X_0, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -60,54 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X03, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(V6X06, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(V6X21, { + initSPIFmumID(V6X_1, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -131,7 +84,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X43, { + initSPIFmumID(V6X_3, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -155,7 +108,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X44, { + initSPIFmumID(V6X_4, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -178,7 +131,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X46, { + initSPIFmumID(V6X_6, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -201,101 +154,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X50, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(V6X53, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(V6X54, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(V6X56, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - initSPIHWVersion(V6X0910, { + initSPIFmumID(V6X_16, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -319,29 +179,6 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X1010, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); From 4b5a709e2712d4bcb5c1bbcff5c0ceb77820b45c Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 12 Jan 2024 10:01:09 -0800 Subject: [PATCH 39/55] PX4:Extend manifest types & add CLI query --- .../px4_platform_common/board_common.h | 45 ++++++++++++++++--- .../nuttx/src/px4/common/px4_manifest.cpp | 7 +++ 2 files changed, 45 insertions(+), 7 deletions(-) diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 90bc82b3f2fd..8e7e096cbd56 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -666,20 +666,51 @@ bool board_booted_by_px4(void); ************************************************************************************/ typedef enum { - PX4_MFT_PX4IO = 0, - PX4_MFT_USB = 1, - PX4_MFT_CAN2 = 2, - PX4_MFT_CAN3 = 3, + PX4_MFT_PX4IO = 0, + PX4_MFT_USB = 1, + PX4_MFT_CAN2 = 2, + PX4_MFT_CAN3 = 3, + PX4_MFT_PM2 = 4, + PX4_MFT_ETHERNET = 5, + PX4_MFT_T1_ETH = 6, + PX4_MFT_T100_ETH = 7, + PX4_MFT_T1000_ETH = 8, } px4_hw_mft_item_id_t; +typedef int (*system_query_func_t)(const char *sub, const char *val, void *out); + +#define PX4_MFT_MFT_TYPES { \ + PX4_MFT_PX4IO, \ + PX4_MFT_USB, \ + PX4_MFT_CAN2, \ + PX4_MFT_CAN3, \ + PX4_MFT_PM2, \ + PX4_MFT_ETHERNET, \ + PX4_MFT_T1_ETH, \ + PX4_MFT_T100_ETH, \ + PX4_MFT_T1000_ETH } + +#define PX4_MFT_MFT_STR_TYPES { \ + "MFT_PX4IO", \ + "MFT_USB", \ + "MFT_CAN2", \ + "MFT_CAN3", \ + "MFT_PM2", \ + "MFT_ETHERNET", \ + "MFT_T1_ETH", \ + "MFT_T100_ETH", \ + "MFT_T1000_ETH", \ + "MFT_T1000_ETH"} + typedef enum { - px4_hw_con_unknown = 0, - px4_hw_con_onboard = 1, + px4_hw_con_unknown = 0, + px4_hw_con_onboard = 1, px4_hw_con_connector = 3, } px4_hw_connection_t; typedef struct { + unsigned int id: 16; /* The id px4_hw_mft_item_id_t */ unsigned int present: 1; /* 1 if this board have this item */ unsigned int mandatory: 1; /* 1 if this item has to be present and working */ unsigned int connection: 2; /* See px4_hw_connection_t */ @@ -691,7 +722,7 @@ typedef const px4_hw_mft_item_t *px4_hw_mft_item; #if defined(BOARD_HAS_VERSIONING) __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id); - +__EXPORT int system_query_manifest(const char *sub, const char *val, void *out); # define PX4_MFT_HW_SUPPORTED(ID) (board_query_manifest((ID))->present) # define PX4_MFT_HW_REQUIRED(ID) (board_query_manifest((ID))->mandatory) # define PX4_MFT_HW_IS_ONBOARD(ID) (board_query_manifest((ID))->connection == px4_hw_con_onboard) diff --git a/platforms/nuttx/src/px4/common/px4_manifest.cpp b/platforms/nuttx/src/px4/common/px4_manifest.cpp index d7402215abdd..f3dc9a5e60bc 100644 --- a/platforms/nuttx/src/px4/common/px4_manifest.cpp +++ b/platforms/nuttx/src/px4/common/px4_manifest.cpp @@ -104,6 +104,13 @@ __EXPORT int px4_mft_query(const px4_mft_s *mft, px4_manifest_types_e type, break; case MFT: + if (mft->mfts[m]->pmft != nullptr) { + system_query_func_t query = (system_query_func_t) mft->mfts[m]->pmft; + return query(sub, val, nullptr); + } + + break; + default: rv = -ENODATA; break; From 4536f3b2e41fc8f6b20c2480f66fceee551cbd86 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 12 Jan 2024 10:29:13 -0800 Subject: [PATCH 40/55] PX4:common add PAB manifest PX4:common add PAB manifest with V5X bases --- platforms/common/CMakeLists.txt | 1 + platforms/common/pab_manifest.c | 404 ++++++++++++++++++++++++++++++++ 2 files changed, 405 insertions(+) create mode 100644 platforms/common/pab_manifest.c diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index a6e06b9d4735..d04f463c5a06 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(px4_platform STATIC px4_cli.cpp shutdown.cpp spi.cpp + pab_manifest.c ${SRCS} ) target_link_libraries(px4_platform prebuild_targets px4_work_queue) diff --git a/platforms/common/pab_manifest.c b/platforms/common/pab_manifest.c new file mode 100644 index 000000000000..d0570708a7ca --- /dev/null +++ b/platforms/common/pab_manifest.c @@ -0,0 +1,404 @@ +/**************************************************************************** + * + * 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 + * 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. + * + ****************************************************************************/ + +/** + * @file pab_manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW_BASE_ID + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + +typedef struct { + hw_base_id_t hw_base_id; /* The ID of the Base */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific base board configuration +// The ids of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h + +// BASE ID 0 Auterion vXx base board +static const px4_hw_mft_item_t base_configuration_0[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 1 vXx base without px4io +static const px4_hw_mft_item_t base_configuration_1[] = { + { + .id = PX4_MFT_PX4IO, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 2 Modal AI Alaised to ID 0 + +// BASE ID 3 NXP T1 PHY +static const px4_hw_mft_item_t base_configuration_3[] = { + { + .id = PX4_MFT_PX4IO, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T1_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 4 HB CM4 Alaised to ID 0 + +// BASE ID 5 HB min +static const px4_hw_mft_item_t base_configuration_5[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 6 Not allocated +// BASE ID 7 Read from EEPROM +// BASE ID 8 Skynode QS with USB - Alaised to ID 0 + +// BASE ID 9 Auterion Skynode base RC9 & older (no usb +static const px4_hw_mft_item_t base_configuration_9[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +// BASE ID 10 Skynode QS no USB Alaised to ID 9 +// BASE ID 16 Auterion Skynode RC10, RC11, RC12, RC13 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 + {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO + {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base + {HW_BASE_ID(3), base_configuration_3, arraySize(base_configuration_3)}, // NXP T1 PHY + {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base + {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini + {HW_BASE_ID(8), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 8 Aliased to 0 + {HW_BASE_ID(9), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 9 + {HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10 + {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 +}; + +/************************************************************************************ + * Name: base_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + hw_base_id_t hw_base_id = GET_HW_BASE_ID(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_base_id == hw_base_id) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %04" PRIx16 " is not supported!\n", hw_base_id); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized) + for (unsigned int ndx = 0; ndx < boards_manifest->entries; ndx++) { + if (boards_manifest->mft[ndx].id == id) { + rv = &boards_manifest->mft[id]; + break; + } + } + + return rv; +} + +__EXPORT int system_query_manifest(const char *sub, const char *val, void *out) +{ + static const char *keys[] = PX4_MFT_MFT_STR_TYPES; + static const px4_hw_mft_item_id_t item_ids[] = PX4_MFT_MFT_TYPES; + px4_hw_mft_item rv = &device_unsupported; + int id = -1; + int intval = atoi(val); + + for (unsigned int k = 0; k < arraySize(keys); k++) { + if (!strcmp(keys[k], sub)) { + id = item_ids[k]; + break; + } + } + + if (id != -1) { + // In case we have to filter when a FMUM is mounted to a BASE + // For now just use the board + rv = board_query_manifest(id); + return rv->present == intval ? OK : -ENXIO; + } + + return -ENOENT; +} +#endif From a2db688f4b9ca649a7f862a310f53c0d6b5ac8f1 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 12 Jan 2024 10:33:21 -0800 Subject: [PATCH 41/55] px4_fmu_v6x:Use common PAB manifest --- boards/px4/fmu-v6x/src/CMakeLists.txt | 1 - boards/px4/fmu-v6x/src/manifest.c | 216 -------------------------- boards/px4/fmu-v6x/src/mtd.cpp | 13 +- 3 files changed, 11 insertions(+), 219 deletions(-) delete mode 100644 boards/px4/fmu-v6x/src/manifest.c diff --git a/boards/px4/fmu-v6x/src/CMakeLists.txt b/boards/px4/fmu-v6x/src/CMakeLists.txt index d4a16ff4f303..a120ebe33623 100644 --- a/boards/px4/fmu-v6x/src/CMakeLists.txt +++ b/boards/px4/fmu-v6x/src/CMakeLists.txt @@ -55,7 +55,6 @@ else() init.c led.c mtd.cpp - manifest.c sdio.c spi.cpp timer_config.cpp diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c deleted file mode 100644 index d62f6eaba10a..000000000000 --- a/boards/px4/fmu-v6x/src/manifest.c +++ /dev/null @@ -1,216 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2021 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. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// declared in board_common.h - -// BASE ID 0 -static const px4_hw_mft_item_t base_configuration_0[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN3 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - -// BASE ID 1 -static const px4_hw_mft_item_t base_configuration_1[] = { - { - // PX4_MFT_PX4IO - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN3 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - -// BASE ID 5 -static const px4_hw_mft_item_t base_configuration_5[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_CAN3 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - - -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 - {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO - {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base - {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base - {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini - {HW_BASE_ID(9), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 9 - {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 -}; - -/************************************************************************************ - * Name: board_query_manifest - * - * Description: - * Optional returns manifest item. - * - * Input Parameters: - * manifest_id - the ID for the manifest item to retrieve - * - * Returned Value: - * 0 - item is not in manifest => assume legacy operations - * pointer to a manifest item - * - ************************************************************************************/ - -__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) -{ - static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - uint32_t ver_rev = board_get_hw_version() << 16; - ver_rev |= board_get_hw_revision(); - - for (unsigned i = 0; i < arraySize(mft_lists); i++) { - if (mft_lists[i].hw_ver_rev == ver_rev) { - boards_manifest = &mft_lists[i]; - break; - } - } - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev); - } - } - - px4_hw_mft_item rv = &device_unsupported; - - if (boards_manifest != px4_hw_mft_list_uninitialized && - id < boards_manifest->entries) { - rv = &boards_manifest->mft[id]; - } - - return rv; -} diff --git a/boards/px4/fmu-v6x/src/mtd.cpp b/boards/px4/fmu-v6x/src/mtd.cpp index fc1e97d2c8b1..d92a4d7aacdf 100644 --- a/boards/px4/fmu-v6x/src/mtd.cpp +++ b/boards/px4/fmu-v6x/src/mtd.cpp @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -120,10 +123,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; From 3e0290b084af4dd2892fa21a789a88002cf5c6f8 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 12 Jan 2024 10:36:04 -0800 Subject: [PATCH 42/55] ROMFS:netman update - dependent on PX4_MFT_ETHERNET not board type --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 19654d9adf54..90520e4e2112 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -162,7 +162,7 @@ else param select-backup $PARAM_BACKUP_FILE fi - if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X + if mft query -q -k MFT -s MFT_ETHERNET -v 1 then netman update -i eth0 fi From e5f4adaa2d2a3c5bb26fc1d30fff55d64cb6f4b2 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 15 Jan 2024 07:02:14 -0800 Subject: [PATCH 43/55] PX4:ver Add base type compare --- src/systemcmds/ver/ver.cpp | 48 +++++++++++++++++++++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.cpp b/src/systemcmds/ver/ver.cpp index ab75009d1ec5..a2b5c1885a5c 100644 --- a/src/systemcmds/ver/ver.cpp +++ b/src/systemcmds/ver/ver.cpp @@ -51,6 +51,9 @@ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_hwtypecmp_str[] = "hwtypecmp"; +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static const char sz_ver_hwbasecmp_str[] = "hwbasecmp"; +#endif static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_buri_str[] = "uri"; @@ -84,6 +87,11 @@ static void usage(const char *reason) PRINT_MODULE_USAGE_COMMAND_DESCR("hwtypecmp", "Compare hardware type (returns 0 on match)"); PRINT_MODULE_USAGE_ARG(" []", "Hardware type to compare against (eg. V2). An OR comparison is used if multiple are specified", false); +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + PRINT_MODULE_USAGE_COMMAND_DESCR("hwbasecmp", "Compare hardware base (returns 0 on match)"); + PRINT_MODULE_USAGE_ARG(" []", + "Hardware type to compare against (eg. V2). An OR comparison is used if multiple are specified", false); +#endif } extern "C" __EXPORT int ver_main(int argc, char *argv[]) @@ -129,12 +137,50 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[]) return 1; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + + if (!strncmp(argv[1], sz_ver_hwbasecmp_str, sizeof(sz_ver_hwbasecmp_str))) { + if (argc >= 3 && argv[2] != nullptr) { + const char *board_type = px4_board_base_type(); + + for (int i = 2; i < argc; ++i) { + if (strcmp(board_type, argv[i]) == 0) { + return 0; // if one of the arguments match, return success + } + } + + } else { + PX4_ERR("Not enough arguments, try 'ver hwbasecmp {000...999}[1:*]'"); + } + + return 1; + } + +#endif /* check if we want to show all */ bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)); if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { PX4_INFO_RAW("HW arch: %s\n", px4_board_name()); -#if defined(BOARD_HAS_VERSIONING) + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + char sbase[14] = "NA"; + char sfmum[14] = "NA"; + int base = GET_HW_BASE_ID(); + int fmu = GET_HW_FMUM_ID(); + + if (base >= 0) { + snprintf(sbase, sizeof(sbase), "0x%0" STRINGIFY(HW_INFO_VER_DIGITS) "X", base); + } + + if (fmu >= 0) { + snprintf(sfmum, sizeof(sfmum), "0x%0" STRINGIFY(HW_INFO_REV_DIGITS) "X", fmu); + } + + PX4_INFO_RAW("HW type: %s\n", strlen(HW_INFO_INIT_PREFIX) ? HW_INFO_INIT_PREFIX : "NA"); + PX4_INFO_RAW("HW FMUM ID: %s\n", sfmum); + PX4_INFO_RAW("HW BASE ID: %s\n", sbase); +#elif defined(BOARD_HAS_VERSIONING) char vb[14] = "NA"; char rb[14] = "NA"; int v = px4_board_hw_version(); From 0c1b5e88c55ff301dd66a7b9f7b3eaa14f2a590a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 15 Jan 2024 07:43:07 -0800 Subject: [PATCH 44/55] px4_fmu-v6x:HAVE_PM2 set by PX4_MFT_PM2 in manifest --- boards/px4/fmu-v6x/init/rc.board_sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 1ee095519d8b..57b33e50c16c 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ set HAVE_PM2 yes -if ver hwtypecmp V6X005000 V6X005001 V6X005003 V6X005004 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi From 23d8b3cf25454884d2c872e12456d674c6ccf796 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 15 Jan 2024 08:24:31 -0800 Subject: [PATCH 45/55] px4_fmu-v6x:rc.board_sensors Use BOARD_HAS_HW_SPLIT_VERSIONING --- boards/px4/fmu-v6x/init/rc.board_sensors | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 57b33e50c16c..475815f8e833 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -48,25 +48,25 @@ then fi -if ver hwtypecmp V6X000006 V6X004006 V6X005006 +if ver hwtypecmp V6X006 then # Internal SPI bus ICM45686 icm45686 -s -R 10 start iim42652 -s -R 6 start adis16470 -s -R 0 start else - if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 + if ver hwtypecmp V6X004 then # Internal SPI bus ICM20649 icm20649 -s -R 6 start else # Internal SPI BMI088 - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 then bmi088 -A -R 6 -s start bmi088 -G -R 6 -s start else - if ver hwtypecmp V6X000010 + if ver hwtypecmp V6X010 then bmi088 -A -R 0 -s start bmi088 -G -R 0 -s start @@ -78,11 +78,11 @@ else fi # Internal SPI bus ICM42688p - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 then icm42688p -R 12 -s start else - if ver hwtypecmp V6X000010 + if ver hwtypecmp V6X010 then icm42688p -R 14 -s start else @@ -90,12 +90,12 @@ else fi fi - if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 + if ver hwtypecmp V6X003 V6X004 then # Internal SPI bus ICM-42670-P (hard-mounted) icm42670p -R 10 -s start else - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 then icm20602 -R 6 -s start else @@ -106,7 +106,7 @@ else fi # Internal magnetometer on I2c -if ver hwtypecmp V6X002001 +if ver hwtypecmp V6X001 then rm3100 -I -b 4 start else @@ -120,7 +120,7 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X002001 V6X000006 V6X004006 V6X005006 + if ver hwtypecmp V6X001 V6X006 then icp201xx -I -a 0x64 start else @@ -129,7 +129,7 @@ then fi #external baro -if ver hwtypecmp V6X002001 +if ver hwtypecmp V6X001 then icp201xx -X start else From f69c6354dd768ec34ba9d75e2a72667d45ba4c2d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 17 Jan 2024 06:08:32 -0800 Subject: [PATCH 46/55] px4_fmu-v5x:Use BOARD_HAS_HW_SPLIT_VERSIONING & common PAB manifest --- boards/px4/fmu-v5x/init/rc.board_mavlink | 2 +- boards/px4/fmu-v5x/init/rc.board_sensors | 30 +-- boards/px4/fmu-v5x/src/CMakeLists.txt | 1 - boards/px4/fmu-v5x/src/board_config.h | 26 +-- boards/px4/fmu-v5x/src/manifest.c | 222 ----------------------- boards/px4/fmu-v5x/src/mtd.cpp | 13 +- boards/px4/fmu-v5x/src/spi.cpp | 99 +--------- 7 files changed, 25 insertions(+), 368 deletions(-) delete mode 100644 boards/px4/fmu-v5x/src/manifest.c diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink index dad84376ce73..8934b003c835 100644 --- a/boards/px4/fmu-v5x/init/rc.board_mavlink +++ b/boards/px4/fmu-v5x/init/rc.board_mavlink @@ -3,7 +3,7 @@ # board specific MAVLink startup script. #------------------------------------------------------------------------------ -if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 +if ver hwbasecmp 008 009 00a 010 then # Start MAVLink on the UART connected to the mission computer mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 4f7c1d078de5..2b8d344ca066 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -5,7 +5,7 @@ set HAVE_PM2 yes -if ver hwtypecmp V5X005000 V5X005001 V5X005002 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi @@ -49,33 +49,11 @@ then fi fi -if ver hwtypecmp V5X000000 V5X000001 V5X000002 V5X001000 V5X004000 V5X004001 V5X004002 V5X005001 V5X005002 +if ver hwbasecmp 008 009 00a 010 then - #FMUv5Xbase board orientation - - if ver hwtypecmp V5X000000 V5X000001 V5X004000 V5X004001 V5X005001 - then - # Internal SPI BMI088 - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start - else - # Internal SPI bus ICM20649 - icm20649 -s -R 6 start - fi - - # Internal SPI bus ICM42688p - icm42688p -R 6 -s start - - # Internal SPI bus ICM-20602 (hard-mounted) - icm20602 -R 10 -s start - - # Internal magnetometer on I2c - bmm150 -I start - -else #SKYNODE base fmu board orientation - if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 + if ver hwtypecmp V5X000 V5X001 then # Internal SPI BMI088 bmi088 -A -R 2 -s start @@ -105,7 +83,7 @@ ist8310 -X -b 1 -R 10 start if param compare SENS_INT_BARO_EN 1 then bmp388 -I -a 0x77 start - if ver hwtypecmp V5X000000 V5X001000 V5X008000 V5X009000 V5X00a000 + if ver hwtypecmp V5X000 then bmp388 -I start else diff --git a/boards/px4/fmu-v5x/src/CMakeLists.txt b/boards/px4/fmu-v5x/src/CMakeLists.txt index 684b11736eda..311b7c9be8c9 100644 --- a/boards/px4/fmu-v5x/src/CMakeLists.txt +++ b/boards/px4/fmu-v5x/src/CMakeLists.txt @@ -36,7 +36,6 @@ add_library(drivers_board i2c.cpp init.cpp led.c - manifest.c mtd.cpp sdio.c spi.cpp diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 090497720a20..38566d403a90 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -179,31 +179,17 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING // migrate to Split +#define BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PF5 */ ADC3_GPIO(15) #define GPIO_HW_VER_SENSE /* PF4 */ ADC3_GPIO(14) #define HW_INFO_INIT_PREFIX "V5X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 7 -// Base FMUM -#define V5X00 HW_VER_REV(0x0,0x0) // FMUV5X, Rev 0 -#define V5X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 -#define V5X01 HW_VER_REV(0x0,0x1) // FMUV5X I2C2 BMP388, Rev 1 -#define V5X02 HW_VER_REV(0x0,0x2) // FMUV5X, Rev 2 -#define V5X40 HW_VER_REV(0x4,0x0) // FMUV5X, HB CM4 base Rev 0 -#define V5X41 HW_VER_REV(0x4,0x1) // FMUV5X I2C2 BMP388, HB CM4 base Rev 1 -#define V5X42 HW_VER_REV(0x4,0x2) // FMUV5X, HB CM4 base Rev 2 -#define V5X50 HW_VER_REV(0x5,0x0) // FMUV5X, HB Mini Rev 0 -#define V5X51 HW_VER_REV(0x5,0x1) // FMUV5X I2C2 BMP388, HB Mini Rev 1 -#define V5X52 HW_VER_REV(0x5,0x2) // FMUV5X, HB Mini Rev 2 -#define V5X90 HW_VER_REV(0x9,0x0) // NO USB, Rev 0 -#define V5X91 HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388, Rev 1 -#define V5X92 HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388, Rev 2 -#define V5Xa0 HW_VER_REV(0xa,0x0) // NO USB (Q), Rev 0 -#define V5Xa1 HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388, Rev 1 -#define V5Xa2 HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388, Rev 2 -#define V5X101 HW_VER_REV(0x10,0x1) // NO USB (Q) I2C2 BMP388, Rev 1 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 + +#define V5X_0 HW_FMUM_ID(0x0) // FMUV5X, Auterion FMUv5x RC13 (baro2 BMP388 on I2C4) Sensor Set Rev 0 +#define V5X_1 HW_FMUM_ID(0x1) // FMUV5X, Auterion, HB FMUv5x RC15 (baro2 BMP388 on I2C2) Sensor Set Rev 1 +#define V5X_2 HW_FMUM_ID(0x2) // FMUV5X, HB FMUv5x Sensor Set Rev 2 #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/px4/fmu-v5x/src/manifest.c b/boards/px4/fmu-v5x/src/manifest.c deleted file mode 100644 index 56f0009f5000..000000000000 --- a/boards/px4/fmu-v5x/src/manifest.c +++ /dev/null @@ -1,222 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2021 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. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// declared in board_common.h -static const px4_hw_mft_item_t hw_mft_list_v0500[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0550[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0510[] = { - { - // PX4_MFT_PX4IO - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0509[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static px4_hw_mft_list_entry_t mft_lists[] = { -// ver_rev - {V5X00, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 0 - {V5X01, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 1 - {V5X02, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 2 - {V5X10, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)}, // NO PX4IO, Rev 0 - {V5X41, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, HB CM4 base Rev 1 - {V5X42, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, HB CM4 base Rev 2 - {V5X51, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 1 - {V5X52, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 2 - {V5X90, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB, Rev 0 - {V5X91, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 1 - {V5X92, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 2 - {V5Xa0, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q), Rev 0 - {V5Xa1, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 1 - {V5Xa2, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 2 - {V5X101, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 1 -}; - -/************************************************************************************ - * Name: board_query_manifest - * - * Description: - * Optional returns manifest item. - * - * Input Parameters: - * manifest_id - the ID for the manifest item to retrieve - * - * Returned Value: - * 0 - item is not in manifest => assume legacy operations - * pointer to a manifest item - * - ************************************************************************************/ - -__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) -{ - static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - uint32_t ver_rev = board_get_hw_version() << 16; - ver_rev |= board_get_hw_revision(); - - for (unsigned i = 0; i < arraySize(mft_lists); i++) { - if (mft_lists[i].hw_ver_rev == ver_rev) { - boards_manifest = &mft_lists[i]; - break; - } - } - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev); - } - } - - px4_hw_mft_item rv = &device_unsupported; - - if (boards_manifest != px4_hw_mft_list_uninitialized && - id < boards_manifest->entries) { - rv = &boards_manifest->mft[id]; - } - - return rv; -} diff --git a/boards/px4/fmu-v5x/src/mtd.cpp b/boards/px4/fmu-v5x/src/mtd.cpp index fc1e97d2c8b1..d92a4d7aacdf 100644 --- a/boards/px4/fmu-v5x/src/mtd.cpp +++ b/boards/px4/fmu-v5x/src/mtd.cpp @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -120,10 +123,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp index 8fa378ee5d4d..dd6bfbb25e1e 100644 --- a/boards/px4/fmu-v5x/src/spi.cpp +++ b/boards/px4/fmu-v5x/src/spi.cpp @@ -36,7 +36,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(V5X00, { + initSPIFmumID(V5X_0, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -60,7 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V5X01, { + initSPIFmumID(V5X_1, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -84,7 +84,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V5X02, { + initSPIFmumID(V5X_2, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -107,99 +107,6 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V5X41, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(V5X42, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(V5X51, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(V5X52, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); From 247067d3928da9494c37a53716a71c0ee9f19cf0 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Sat, 27 Jan 2024 11:05:39 -0700 Subject: [PATCH 47/55] boards: arkv6x migrate to split versioning --- boards/ark/fmu-v6x/init/rc.board_defaults | 4 +- boards/ark/fmu-v6x/init/rc.board_sensors | 6 +- boards/ark/fmu-v6x/src/CMakeLists.txt | 1 - boards/ark/fmu-v6x/src/board_config.h | 18 +- boards/ark/fmu-v6x/src/manifest.c | 216 ---------------------- boards/ark/fmu-v6x/src/mtd.cpp | 15 +- boards/ark/fmu-v6x/src/spi.cpp | 144 +-------------- 7 files changed, 25 insertions(+), 379 deletions(-) delete mode 100644 boards/ark/fmu-v6x/src/manifest.c diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 87451b0c52ea..52d098101df1 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -22,12 +22,12 @@ param set-default SENS_IMU_TEMP 10.0 #param set-default SENS_IMU_TEMP_I 0.025 #param set-default SENS_IMU_TEMP_P 1.0 -if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +if ver hwtypecmp ARKV6X000 then param set-default SENS_TEMP_ID 2818058 fi -if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +if ver hwtypecmp ARKV6X001 then param set-default SENS_TEMP_ID 3014666 fi diff --git a/boards/ark/fmu-v6x/init/rc.board_sensors b/boards/ark/fmu-v6x/init/rc.board_sensors index a2b993f928d1..e120ba6017d2 100644 --- a/boards/ark/fmu-v6x/init/rc.board_sensors +++ b/boards/ark/fmu-v6x/init/rc.board_sensors @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ set HAVE_PM2 yes -if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X005004 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi @@ -47,7 +47,7 @@ then fi fi -if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +if ver hwtypecmp ARKV6X000 then # Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz iim42652 -R 3 -s -b 1 -C 32051 start @@ -59,7 +59,7 @@ then icm42688p -R 6 -s -b 3 -C 32051 start fi -if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +if ver hwtypecmp ARKV6X001 then # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz iim42653 -R 3 -s -b 1 -C 32051 start diff --git a/boards/ark/fmu-v6x/src/CMakeLists.txt b/boards/ark/fmu-v6x/src/CMakeLists.txt index 3135751c666e..78b8222f19d8 100644 --- a/boards/ark/fmu-v6x/src/CMakeLists.txt +++ b/boards/ark/fmu-v6x/src/CMakeLists.txt @@ -55,7 +55,6 @@ else() init.c led.c mtd.cpp - manifest.c sdio.c spi.cpp spix_sync.c diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 9d940d59d955..04b84631c760 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -204,25 +204,17 @@ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING // migrate to Split +#define BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "ARKV6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Base/FMUM -#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Sensor Set Rev 0 -#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, Sensor Set Rev 1 -//#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3 -//#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4 -#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Sensor Set Rev 0 -#define ARKV6X11 HW_VER_REV(0x1,0x1) // NO PX4IO, Sensor Set Rev 1 -#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3 -#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4 -#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5 -#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped +#define ARKV6X_0 HW_FMUM_ID(0x0) // ARKV6X, Sensor Set Rev 0 +#define ARKV6X_1 HW_FMUM_ID(0x1) // ARKV6X, Sensor Set Rev 1 #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/ark/fmu-v6x/src/manifest.c b/boards/ark/fmu-v6x/src/manifest.c deleted file mode 100644 index f4e012bfb068..000000000000 --- a/boards/ark/fmu-v6x/src/manifest.c +++ /dev/null @@ -1,216 +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. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// declared in board_common.h -static const px4_hw_mft_item_t hw_mft_list_v0600[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0610[] = { - { - // PX4_MFT_PX4IO - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0640[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_v0650[] = { - { - // PX4_MFT_PX4IO - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_unknown, - }, - { - // PX4_MFT_USB - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, - { - // PX4_MFT_CAN2 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - - -static px4_hw_mft_list_entry_t mft_lists[] = { -// ver_rev - {ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 0 - {ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 1 - {ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 0 - {ARKV6X11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 1 - {ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3 - {ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4 - {ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5 - {ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped -}; - -/************************************************************************************ - * Name: board_query_manifest - * - * Description: - * Optional returns manifest item. - * - * Input Parameters: - * manifest_id - the ID for the manifest item to retrieve - * - * Returned Value: - * 0 - item is not in manifest => assume legacy operations - * pointer to a manifest item - * - ************************************************************************************/ - -__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) -{ - static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - uint32_t ver_rev = board_get_hw_version() << 16; - ver_rev |= board_get_hw_revision(); - - for (unsigned i = 0; i < arraySize(mft_lists); i++) { - if (mft_lists[i].hw_ver_rev == ver_rev) { - boards_manifest = &mft_lists[i]; - break; - } - } - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev); - } - } - - px4_hw_mft_item rv = &device_unsupported; - - if (boards_manifest != px4_hw_mft_list_uninitialized && - id < boards_manifest->entries) { - rv = &boards_manifest->mft[id]; - } - - return rv; -} diff --git a/boards/ark/fmu-v6x/src/mtd.cpp b/boards/ark/fmu-v6x/src/mtd.cpp index 3ece10aeca4f..6e89d3d1f89a 100644 --- a/boards/ark/fmu-v6x/src/mtd.cpp +++ b/boards/ark/fmu-v6x/src/mtd.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 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 @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -92,10 +95,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/ark/fmu-v6x/src/spi.cpp b/boards/ark/fmu-v6x/src/spi.cpp index f83a5a91cd4b..fc60153efedf 100644 --- a/boards/ark/fmu-v6x/src/spi.cpp +++ b/boards/ark/fmu-v6x/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 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 @@ -36,7 +36,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(ARKV6X00, { + initSPIFmumID(ARKV6X_0, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -59,145 +59,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(ARKV6X01, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(ARKV6X10, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(ARKV6X11, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(ARKV6X40, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(ARKV6X41, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(ARKV6X50, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - initSPIHWVersion(ARKV6X51, { + initSPIFmumID(ARKV6X_1, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), From f6ec71d39f3bc1b0bd44a85e30452ec32d09e4c0 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 31 Jan 2024 06:01:05 -0800 Subject: [PATCH 48/55] px4_fmu-v6x:Add Sensor set 8 --- boards/px4/fmu-v6x/init/rc.board_sensors | 21 +++++++++++++++------ boards/px4/fmu-v6x/src/board_config.h | 2 +- boards/px4/fmu-v6x/src/spi.cpp | 23 +++++++++++++++++++++++ 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 475815f8e833..86242bf08700 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -48,12 +48,21 @@ then fi -if ver hwtypecmp V6X006 +# Keep nesting shallow +if ver hwtypecmp V6X006 V6X008 then - # Internal SPI bus ICM45686 - icm45686 -s -R 10 start - iim42652 -s -R 6 start - adis16470 -s -R 0 start + if ver hwtypecmp V6X006 + then + # Internal SPI bus ICM45686 + icm45686 -s -R 10 start + iim42652 -s -R 6 start + adis16470 -s -R 0 start + else + # Internal SPI bus 3x ICM45686 + icm45686 -b 3 -s -R 0 start + icm45686 -b 2 -s -R 0 start + icm45686 -b 1 -s -R 10 start + fi else if ver hwtypecmp V6X004 then @@ -120,7 +129,7 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X001 V6X006 + if ver hwtypecmp V6X001 V6X006 V6X008 then icp201xx -I -a 0x64 start else diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index 6d79c1b1518e..7cad5497ed2b 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -215,7 +215,7 @@ #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "V6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 6 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 7 // Base/FMUM #define V6X_0 HW_FMUM_ID(0x0) // FMUV6X, Auterion,HB Sensor Set Rev 0 #define V6X_1 HW_FMUM_ID(0x1) // FMUV6X, CUAV Sensor Set Rev 1 diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index c0150720838f..8c633dd78f14 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -154,6 +154,29 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), + initSPIFmumID(V6X_8, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIFmumID(V6X_16, { initSPIBus(SPI::Bus::SPI1, { From f75a92d19977061eba411bd5a134a59f9fb0407e Mon Sep 17 00:00:00 2001 From: Vincent Poon Date: Fri, 2 Feb 2024 15:54:38 +0800 Subject: [PATCH 49/55] Change FMU-v6x REV 6 IMU Order Change IMU Order, make adis16470 in 1st priority. --- boards/px4/fmu-v6x/init/rc.board_sensors | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 86242bf08700..06554a688de8 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -54,9 +54,9 @@ then if ver hwtypecmp V6X006 then # Internal SPI bus ICM45686 - icm45686 -s -R 10 start - iim42652 -s -R 6 start adis16470 -s -R 0 start + iim42652 -s -R 6 start + icm45686 -s -R 10 start else # Internal SPI bus 3x ICM45686 icm45686 -b 3 -s -R 0 start From 011af5f7971eab9837fd209a05a63af509aeb492 Mon Sep 17 00:00:00 2001 From: jamming Date: Wed, 27 Mar 2024 07:49:11 +0800 Subject: [PATCH 50/55] boards/holybro/kakuteh7: fix icm42688p IMU - the mass-produced kakuteH7 did not use ICM20689 IMU --- boards/holybro/kakuteh7/src/spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/holybro/kakuteh7/src/spi.cpp b/boards/holybro/kakuteh7/src/spi.cpp index 1ff5b7f35c1c..e0f4db0edd6e 100644 --- a/boards/holybro/kakuteh7/src/spi.cpp +++ b/boards/holybro/kakuteh7/src/spi.cpp @@ -43,7 +43,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), }), initSPIBus(SPI::Bus::SPI4, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), }), From 1dacb4cdef2d7145754fc788fa8dc482eed74b40 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 3 Apr 2024 06:48:09 -0700 Subject: [PATCH 51/55] [BACKPORT] px4_fmu-v6x:Add Holybro Pixhawk Jetson Baseboard ver 0x100 --- platforms/common/pab_manifest.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/platforms/common/pab_manifest.c b/platforms/common/pab_manifest.c index d0570708a7ca..ba4835a5b13a 100644 --- a/platforms/common/pab_manifest.c +++ b/platforms/common/pab_manifest.c @@ -315,19 +315,19 @@ static const px4_hw_mft_item_t base_configuration_9[] = { // BASE ID 10 Skynode QS no USB Alaised to ID 9 // BASE ID 16 Auterion Skynode RC10, RC11, RC12, RC13 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 - {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO - {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base - {HW_BASE_ID(3), base_configuration_3, arraySize(base_configuration_3)}, // NXP T1 PHY - {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base - {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini - {HW_BASE_ID(8), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 8 Aliased to 0 - {HW_BASE_ID(9), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 9 - {HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10 - {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 + {HW_BASE_ID(0), base_configuration_0, arraySize(base_configuration_0)}, // std Base with PX4IO + {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO + {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base + {HW_BASE_ID(3), base_configuration_3, arraySize(base_configuration_3)}, // NXP T1 PHY + {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base + {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini + {HW_BASE_ID(8), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 8 Aliased to 0 + {HW_BASE_ID(9), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 9 + {HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10 + {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 + {HW_BASE_ID(0x100), base_configuration_0, arraySize(base_configuration_0)}, // Holybro Pixhawk Jetson Baseboard ver 0x100 Alaised to ID 0 }; /************************************************************************************ From 08e14a80728eb2cca885912987861831eda9345a Mon Sep 17 00:00:00 2001 From: Alexis Guijarro Date: Tue, 16 Jan 2024 14:00:19 -0600 Subject: [PATCH 52/55] github actions compile nuttx add mro_ctrl-zero-classic --- .github/workflows/compile_nuttx.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 67b055fdd97b..36e77d9f2d0d 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -45,6 +45,7 @@ jobs: matek_h743-slim, modalai_fc-v1, modalai_fc-v2, + mro_ctrl-zero-classic, mro_ctrl-zero-f7, mro_ctrl-zero-f7-oem, mro_ctrl-zero-h7, From 8cb7d1c0502bd86d8c1f62803c2c44f652259983 Mon Sep 17 00:00:00 2001 From: Alexis Guijarro Date: Sat, 9 Mar 2024 02:50:53 +0700 Subject: [PATCH 53/55] boards/mro/ctrl-zero-classic: corrections for mRo Control Zero Classic Board (#22745) - Build target changed from STM32H743II to STM32H743ZI - Missing external SPI interface added - Nonexistent I2C3 interface removed - I2C4 pins changed - Red and Green LED lights remapped - Missing ADC inputs added and already present ones corrected - CAN Silent interfaces corrected - Power pins corrected and Level Shifter pin added to enable ICM20948 - Buzzer pin remapped - HRT channel and PPM pin changed - RSSI input remapped - ICM20602 and BMI088 pins corrected - Serial ports remapped --- boards/mro/ctrl-zero-classic/default.px4board | 12 ++-- .../ctrl-zero-classic/init/rc.board_defaults | 2 +- .../ctrl-zero-classic/init/rc.board_sensors | 2 + .../nuttx-config/bootloader/defconfig | 2 +- .../nuttx-config/include/board.h | 17 ++--- .../nuttx-config/nsh/defconfig | 15 ++--- .../mro/ctrl-zero-classic/src/board_config.h | 67 ++++++++++++------- boards/mro/ctrl-zero-classic/src/i2c.cpp | 1 - boards/mro/ctrl-zero-classic/src/spi.cpp | 7 +- .../ctrl-zero-classic/src/timer_config.cpp | 22 ++++++ 10 files changed, 97 insertions(+), 50 deletions(-) diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index f88c94880fda..c16095b6a096 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -1,9 +1,11 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m7" -CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" -CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" -CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" -CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0" CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y @@ -25,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y @@ -96,4 +99,3 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y -CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/ctrl-zero-classic/init/rc.board_defaults b/boards/mro/ctrl-zero-classic/init/rc.board_defaults index 32117f4a5886..110c240ee8a0 100644 --- a/boards/mro/ctrl-zero-classic/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-classic/init/rc.board_defaults @@ -6,4 +6,4 @@ param set-default BAT1_V_DIV 10.1 param set-default BAT1_A_PER_V 17 -safety_button start +param set-default TEL_FRSKY_CONFIG 103 diff --git a/boards/mro/ctrl-zero-classic/init/rc.board_sensors b/boards/mro/ctrl-zero-classic/init/rc.board_sensors index 111e3eb0440b..46f488794516 100644 --- a/boards/mro/ctrl-zero-classic/init/rc.board_sensors +++ b/boards/mro/ctrl-zero-classic/init/rc.board_sensors @@ -16,3 +16,5 @@ icm20948 -s -b 1 -R 8 -M start # Interal DPS310 (barometer) dps310 -s -b 2 start + +safety_button start diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig index c8a5905f548c..241e130b4cf2 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig @@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" -CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H743ZI=y CONFIG_ARCH_CHIP_STM32H7=y CONFIG_ARCH_INTERRUPTSTACK=768 CONFIG_ARMV7M_BASEPRI_WAR=y diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h index 9b1ae6342d33..0df441c6a778 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h @@ -222,6 +222,9 @@ /* UART/USART */ +#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */ +#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */ + #define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ #define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ #define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ @@ -235,9 +238,6 @@ #define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ #define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ -#define GPIO_USART6_TX 0 /* USART6 is RX-only */ -#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ - #define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ #define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ @@ -268,13 +268,14 @@ #define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ #define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ +#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13 */ +#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */ +#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */ + /* I2C */ #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ -#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ -#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ - -#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */ -#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */ +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */ diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig index e5169e78ede7..df9639f35251 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig @@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" -CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H743ZI=y CONFIG_ARCH_CHIP_STM32H7=y CONFIG_ARCH_INTERRUPTSTACK=768 CONFIG_ARCH_STACKDUMP=y @@ -190,7 +190,6 @@ CONFIG_STM32H7_DMA2=y CONFIG_STM32H7_DMACAPABLE=y CONFIG_STM32H7_FLOWCONTROL_BROKEN=y CONFIG_STM32H7_I2C1=y -CONFIG_STM32H7_I2C3=y CONFIG_STM32H7_I2C4=y CONFIG_STM32H7_I2C_DYNTIMEO=y CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 @@ -210,17 +209,20 @@ CONFIG_STM32H7_SPI2=y CONFIG_STM32H7_SPI5=y CONFIG_STM32H7_SPI5_DMA=y CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI6=y +CONFIG_STM32H7_TIM15=y +CONFIG_STM32H7_TIM16=y CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM2=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y CONFIG_STM32H7_UART4=y CONFIG_STM32H7_UART7=y CONFIG_STM32H7_UART8=y -CONFIG_STM32H7_USART2=y -CONFIG_STM32H7_USART3=y -CONFIG_STM32H7_USART6=y CONFIG_STM32H7_USART_BREAKS=y CONFIG_STM32H7_USART_INVERT=y CONFIG_STM32H7_USART_SINGLEWIRE=y @@ -250,9 +252,6 @@ CONFIG_USART3_IFLOWCONTROL=y CONFIG_USART3_OFLOWCONTROL=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=3000 -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=600 -CONFIG_USART6_TXBUFSIZE=1500 CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/mro/ctrl-zero-classic/src/board_config.h b/boards/mro/ctrl-zero-classic/src/board_config.h index dfb0dfcb1a78..d40eb1e56ffc 100644 --- a/boards/mro/ctrl-zero-classic/src/board_config.h +++ b/boards/mro/ctrl-zero-classic/src/board_config.h @@ -45,95 +45,111 @@ #include /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ -#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) -#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) +#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) #define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) +#define GPIO_LED_SAFETY /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) + #define BOARD_HAS_CONTROL_STATUS_LEDS 1 #define BOARD_OVERLOAD_LED LED_RED #define BOARD_ARMED_STATE_LED LED_BLUE /* ADC channels */ #define PX4_ADC_GPIO \ - /* PA2 */ GPIO_ADC12_INP14, \ + /* PC4 */ GPIO_ADC12_INP4, \ /* PA3 */ GPIO_ADC12_INP15, \ /* PA4 */ GPIO_ADC12_INP18, \ - /* PC1 */ GPIO_ADC123_INP11 + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC5 */ GPIO_ADC12_INP8, \ + /* PB0 */ GPIO_ADC12_INP9, \ + /* PB1 */ GPIO_ADC12_INP5 /* Define Channel numbers must match above GPIO pins */ -#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 4 /* PC4 BATT_VOLT_SENS */ #define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ #define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ -#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ +#define ADC_RC_RSSI_CHANNEL 10 /* PC0 */ +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 8 /* PC5 */ +#define ADC_AUX1_VOLTAGE_CHANNEL 9 /* PB0 */ +#define ADC_AUX2_VOLTAGE_CHANNEL 5 /* PB1 */ #define ADC_CHANNELS \ ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ (1 << ADC_SCALED_V5_CHANNEL) | \ - (1 << ADC_RC_RSSI_CHANNEL)) + (1 << ADC_RC_RSSI_CHANNEL) | \ + (1 << ADC_AIRSPEED_VOLTAGE_CHANNEL) | \ + (1 << ADC_AUX1_VOLTAGE_CHANNEL) | \ + (1 << ADC_AUX2_VOLTAGE_CHANNEL)) /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* CAN Silence: Silent mode control */ -#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) -#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) +#define GPIO_CAN1_SILENT_S0 /* PF11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11) +#define GPIO_CAN2_SILENT_S0 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12) /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 12 /* Power supply control and monitoring GPIOs */ -#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_nPOWER_IN_A /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) #define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define BOARD_NUMBER_BRICKS 1 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + /* Define True logic Power Control in arch agnostic form */ #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) #define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) /* Tone alarm output */ -#define TONE_ALARM_TIMER 2 /* timer 2 */ -#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ +#define TONE_ALARM_TIMER 16 /* timer 16 */ +#define TONE_ALARM_CHANNEL 1 /* PF6 TIM16_CH1 */ -#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_BUZZER_1 /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6) #define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 -#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 +#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2 /* USB OTG FS */ #define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ #define HRT_TIMER 3 /* use timer3 for the HRT */ -#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */ -#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ -#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 +#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3 /* RC Serial port */ -#define RC_SERIAL_PORT "/dev/ttyS3" +#define RC_SERIAL_PORT "/dev/ttyS5" -#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_RSSI_IN /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) /* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ -#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) -/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_SAFETY_SWITCH_IN /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */ #define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ +/* No PX4IO processor present */ +#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0 + /* Power switch controls ******************************************************/ #define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) /* * Board has a separate RC_IN * - * GPIO PPM_IN on PB0 T3CH3 + * GPIO PPM_IN on PC7 T3CH2 * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output */ -#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) #define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) #define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ #define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) @@ -156,7 +172,7 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_NUM_IO_TIMERS 5 +#define BOARD_NUM_IO_TIMERS 6 #define BOARD_ENABLE_CONSOLE_BUFFER @@ -171,9 +187,12 @@ GPIO_CAN2_SILENT_S0, \ GPIO_nPOWER_IN_A, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_LEVEL_SHIFTER_OE, \ GPIO_TONE_ALARM_IDLE, \ GPIO_SAFETY_SWITCH_IN, \ GPIO_OTGFS_VBUS, \ + GPIO_BTN_SAFETY, \ + GPIO_LED_SAFETY, \ } __BEGIN_DECLS diff --git a/boards/mro/ctrl-zero-classic/src/i2c.cpp b/boards/mro/ctrl-zero-classic/src/i2c.cpp index 1b8927c69939..49c9ea1c7efd 100644 --- a/boards/mro/ctrl-zero-classic/src/i2c.cpp +++ b/boards/mro/ctrl-zero-classic/src/i2c.cpp @@ -35,6 +35,5 @@ constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusExternal(1), - initI2CBusExternal(3), initI2CBusExternal(4), }; diff --git a/boards/mro/ctrl-zero-classic/src/spi.cpp b/boards/mro/ctrl-zero-classic/src/spi.cpp index 4a4c3502bbd9..674ae3f09b2f 100644 --- a/boards/mro/ctrl-zero-classic/src/spi.cpp +++ b/boards/mro/ctrl-zero-classic/src/spi.cpp @@ -37,7 +37,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}), initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), }, {GPIO::PortE, GPIO::Pin3}), initSPIBus(SPI::Bus::SPI2, { @@ -46,7 +46,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { }), initSPIBus(SPI::Bus::SPI5, { initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}), }), }; diff --git a/boards/mro/ctrl-zero-classic/src/timer_config.cpp b/boards/mro/ctrl-zero-classic/src/timer_config.cpp index 23bb7a1decc7..50760e387bba 100644 --- a/boards/mro/ctrl-zero-classic/src/timer_config.cpp +++ b/boards/mro/ctrl-zero-classic/src/timer_config.cpp @@ -33,6 +33,28 @@ #include +/* Timer allocation + * + * TIM1_CH4 T FMU_CH1 + * TIM1_CH3 T FMU_CH2 + * TIM1_CH2 T FMU_CH3 + * TIM1_CH1 T FMU_CH4 + * + * TIM4_CH2 T FMU_CH5 + * TIM4_CH3 T FMU_CH6 + * TIM2_CH3 T FMU_CH7 + * TIM2_CH1 T FMU_CH8 + * + * TIM2_CH4 T FMU_CH9 + * TIM15_CH1 T FMU_CH10 + * + * TIM8_CH1 T FMU_CH11 + * + * TIM4_CH4 T FMU_CH12 + * + * TIM16_CH1 T BUZZER - Driven by other driver + */ + constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer1, DMA{DMA::Index1}), initIOTimer(Timer::Timer4, DMA{DMA::Index1}), From caa45c1caa8a59b73597c91b87b110932d590525 Mon Sep 17 00:00:00 2001 From: Alexis Guijarro Date: Thu, 2 May 2024 19:22:19 +0000 Subject: [PATCH 54/55] mRo Control Zero Classic: Definition for GPS2 by default added --- boards/mro/ctrl-zero-classic/init/rc.board_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/mro/ctrl-zero-classic/init/rc.board_defaults b/boards/mro/ctrl-zero-classic/init/rc.board_defaults index 110c240ee8a0..0495329e1a47 100644 --- a/boards/mro/ctrl-zero-classic/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-classic/init/rc.board_defaults @@ -6,4 +6,5 @@ param set-default BAT1_V_DIV 10.1 param set-default BAT1_A_PER_V 17 +param set-default GPS_2_CONFIG 202 param set-default TEL_FRSKY_CONFIG 103 From 1555f2bd2229544c43966ab5f94879c41d8e1e01 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Aug 2024 16:30:44 -0400 Subject: [PATCH 55/55] boards/px4/fmu-v5x: rc.board_sensors fix missing Split PAB ID and FMUM backport --- boards/px4/fmu-v5x/init/rc.board_sensors | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 2b8d344ca066..a54eab13ed4a 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -49,7 +49,7 @@ then fi fi -if ver hwbasecmp 008 009 00a 010 +if ver hwbasecmp 008 009 00a 010 011 then #SKYNODE base fmu board orientation @@ -72,6 +72,27 @@ then # Internal magnetometer on I2c bmm150 -I -R 6 start +else + #FMUv5Xbase board orientation + + if ver hwtypecmp V5X000 V5X001 + then + # Internal SPI BMI088 + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start + else + # Internal SPI bus ICM20649 + icm20649 -s -R 6 start + fi + + # Internal SPI bus ICM42688p + icm42688p -R 6 -s start + + # Internal SPI bus ICM-20602 (hard-mounted) + icm20602 -R 10 -s start + + # Internal magnetometer on I2c + bmm150 -I start fi # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)