Merge branch 'tf/s35770' of github.com:ThunderFly-aerospace/PX4-Autop…

…ilot into tf/s35770
ThunderFly-aerospace · Oct 15, 2024 · 7642b5f · 7642b5f
2 parents 2426f8e + c811cc9
commit 7642b5f
Show file tree

Hide file tree

Showing 6 changed files with 44 additions and 14 deletions.
diff --git a/platforms/nuttx/init/stm32f7/rc.board_arch_defaults b/platforms/nuttx/init/stm32f7/rc.board_arch_defaults
@@ -13,4 +13,4 @@ param set-default -s MC_AT_EN 1
 
 param set-default -s UAVCAN_ENABLE 2
 
-set LOGGER_BUF 64
+set LOGGER_BUF 40
diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
@@ -217,7 +217,9 @@ void Ekf::resetFlowFusion()
 {
 	ECL_INFO("reset velocity to flow");
 	_information_events.flags.reset_vel_to_flow = true;
-	resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
+
+	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(_flow_sample_delayed);
+	resetHorizontalVelocityTo(_flow_vel_ne, flow_vel_var);
 
 	// reset position, estimate is relative to initial position in this mode, so we start with zero error
 	if (!_control_status.flags.in_air) {

diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp
@@ -42,9 +42,11 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/crypto.h>
 #include <px4_platform_common/log.h>
-#ifdef __PX4_NUTTX
-#include <systemlib/hardfault_log.h>
-#endif /* __PX4_NUTTX */
+
+#if defined(__PX4_NUTTX)
+# include <malloc.h>
+# include <systemlib/hardfault_log.h>
+#endif // __PX4_NUTTX
 
 using namespace time_literals;
 
@@ -60,11 +62,13 @@ LogWriterFile::LogWriterFile(size_t buffer_size)
 	//We always write larger chunks (orb messages) to the buffer, so the buffer
 	//needs to be larger than the minimum write chunk (300 is somewhat arbitrary)
 	{
-		math::max(buffer_size, _min_write_chunk + 300),
+		buffer_size,
+		_min_write_chunk + 300,
 		perf_alloc(PC_ELAPSED, "logger_sd_write"), perf_alloc(PC_ELAPSED, "logger_sd_fsync")},
 
 	{
 		300, // buffer size for the mission log (can be kept fairly small)
+		1,
 		perf_alloc(PC_ELAPSED, "logger_sd_write_mission"), perf_alloc(PC_ELAPSED, "logger_sd_fsync_mission")}
 }
 {
@@ -590,9 +594,12 @@ const char *log_type_str(LogType type)
 	return "unknown";
 }
 
-LogWriterFile::LogFileBuffer::LogFileBuffer(size_t log_buffer_size, perf_counter_t perf_write,
-		perf_counter_t perf_fsync)
-	: _buffer_size(log_buffer_size), _perf_write(perf_write), _perf_fsync(perf_fsync)
+LogWriterFile::LogFileBuffer::LogFileBuffer(size_t log_buffer_desired_size, size_t log_buffer_min_size,
+		perf_counter_t perf_write, perf_counter_t perf_fsync) :
+	_buffer_size(log_buffer_desired_size),
+	_buffer_size_min(log_buffer_min_size),
+	_perf_write(perf_write),
+	_perf_fsync(perf_fsync)
 {
 }
 
@@ -660,6 +667,25 @@ bool LogWriterFile::LogFileBuffer::start_log(const char *filename)
 	}
 
 	if (_buffer == nullptr) {
+		_buffer_size = math::max(_buffer_size, _buffer_size_min);
+
+#if defined(__PX4_NUTTX)
+		struct mallinfo alloc_info = mallinfo();
+
+		// reduced to largest available free chunk, but leave at least 1 kB available
+		static constexpr ssize_t one_kb = 1024;
+		const ssize_t reduced_buffer_size = math::max((alloc_info.mxordblk - one_kb) / one_kb * one_kb,
+						    (ssize_t)_buffer_size_min);
+
+		if ((reduced_buffer_size > 0) && ((ssize_t)_buffer_size > reduced_buffer_size)) {
+			PX4_WARN("requested buffer size %dB limited to available %dB (available plus 1 kB margin)",
+				 _buffer_size, reduced_buffer_size);
+
+			_buffer_size = reduced_buffer_size;
+		}
+
+#endif // __PX4_NUTTX
+
 		_buffer = (uint8_t *) px4_cache_aligned_alloc(_buffer_size);
 
 		if (_buffer == nullptr) {

diff --git a/src/modules/logger/log_writer_file.h b/src/modules/logger/log_writer_file.h
@@ -169,7 +169,8 @@ class LogWriterFile
 	class LogFileBuffer
 	{
 	public:
-		LogFileBuffer(size_t log_buffer_size, perf_counter_t perf_write, perf_counter_t perf_fsync);
+		LogFileBuffer(size_t log_buffer_desired_size, size_t log_buffer_min_size,
+			      perf_counter_t perf_write, perf_counter_t perf_fsync);
 
 		~LogFileBuffer();
 
@@ -203,7 +204,8 @@ class LogWriterFile
 		bool _should_run = false;
 		px4::atomic_bool _had_write_error{false};
 	private:
-		const size_t _buffer_size;
+		size_t _buffer_size;
+		const size_t _buffer_size_min;
 		int	_fd = -1;
 		uint8_t *_buffer = nullptr;
 		size_t _head = 0; ///< next position to write to

diff --git a/src/modules/sensors/sensor_params_flow.c b/src/modules/sensors/sensor_params_flow.c
@@ -60,7 +60,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
  * @min 0.0
  * @max 1.0
  * @increment 0.1
- * @decimal 1
+ * @decimal 2
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.08f);
@@ -77,7 +77,7 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.08f);
  * @min 1.0
  * @max 100.0
  * @increment 0.1
- * @decimal 1
+ * @decimal 2
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 100.f);

diff --git a/test/mavsdk_tests/test_vtol_rtl.cpp b/test/mavsdk_tests/test_vtol_rtl.cpp
@@ -74,7 +74,7 @@ TEST_CASE("RTL with Mission Landing", "[vtol]")
 	tester.set_rtl_type(2);
 	tester.arm();
 	tester.execute_rtl_when_reaching_mission_sequence(2);
-	tester.check_tracks_mission_raw(35.0f);
+	tester.check_tracks_mission_raw(40.0f);
 	tester.wait_until_disarmed(std::chrono::seconds(120));
 }