From 7ff52a71fb43c2f632eea6ca01105c3d5da119fc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 20 Jan 2025 17:51:46 +1100 Subject: [PATCH 01/77] AP_DroneCAN: don't delcare xacti publishers if xacti not compiled in --- libraries/AP_DroneCAN/AP_DroneCAN.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 14b71150218ca..6568a04509c6c 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -37,6 +37,7 @@ #include #include #include +#include #ifndef DRONECAN_SRV_NUMBER #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS @@ -167,10 +168,12 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { Canard::Publisher buzzer{canard_iface}; Canard::Publisher rtcm_stream{canard_iface}; +#if HAL_MOUNT_XACTI_ENABLED // xacti specific publishers Canard::Publisher xacti_copter_att_status{canard_iface}; Canard::Publisher xacti_gimbal_control_data{canard_iface}; Canard::Publisher xacti_gnss_status{canard_iface}; +#endif // HAL_MOUNT_XACTI_ENABLED #if AP_RELAY_DRONECAN_ENABLED // Hardpoint for relay From c681bb493492f41cb3efbcd1babd05dbbb2de033 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH 02/77] AP_DAL: allow for more than 327m range rangefinders --- libraries/AP_DAL/AP_DAL_RangeFinder.cpp | 18 +++++++++--------- libraries/AP_DAL/AP_DAL_RangeFinder.h | 6 +++--- libraries/AP_DAL/LogStructure.h | 10 +++++----- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL_RangeFinder.cpp b/libraries/AP_DAL/AP_DAL_RangeFinder.cpp index 9603a2d39499c..467e5109937da 100644 --- a/libraries/AP_DAL/AP_DAL_RangeFinder.cpp +++ b/libraries/AP_DAL/AP_DAL_RangeFinder.cpp @@ -36,7 +36,7 @@ AP_DAL_RangeFinder::AP_DAL_RangeFinder() #endif } -int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const +float AP_DAL_RangeFinder::ground_clearance_orient(enum Rotation orientation) const { #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) const auto *rangefinder = AP::rangefinder(); @@ -44,25 +44,25 @@ int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation if (orientation != ROTATION_PITCH_270) { // the EKF only asks for this from a specific orientation. Thankfully. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return rangefinder->ground_clearance_cm_orient(orientation); + return rangefinder->ground_clearance_orient(orientation); } #endif - return _RRNH.ground_clearance_cm; + return _RRNH.ground_clearance; } -int16_t AP_DAL_RangeFinder::max_distance_cm_orient(enum Rotation orientation) const +float AP_DAL_RangeFinder::max_distance_orient(enum Rotation orientation) const { #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) if (orientation != ROTATION_PITCH_270) { const auto *rangefinder = AP::rangefinder(); // the EKF only asks for this from a specific orientation. Thankfully. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return rangefinder->max_distance_cm_orient(orientation); + return rangefinder->max_distance_orient(orientation); } #endif - return _RRNH.max_distance_cm; + return _RRNH.max_distance; } void AP_DAL_RangeFinder::start_frame() @@ -75,8 +75,8 @@ void AP_DAL_RangeFinder::start_frame() const log_RRNH old = _RRNH; // EKF only asks for this *down*. - _RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270); - _RRNH.max_distance_cm = rangefinder->max_distance_cm_orient(ROTATION_PITCH_270); + _RRNH.ground_clearance = rangefinder->ground_clearance_orient(ROTATION_PITCH_270); + _RRNH.max_distance = rangefinder->max_distance_orient(ROTATION_PITCH_270); WRITE_REPLAY_BLOCK_IFCHANGED(RRNH, _RRNH, old); @@ -102,7 +102,7 @@ void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) { _RRNI.orientation = backend->orientation(); _RRNI.status = (uint8_t)backend->status(); _RRNI.pos_offset = backend->get_pos_offset(); - _RRNI.distance_cm = backend->distance_cm(); + _RRNI.distance = backend->distance(); WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old); } diff --git a/libraries/AP_DAL/AP_DAL_RangeFinder.h b/libraries/AP_DAL/AP_DAL_RangeFinder.h index 5bb60392cb02d..cb2372d2241bc 100644 --- a/libraries/AP_DAL/AP_DAL_RangeFinder.h +++ b/libraries/AP_DAL/AP_DAL_RangeFinder.h @@ -20,8 +20,8 @@ class AP_DAL_RangeFinder { Good }; - int16_t ground_clearance_cm_orient(enum Rotation orientation) const; - int16_t max_distance_cm_orient(enum Rotation orientation) const; + float ground_clearance_orient(enum Rotation orientation) const; + float max_distance_orient(enum Rotation orientation) const; // return true if we have a range finder with the specified orientation bool has_orientation(enum Rotation orientation) const; @@ -59,7 +59,7 @@ class AP_DAL_RangeFinder_Backend { return (AP_DAL_RangeFinder::Status)_RRNI.status; } - uint16_t distance_cm() const { return _RRNI.distance_cm; } + float distance() const { return _RRNI.distance; } const Vector3f &get_pos_offset() const { return _RRNI.pos_offset; } diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index c8ef343584438..4a42e38b1b39b 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -172,8 +172,8 @@ struct log_RBRI { // @Description: Replay Data Rangefinder Header struct log_RRNH { // this is rotation-pitch-270! - int16_t ground_clearance_cm; - int16_t max_distance_cm; + float ground_clearance; + float max_distance; uint8_t num_sensors; uint8_t _end; }; @@ -182,7 +182,7 @@ struct log_RRNH { // @Description: Replay Data Rangefinder Instance struct log_RRNI { Vector3f pos_offset; - uint16_t distance_cm; + float distance; uint8_t orientation; uint8_t status; uint8_t instance; @@ -420,9 +420,9 @@ struct log_RBOH { { LOG_RBRI_MSG, RLOG_SIZE(RBRI), \ "RBRI", "IfBB", "LastUpdate,Alt,H,I", "---#", "----" }, \ { LOG_RRNH_MSG, RLOG_SIZE(RRNH), \ - "RRNH", "hhB", "GCl,MaxD,NumSensors", "???", "???" }, \ + "RRNH", "ffB", "GCl,MaxD,NumSensors", "mm-", "00-" }, \ { LOG_RRNI_MSG, RLOG_SIZE(RRNI), \ - "RRNI", "fffHBBB", "PX,PY,PZ,Dist,Orient,Status,I", "------#", "-------" }, \ + "RRNI", "ffffBBB", "PX,PY,PZ,Dist,Orient,Status,I", "---m--#", "---0---" }, \ { LOG_RGPH_MSG, RLOG_SIZE(RGPH), \ "RGPH", "BB", "NumInst,Primary", "--", "--" }, \ { LOG_RGPI_MSG, RLOG_SIZE(RGPI), \ From 680fb999c58bfc0eb0e278a380e031ac965dcfc5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH 03/77] AP_Logger: allow for more than 327m range rangefinders --- libraries/AP_Logger/LogStructure.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 3176e5d8931c6..13584cddca771 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -456,7 +456,7 @@ struct PACKED log_RFND { LOG_PACKET_HEADER; uint64_t time_us; uint8_t instance; - uint16_t dist; + float dist; uint8_t status; uint8_t orient; int8_t quality; @@ -1181,7 +1181,7 @@ LOG_STRUCTURE_FROM_MOUNT \ { LOG_MODE_MSG, sizeof(log_Mode), \ "MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \ { LOG_RFND_MSG, sizeof(log_RFND), \ - "RFND", "QBCBBb", "TimeUS,Instance,Dist,Stat,Orient,Quality", "s#m--%", "F-B---", true }, \ + "RFND", "QBfBBb", "TimeUS,Instance,Dist,Stat,Orient,Quality", "s#m--%", "F-0---", true }, \ { LOG_MAV_STATS, sizeof(log_MAV_Stats), \ "DMS", "QIIIIBBBBBBBBB", "TimeUS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "F-------------" }, \ LOG_STRUCTURE_FROM_BEACON \ From e85539acf25a2d1ee69e3eac1c7d616445f6f144 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH 04/77] AP_NavEKF2: allow for more than 327m range rangefinders --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 4 ++-- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index b58f2254dbbbf..be95fcbc742a7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -483,7 +483,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Param: RNG_USE_HGT // @DisplayName: Range finder switch height percentage - // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND_MAX_CM). This will not work unless Baro or GPS height is selected as the primary height source vis EK2_ALT_SOURCE = 0 or 2 respectively. This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. + // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND*_MAX). This will not work unless Baro or GPS height is selected as the primary height source vis EK2_ALT_SOURCE = 0 or 2 respectively. This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. // @Range: -1 70 // @Increment: 1 // @User: Advanced diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index cdd25bc13383c..724b2a2e05eaa 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -28,7 +28,7 @@ void NavEKF2_core::readRangeFinder(void) return; } - rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f); + rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f); // read range finder at 20Hz // TODO better way of knowing if it has new data @@ -51,7 +51,7 @@ void NavEKF2_core::readRangeFinder(void) rngMeasIndex[sensorIndex] = 0; } storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; - storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f; + storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance(); } else { continue; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 6d5937dd2247b..7f209b6b28d25 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -66,7 +66,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const // we really, really shouldn't be here. return false; } - height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f); + height = MAX(float(_rng->max_distance_orient(ROTATION_PITCH_270)) * 0.7f - 1.0f, 1.0f); #else return false; #endif diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index debcaaf2c29e8..2f3382a8e9c40 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -979,7 +979,7 @@ void NavEKF2_core::selectHeightForFusion() activeHgtSource = HGT_SOURCE_RNG; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) { // determine if we are above or below the height switch region - ftype rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; + const ftype rangeMaxUse = 1e-2f * _rng->max_distance_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; From b633084df3c694b499a95c1a962d6c957007d92d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH 05/77] AP_NavEKF3: allow for more than 327m range rangefinders --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 57b837d9e47d0..44eb4f22dadc7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -480,7 +480,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: RNG_USE_HGT // @DisplayName: Range finder switch height percentage - // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFNDx_MAX_CM) and the primary height source is Baro or GPS (see EK3_SRCx_POSZ). This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. + // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFNDx_MAX) and the primary height source is Baro or GPS (see EK3_SRCx_POSZ). This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. // @Range: -1 70 // @Increment: 1 // @User: Advanced diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index a7146dd06e71b..b4c09f9835db5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -25,7 +25,7 @@ void NavEKF3_core::readRangeFinder(void) if (_rng == nullptr) { return; } - rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f); + rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f); // limit update rate to maximum allowed by data buffers if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) { @@ -47,7 +47,7 @@ void NavEKF3_core::readRangeFinder(void) rngMeasIndex[sensorIndex] = 0; } storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; - storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f; + storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance(); } else { continue; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 8fc369c392f7d..c358e1d07cbf2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -74,7 +74,7 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const // we really, really shouldn't be here. return false; } - height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f); + height = MAX(float(_rng->max_distance_orient(ROTATION_PITCH_270)) * 0.7f - 1.0f, 1.0f); #else return false; #endif diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 98aaf8cd27fbf..a73d177e620f7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1208,7 +1208,7 @@ void NavEKF3_core::selectHeightForFusion() activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) || (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) { // determine if we are above or below the height switch region - ftype rangeMaxUse = 1e-4 * (ftype)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; + const ftype rangeMaxUse = 1e-2 * (ftype)_rng->max_distance_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; bool belowLowerSwHgt = ((terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse) && (imuSampleTime_ms - gndHgtValidTime_ms < 1000); From 6ecbc0709072ba134bd63997f4dd21b9a219ec82 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 06/77] AP_Proximity: allow for more than 327m range rangefinders --- libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp index 4ec86b7f6e943..b69d3e5e91005 100644 --- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp @@ -51,8 +51,8 @@ void AP_Proximity_RangeFinder::update(void) const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle); // distance in meters const float distance = sensor->distance(); - _distance_min = sensor->min_distance_cm() * 0.01f; - _distance_max = sensor->max_distance_cm() * 0.01f; + _distance_min = sensor->min_distance(); + _distance_max = sensor->max_distance(); if ((distance <= _distance_max) && (distance >= _distance_min) && !ignore_reading(angle, distance, false)) { frontend.boundary.set_face_attributes(face, angle, distance, state.instance); // update OA database @@ -64,11 +64,11 @@ void AP_Proximity_RangeFinder::update(void) } // check upward facing range finder if (sensor->orientation() == ROTATION_PITCH_90) { - int16_t distance_upward = sensor->distance_cm(); - int16_t up_distance_min = sensor->min_distance_cm(); - int16_t up_distance_max = sensor->max_distance_cm(); + const float distance_upward = sensor->distance(); + const float up_distance_min = sensor->min_distance(); + const float up_distance_max = sensor->max_distance(); if ((distance_upward >= up_distance_min) && (distance_upward <= up_distance_max)) { - _distance_upward = distance_upward * 0.01f; + _distance_upward = distance_upward; } else { _distance_upward = -1.0; // mark an valid reading } From fb7925d029dc716613360c63c5d1037967cfef6e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 07/77] AP_RangeFinder: allow for more than 327m range rangefinders --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 35 ++++++++------- libraries/AP_RangeFinder/AP_RangeFinder.h | 31 ++++++++++--- .../AP_RangeFinder_Ainstein_LR_D1.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_Backend.cpp | 4 +- .../AP_RangeFinder/AP_RangeFinder_Backend.h | 7 ++- .../AP_RangeFinder/AP_RangeFinder_Bebop.h | 4 +- .../AP_RangeFinder_Benewake.cpp | 4 +- .../AP_RangeFinder_Benewake_TFMiniPlus.cpp | 2 +- .../AP_RangeFinder_JRE_Serial.cpp | 6 +-- .../AP_RangeFinder_LeddarVu8.cpp | 6 +-- .../AP_RangeFinder_LightWareI2C.cpp | 12 ++--- .../AP_RangeFinder_LightWareI2C.h | 5 +-- .../AP_RangeFinder_LightWareSerial.cpp | 4 +- .../AP_RangeFinder/AP_RangeFinder_MAVLink.cpp | 39 ++++++++-------- .../AP_RangeFinder/AP_RangeFinder_MAVLink.h | 10 ++--- .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 44 ++++++++++++------- .../AP_RangeFinder/AP_RangeFinder_Params.h | 7 +-- .../AP_RangeFinder/AP_RangeFinder_RDS02UF.h | 12 +++-- .../AP_RangeFinder_TeraRangerI2C.cpp | 4 +- .../AP_RangeFinder_TeraRanger_Serial.cpp | 11 ++--- .../AP_RangeFinder/AP_RangeFinder_analog.cpp | 6 +-- .../examples/RFIND_test/RFIND_test.cpp | 4 +- 22 files changed, 147 insertions(+), 112 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 82e23c11e30b0..f6b25ea33e335 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -190,6 +190,14 @@ RangeFinder::RangeFinder() _singleton = this; } +void RangeFinder::convert_params(void) +{ + // PARAMETER_CONVERSION - Added: Dec-2024 for 4.6->4.7 + for (auto &p : params) { + p.convert_min_max_params(); + } +} + /* initialise the RangeFinder class. We do detection of attached range finders here. For now we won't allow for hot-plugging of @@ -197,6 +205,8 @@ RangeFinder::RangeFinder() */ void RangeFinder::init(enum Rotation orientation_default) { + convert_params(); + if (num_instances != 0) { // don't re-init if we've found some sensors already return; @@ -702,11 +712,6 @@ float RangeFinder::distance_orient(enum Rotation orientation) const return backend->distance(); } -uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const -{ - return distance_orient(orientation) * 100.0; -} - int8_t RangeFinder::signal_quality_pct_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); @@ -716,31 +721,31 @@ int8_t RangeFinder::signal_quality_pct_orient(enum Rotation orientation) const return backend->signal_quality_pct(); } -int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const +float RangeFinder::min_distance_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); if (backend == nullptr) { - return 0; + return 0; // consider NaN } - return backend->max_distance_cm(); + return backend->min_distance(); } -int16_t RangeFinder::min_distance_cm_orient(enum Rotation orientation) const +float RangeFinder::max_distance_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); if (backend == nullptr) { - return 0; + return 0; // consider NaN } - return backend->min_distance_cm(); + return backend->max_distance(); } -int16_t RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const +float RangeFinder::ground_clearance_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); if (backend == nullptr) { - return 0; + return 0; // consider NaN } - return backend->ground_clearance_cm(); + return backend->ground_clearance(); } bool RangeFinder::has_data_orient(enum Rotation orientation) const @@ -821,7 +826,7 @@ void RangeFinder::Log_RFND() const LOG_PACKET_HEADER_INIT(LOG_RFND_MSG), time_us : AP_HAL::micros64(), instance : i, - dist : s->distance_cm(), + dist : s->distance(), status : (uint8_t)s->status(), orient : s->orientation(), quality : s->signal_quality_pct(), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 4d66101b8b6ab..53734fd1d5a81 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -35,7 +35,7 @@ #endif #endif -#define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10 +#define RANGEFINDER_GROUND_CLEARANCE_DEFAULT 0.10 #define RANGEFINDER_PREARM_ALT_MAX_CM 200 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 0 @@ -268,15 +268,30 @@ class RangeFinder uint8_t get_address(uint8_t id) const { return id >= RANGEFINDER_MAX_INSTANCES? 0 : uint8_t(params[id].address.get()); } - + // methods to return a distance on a particular orientation from // any sensor which can current supply it - float distance_orient(enum Rotation orientation) const; - uint16_t distance_cm_orient(enum Rotation orientation) const; int8_t signal_quality_pct_orient(enum Rotation orientation) const; - int16_t max_distance_cm_orient(enum Rotation orientation) const; - int16_t min_distance_cm_orient(enum Rotation orientation) const; - int16_t ground_clearance_cm_orient(enum Rotation orientation) const; +#if AP_SCRIPTING_ENABLED + // centimetre accessors - do not use, reduce use where possible + uint16_t distance_cm_orient(enum Rotation orientation) const { + return distance_orient(orientation) * 100.0; + } + int32_t max_distance_cm_orient(enum Rotation orientation) const { + return max_distance_orient(orientation) * 100; + } + int32_t min_distance_cm_orient(enum Rotation orientation) const { + return min_distance_orient(orientation) * 100; + } + int32_t ground_clearance_cm_orient(enum Rotation orientation) const { + return ground_clearance_orient(orientation) * 100; + } +#endif + // metre accessors - use these in preference to the cm accessors + float distance_orient(enum Rotation orientation) const; + float max_distance_orient(enum Rotation orientation) const; + float min_distance_orient(enum Rotation orientation) const; + float ground_clearance_orient(enum Rotation orientation) const; MAV_DISTANCE_SENSOR get_mav_distance_sensor_type_orient(enum Rotation orientation) const; RangeFinder::Status status_orient(enum Rotation orientation) const; bool has_data_orient(enum Rotation orientation) const; @@ -310,6 +325,8 @@ class RangeFinder float estimated_terrain_height; Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests + void convert_params(void); + void detect_instance(uint8_t instance, uint8_t& serial_instance); bool _add_backend(AP_RangeFinder_Backend *driver, uint8_t instance, uint8_t serial_instance=0); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp index e8a2f8c319b0c..1da5f61ecd5b8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp @@ -95,7 +95,7 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m) has_data = false; if (snr == 0) { state.status = RangeFinder::Status::OutOfRangeHigh; - reading_m = MAX(656, max_distance_cm() * 0.01 + 1); + reading_m = MAX(656, max_distance() + 1); } else { state.status = RangeFinder::Status::NoData; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp index 3ae267a928cf3..bf5df397bc070 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp @@ -60,9 +60,9 @@ bool AP_RangeFinder_Backend::has_data() const { void AP_RangeFinder_Backend::update_status(RangeFinder::RangeFinder_State &state_arg) const { // check distance - if (state_arg.distance_m > max_distance_cm() * 0.01f) { + if (state_arg.distance_m > max_distance()) { set_status(state_arg, RangeFinder::Status::OutOfRangeHigh); - } else if (state_arg.distance_m < min_distance_cm() * 0.01f) { + } else if (state_arg.distance_m < min_distance()) { set_status(state_arg, RangeFinder::Status::OutOfRangeLow); } else { set_status(state_arg, RangeFinder::Status::Good); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index e1ae5b50122c1..087b504ed7952 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -53,12 +53,11 @@ class AP_RangeFinder_Backend enum Rotation orientation() const { return (Rotation)params.orientation.get(); } float distance() const { return state.distance_m; } - uint16_t distance_cm() const { return state.distance_m*100.0f; } int8_t signal_quality_pct() const WARN_IF_UNUSED { return state.signal_quality_pct; } uint16_t voltage_mv() const { return state.voltage_mv; } - virtual int16_t max_distance_cm() const { return params.max_distance_cm; } - virtual int16_t min_distance_cm() const { return params.min_distance_cm; } - int16_t ground_clearance_cm() const { return params.ground_clearance_cm; } + virtual float max_distance() const { return params.max_distance; } + virtual float min_distance() const { return params.min_distance; } + float ground_clearance() const { return params.ground_clearance; } MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const; RangeFinder::Status status() const; RangeFinder::Type type() const { return (RangeFinder::Type)params.type.get(); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h index e32be0ebe84f0..6d1821a4a616f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h @@ -141,8 +141,8 @@ class AP_RangeFinder_Bebop : public AP_RangeFinder_Backend { unsigned int _filtered_capture_size; struct echo _echoes[RNFD_BEBOP_MAX_ECHOES]; unsigned int _filter_average = 4; - int16_t _last_max_distance_cm = 850; - int16_t _last_min_distance_cm = 32; + float _last_max_distance = 8.50; + float _last_min_distance = 0.32; }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index f04056588ddb0..95d149182d60d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal; #define BENEWAKE_FRAME_HEADER 0x59 #define BENEWAKE_FRAME_LENGTH 9 #define BENEWAKE_DIST_MAX_CM 32768 -#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100 +#define BENEWAKE_OUT_OF_RANGE_ADD 1.00 // metres // format of serial packets received from benewake lidar // @@ -129,7 +129,7 @@ bool AP_RangeFinder_Benewake::get_reading(float &reading_m) if (count_out_of_range > 0) { // if only out of range readings return larger of // driver defined maximum range for the model and user defined max range + 1m - reading_m = MAX(model_dist_max_cm(), max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM) * 0.01f; + reading_m = MAX(model_dist_max_cm() * 0.01, max_distance() + BENEWAKE_OUT_OF_RANGE_ADD); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp index 0e3e954816611..76f8d0c2ccfea 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp @@ -169,7 +169,7 @@ void AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw * value to 0." - force it to the max distance so status is set to OutOfRangeHigh * rather than NoData. */ - output_distance_cm = MAX(MAX_DIST_CM, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM); + output_distance_cm = MAX(MAX_DIST_CM, max_distance()*100 + BENEWAKE_OUT_OF_RANGE_ADD_CM); } else { output_distance_cm = constrain_int16(output_distance_cm, MIN_DIST_CM, MAX_DIST_CM); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp index d54ef55d5f081..7bc16f2c11c3b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -25,8 +25,8 @@ #define FRAME_HEADER_2_B 'B' // 0x42 #define FRAME_HEADER_2_C 'C' // 0x43 -#define DIST_MAX_CM 5000 -#define OUT_OF_RANGE_ADD_CM 100 +#define DIST_MAX 50.00 +#define OUT_OF_RANGE_ADD 1.0 // metres void AP_RangeFinder_JRE_Serial::move_preamble_in_buffer(uint8_t search_start_pos) { @@ -134,7 +134,7 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) // all readings were invalid so return out-of-range-high value if (invalid_count > 0) { no_signal = true; - reading_m = MIN(MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f; + reading_m = MAX(DIST_MAX, max_distance() + OUT_OF_RANGE_ADD); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp index 6a2f1bf169714..c8ac42120a91a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp @@ -26,8 +26,8 @@ extern const AP_HAL::HAL& hal; // https://autonomoustuff.com/product/leddartech-vu8/ #define LEDDARVU8_ADDR_DEFAULT 0x01 // modbus default device id -#define LEDDARVU8_DIST_MAX_CM 18500 // maximum possible distance reported by lidar -#define LEDDARVU8_OUT_OF_RANGE_ADD_CM 100 // add this many cm to out-of-range values +#define LEDDARVU8_DIST_MAX 185.00 // maximum possible distance reported by lidar +#define LEDDARVU8_OUT_OF_RANGE_ADD 1.00 // add this many metres to out-of-range values #define LEDDARVU8_TIMEOUT_MS 200 // timeout in milliseconds if no distance messages received // distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal @@ -78,7 +78,7 @@ bool AP_RangeFinder_LeddarVu8::get_reading(float &reading_m) } else { // if only out of range readings return larger of // driver defined maximum range for the model and user defined max range + 1m - reading_m = MAX(LEDDARVU8_DIST_MAX_CM, max_distance_cm() + LEDDARVU8_OUT_OF_RANGE_ADD_CM)*0.01f; + reading_m = MAX(LEDDARVU8_DIST_MAX, max_distance() + LEDDARVU8_OUT_OF_RANGE_ADD); } return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index 5e600ca463dd2..940d15943bf87 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal; #define LIGHTWARE_LOST_SIGNAL_TIMEOUT_WRITE_REG 23 #define LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE 20 // number of lost signal confirmations for legacy protocol only -#define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100 +#define LIGHTWARE_OUT_OF_RANGE_ADD 1.00 // metres static const size_t lx20_max_reply_len_bytes = 32; static const size_t lx20_max_expected_stream_reply_len_bytes = 14; @@ -351,7 +351,7 @@ bool AP_RangeFinder_LightWareI2C::legacy_get_reading(float &reading_m) int16_t signed_val = int16_t(be16toh(val)); if (signed_val < 0) { // some lidar firmwares will return 65436 for out of range - reading_m = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM) * 0.01f; + reading_m = max_distance() + LIGHTWARE_OUT_OF_RANGE_ADD; } else { reading_m = uint16_t(signed_val) * 0.01f; } @@ -381,7 +381,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_get_reading(float &reading_m) } if (i==0) { - reading_m = sf20_stream_val[0] * 0.01f; + reading_m = sf20_stream_val[0]; } // Increment the stream sequence @@ -400,7 +400,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_get_reading(float &reading_m) bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf, size_t *p_num_processed_chars, const char *string_identifier, - uint16_t &val) + float &val) { size_t string_identifier_len = strlen(string_identifier); for (uint32_t i = 0 ; i < string_identifier_len ; i++) { @@ -416,7 +416,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf, we will return max distance */ if (strncmp((const char *)&stream_buf[*p_num_processed_chars], "-1.00", 5) == 0) { - val = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM); + val = max_distance() + LIGHTWARE_OUT_OF_RANGE_ADD; (*p_num_processed_chars) += 5; return true; } @@ -451,7 +451,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf, } accumulator *= final_multiplier; - val = accumulator; + val = accumulator * 0.01; return number_found; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h index ab15b6a9e55d6..fa10dc89efbaf 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h @@ -32,7 +32,7 @@ class AP_RangeFinder_LightWareI2C : public AP_RangeFinder_Backend private: - uint16_t sf20_stream_val[NUM_SF20_DATA_STREAMS]; + float sf20_stream_val[NUM_SF20_DATA_STREAMS]; int currentStreamSequenceIndex = 0; // constructor @@ -59,8 +59,7 @@ class AP_RangeFinder_LightWareI2C : public AP_RangeFinder_Backend bool sf20_parse_stream(uint8_t *stream_buf, size_t *p_num_processed_chars, const char *string_identifier, - uint16_t &val); - void data_log(uint16_t *val); + float &val); AP_HAL::OwnPtr _dev; }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index 61c2ed9c2d975..4eebdf66d2682 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -37,7 +37,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m) uint16_t invalid_count = 0; // number of invalid readings // max distance the sensor can reliably measure - read from parameters - const int16_t distance_cm_max = max_distance_cm(); + const auto distance_cm_max = max_distance()*100; // read any available lines from the lidar for (auto i=0; i<8192; i++) { @@ -128,7 +128,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m) // all readings were invalid so return out-of-range-high value if (invalid_count > 0) { - reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f; + reading_m = MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM); no_signal = true; return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index b4fc5fb1ff597..63e7642bc1ea1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -30,9 +30,9 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg) // only accept distances for the configured orientation if (packet.orientation == orientation()) { state.last_reading_ms = AP_HAL::millis(); - distance_cm = packet.current_distance; - _max_distance_cm = packet.max_distance; - _min_distance_cm = packet.min_distance; + distance = packet.current_distance * 0.01; + _max_distance = packet.max_distance * 0.01; + _min_distance = packet.min_distance * 0.01; sensor_type = (MAV_DISTANCE_SENSOR)packet.type; signal_quality = packet.signal_quality; if (signal_quality == 0) { @@ -45,28 +45,31 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg) } } -int16_t AP_RangeFinder_MAVLink::max_distance_cm() const +float AP_RangeFinder_MAVLink::max_distance() const { - if (_max_distance_cm == 0 && _min_distance_cm == 0) { + const auto baseclass_max_distance = AP_RangeFinder_Backend::max_distance(); + + if (is_zero(_max_distance) && is_zero(_min_distance)) { // we assume if both of these are zero that we ignore both - return params.max_distance_cm; + return baseclass_max_distance; } - if (params.max_distance_cm < _max_distance_cm) { - return params.max_distance_cm; - } - return _max_distance_cm; + // return the smaller of the base class's distance and what we + // receive from the network: + return MIN(baseclass_max_distance, _max_distance); } -int16_t AP_RangeFinder_MAVLink::min_distance_cm() const +float AP_RangeFinder_MAVLink::min_distance() const { - if (_max_distance_cm == 0 && _min_distance_cm == 0) { + const auto baseclass_min_distance = AP_RangeFinder_Backend::min_distance(); + + if (is_zero(_max_distance) && is_zero(_min_distance)) { // we assume if both of these are zero that we ignore both - return params.min_distance_cm; + return baseclass_min_distance; } - if (params.min_distance_cm > _min_distance_cm) { - return params.min_distance_cm; - } - return _min_distance_cm; + + // return the smaller of the base class's distance and what we + // receive from the network: + return MIN(baseclass_min_distance, _min_distance); } /* @@ -81,7 +84,7 @@ void AP_RangeFinder_MAVLink::update(void) state.distance_m = 0.0f; state.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN; } else { - state.distance_m = distance_cm * 0.01f; + state.distance_m = distance; state.signal_quality_pct = signal_quality; update_status(); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h index 47cd080c13f9a..89f4232367505 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h @@ -28,8 +28,8 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend // Get update from mavlink void handle_msg(const mavlink_message_t &msg) override; - int16_t max_distance_cm() const override; - int16_t min_distance_cm() const override; + float max_distance() const override; + float min_distance() const override; protected: @@ -40,9 +40,9 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend private: // stored data from packet: - uint16_t distance_cm; - uint16_t _max_distance_cm; - uint16_t _min_distance_cm; + float distance; + float _max_distance; + float _min_distance; int8_t signal_quality; // start a reading diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 72630f95f3949..e068295694ff5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -49,21 +49,21 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @User: Standard AP_GROUPINFO("FUNCTION", 5, AP_RangeFinder_Params, function, 0), - // @Param: MIN_CM + // @Param: MIN // @DisplayName: Rangefinder minimum distance - // @Description: Minimum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 + // @Description: Minimum distance in metres that rangefinder can reliably read + // @Units: m + // @Increment: 0.01 // @User: Standard - AP_GROUPINFO("MIN_CM", 6, AP_RangeFinder_Params, min_distance_cm, 20), + AP_GROUPINFO("MIN", 6, AP_RangeFinder_Params, min_distance, 0.20), - // @Param: MAX_CM + // @Param: MAX // @DisplayName: Rangefinder maximum distance - // @Description: Maximum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 + // @Description: Maximum distance in metres that rangefinder can reliably read + // @Units: m + // @Increment: 0.01 // @User: Standard - AP_GROUPINFO("MAX_CM", 7, AP_RangeFinder_Params, max_distance_cm, 700), + AP_GROUPINFO("MAX", 7, AP_RangeFinder_Params, max_distance, 7.00), // @Param: STOP_PIN // @DisplayName: Rangefinder stop pin @@ -89,14 +89,14 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @User: Standard AP_GROUPINFO("PWRRNG", 11, AP_RangeFinder_Params, powersave_range, 0), - // @Param: GNDCLEAR - // @DisplayName: Distance (in cm) from the range finder to the ground - // @Description: This parameter sets the expected range measurement(in cm) that the range finder should return when the vehicle is on the ground. - // @Units: cm - // @Range: 5 127 - // @Increment: 1 + // @Param: GNDCLR + // @DisplayName: Distance from the range finder to the ground + // @Description: This parameter sets the expected range measurement that the range finder should return when the vehicle is on the ground. + // @Units: m + // @Range: 0.05 1.5 + // @Increment: 0.01 // @User: Standard - AP_GROUPINFO("GNDCLEAR", 12, AP_RangeFinder_Params, ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), + AP_GROUPINFO("GNDCLR", 12, AP_RangeFinder_Params, ground_clearance, RANGEFINDER_GROUND_CLEARANCE_DEFAULT), // @Param: ADDR // @DisplayName: Bus address of sensor @@ -141,6 +141,16 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { AP_GROUPEND }; + +// PARAMETER_CONVERSION - Added: Dec-2024 for 4.7 +void AP_RangeFinder_Params::convert_min_max_params(void) +{ + // ./Tools/autotest/test_param_upgrade.py --vehicle=arducopter --param "RNGFND1_MAX_CM=300->RNGFND1_MAX=3.00" --param "RNGFND2_MIN_CM=678->RNGFND2_MIN=6.78" --param "RNGFNDA_MIN_CM=1->RNGFNDA_MIN=0.01" --param "RNGFND5_GNDCLEAR=103->RNGFND5_GNDCLR=1.03" + max_distance.convert_parameter_width(AP_PARAM_INT16, 0.01); + min_distance.convert_parameter_width(AP_PARAM_INT16, 0.01); + ground_clearance.convert_parameter_width(AP_PARAM_INT8, 0.01); +} + AP_RangeFinder_Params::AP_RangeFinder_Params(void) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.h b/libraries/AP_RangeFinder/AP_RangeFinder_Params.h index 17893c3e6812f..f0b58b31642df 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.h @@ -12,6 +12,7 @@ class AP_RangeFinder_Params { static const struct AP_Param::GroupInfo var_info[]; AP_RangeFinder_Params(void); + void convert_min_max_params(); /* Do not allow copies */ CLASS_NO_COPY(AP_RangeFinder_Params); @@ -20,14 +21,14 @@ class AP_RangeFinder_Params { AP_Float scaling; AP_Float offset; AP_Int16 powersave_range; - AP_Int16 min_distance_cm; - AP_Int16 max_distance_cm; + AP_Float min_distance; + AP_Float max_distance; AP_Int8 type; AP_Int8 pin; AP_Int8 ratiometric; AP_Int8 stop_pin; AP_Int8 function; - AP_Int8 ground_clearance_cm; + AP_Float ground_clearance; AP_Int8 address; AP_Int8 orientation; }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h index 6b480a17765c8..cccb2689ff190 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h @@ -23,8 +23,8 @@ #include "AP_RangeFinder_Backend_Serial.h" #define RDS02_BUFFER_SIZE 50 -#define RDS02UF_DIST_MAX_CM 2000 -#define RDS02UF_DIST_MIN_CM 150 +#define RDS02UF_DIST_MAX 20.00 +#define RDS02UF_DIST_MIN 1.50 #define RDS02UF_DATA_LEN 10 class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial @@ -57,8 +57,12 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial uint16_t read_timeout_ms() const override { return 500; } // make sure readings go out-of-range when necessary - int16_t max_distance_cm()const override { return MIN(params.max_distance_cm, RDS02UF_DIST_MAX_CM); } - int16_t min_distance_cm() const override { return MAX(params.min_distance_cm, RDS02UF_DIST_MIN_CM); } + float max_distance() const override { + return MIN(AP_RangeFinder_Backend::max_distance(), RDS02UF_DIST_MAX); + } + float min_distance() const override { + return MAX(AP_RangeFinder_Backend::min_distance(), RDS02UF_DIST_MIN); + } // Data Format for Benewake Rds02UF // =============================== diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp index ab63b192dd115..fe034246d8cc8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp @@ -139,14 +139,14 @@ bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, ui // Check for error codes if (raw_distance == 0xFFFF) { // Too far away - output_distance_cm = max_distance_cm() + TR_OUT_OF_RANGE_ADD_CM; + output_distance_cm = max_distance()*100 + TR_OUT_OF_RANGE_ADD_CM; } else if (raw_distance == 0x0000) { // Too close output_distance_cm = 0; } else if (raw_distance == 0x0001) { // Unable to measure // This can also include the sensor pointing to the horizon when used as a proximity sensor - output_distance_cm = max_distance_cm() + TR_OUT_OF_RANGE_ADD_CM; + output_distance_cm = max_distance()*100 + TR_OUT_OF_RANGE_ADD_CM; } else { output_distance_cm = raw_distance/10; // Conversion to centimeters } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp index 088472b6b8387..bf1b3fd513877 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp @@ -18,17 +18,14 @@ #if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED -#include #include #include #include -extern const AP_HAL::HAL& hal; - #define FRAME_HEADER 0x54 #define FRAME_LENGTH 5 -#define DIST_MAX_CM 3000 -#define OUT_OF_RANGE_ADD_CM 1000 +#define DIST_MAX 30.00 +#define OUT_OF_RANGE_ADD 10.00 #define STATUS_MASK 0x1F #define DISTANCE_ERROR 0x0001 @@ -77,7 +74,7 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m) if (crc == linebuf[FRAME_LENGTH-1]) { // calculate distance uint16_t dist = ((uint16_t)linebuf[1] << 8) | linebuf[2]; - if (dist >= DIST_MAX_CM *10) { + if (dist >= DIST_MAX *1000) { // this reading is out of range and a bad read bad_read++; } else { @@ -107,7 +104,7 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m) if (bad_read > 0) { // if a bad read has occurred this update overwrite return with larger of // driver defined maximum range for the model and user defined max range + 1m - reading_m = MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM) * 0.01f; + reading_m = MAX(DIST_MAX, max_distance() + OUT_OF_RANGE_ADD); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index b9ffb1f06c674..8cba8ac052574 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -89,7 +89,7 @@ void AP_RangeFinder_analog::update(void) float scaling = params.scaling; float offset = params.offset; RangeFinder::Function function = (RangeFinder::Function)params.function.get(); - int16_t _max_distance_cm = params.max_distance_cm; + const float _max_distance = max_distance(); switch (function) { case RangeFinder::Function::LINEAR: @@ -106,8 +106,8 @@ void AP_RangeFinder_analog::update(void) } else { dist_m = scaling / (v - offset); } - if (dist_m > _max_distance_cm * 0.01f) { - dist_m = _max_distance_cm * 0.01f; + if (dist_m > _max_distance) { + dist_m = _max_distance; } break; } diff --git a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp index a281962e1b473..2cf3fc04cb5e7 100644 --- a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp +++ b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp @@ -50,11 +50,11 @@ void loop() if (!sensor->has_data()) { continue; } - hal.console->printf("All: device_%u type %d status %d distance_cm %d\n", + hal.console->printf("All: device_%u type=%d status=%d distance=%f\n", i, (int)sensor->type(), (int)sensor->status(), - sensor->distance_cm()); + sensor->distance()); had_data = true; } if (!had_data) { From 8ea5ef2784058036a99d73f1bc65fd9f5427e9d8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 08/77] AP_Scripting: allow for more than 327m range rangefinders --- libraries/AP_Scripting/docs/docs.lua | 26 ++++++++++++++++++- .../examples/rangefinder_quality_test.lua | 12 ++++----- .../examples/rangefinder_test.lua | 23 ++++++++++++---- .../generator/description/bindings.desc | 12 +++++++-- 4 files changed, 59 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 7eeae5de6e1f9..d609dc4319ea5 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3157,29 +3157,53 @@ function rangefinder:status_orient(orientation) end -- desc ---@param orientation integer ---@return integer +---@deprecated -- Use ground_clearance_orient (in metres) function rangefinder:ground_clearance_cm_orient(orientation) end -- desc ---@param orientation integer ---@return integer +---@deprecated -- Use min_distance_orient (in metres) function rangefinder:min_distance_cm_orient(orientation) end -- desc ---@param orientation integer ---@return integer +---@deprecated -- Use max_distance_orient (in metres) function rangefinder:max_distance_cm_orient(orientation) end -- desc ---@param orientation integer ---@return integer +---@deprecated -- Use distance_orient (in metres) function rangefinder:distance_cm_orient(orientation) end +-- Configured distance between rangefinder of a particular orientation and the vehicle's side which would rest on the ground. Usually used for a downward-facing rangefinder to give ground clearance. +---@param orientation integer +---@return number +function rangefinder:ground_clearance_orient(orientation) end + +-- Configured minimum distance the rangefinder in supplied orientation can measure +---@param orientation integer +---@return number +function rangefinder:min_distance_orient(orientation) end + +-- Configured maximum distance the rangefinder in supplied orientation can measure +---@param orientation integer +---@return number +function rangefinder:max_distance_orient(orientation) end + +-- Distance rangefinder in supplied orientation is currently measuring +---@param orientation integer +---@return number +function rangefinder:distance_orient(orientation) end + -- Current distance measurement signal quality for range finder at this orientation ---@param orientation integer ---@return integer function rangefinder:signal_quality_pct_orient(orientation) end --- desc +-- Returns true if there is a rangefinder measuring in supplied orientation ---@param orientation integer ---@return boolean function rangefinder:has_orientation(orientation) end diff --git a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua index 45772d4d35d70..1767b6bb8540d 100644 --- a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua +++ b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua @@ -7,8 +7,8 @@ -- Parameters should be set as follows before this test is loaded. -- "RNGFND1_TYPE": 36, -- "RNGFND1_ORIENT": 25, --- "RNGFND1_MIN_CM": 10, --- "RNGFND1_MAX_CM": 5000, +-- "RNGFND1_MIN": 0.10, +-- "RNGFND1_MAX": 50.00, ---@diagnostic disable: cast-local-type @@ -43,8 +43,8 @@ local SIGNAL_QUALITY_MAX = 100 local SIGNAL_QUALITY_UNKNOWN = -1 -- Read parameters for min and max valid range finder ranges. -local RNGFND1_MIN_CM = Parameter("RNGFND1_MIN_CM"):get() -local RNGFND1_MAX_CM = Parameter("RNGFND1_MAX_CM"):get() +local RNGFND1_MIN = Parameter("RNGFND1_MIN"):get() +local RNGFND1_MAX = Parameter("RNGFND1_MAX"):get() local function send(str) gcs:send_text(3, string.format("%s %s", TEST_ID_STR, str)) @@ -253,9 +253,9 @@ local function _update_begin_test() -- The full state udata must be initialized. local rf_state = RangeFinder_State() -- Set the status - if dist_m_in < RNGFND1_MIN_CM * 0.01 then + if dist_m_in < RNGFND1_MIN then rf_state:status(RNDFND_STATUS_OUT_OF_RANGE_LOW) - elseif dist_m_in > RNGFND1_MAX_CM * 0.01 then + elseif dist_m_in > RNGFND1_MAX then rf_state:status(RNDFND_STATUS_OUT_OF_RANGE_HIGH) else rf_state:status(RNDFND_STATUS_GOOD) diff --git a/libraries/AP_Scripting/examples/rangefinder_test.lua b/libraries/AP_Scripting/examples/rangefinder_test.lua index b6832bf54be35..ca5078f7b0acf 100644 --- a/libraries/AP_Scripting/examples/rangefinder_test.lua +++ b/libraries/AP_Scripting/examples/rangefinder_test.lua @@ -19,13 +19,26 @@ function update() end function info(rotation) - local ground_clearance = rangefinder:ground_clearance_cm_orient(rotation) - local distance_min = rangefinder:min_distance_cm_orient(rotation) - local distance_max = rangefinder:max_distance_cm_orient(rotation) + local ground_clearance = rangefinder:ground_clearance_orient(rotation) + local distance_min = rangefinder:min_distance_orient(rotation) + local distance_max = rangefinder:max_distance_orient(rotation) local offset = rangefinder:get_pos_offset_orient(rotation) - local distance_cm = rangefinder:distance_cm_orient(rotation) + local distance = rangefinder:distance_orient(rotation) - gcs:send_text(0, string.format("Rotation %d %.0f cm range %d - %d offset %.0f %.0f %.0f ground clearance %.0f", rotation, distance_cm, distance_min, distance_max, offset:x(), offset:y(), offset:z(), ground_clearance)) + gcs:send_text( + 0, + string.format( + "rot=%d distance=%.2f min=%.2f max=%.2f offset=(%.0f,%.0f,%.0f) ground-clearance=%.2f", + rotation, + distance, + distance_min, + distance_max, + offset:x(), + offset:y(), + offset:z(), + ground_clearance + ) + ) end return update(), 1000 -- first message may be displayed 1 seconds after start-up diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 18ae2c6d05110..f26dd062427ec 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -252,10 +252,18 @@ singleton RangeFinder rename rangefinder singleton RangeFinder method num_sensors uint8_t singleton RangeFinder method has_orientation boolean Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method distance_cm_orient deprecate Use distance_orient (in metres) +singleton RangeFinder method distance_orient float Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method signal_quality_pct_orient int8_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 -singleton RangeFinder method max_distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 -singleton RangeFinder method min_distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method max_distance_cm_orient int32_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method max_distance_cm_orient deprecate Use max_distance_orient (in metres) +singleton RangeFinder method min_distance_cm_orient int32_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method min_distance_cm_orient deprecate Use min_distance_orient (in metres) singleton RangeFinder method ground_clearance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method ground_clearance_cm_orient deprecate Use ground_clearance_orient (in metres) +singleton RangeFinder method max_distance_orient float Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method min_distance_orient float Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method ground_clearance_orient float Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method status_orient uint8_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method has_data_orient boolean Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method get_pos_offset_orient Vector3f Rotation'enum ROTATION_NONE ROTATION_MAX-1 From b54e795a0eb6b3cff94394bc44121b9f94b7d447 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 09/77] GCS_MAVLink: allow for more than 327m range rangefinders --- libraries/GCS_MAVLink/GCS_Common.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index cb89231557741..d355e172fae5e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -429,9 +429,9 @@ void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, con mavlink_msg_distance_sensor_send( chan, AP_HAL::millis(), // time since system boot TODO: take time of measurement - sensor->min_distance_cm(), // minimum distance the sensor can measure in centimeters - sensor->max_distance_cm(), // maximum distance the sensor can measure in centimeters - sensor->distance_cm(), // current distance reading + MIN(sensor->min_distance() * 100, 65535),// minimum distance the sensor can measure in centimeters + MIN(sensor->max_distance() * 100, 65535),// maximum distance the sensor can measure in centimeters + MIN(sensor->distance() * 100, 65535), // current distance reading sensor->get_mav_distance_sensor_type(), // type from MAV_DISTANCE_SENSOR enum instance, // onboard ID of the sensor == instance sensor->orientation(), // direction the sensor faces from MAV_SENSOR_ORIENTATION enum From 00f713cda86e845b7793104f83705b4513e2afd1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 10/77] SITL: allow for more than 327m range rangefinders --- .../SilentWings/Params/Rolladen-Schneider-LS8b.param | 12 ++++++------ .../SilentWings/Params/Schleicher-ASW27B.param | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index ccba94be53b18..5bf3c82c55360 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -451,10 +451,10 @@ RLL2SRV_RMAX,75 RLL2SRV_TCONST,0.45 RNGFND_ADDR,0 RNGFND_FUNCTION,0 -RNGFND_GNDCLEAR,10 +RNGFND_GNDCLR,0.1 RNGFND_LANDING,0 -RNGFND_MAX_CM,700 -RNGFND_MIN_CM,20 +RNGFND_MAX,7.00 +RNGFND_MIN,0.20 RNGFND_OFFSET,0 RNGFND_ORIENT,25 RNGFND_PIN,-1 @@ -469,9 +469,9 @@ RNGFND_STOP_PIN,-1 RNGFND_TYPE,0 RNGFND2_ADDR,0 RNGFND2_FUNCTION,0 -RNGFND2_GNDCLEAR,10 -RNGFND2_MAX_CM,700 -RNGFND2_MIN_CM,20 +RNGFND2_GNDCLR,0.1 +RNGFND2_MAX,7.00 +RNGFND2_MIN,0.20 RNGFND2_OFFSET,0 RNGFND2_ORIENT,25 RNGFND2_PIN,-1 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index 353379fa5a10d..5eb9fc9cde642 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -471,10 +471,10 @@ RLL2SRV_RMAX,50 RLL2SRV_TCONST,0.45 RNGFND_ADDR,0 RNGFND_FUNCTION,0 -RNGFND_GNDCLEAR,10 +RNGFND_GNDCLR,0.1 RNGFND_LANDING,0 -RNGFND_MAX_CM,700 -RNGFND_MIN_CM,20 +RNGFND_MAX,7.00 +RNGFND_MIN,0.20 RNGFND_OFFSET,0 RNGFND_ORIENT,25 RNGFND_PIN,-1 @@ -489,9 +489,9 @@ RNGFND_STOP_PIN,-1 RNGFND_TYPE,0 RNGFND2_ADDR,0 RNGFND2_FUNCTION,0 -RNGFND2_GNDCLEAR,10 -RNGFND2_MAX_CM,700 -RNGFND2_MIN_CM,20 +RNGFND2_GNDCLR,0.1 +RNGFND2_MAX,7.00 +RNGFND2_MIN,0.20 RNGFND2_OFFSET,0 RNGFND2_ORIENT,25 RNGFND2_PIN,-1 From a5eb9326cf9823f55530862d935abe116867d8a9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 11/77] ArduCopter: allow for more than 327m range rangefinders --- ArduCopter/AP_Arming.cpp | 4 ++-- ArduCopter/heli.cpp | 2 +- ArduCopter/mode_guided.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 6ef06d421641a..3e2c45ae3fe83 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -267,8 +267,8 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) return false; } // check if RTL_ALT is higher than rangefinder's max range - if (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT>RNGFND_MAX_CM"); + if (copter.g.rtl_altitude > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270) * 100) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT (in cm) above RNGFND_MAX (in metres)"); return false; } #else diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index bff2a3de17041..4ca8cffae8a73 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -46,7 +46,7 @@ void Copter::check_dynamic_flight(void) if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) { // when we are more than 2m from the ground with good // rangefinder lock consider it to be dynamic flight - moving = (rangefinder.distance_cm_orient(ROTATION_PITCH_270) > 200); + moving = (rangefinder.distance_orient(ROTATION_PITCH_270) > 2); } #endif diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index ba0321a1444ec..cfc8e27eff747 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -139,7 +139,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) #if AP_RANGEFINDER_ENABLED if (wp_nav->rangefinder_used_and_healthy() && wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER && - takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { + takeoff_alt_cm < copter.rangefinder.max_distance_orient(ROTATION_PITCH_270)*100) { // can't takeoff downwards if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) { return false; From cf9b74d2eaf2b640ee80efe3c4d48cddcf62a5c7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:49 +1100 Subject: [PATCH 12/77] ArduSub: allow for more than 327m range rangefinders --- ArduSub/Log.cpp | 6 +++--- ArduSub/Sub.h | 12 ++++++------ ArduSub/mode_surftrak.cpp | 4 ++-- ArduSub/sensors.cpp | 18 +++++++++--------- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 51d02ea560ac7..865fb215e9332 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -16,7 +16,7 @@ struct PACKED log_Control_Tuning { float inav_alt; float baro_alt; int16_t desired_rangefinder_alt; - int16_t rangefinder_alt; + float rangefinder_alt; float terr_alt; int16_t target_climb_rate; int16_t climb_rate; @@ -48,7 +48,7 @@ void Sub::Log_Write_Control_Tuning() inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : barometer.get_altitude(), desired_rangefinder_alt : (int16_t)mode_surftrak.get_rangefinder_target_cm(), - rangefinder_alt : rangefinder_state.alt_cm, + rangefinder_alt : rangefinder_state.alt, terr_alt : terr_alt, target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(), climb_rate : climb_rate @@ -264,7 +264,7 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target const struct LogStructure Sub::log_structure[] = { LOG_COMMON_STRUCTURES, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qfffffffccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" }, + "CTUN", "Qffffffffcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BB0BBB" }, { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t), "D16", "QBh", "TimeUS,Id,Value", "s--", "F--" }, { LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 91af53aa83292..bdf70e40d3fc1 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -145,15 +145,15 @@ class Sub : public AP_Vehicle { AP_LeakDetector leak_detector; struct { - bool enabled:1; - bool alt_healthy:1; // true if we can trust the altitude from the rangefinder - int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder - int16_t min_cm; // min rangefinder distance (in cm) - int16_t max_cm; // max rangefinder distance (in cm) + bool enabled; + bool alt_healthy; // true if we can trust the altitude from the rangefinder + float alt; // tilt compensated altitude from rangefinder + float min; // min rangefinder distance + float max; // max rangefinder distance uint32_t last_healthy_ms; float inertial_alt_cm; // inertial alt at time of last rangefinder sample float rangefinder_terrain_offset_cm; // terrain height above EKF origin - LowPassFilterFloat alt_cm_filt; // altitude filter + LowPassFilterFloat alt_filt; // altitude filter } rangefinder_state = { false, false, 0, 0, 0, 0, 0, 0 }; #if AP_RPM_ENABLED diff --git a/ArduSub/mode_surftrak.cpp b/ArduSub/mode_surftrak.cpp index f119e02a8981f..c5ac051040c80 100644 --- a/ArduSub/mode_surftrak.cpp +++ b/ArduSub/mode_surftrak.cpp @@ -67,9 +67,9 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm) sub.gcs().send_text(MAV_SEVERITY_WARNING, "wrong mode, rangefinder target not set"); } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) { sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to set rangefinder target", sub.g.surftrak_depth * 0.01f); - } else if (target_cm < (float)sub.rangefinder_state.min_cm) { + } else if (target_cm < sub.rangefinder_state.min*100) { sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target below minimum, ignored"); - } else if (target_cm > (float)sub.rangefinder_state.max_cm) { + } else if (target_cm > sub.rangefinder_state.max*100) { sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target above maximum, ignored"); } else { success = true; diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index f918e81a71f51..834595659a25e 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -20,7 +20,7 @@ void Sub::init_rangefinder() #if AP_RANGEFINDER_ENABLED rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN); rangefinder.init(ROTATION_PITCH_270); - rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); + rangefinder_state.alt_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270); #endif } @@ -39,30 +39,30 @@ void Sub::read_rangefinder() (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX) && (signal_quality_pct == -1 || signal_quality_pct >= g.rangefinder_signal_min); - int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270); + float temp_alt_m = rangefinder.distance_orient(ROTATION_PITCH_270); #if RANGEFINDER_TILT_CORRECTION // correct alt for angle of the rangefinder - temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); + temp_alt_m = temp_alt_m * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); #endif - rangefinder_state.alt_cm = temp_alt; + rangefinder_state.alt = temp_alt_m; rangefinder_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm(); - rangefinder_state.min_cm = rangefinder.min_distance_cm_orient(ROTATION_PITCH_270); - rangefinder_state.max_cm = rangefinder.max_distance_cm_orient(ROTATION_PITCH_270); + rangefinder_state.min = rangefinder.min_distance_orient(ROTATION_PITCH_270); + rangefinder_state.max = rangefinder.max_distance_orient(ROTATION_PITCH_270); // calculate rangefinder_terrain_offset_cm if (rangefinder_state.alt_healthy) { uint32_t now = AP_HAL::millis(); if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) { // reset filter if we haven't used it within the last second - rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm); + rangefinder_state.alt_filt.reset(rangefinder_state.alt); } else { - rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f); + rangefinder_state.alt_filt.apply(rangefinder_state.alt, 0.05f); } rangefinder_state.last_healthy_ms = now; rangefinder_state.rangefinder_terrain_offset_cm = - sub.rangefinder_state.inertial_alt_cm - sub.rangefinder_state.alt_cm_filt.get(); + sub.rangefinder_state.inertial_alt_cm - (sub.rangefinder_state.alt_filt.get() * 100); } // send rangefinder altitude and health to waypoint navigation library From 1418935c87944aa914a07b560559e543472b80b0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 15:13:10 +1100 Subject: [PATCH 13/77] ArduPlane: allow for more than 327m range rangefinders --- ArduPlane/altitude.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 230a16d067d0e..e96e778b82fa5 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -719,10 +719,10 @@ void Plane::rangefinder_height_update(void) // to misconfiguration or a faulty sensor if (rangefinder_state.in_range_count < 10) { if (!is_equal(distance, rangefinder_state.last_distance) && - fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01f) { + fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_orient(rangefinder_orientation())) { rangefinder_state.in_range_count++; } - if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01*0.2) { + if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_orient(rangefinder_orientation())*0.2) { // changes by more than 20% of full range will reset counter rangefinder_state.in_range_count = 0; } From 2b2103b273e87ab1b71bef847ac1c28d43a3c039 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 15:13:10 +1100 Subject: [PATCH 14/77] AP_Frsky_Telem: allow for more than 327m range rangefinders --- libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index c6e69d626c3e2..fbe31d6b1ad62 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -696,7 +696,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void) attiandrng |= ((uint16_t)roundf((pitch * RAD_TO_DEG * 100 + 9000) * 0.05f) & ATTIANDRNG_PITCH_LIMIT)<distance_cm_orient(ROTATION_PITCH_270) : 0, 3, 1)<distance_orient(ROTATION_PITCH_270)*100 : 0, 3, 1)< Date: Fri, 11 Oct 2024 15:13:10 +1100 Subject: [PATCH 15/77] AP_SurfaceDistance: allow for more than 327m range rangefinders --- libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp | 2 +- libraries/AP_SurfaceDistance/AP_SurfaceDistance.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp index af8ec0b42f091..9dfdb232d55d7 100644 --- a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp +++ b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp @@ -69,7 +69,7 @@ void AP_SurfaceDistance::update() } // tilt corrected but unfiltered, not glitch protected alt - alt_cm = tilt_correction * rangefinder->distance_cm_orient(rotation); + alt_cm = tilt_correction * rangefinder->distance_orient(rotation)*100; // remember inertial alt to allow us to interpolate rangefinder inertial_alt_cm = inertial_nav.get_position_z_up_cm(); diff --git a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h index 1b331a30810c1..c8aefad62e0cd 100644 --- a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h +++ b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h @@ -26,10 +26,10 @@ class AP_SurfaceDistance { bool enabled; // not to be confused with rangefinder enabled, this state is to be set by the vehicle. bool alt_healthy; // true if we can trust the altitude from the rangefinder - int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder + int32_t alt_cm; // tilt compensated altitude (in cm) from rangefinder float inertial_alt_cm; // inertial alt at time of last rangefinder sample LowPassFilterFloat alt_cm_filt {0.5}; // altitude filter - int16_t alt_cm_glitch_protected; // last glitch protected altitude + int32_t alt_cm_glitch_protected; // last glitch protected altitude int8_t glitch_count; // non-zero number indicates rangefinder is glitching uint32_t glitch_cleared_ms; // system time glitch cleared float terrain_offset_cm; // filtered terrain offset (e.g. terrain's height above EKF origin) From 18f95ba41b957628bda07d1f61560e5907a82812 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 15:13:11 +1100 Subject: [PATCH 16/77] Tools: allow for more than 327m range rangefinders --- .../Parrot_Disco/Parrot_Disco.param | 4 +- Tools/Frame_params/deset-mapping-boat.param | 4 +- Tools/Frame_params/eLAB_EX700_AC34.param | 4 +- Tools/autotest/arducopter.py | 188 +++++++++++++++++- Tools/autotest/ardusub.py | 15 +- .../default_params/copter-optflow.parm | 6 +- .../default_params/copter-rangefinder.parm | 6 +- .../autotest/default_params/gazebo-iris.parm | 2 +- Tools/autotest/default_params/periph.parm | 2 +- Tools/autotest/default_params/quad-can.parm | 2 +- .../default_params/quadplane-can.parm | 2 +- Tools/autotest/default_params/sub.parm | 2 +- .../default_params/vee-gull_005.param | 12 +- Tools/autotest/models/Callisto.param | 2 +- Tools/autotest/quadplane.py | 2 +- Tools/autotest/vehicle_test_suite.py | 6 +- 16 files changed, 216 insertions(+), 43 deletions(-) diff --git a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param index 0d26d2c7c84cf..9f88fe5e11f08 100644 --- a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param +++ b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param @@ -43,8 +43,8 @@ RLL_RATE_P 0.040758 RLL2SRV_RMAX 75.000000 RLL2SRV_TCONST 0.450000 RNGFND_LANDING 1 -RNGFND_MAX_CM 700 -RNGFND_MIN_CM 20 +RNGFND_MAX 7.00 +RNGFND_MIN 0.20 RNGFND_PIN -1 RNGFND_SCALING 1 RNGFND_TYPE 9 diff --git a/Tools/Frame_params/deset-mapping-boat.param b/Tools/Frame_params/deset-mapping-boat.param index 44b9baeea3f99..c9e84cf411aa7 100644 --- a/Tools/Frame_params/deset-mapping-boat.param +++ b/Tools/Frame_params/deset-mapping-boat.param @@ -25,8 +25,8 @@ MOT_SLEWRATE,25 MOT_VEC_ANGLEMAX,30 NAVL1_DAMPING,0.8 NAVL1_PERIOD,16 -RNGFND1_MAX_CM,20000 -RNGFND1_MIN_CM,0 +RNGFND1_MAX,200.00 +RNGFND1_MIN,0 RNGFND1_TYPE,17 SERIAL2_BAUD,19 SERIAL2_PROTOCOL,39 diff --git a/Tools/Frame_params/eLAB_EX700_AC34.param b/Tools/Frame_params/eLAB_EX700_AC34.param index 85fce24f5652e..e8453feb39e27 100644 --- a/Tools/Frame_params/eLAB_EX700_AC34.param +++ b/Tools/Frame_params/eLAB_EX700_AC34.param @@ -39,8 +39,8 @@ MOT_THST_HOVER,0.25 MOT_THST_EXPO,0.7 MOT_BAT_VOLT_MAX,25.2 MOT_BAT_VOLT_MIN,21.0 -RNGFND_MAX_CM,5000 -RNGFND_MIN_CM,5 +RNGFND_MAX,50.00 +RNGFND_MIN,0.05 RNGFND_SCALING,1 RNGFND_TYPE,8 PILOT_THR_BHV,1 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d11aaa1e636fa..07f920e6cd87a 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2870,7 +2870,7 @@ def EK3_RNG_USE_HGT(self): self.set_analog_rangefinder_parameters() # set use-height to 20m (the parameter is a percentage of max range) self.set_parameters({ - 'EK3_RNG_USE_HGT': 200000 / self.get_parameter('RNGFND1_MAX_CM'), + 'EK3_RNG_USE_HGT': (20 / self.get_parameter('RNGFND1_MAX')) * 100, }) self.reboot_sitl() @@ -3044,7 +3044,7 @@ def CANGPSCopterMission(self): "COMPASS_USE3" : 0, # use DroneCAN rangefinder "RNGFND1_TYPE" : 24, - "RNGFND1_MAX_CM" : 11000, + "RNGFND1_MAX" : 110.00, # use DroneCAN battery monitoring, and enforce with a arming voltage "BATT_MONITOR" : 8, "BATT_ARM_VOLT" : 12.0, @@ -4026,7 +4026,7 @@ def test_rangefinder_switchover(self): self.set_analog_rangefinder_parameters() self.set_parameters({ - "RNGFND1_MAX_CM": 1500 + "RNGFND1_MAX": 15.00 }) # configure EKF to use rangefinder for altitude at low altitudes @@ -4757,7 +4757,9 @@ def fly_guided_move_local(self, x, y, z_up, timeout=100): while True: if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Did not reach destination") - if self.distance_to_local_position((x, y, -z_up)) < 1: + dist = self.distance_to_local_position((x, y, -z_up)) + if dist < 1: + self.progress(f"Reach distance ({dist})") break def test_guided_local_position_target(self, x, y, z_up): @@ -8522,7 +8524,7 @@ def fly_rangefinder_mavlink_distance_sensor(self): self.set_parameter("SERIAL5_PROTOCOL", 1) self.set_parameter("RNGFND1_TYPE", 10) self.reboot_sitl() - self.set_parameter("RNGFND1_MAX_CM", 32767) + self.set_parameter("RNGFND1_MAX", 327.67) self.progress("Should be unhealthy while we don't send messages") self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, False) @@ -8787,10 +8789,10 @@ def MaxBotixI2CXL(self): self.set_parameters({ "RNGFND1_TYPE": 2, # maxbotix "RNGFND1_ADDR": 0x70, - "RNGFND1_MIN_CM": 150, + "RNGFND1_MIN": 1.50, "RNGFND2_TYPE": 2, # maxbotix "RNGFND2_ADDR": 0x71, - "RNGFND2_MIN_CM": 250, + "RNGFND2_MIN": 2.50, }) self.reboot_sitl() self.do_timesync_roundtrip() @@ -8932,6 +8934,176 @@ def RangeFinderDriversMaxAlt(self): self.do_RTL() + def RangeFinderDriversLongRange(self): + '''test rangefinder above 327m''' + # FIXME: when we get a driver+simulator for a rangefinder with + # >327m range change this to use that driver + self.set_parameters({ + "SERIAL4_PROTOCOL": 9, + "RNGFND1_TYPE": 19, # BenewakeTF02 + "WPNAV_SPEED_UP": 1000, # cm/s + }) + self.customise_SITL_commandline([ + "--serial4=sim:benewake_tf02", + ]) + + text_good = "GOOD" + text_out_of_range_high = "OUT_OF_RANGE_HIGH" + + rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION + + alt = 3 + self.takeoff(alt, mode='GUIDED') + self.assert_parameter_value("RNGFND1_MAX", 7.00) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + + self.send_statustext(text_good) + + alt = 10 + self.fly_guided_move_local(0, 0, alt) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + self.send_statustext(text_out_of_range_high) + + self.progress("Moving the goalposts") + self.set_parameter("RNGFND1_MAX", 20) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + self.send_statustext(text_good) + self.delay_sim_time(2) + + dfreader = self.dfreader_for_current_onboard_log() + + self.do_RTL() + + self.progress("Checking in/out of range markers in log") + required_range = None + count = 0 + while True: + m = dfreader.recv_match(type=["MSG", "RFND"]) + if m is None: + break + m_type = m.get_type() + if m_type == "MSG": + for t in [text_good, text_out_of_range_high]: + if t in m.Message: + required_range = t + continue + if m_type == "RFND": + if required_range is None: + continue + if required_range == text_good: + required_state = 4 + elif required_range == text_out_of_range_high: + required_state = 3 + else: + raise ValueError(f"Unexpected text {required_range}") + if m.Stat != required_state: + raise NotAchievedException(f"Not in expected state want={required_state} got={m.Stat} dist={m.Dist}") + self.progress(f"In expected state {required_range}") + required_range = None + count += 1 + + if count < 3: + raise NotAchievedException("Did not see range markers") + + def RangeFinderSITLLongRange(self): + '''test rangefinder above 327m''' + # FIXME: when we get a driver+simulator for a rangefinder with + # >327m range change this to use that driver + self.set_parameters({ + "RNGFND1_TYPE": 100, # SITL + "WPNAV_SPEED_UP": 1000, # cm/s + "RNGFND1_MAX": 7000, # metres + }) + self.reboot_sitl() + + text_good = "GOOD" + text_high = "HIGH" + + rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION + + alt = 3 + self.takeoff(alt, mode='GUIDED') + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + got = m.current_distance * 0.01 + if abs(got - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range {got=}") + + self.send_statustext(text_good) + + alt = 635 + self.fly_guided_move_local(0, 0, alt) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + + self.send_statustext(text_high) + + dfreader = self.dfreader_for_current_onboard_log() + + # fast descent! + def hook(msg, m): + if m.get_type() != 'HEARTBEAT': + return + # tell vehicle to only pay attention to the attitude: + bitmask = 0 + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE + + self.mav.mav.set_attitude_target_send( + 0, # time_boot_ms + 1, # target sysid + 1, # target compid + 0, # bitmask of things to ignore + mavextra.euler_to_quat([ + math.radians(-180), + math.radians(0), + math.radians(0)]), # att + 0, # roll rate (rad/s) + 0, # pitch rate (rad/s) + 0, # yaw rate (rad/s) + 1 # thrust, 0 to 1, translated to a climb/descent rate + ) + + self.install_message_hook_context(hook) + + self.wait_altitude(0, 30, timeout=200, relative=True) + self.do_RTL() + + self.progress("Checking in/out of range markers in log") + good = False + while True: + m = dfreader.recv_match(type=["MSG", "RFND"]) + if m is None: + break + m_type = m.get_type() + if m_type == "RFND": + if m.Dist < 600: + continue + good = True + break + + if not good: + raise NotAchievedException("Did not see good alt") + def ShipTakeoff(self): '''Fly Simulated Ship Takeoff''' # test ship takeoff @@ -10876,6 +11048,8 @@ def tests1e(self): self.ModeFollow, self.RangeFinderDrivers, self.RangeFinderDriversMaxAlt, + self.RangeFinderDriversLongRange, + self.RangeFinderSITLLongRange, self.MaxBotixI2CXL, self.MAVProximity, self.ParameterValidation, diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 5947640790b44..88b498721f6c8 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -16,7 +16,6 @@ import vehicle_test_suite from vehicle_test_suite import NotAchievedException from vehicle_test_suite import AutoTestTimeoutException -from vehicle_test_suite import PreconditionFailedException if sys.version_info[0] < 3: ConnectionResetError = AutoTestTimeoutException @@ -196,8 +195,8 @@ def RngfndQuality(self): "SCR_ENABLE": 1, "RNGFND1_TYPE": 36, "RNGFND1_ORIENT": 25, - "RNGFND1_MIN_CM": 10, - "RNGFND1_MAX_CM": 5000, + "RNGFND1_MIN": 0.10, + "RNGFND1_MAX": 50.00, }) self.install_example_script_context("rangefinder_quality_test.lua") @@ -244,8 +243,7 @@ def watch_distance_maintained(self, delta=0.3, timeout=5.0): def Surftrak(self): """Test SURFTRAK mode""" - if self.get_parameter('RNGFND1_MAX_CM') != 3000.0: - raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0) + self.assert_parameter_value('RNGFND1_MAX', 30) # Something closer to Bar30 noise self.context_push() @@ -292,8 +290,8 @@ def prepare_synthetic_seafloor_test(self, sea_floor_depth, rf_target): "SCR_ENABLE": 1, "RNGFND1_TYPE": 36, "RNGFND1_ORIENT": 25, - "RNGFND1_MIN_CM": 10, - "RNGFND1_MAX_CM": 3000, + "RNGFND1_MIN": 0.10, + "RNGFND1_MAX": 30.00, "SCR_USER1": 2, # Configuration bundle "SCR_USER2": sea_floor_depth, # Depth in meters "SCR_USER3": 101, # Output log records @@ -788,8 +786,7 @@ def MAV_CMD_DO_REPOSITION(self): def TerrainMission(self): """Mission using surface tracking""" - if self.get_parameter('RNGFND1_MAX_CM') != 3000.0: - raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0) + self.assert_parameter_value('RNGFND1_MAX', 30) filename = "terrain_mission.txt" self.progress("Executing mission %s" % filename) diff --git a/Tools/autotest/default_params/copter-optflow.parm b/Tools/autotest/default_params/copter-optflow.parm index 162c037074101..90f4ee41937c6 100644 --- a/Tools/autotest/default_params/copter-optflow.parm +++ b/Tools/autotest/default_params/copter-optflow.parm @@ -5,8 +5,8 @@ EK3_SRC1_VELXY 5 EK3_SRC1_VELZ 0 FLOW_TYPE 10 RNGFND1_TYPE 1 -RNGFND1_MIN_CM 0 -RNGFND1_MAX_CM 4000 +RNGFND1_MIN 0 +RNGFND1_MAX 40.00 RNGFND1_PIN 0 RNGFND1_SCALING 12.12 -SIM_FLOW_ENABLE 1 \ No newline at end of file +SIM_FLOW_ENABLE 1 diff --git a/Tools/autotest/default_params/copter-rangefinder.parm b/Tools/autotest/default_params/copter-rangefinder.parm index 0005a89571eea..1963fe33b4987 100644 --- a/Tools/autotest/default_params/copter-rangefinder.parm +++ b/Tools/autotest/default_params/copter-rangefinder.parm @@ -1,5 +1,5 @@ RNGFND1_TYPE 1 -RNGFND1_MIN_CM 0 -RNGFND1_MAX_CM 4000 +RNGFND1_MIN 0 +RNGFND1_MAX 40.00 RNGFND1_PIN 0 -RNGFND1_SCALING 12.12 \ No newline at end of file +RNGFND1_SCALING 12.12 diff --git a/Tools/autotest/default_params/gazebo-iris.parm b/Tools/autotest/default_params/gazebo-iris.parm index 9e586a53fdd18..7fba567f3fdb0 100644 --- a/Tools/autotest/default_params/gazebo-iris.parm +++ b/Tools/autotest/default_params/gazebo-iris.parm @@ -10,4 +10,4 @@ SIM_SONAR_SCALE 10 RNGFND1_TYPE 1 RNGFND1_SCALING 10 RNGFND1_PIN 0 -RNGFND1_MAX_CM 5000 +RNGFND1_MAX 50.00 diff --git a/Tools/autotest/default_params/periph.parm b/Tools/autotest/default_params/periph.parm index d1674bb220f22..ca13f126fdbac 100644 --- a/Tools/autotest/default_params/periph.parm +++ b/Tools/autotest/default_params/periph.parm @@ -5,7 +5,7 @@ COMPASS_ENABLE 1 BARO_ENABLE 1 ARSPD_TYPE 100 RNGFND1_TYPE 100 -RNGFND1_MAX_CM 12000 +RNGFND1_MAX 120.00 BATT_MONITOR 16 BATT_I2C_BUS 2 diff --git a/Tools/autotest/default_params/quad-can.parm b/Tools/autotest/default_params/quad-can.parm index 2f6df0c33ba05..90711c5bed888 100644 --- a/Tools/autotest/default_params/quad-can.parm +++ b/Tools/autotest/default_params/quad-can.parm @@ -4,5 +4,5 @@ SIM_CAN_SRV_MSK 0xFf SIM_VIB_MOT_MAX 270 GPS1_TYPE 9 RNGFND1_TYPE 24 -RNGFND1_MAX_CM 11000 +RNGFND1_MAX 110.00 BATT_MONITOR 8 diff --git a/Tools/autotest/default_params/quadplane-can.parm b/Tools/autotest/default_params/quadplane-can.parm index 8bb2195a7f783..2596901289a66 100644 --- a/Tools/autotest/default_params/quadplane-can.parm +++ b/Tools/autotest/default_params/quadplane-can.parm @@ -8,6 +8,6 @@ SIM_VIB_MOT_MAX 270 GPS1_TYPE 9 ARSPD_TYPE 8 RNGFND1_TYPE 24 -RNGFND1_MAX_CM 11000 +RNGFND1_MAX 110.00 RNGFND_LANDING 1 BATT_MONITOR 8 diff --git a/Tools/autotest/default_params/sub.parm b/Tools/autotest/default_params/sub.parm index 65615aa5f2abd..bf0b9ddfafd00 100644 --- a/Tools/autotest/default_params/sub.parm +++ b/Tools/autotest/default_params/sub.parm @@ -75,7 +75,7 @@ PSC_POSXY_P 2.5 PSC_VELXY_D 0.8 PSC_VELXY_I 0.5 PSC_VELXY_P 5.0 -RNGFND1_MAX_CM 3000 +RNGFND1_MAX 30.00 RNGFND1_PIN 0 RNGFND1_SCALING 12.12 RNGFND1_TYPE 1 diff --git a/Tools/autotest/default_params/vee-gull_005.param b/Tools/autotest/default_params/vee-gull_005.param index 9415f055bdedc..c5305222954ab 100644 --- a/Tools/autotest/default_params/vee-gull_005.param +++ b/Tools/autotest/default_params/vee-gull_005.param @@ -465,10 +465,10 @@ RLL2SRV_RMAX,0 RLL2SRV_TCONST,0.5 RNGFND1_ADDR,0 RNGFND1_FUNCTION,0 -RNGFND1_GNDCLEAR,10 +RNGFND1_GNDCLR,0.01 RNGFND1_LANDING,0 -RNGFND1_MAX_CM,700 -RNGFND1_MIN_CM,20 +RNGFND1_MAX,7.00 +RNGFND1_MIN,0.20 RNGFND1_OFFSET,0 RNGFND1_ORIENT,25 RNGFND1_PIN,-1 @@ -483,9 +483,9 @@ RNGFND1_STOP_PIN,-1 RNGFND1_TYPE,0 RNGFND2_ADDR,0 RNGFND2_FUNCTION,0 -RNGFND2_GNDCLEAR,10 -RNGFND2_MAX_CM,700 -RNGFND2_MIN_CM,20 +RNGFND2_GNDCLR,0.01 +RNGFND2_MAX,7.00 +RNGFND2_MIN,0.20 RNGFND2_OFFSET,0 RNGFND2_ORIENT,25 RNGFND2_PIN,-1 diff --git a/Tools/autotest/models/Callisto.param b/Tools/autotest/models/Callisto.param index df57aea2e9064..b3789db7da7b0 100644 --- a/Tools/autotest/models/Callisto.param +++ b/Tools/autotest/models/Callisto.param @@ -55,7 +55,7 @@ PSC_ANGLE_MAX,45 PSC_JERK_Z,10 PSC_POSZ_P,0.5 PSC_VELZ_P,2.5 -RNGFND1_MAX_CM,10000 +RNGFND1_MAX,100.00 RNGFND1_PIN,0 RNGFND1_SCALING,12.1212 RNGFND1_TYPE,1 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 69fdd6eb0cb5b..8e2500a9e04c1 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1524,7 +1524,7 @@ def PrecisionLanding(self): "RNGFND1_TYPE": 100, "RNGFND1_PIN" : 0, "RNGFND1_SCALING" : 12.2, - "RNGFND1_MAX_CM" : 5000, + "RNGFND1_MAX" : 50.00, "RNGFND_LANDING" : 1, }) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index e2022f21b7e0b..9557c0f72c93d 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5823,8 +5823,8 @@ def send_mavlink_run_prearms_command(self): def analog_rangefinder_parameters(self): return { "RNGFND1_TYPE": 1, - "RNGFND1_MIN_CM": 0, - "RNGFND1_MAX_CM": 4000, + "RNGFND1_MIN": 0, + "RNGFND1_MAX": 40.00, "RNGFND1_SCALING": 12.12, "RNGFND1_PIN": 0, } @@ -7209,6 +7209,8 @@ def assert_rangefinder_distance_between(self, dist_min, dist_max): raise NotAchievedException("above max height (%f > %f)" % (m.distance, dist_max)) + self.progress(f"Rangefinder distance {m.distance} is between {dist_min} and {dist_max}") + def assert_distance_sensor_quality(self, quality): m = self.assert_receive_message('DISTANCE_SENSOR') From 9ba3206097bfb651cc3012f65ce4a21ec779768b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 24 Dec 2024 08:24:26 +1100 Subject: [PATCH 17/77] Sub: log desired rangefinder alt (DSAlt) in metres not cm --- ArduSub/Log.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 865fb215e9332..0da4731a153e3 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -15,7 +15,7 @@ struct PACKED log_Control_Tuning { float desired_alt; float inav_alt; float baro_alt; - int16_t desired_rangefinder_alt; + float desired_rangefinder_alt; float rangefinder_alt; float terr_alt; int16_t target_climb_rate; @@ -47,7 +47,7 @@ void Sub::Log_Write_Control_Tuning() desired_alt : pos_control.get_pos_target_z_cm() * 0.01f, inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : barometer.get_altitude(), - desired_rangefinder_alt : (int16_t)mode_surftrak.get_rangefinder_target_cm(), + desired_rangefinder_alt : mode_surftrak.get_rangefinder_target_cm() * 0.01, rangefinder_alt : rangefinder_state.alt, terr_alt : terr_alt, target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(), @@ -264,7 +264,7 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target const struct LogStructure Sub::log_structure[] = { LOG_COMMON_STRUCTURES, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qffffffffcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BB0BBB" }, + "CTUN", "Qffffffffffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B00BBB" }, { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t), "D16", "QBh", "TimeUS,Id,Value", "s--", "F--" }, { LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t), From 98dbcd5d08e772f5453481db047bb45fe86ade6a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 24 Dec 2024 08:44:25 +1100 Subject: [PATCH 18/77] AP_Scripting: update examples to not use deprecated methods --- libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua | 2 +- libraries/AP_Scripting/applets/plane_package_place.lua | 2 +- libraries/AP_Scripting/applets/plane_precland.lua | 2 +- libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua | 2 +- libraries/AP_Scripting/examples/ahrs-source.lua | 2 +- libraries/AP_Scripting/examples/copter-wall-climber.lua | 2 +- libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua | 2 +- libraries/AP_Scripting/examples/land_hagl.lua | 2 +- libraries/AP_Scripting/examples/rangefinder_quality_test.lua | 4 ++-- 9 files changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua b/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua index 7b980ce88fa31..7f4fac241e567 100644 --- a/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua +++ b/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua @@ -184,7 +184,7 @@ function update() -- get rangefinder distance local rngfnd_distance_m = 0 if rangefinder:has_data_orient(rangefinder_rotation) then - rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_rotation) * 0.01 + rngfnd_distance_m = rangefinder:distance_orient(rangefinder_rotation) end local rngfnd_over_threshold = (rngfnd_distance_m == 0) or (rngfnd_distance_m > ESRC_RNGFND_MAX:get()) diff --git a/libraries/AP_Scripting/applets/plane_package_place.lua b/libraries/AP_Scripting/applets/plane_package_place.lua index 360d958a1b0ca..7985d107c16bf 100644 --- a/libraries/AP_Scripting/applets/plane_package_place.lua +++ b/libraries/AP_Scripting/applets/plane_package_place.lua @@ -108,7 +108,7 @@ function update() -- check the distance, if less than RNG_ORIENT_DOWN then release local dist_m if not landed then - dist_m = rangefinder:distance_cm_orient(RNG_ORIENT_DOWN) * 0.01 + dist_m = rangefinder:distance_orient(RNG_ORIENT_DOWN) else dist_m = 0.0 end diff --git a/libraries/AP_Scripting/applets/plane_precland.lua b/libraries/AP_Scripting/applets/plane_precland.lua index 1d5dc96cd8f00..74833af8b4610 100644 --- a/libraries/AP_Scripting/applets/plane_precland.lua +++ b/libraries/AP_Scripting/applets/plane_precland.lua @@ -166,7 +166,7 @@ local function update() --[[ get rangefinder distance, and if PLND_ALT_CUTOFF is set then stop precland operation if below the cutoff --]] - local rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_orient) * 0.01 + local rngfnd_distance_m = rangefinder:distance_orient(rangefinder_orient) if PLND_ALT_CUTOFF:get() > 0 and rngfnd_distance_m < PLND_ALT_CUTOFF:get() then return end diff --git a/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua b/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua index 87d9bf3a232fc..4d1634fd3f4ff 100644 --- a/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua +++ b/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua @@ -121,7 +121,7 @@ function update() -- get rangefinder distance local rngfnd_distance_m = 0 if rangefinder:has_data_orient(rangefinder_rotation) then - rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_rotation) * 0.01 + rngfnd_distance_m = rangefinder:distance_orient(rangefinder_rotation) end local rngfnd_over_threshold = (rngfnd_distance_m == 0) or (rngfnd_distance_m > rangefinder_thresh_dist) diff --git a/libraries/AP_Scripting/examples/ahrs-source.lua b/libraries/AP_Scripting/examples/ahrs-source.lua index d65104dfea40c..9eb050e48fc8e 100644 --- a/libraries/AP_Scripting/examples/ahrs-source.lua +++ b/libraries/AP_Scripting/examples/ahrs-source.lua @@ -85,7 +85,7 @@ function update() -- get rangefinder distance local rngfnd_distance_m = 0 if rangefinder:has_data_orient(rangefinder_rotation) then - rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_rotation) * 0.01 + rngfnd_distance_m = rangefinder:distance_orient(rangefinder_rotation) end local rngfnd_over_threshold = (rngfnd_distance_m == 0) or (rngfnd_distance_m > rangefinder_thresh_dist) diff --git a/libraries/AP_Scripting/examples/copter-wall-climber.lua b/libraries/AP_Scripting/examples/copter-wall-climber.lua index 0028776c30442..714c9344896ba 100644 --- a/libraries/AP_Scripting/examples/copter-wall-climber.lua +++ b/libraries/AP_Scripting/examples/copter-wall-climber.lua @@ -140,7 +140,7 @@ function update() if (climb_mode == 1) then -- convert rangefinder distance to pitch speed if rangefinder:has_data_orient(0) then - local distance_m = rangefinder:distance_cm_orient(0) * 0.01 + local distance_m = rangefinder:distance_orient(0) wall_pitch_speed = (distance_m - wall_dist_target) * wall_dist_to_speed_P wall_pitch_speed = math.min(wall_pitch_speed, wall_pitch_speed_max) wall_pitch_speed = math.max(wall_pitch_speed, -wall_pitch_speed_max) diff --git a/libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua b/libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua index 582e0ce08967b..9d6b53a271674 100644 --- a/libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua +++ b/libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua @@ -61,7 +61,7 @@ function sample_rangefinder_to_get_AGL() end -- we're actively sampling rangefinder distance to ground - local distance_raw_m = rangefinder:distance_cm_orient(ROTATION_PITCH_270) * 0.01 + local distance_raw_m = rangefinder:distance_orient(ROTATION_PITCH_270) -- correct the range for attitude (multiply by DCM.c.z, which is cos(roll)*cos(pitch)) local ahrs_get_rotation_body_to_ned_c_z = math.cos(ahrs:get_roll())*math.cos(ahrs:get_pitch()) diff --git a/libraries/AP_Scripting/examples/land_hagl.lua b/libraries/AP_Scripting/examples/land_hagl.lua index 3bdbc36329aea..d630633cff4f5 100644 --- a/libraries/AP_Scripting/examples/land_hagl.lua +++ b/libraries/AP_Scripting/examples/land_hagl.lua @@ -33,7 +33,7 @@ local function send_HAGL() last_active = false return end - local rangefinder_dist = rangefinder:distance_cm_orient(RANGEFINDER_ORIENT)*0.01 + local rangefinder_dist = rangefinder:distance_orient(RANGEFINDER_ORIENT) local correction = math.cos(ahrs:get_roll())*math.cos(ahrs:get_pitch()) local rangefinder_corrected = rangefinder_dist * correction if RANGEFINDER_ORIENT == ROTATION_PITCH_90 then diff --git a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua index 1767b6bb8540d..4c93bf4ac289d 100644 --- a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua +++ b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua @@ -88,12 +88,12 @@ local function get_and_eval(test_idx, dist_m_in, signal_quality_pct_in, status_e -- L U A I N T E R F A C E T E S T -- Check that the distance and signal_quality from the frontend are as expected - local distance1_cm_out = rangefinder:distance_cm_orient(RNGFND_ORIENTATION_DOWN) + local distance1_cm_out = rangefinder:distance_orient(RNGFND_ORIENTATION_DOWN) * 100 local signal_quality1_pct_out = rangefinder:signal_quality_pct_orient(RNGFND_ORIENTATION_DOWN) -- Make sure data was returned if not distance1_cm_out or not signal_quality1_pct_out then - return "No data returned from rangefinder:distance_cm_orient()" + return "No data returned from rangefinder:distance_orient()" end send(string.format("Frontend test %i dist in_m: %.2f out_cm: %.2f, signal_quality_pct in: %.1f out: %.1f", From aff20db1a169343605a3635184323fbc07f49b3c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Dec 2024 09:31:13 +1100 Subject: [PATCH 19/77] autotest: correct copter-tailsitter parameter file for RNGFND1_MIN_CM rename also fix a race condition in the quadplane-tailsitter test - which is unlikely to ever trigger --- .../autotest/default_params/quadplane-copter_tailsitter.parm | 4 ++-- Tools/autotest/quadplane.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/default_params/quadplane-copter_tailsitter.parm b/Tools/autotest/default_params/quadplane-copter_tailsitter.parm index a936774c6ce33..901bd7eda8d6a 100644 --- a/Tools/autotest/default_params/quadplane-copter_tailsitter.parm +++ b/Tools/autotest/default_params/quadplane-copter_tailsitter.parm @@ -38,8 +38,8 @@ SIM_SONAR_SCALE 10 RNGFND1_TYPE 100 RNGFND1_PIN 0 RNGFND1_SCALING 10 -RNGFND1_MIN_CM 10 -RNGFND1_MAX_CM 5000 +RNGFND1_MIN 0.10 +RNGFND1_MAX 50 RNGFND1_ORIENT 12 RNGFND_LANDING 1 RNGFND_LND_ORNT 12 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 8e2500a9e04c1..4583846883863 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1077,7 +1077,7 @@ def CopterTailsitter(self): self.context_collect("STATUSTEXT") self.progress("Starting QLAND") self.change_mode("QLAND") - self.wait_statustext("Rangefinder engaged") + self.wait_statustext("Rangefinder engaged", check_context=True) self.wait_disarmed(timeout=100) def setup_ICEngine_vehicle(self): From 7c101a0846b2e3b96d9d2e198d9d37413371d24a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 1 Jan 2025 13:57:57 +1100 Subject: [PATCH 20/77] Replay: add example script for converting from cm to m in RRNI and RRNH messages --- .../examples/rewrite-RFND-values-to-floats.py | 132 ++++++++++++++++++ .../AP_RangeFinder/AP_RangeFinder_MAVLink.cpp | 4 +- 2 files changed, 134 insertions(+), 2 deletions(-) create mode 100755 Tools/Replay/examples/rewrite-RFND-values-to-floats.py diff --git a/Tools/Replay/examples/rewrite-RFND-values-to-floats.py b/Tools/Replay/examples/rewrite-RFND-values-to-floats.py new file mode 100755 index 0000000000000..7688d30359dc3 --- /dev/null +++ b/Tools/Replay/examples/rewrite-RFND-values-to-floats.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python +'''In ArduPilot 4.7 the distance values were moved to float to +facilitate supporting rangefinders with more than 327m of range. + +The dataflash Replay log messages changes to support this. + +This script will take an older log and convert those messages such +that older logs can be replayed with newer code. + +AP_FLAKE8_CLEAN +''' + +from argparse import ArgumentParser + +import struct + +from pymavlink import DFReader +from pymavlink import mavutil + +new_RRNH_format = "ffB" +new_RRNI_format = "ffffBBB" + + +class Rewrite(): + def __init__(self, rewrite_fmt, rewrite_fmtu, rewrite_instance): + self.rewrite_fmt = rewrite_fmt + self.rewrite_fmtu = rewrite_fmtu + self.rewrite_instance = rewrite_instance + + def format_to_struct(fmt): + ret = bytes("<", 'ascii') + for c in fmt: + (s, mul, type) = DFReader.FORMAT_TO_STRUCT[c] + ret += bytes(s, 'ascii') + return bytes(ret) + + +rewrites = { + # convert RRNI.distance_cm > RRNI.distance + "RRNH": Rewrite( + rewrite_fmt=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct("BBnNZ"), + m.Type, + struct.calcsize(Rewrite.format_to_struct(new_RRNH_format)) + 3, # m.Length + bytes(m.Name, 'ascii'), + bytes(new_RRNH_format, 'ascii'), + bytes(m.Columns, 'ascii') + ), + rewrite_fmtu=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct("QBNN"), + m.TimeUS, + m.FmtType, + bytes("mm-", 'ascii'), # new units + bytes("00-", 'ascii') # new mults + ), + rewrite_instance=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct(new_RRNH_format), + m.GCl * 0.01, + m.MaxD * 0.01, + m.NumSensors + ), + ), + "RRNI": Rewrite( + rewrite_fmt=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct("BBnNZ"), + m.Type, + struct.calcsize(Rewrite.format_to_struct(new_RRNI_format)) + 3, # m.Length + bytes(m.Name, 'ascii'), + bytes(new_RRNI_format, 'ascii'), + bytes(m.Columns, 'ascii') + ), + rewrite_fmtu=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct("QBNN"), + m.TimeUS, + m.FmtType, + bytes("???m--#", 'ascii'), # new units + bytes("???0---", 'ascii') # new mults + ), + rewrite_instance=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct(new_RRNI_format), + m.PX, + m.PY, + m.PZ, + m.Dist * 0.01, + m.Orient, + m.Status, + m.I + ), + ), +} + + +parser = ArgumentParser(description=__doc__) + +parser.add_argument("login") +parser.add_argument("logout") + +args = parser.parse_args() + +login = mavutil.mavlink_connection(args.login) +output = open(args.logout, mode='wb') + +type_name_map = {} + + +def rewrite_message(m): + buf = bytearray(m.get_msgbuf()) + + mtype = m.get_type() + if mtype == 'FMT': + type_name_map[m.Type] = m.Name + if m.Name in rewrites: + return rewrites[m.Name].rewrite_fmt(buf, m) + + if mtype == 'FMTU': + if m.FmtType not in type_name_map: + raise ValueError(f"Have not seen format for ID {m.FmtType}") + name = type_name_map[m.FmtType] + if name in rewrites: + return rewrites[name].rewrite_fmtu(buf, m) + + if mtype in rewrites: + return rewrites[mtype].rewrite_instance(buf, m) + + return buf + + +while True: + m = login.recv_msg() + if m is None: + break + output.write(rewrite_message(m)) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index 63e7642bc1ea1..2a1a1a36ce639 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -67,9 +67,9 @@ float AP_RangeFinder_MAVLink::min_distance() const return baseclass_min_distance; } - // return the smaller of the base class's distance and what we + // return the larger of the base class's distance and what we // receive from the network: - return MIN(baseclass_min_distance, _min_distance); + return MAX(baseclass_min_distance, _min_distance); } /* From 25e8277fdaa95732a664a9ab0a0473ca5c50f533 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 12 Jun 2023 11:27:24 -0400 Subject: [PATCH 21/77] AP_HAL_ChibiOS: add support for littlefs --- libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h | 2 +- libraries/AP_HAL_ChibiOS/Storage.cpp | 2 +- .../AP_HAL_ChibiOS/hwdef/common/malloc.c | 4 +-- .../hwdef/scripts/chibios_hwdef.py | 27 ++++++++++++++++--- libraries/AP_HAL_ChibiOS/sdcard.cpp | 10 +++---- 5 files changed, 32 insertions(+), 13 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h index 4ed88397bf1df..4f0ff4ca8c1f2 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h @@ -21,7 +21,7 @@ #include #include #include "hwdef/common/halconf.h" -#ifdef USE_POSIX +#ifdef USE_POSIX_FATFS #include #endif #include diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index dc0ce2f0740d9..39f4540cc0b48 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -128,7 +128,7 @@ void Storage::_storage_open(void) void Storage::_save_backup(void) { #ifdef USE_POSIX - // allow for fallback to microSD based storage + // allow for fallback to microSD or dataflash based storage // create the backup directory if need be int ret; const char* _storage_bak_directory = HAL_STORAGE_BACKUP_FOLDER; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c index 97b837b12a480..b733b20e71f9c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -585,7 +585,7 @@ void __wrap__free_r(void *rptr, void *ptr) return free(ptr); } -#ifdef USE_POSIX +#ifdef USE_POSIX_FATFS /* allocation functions for FATFS */ @@ -615,7 +615,7 @@ void ff_memfree(void* mblock) { free(mblock); } -#endif // USE_POSIX +#endif // USE_POSIX_FATFS /* return true if a memory region is safe for a DMA operation diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 211e4443c76c0..e9f227d6358b6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -762,6 +762,13 @@ def enable_can(self, f): f.write('#define HAL_CANFD_SUPPORTED %d\n' % canfd_supported) self.env_vars['HAL_CANFD_SUPPORTED'] = canfd_supported + def has_dataflash_spi(self): + '''check for dataflash connected to spi bus''' + for dev in self.spidev: + if(dev[0] == 'dataflash'): + return True + return False + def has_sdcard_spi(self): '''check for sdcard connected to spi bus''' for dev in self.spidev: @@ -943,13 +950,15 @@ def write_mcu_config(self, f): f.write('#define HAL_STDOUT_BAUDRATE %u\n\n' % self.get_config('STDOUT_BAUDRATE', type=int)) if self.have_type_prefix('SDIO'): f.write('// SDIO available, enable POSIX filesystem support\n') - f.write('#define USE_POSIX\n\n') + f.write('#define USE_POSIX\n') + f.write('#define USE_POSIX_FATFS\n\n') f.write('#define HAL_USE_SDC TRUE\n') self.build_flags.append('USE_FATFS=yes') self.env_vars['WITH_FATFS'] = "1" elif self.have_type_prefix('SDMMC2'): f.write('// SDMMC2 available, enable POSIX filesystem support\n') - f.write('#define USE_POSIX\n\n') + f.write('#define USE_POSIX\n') + f.write('#define USE_POSIX_FATFS\n\n') f.write('#define HAL_USE_SDC TRUE\n') f.write('#define STM32_SDC_USE_SDMMC2 TRUE\n') f.write('#define HAL_USE_SDMMC 1\n') @@ -957,7 +966,8 @@ def write_mcu_config(self, f): self.env_vars['WITH_FATFS'] = "1" elif self.have_type_prefix('SDMMC'): f.write('// SDMMC available, enable POSIX filesystem support\n') - f.write('#define USE_POSIX\n\n') + f.write('#define USE_POSIX\n') + f.write('#define USE_POSIX_FATFS\n\n') f.write('#define HAL_USE_SDC TRUE\n') f.write('#define STM32_SDC_USE_SDMMC1 TRUE\n') f.write('#define HAL_USE_SDMMC 1\n') @@ -965,12 +975,21 @@ def write_mcu_config(self, f): self.env_vars['WITH_FATFS'] = "1" elif self.has_sdcard_spi(): f.write('// MMC via SPI available, enable POSIX filesystem support\n') - f.write('#define USE_POSIX\n\n') + f.write('#define USE_POSIX\n') + f.write('#define USE_POSIX_FATFS\n\n') f.write('#define HAL_USE_MMC_SPI TRUE\n') f.write('#define HAL_USE_SDC FALSE\n') f.write('#define HAL_SDCARD_SPI_HOOK TRUE\n') self.build_flags.append('USE_FATFS=yes') self.env_vars['WITH_FATFS'] = "1" + elif self.has_dataflash_spi(): + f.write('// Dataflash memory via SPI available, enable POSIX filesystem support\n') + f.write('#define USE_POSIX\n') + f.write('#define USE_POSIX_LITTLEFS\n\n') + f.write('#define HAL_USE_SDC FALSE\n') + self.build_flags.append('USE_FATFS=no') + self.env_vars['WITH_LITTLEFS'] = "1" + self.env_vars['DISABLE_SCRIPTING'] = True else: f.write('#define HAL_USE_SDC FALSE\n') self.build_flags.append('USE_FATFS=no') diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index 922440826b06d..e2750960572be 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal; -#ifdef USE_POSIX +#ifdef USE_POSIX_FATFS static FATFS SDC_FS; // FATFS object #ifndef HAL_BOOTLOADER_BUILD static HAL_Semaphore sem; @@ -54,7 +54,7 @@ static SPIConfig highspeed; */ bool sdcard_init() { -#ifdef USE_POSIX +#ifdef USE_POSIX_FATFS #ifndef HAL_BOOTLOADER_BUILD WITH_SEMAPHORE(sem); @@ -150,7 +150,7 @@ bool sdcard_init() } #endif sdcard_running = false; -#endif // USE_POSIX +#endif // USE_POSIX_FATFS return false; } @@ -159,7 +159,7 @@ bool sdcard_init() */ void sdcard_stop(void) { -#ifdef USE_POSIX +#ifdef USE_POSIX_FATFS // unmount f_mount(nullptr, "/", 1); #endif @@ -185,7 +185,7 @@ void sdcard_stop(void) bool sdcard_retry(void) { -#ifdef USE_POSIX +#ifdef USE_POSIX_FATFS if (!sdcard_running) { if (sdcard_init()) { #if AP_FILESYSTEM_FILE_WRITING_ENABLED From 1faf1b3d9c1a303997f8998bc7f3311c088112eb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 29 Nov 2024 19:40:43 +0000 Subject: [PATCH 22/77] AP_HAL_ChibiOS: don't backup storage with littlefs --- libraries/AP_HAL_ChibiOS/Storage.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 39f4540cc0b48..2663403227878 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -42,8 +42,12 @@ extern const AP_HAL::HAL& hal; #endif #ifndef HAL_STORAGE_BACKUP_COUNT +#if AP_FILESYSTEM_LITTLEFS_ENABLED +#define HAL_STORAGE_BACKUP_COUNT 0 +#else #define HAL_STORAGE_BACKUP_COUNT 100 #endif +#endif #define STORAGE_FLASH_RETRIES 5 From 948a38c62392cbefe4e29889ca1da135dfcb3d66 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 12 Jun 2023 11:28:26 -0400 Subject: [PATCH 23/77] AP_HAL: add littlefs build option --- libraries/AP_HAL/AP_HAL_Boards.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index af233e5596d55..fb85427706ac7 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -214,6 +214,10 @@ #define HAL_OS_FATFS_IO 0 #endif +#ifndef HAL_OS_LITTLEFS_IO +#define HAL_OS_LITTLEFS_IO 0 +#endif + #ifndef HAL_BARO_DEFAULT #define HAL_BARO_DEFAULT HAL_BARO_NONE #endif From dc9c0fc165f22340f0a2705582341d97815d0efa Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 12 Jun 2023 11:30:02 -0400 Subject: [PATCH 24/77] waf: build in littlefs support --- Tools/ardupilotwaf/ardupilotwaf.py | 4 ++++ Tools/ardupilotwaf/littlefs.py | 25 +++++++++++++++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 Tools/ardupilotwaf/littlefs.py diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index c8634444135ec..1695be2465cdc 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -445,6 +445,10 @@ def ap_stlib(bld, **kw): kw['target'] = kw['name'] kw['source'] = [] + if 'use' not in kw: + kw['use'] = [] + kw['use'].append('littlefs') + bld.stlib(**kw) _created_program_dirs = set() diff --git a/Tools/ardupilotwaf/littlefs.py b/Tools/ardupilotwaf/littlefs.py new file mode 100644 index 0000000000000..864983e20ffab --- /dev/null +++ b/Tools/ardupilotwaf/littlefs.py @@ -0,0 +1,25 @@ +# encoding: utf-8 + +""" +Adds support for building littlefs as part of a Waf build +""" + +from waflib.Configure import conf + +def configure(cfg): + cfg.env.append_value('GIT_SUBMODULES', 'littlefs') + cfg.env.prepend_value('INCLUDES', [ + cfg.srcnode.abspath() + '/modules/littlefs/', + ]) + + +@conf +def littlefs(bld, **kw): + kw.update( + name='littlefs', + source=['modules/littlefs/lfs.c', 'modules/littlefs/lfs_util.c'], + target='littlefs', + defines=['LFS_NO_DEBUG'], + cflags=['-Wno-format', '-Wno-format-extra-args', '-Wno-shadow', '-Wno-unused-function', '-Wno-missing-declarations'] + ) + return bld.stlib(**kw) From 35b6dd9814f332f88528070bb542997ca3a76c71 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 12 Jun 2023 11:30:28 -0400 Subject: [PATCH 25/77] wscript: build in littlefs --- wscript | 3 +++ 1 file changed, 3 insertions(+) diff --git a/wscript b/wscript index fff0a563f2111..803f7dbaf57f9 100644 --- a/wscript +++ b/wscript @@ -565,6 +565,7 @@ def configure(cfg): if cfg.options.enable_benchmarks: cfg.load('gbenchmark') cfg.load('gtest') + cfg.load('littlefs') cfg.load('static_linking') cfg.load('build_summary') @@ -795,6 +796,8 @@ def _build_common_taskgens(bld): ap_libraries=bld.ap_get_all_libraries(), ) + bld.littlefs() + if bld.env.HAS_GTEST: bld.libgtest(cxxflags=['-include', 'ap_config.h']) From 44ca13c5f38925a062a5cae12d73c772e89a98f4 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 12 Jun 2023 11:30:57 -0400 Subject: [PATCH 26/77] Replay: account for littlefs when building --- Tools/Replay/wscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/Replay/wscript b/Tools/Replay/wscript index 0e3b9c28ba508..bdf08a411a5c6 100644 --- a/Tools/Replay/wscript +++ b/Tools/Replay/wscript @@ -7,7 +7,7 @@ def configure(cfg): cfg.env.HAL_GCS_ENABLED = 0 def build(bld): - if isinstance(bld.get_board(), boards.chibios) and bld.env['WITH_FATFS'] != '1': + if isinstance(bld.get_board(), boards.chibios) and bld.env['WITH_FATFS'] != '1' and bld.env['WITH_LITTLEFS'] != 1: # we need a filesystem for replay return From 514cad3c25f216326bd39868f42992ac1fc1c554 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 25 Nov 2024 21:15:04 +0000 Subject: [PATCH 27/77] AP_Bootloader: correct use usage --- Tools/AP_Bootloader/wscript | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Bootloader/wscript b/Tools/AP_Bootloader/wscript index 172db17626123..9e951d638f8c1 100644 --- a/Tools/AP_Bootloader/wscript +++ b/Tools/AP_Bootloader/wscript @@ -12,7 +12,7 @@ def build(bld): bld.ap_stlib( name= 'AP_Bootloader_libs', - use='dronecan', + use=['dronecan'], dynamic_source='modules/DroneCAN/libcanard/dsdlc_generated/src/**.c', ap_vehicle='AP_Bootloader', ap_libraries= flashiface_lib + [ @@ -27,7 +27,7 @@ def build(bld): # build external libcanard library bld.stlib(source='../../modules/DroneCAN/libcanard/canard.c', name='libcanard', - use='dronecan', + use=['dronecan'], target='libcanard') bld.ap_program( From bc94df6d54539f1fdaf90fb4b6348b8286f9fd01 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 25 Nov 2024 21:15:18 +0000 Subject: [PATCH 28/77] AP_Periph: correct use usage --- Tools/AP_Periph/wscript | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index a65b269c32f32..0d275bdf80a8d 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -91,7 +91,7 @@ def build(bld): dynamic_source='modules/DroneCAN/libcanard/dsdlc_generated/src/**.c', ap_vehicle='AP_Periph', ap_libraries= libraries, - use='dronecan', + use=['dronecan'], exclude_src=[ 'libraries/AP_HAL_ChibiOS/Storage.cpp' ] @@ -100,7 +100,7 @@ def build(bld): # build external libcanard library bld.stlib(source='../../modules/DroneCAN/libcanard/canard.c', name='libcanard', - use='dronecan', + use=['dronecan'], target='libcanard') bld.ap_program( From 2c20e112c6f5709d0d27839159b0d91dcda2508d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 29 Nov 2024 19:23:46 +0000 Subject: [PATCH 29/77] AP_Logger: sync littlefs writes on block boundaries optimize free space when using littlefs --- libraries/AP_Logger/AP_Logger.cpp | 10 +++++- libraries/AP_Logger/AP_Logger_File.cpp | 49 +++++++++++++++++++++----- libraries/AP_Logger/AP_Logger_File.h | 8 +++++ libraries/AP_Logger/AP_Logger_config.h | 2 +- 4 files changed, 59 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 60d4e4a1b2dd0..3d834946edf5c 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -56,6 +56,14 @@ extern const AP_HAL::HAL& hal; #define HAL_LOGGER_ARM_PERSIST 15 #endif +#ifndef HAL_LOGGER_MIN_MB_FREE +#if AP_FILESYSTEM_LITTLEFS_ENABLED +#define HAL_LOGGER_MIN_MB_FREE 1 +#else +#define HAL_LOGGER_MIN_MB_FREE 500 +#endif +#endif + #ifndef HAL_LOGGING_BACKENDS_DEFAULT # if HAL_LOGGING_FILESYSTEM_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) # define HAL_LOGGING_BACKENDS_DEFAULT Backend_Type::FILESYSTEM @@ -137,7 +145,7 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = { // @Units: MB // @Range: 10 1000 // @User: Standard - AP_GROUPINFO("_FILE_MB_FREE", 7, AP_Logger, _params.min_MB_free, 500), + AP_GROUPINFO("_FILE_MB_FREE", 7, AP_Logger, _params.min_MB_free, HAL_LOGGER_MIN_MB_FREE), // @Param: _FILE_RATEMAX // @DisplayName: Maximum logging rate for file backend diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 4be657072997d..71d8e8d605a42 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -941,6 +941,7 @@ void AP_Logger_File::io_timer(void) // least once per 2 seconds if data is available return; } + if (tnow - _free_space_last_check_time > _free_space_check_interval) { _free_space_last_check_time = tnow; last_io_operation = "disk_space_avail"; @@ -964,6 +965,7 @@ void AP_Logger_File::io_timer(void) const uint8_t *head = _writebuf.readptr(size); nbytes = MIN(nbytes, size); +#if !AP_FILESYSTEM_LITTLEFS_ENABLED // try to align writes on a 512 byte boundary to avoid filesystem reads if ((nbytes + _write_offset) % 512 != 0) { uint32_t ofs = (nbytes + _write_offset) % 512; @@ -971,7 +973,7 @@ void AP_Logger_File::io_timer(void) nbytes -= ofs; } } - +#endif last_io_operation = "write"; if (!write_fd_semaphore.take(1)) { return; @@ -980,6 +982,33 @@ void AP_Logger_File::io_timer(void) write_fd_semaphore.give(); return; } + +#if AP_FILESYSTEM_LITTLEFS_ENABLED + // see https://github.com/littlefs-project/littlefs/issues/564#issuecomment-2363032827 + // n = (N − w/8 ( popcount( N/(B − 2w/8) − 1) + 2))/(B − 2w/8)) + // off = N − ( B − 2w/8 ) n − w/8popcount( n ) +#define BLOCK_INDEX(N, B) \ + (N - sizeof(uint32_t) * (__builtin_popcount(N/(B - 2 * sizeof(uint32_t)) -1) + 2))/(B - 2 * sizeof(uint32_t)) + +#define BLOCK_OFFSET(N, B, n) \ + (N - (B - 2*sizeof(uint32_t)) * n - sizeof(uint32_t) * __builtin_popcount(n)) + + uint32_t blocksize = AP::FS().get_sync_size(); + uint32_t block_index = BLOCK_INDEX(_write_offset, blocksize); + uint32_t block_offset = BLOCK_OFFSET(_write_offset, blocksize, block_index); + bool end_of_block = false; + if (blocksize - block_offset <= nbytes) { + if (blocksize == block_offset) { + // exactly at the end of the block, sync and then write all the data + AP::FS().fsync(_write_fd); + } else { + // near the end of the block, fill in the remaining gap + nbytes = blocksize - block_offset; + end_of_block = true; + } + } +#endif // AP_FILESYSTEM_LITTLEFS_ENABLED + ssize_t nwritten = AP::FS().write(_write_fd, head, nbytes); last_io_operation = ""; if (nwritten <= 0) { @@ -999,16 +1028,20 @@ void AP_Logger_File::io_timer(void) _last_write_ms = tnow; _write_offset += nwritten; _writebuf.advance(nwritten); + /* - the best strategy for minimizing corruption on microSD cards - seems to be to write in 4k chunks and fsync the file on each - chunk, ensuring the directory entry is updated after each - write. + fsync on littlefs is extremely expensive (20% CPU on an H7) and not + required since the whole point of the filesystem is to avoid corruption */ +#if AP_FILESYSTEM_LITTLEFS_ENABLED + if (end_of_block) +#endif // AP_FILESYSTEM_LITTLEFS_ENABLED #if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE - last_io_operation = "fsync"; - AP::FS().fsync(_write_fd); - last_io_operation = ""; + { + last_io_operation = "fsync"; + AP::FS().fsync(_write_fd); + last_io_operation = ""; + } #endif #if AP_RTC_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/libraries/AP_Logger/AP_Logger_File.h b/libraries/AP_Logger/AP_Logger_File.h index 3cfa02b132edf..c465b96b3457a 100644 --- a/libraries/AP_Logger/AP_Logger_File.h +++ b/libraries/AP_Logger/AP_Logger_File.h @@ -14,8 +14,12 @@ #if HAL_LOGGING_FILESYSTEM_ENABLED #ifndef HAL_LOGGER_WRITE_CHUNK_SIZE +#if AP_FILESYSTEM_LITTLEFS_ENABLED +#define HAL_LOGGER_WRITE_CHUNK_SIZE 2048 +#else #define HAL_LOGGER_WRITE_CHUNK_SIZE 4096 #endif +#endif class AP_Logger_File : public AP_Logger_Backend { @@ -125,7 +129,11 @@ class AP_Logger_File : public AP_Logger_Backend // data and failures-to-boot. uint32_t _free_space_last_check_time; // milliseconds const uint32_t _free_space_check_interval = 1000UL; // milliseconds +#if AP_FILESYSTEM_LITTLEFS_ENABLED + const uint32_t _free_space_min_avail = 4096; // bytes +#else const uint32_t _free_space_min_avail = 8388608; // bytes +#endif // semaphore mediates access to the ringbuffer HAL_Semaphore semaphore; diff --git a/libraries/AP_Logger/AP_Logger_config.h b/libraries/AP_Logger/AP_Logger_config.h index a5a3508043c34..16553f2bde923 100644 --- a/libraries/AP_Logger/AP_Logger_config.h +++ b/libraries/AP_Logger/AP_Logger_config.h @@ -48,7 +48,7 @@ #endif #ifndef HAL_LOGGER_FILE_CONTENTS_ENABLED -#define HAL_LOGGER_FILE_CONTENTS_ENABLED HAL_LOGGING_FILESYSTEM_ENABLED +#define HAL_LOGGER_FILE_CONTENTS_ENABLED HAL_LOGGING_FILESYSTEM_ENABLED && !AP_FILESYSTEM_LITTLEFS_ENABLED #endif // range of IDs to allow for new messages during replay. It is very From 1396f347b7ab839297ee611b88a23ce3d7fc9ee9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 26 Jul 2023 17:10:20 +0200 Subject: [PATCH 30/77] AP_HAL_ChibiOS: littlefs for MambaH743v4 --- libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat index f3179a3cb4832..9a221ff10a7cd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat @@ -27,9 +27,6 @@ env OPTIMIZE -O2 STORAGE_FLASH_PAGE 1 define HAL_STORAGE_SIZE 32768 -# enable logging to dataflash -define HAL_LOGGING_DATAFLASH_ENABLED 1 - # one I2C bus I2C_ORDER I2C1 I2C2 @@ -183,7 +180,8 @@ IMU BMI270 SPI:bmi270 ROTATION_PITCH_180 IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180 IMU Invensensev3 SPI:icm42688_2 ROTATION_ROLL_180_YAW_270 -define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_W25NXX +define HAL_OS_LITTLEFS_IO 1 +define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX # DPS280 or SPL06 integrated on I2C bus BARO DPS280 I2C:0:0x76 @@ -195,5 +193,3 @@ define HAL_OSD_TYPE_DEFAULT 1 ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin define DEFAULT_NTF_LED_TYPES 257 - -define AP_SCRIPTING_ENABLED 0 From 5c03e27068f5f045afe3eb25b0b11b0d412cf452 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 29 Nov 2024 19:25:43 +0000 Subject: [PATCH 31/77] AP_HAL_ChibiOS: littlefs for MatekH7A3 --- libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat index cb578f84b2aec..006b6bc7fe612 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat @@ -36,6 +36,10 @@ define AP_FLASH_STORAGE_DOUBLE_PAGE 1 PA11 OTG_HS_DM OTG1 PA12 OTG_HS_DP OTG1 +# Debug pins, PA14/PA13 are shared with the LEDs so that needs to be disabled +#PA13 JTMS-SWDIO SWD +#PA14 JTCK-SWCLK SWD + # SPI1 for ICM42688P & MAX7456 PA5 SPI1_SCK SPI1 PA6 SPI1_MISO SPI1 @@ -156,12 +160,10 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin # --------------------- SD & FLASH ---------------------- SPIDEV sdcard SPI2 DEVID3 SD_CS MODE0 400*KHZ 25*MHZ -SPIDEV dataflash SPI2 DEVID1 FLASH_CS MODE3 32*MHZ 32*MHZ - -define HAL_LOGGING_DATAFLASH_ENABLED 1 -define HAL_OS_FATFS_IO 1 -define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_W25NXX +SPIDEV dataflash SPI2 DEVID1 FLASH_CS MODE3 104*MHZ 104*MHZ +define HAL_OS_LITTLEFS_IO 1 +define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX # ----------------- I2C compass & Baro ----------------- # no built-in compass, but probe the i2c bus for all possible From 3626f5fd0ceb0cd6703c7ed8b6e15f7126749de1 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 30 Nov 2024 17:00:49 +0000 Subject: [PATCH 32/77] AP_HAL_ChibiOS: use littlefs on KakuteH7Mini-Nand --- libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat index 5e3a7b1bdbd70..fd5eb4879da6b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat @@ -9,4 +9,6 @@ SPIDEV bmi270 SPI4 DEVID1 MPU6000_CS MODE3 1*MHZ 4*MHZ IMU BMI270 SPI:bmi270 ROTATION_PITCH_180_YAW_90 -define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_W25NXX +undef HAL_LOGGING_DATAFLASH_ENABLED +define HAL_OS_LITTLEFS_IO 1 +define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX From f706b8615e32e7b6d8c8c89ed86e103a4c592ad0 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 30 Nov 2024 10:20:04 +0000 Subject: [PATCH 33/77] AP_Scripting: allow scripting on 2Mb boards with littlefs --- libraries/AP_Scripting/AP_Scripting_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/AP_Scripting_config.h b/libraries/AP_Scripting/AP_Scripting_config.h index 768e751ca7bb1..09f067ae522a6 100644 --- a/libraries/AP_Scripting/AP_Scripting_config.h +++ b/libraries/AP_Scripting/AP_Scripting_config.h @@ -10,7 +10,7 @@ #if AP_SCRIPTING_ENABLED #include // enumerate all of the possible places we can read a script from. - #if !AP_FILESYSTEM_POSIX_ENABLED && !AP_FILESYSTEM_FATFS_ENABLED && !AP_FILESYSTEM_ESP32_ENABLED && !AP_FILESYSTEM_ROMFS_ENABLED + #if !AP_FILESYSTEM_POSIX_ENABLED && !AP_FILESYSTEM_FATFS_ENABLED && !AP_FILESYSTEM_ESP32_ENABLED && !AP_FILESYSTEM_ROMFS_ENABLED && !AP_FILESYSTEM_LITTLEFS_ENABLED #error "Scripting requires a filesystem" #endif #endif From 6390285598ed805db6b45918bd76d0dee33e016b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 12 Jun 2023 11:27:48 -0400 Subject: [PATCH 34/77] AP_Filesystem: add littlefs based filesystem support use correct read status for nor flash implement format on littlefs optimize device calls in littlefs flash usage check for fileops allowed in littlefs littlefs optimization and support for mtime --- libraries/AP_Filesystem/AP_Filesystem.cpp | 9 + libraries/AP_Filesystem/AP_Filesystem.h | 3 + .../AP_Filesystem_FlashMemory_LittleFS.cpp | 1111 +++++++++++++++++ .../AP_Filesystem_FlashMemory_LittleFS.h | 125 ++ .../AP_Filesystem/AP_Filesystem_backend.h | 3 + .../AP_Filesystem/AP_Filesystem_config.h | 13 +- 6 files changed, 1263 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp create mode 100644 libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index 63fdbee0556b0..b7240a6f97b5d 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -35,6 +35,9 @@ static AP_Filesystem_ESP32 fs_local; #elif AP_FILESYSTEM_POSIX_ENABLED #include "AP_Filesystem_posix.h" static AP_Filesystem_Posix fs_local; +#elif AP_FILESYSTEM_LITTLEFS_ENABLED +#include "AP_Filesystem_FlashMemory_LittleFS.h" +static AP_Filesystem_FlashMemory_LittleFS fs_local; #else static AP_Filesystem_Backend fs_local; int errno; @@ -319,6 +322,12 @@ void AP_Filesystem::unmount(void) return LOCAL_BACKEND.fs.unmount(); } +// if non-zero size at which syncs should be peformed, only used by flash fs +uint32_t AP_Filesystem::get_sync_size(void) const +{ + return LOCAL_BACKEND.fs.get_sync_size(); +} + /* Load a file's contents into memory. Returned object must be `delete`d to free the data. The data is guaranteed to be null-terminated such that it can be diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index cc8619ca52152..fe5214fd55809 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -128,6 +128,9 @@ class AP_Filesystem { // format filesystem. This is async, monitor get_format_status for progress bool format(void); + // if non-zero size at which syncs should be peformed, only used by flash fs + uint32_t get_sync_size() const; + // retrieve status of format process: AP_Filesystem_Backend::FormatStatus get_format_status() const; diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp new file mode 100644 index 0000000000000..b717d706bdcdd --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp @@ -0,0 +1,1111 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + ArduPilot filesystem interface for systems using the LittleFS filesystem in + flash memory + + littlefs integration by Tamas Nepusz +*/ +#include "AP_Filesystem_config.h" + +#if AP_FILESYSTEM_LITTLEFS_ENABLED + +#include "AP_Filesystem.h" +#include "AP_Filesystem_FlashMemory_LittleFS.h" +#include +#include + +#include "lfs.h" + +//#define AP_LFS_DEBUG +#ifdef AP_LFS_DEBUG +#define debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#else +#define debug(fmt, args ...) +#endif + +#define ENSURE_MOUNTED() do { if (!mounted && !mount_filesystem()) { errno = EIO; return -1; }} while (0) +#define ENSURE_MOUNTED_NULL() do { if (!mounted && !mount_filesystem()) { errno = EIO; return nullptr; }} while (0) +#define LFS_CHECK(func) do { int __retval = func; if (__retval < 0) { errno = errno_from_lfs_error(__retval); return -1; }} while (0) +#define LFS_CHECK_NULL(func) do { int __retval = func; if (__retval < 0) { errno = errno_from_lfs_error(__retval); return nullptr; }} while (0) +#define LFS_ATTR_MTIME 'M' + +static int flashmem_read( + const struct lfs_config *cfg, lfs_block_t block, lfs_off_t off, + void* buffer, lfs_size_t size +); +static int flashmem_prog( + const struct lfs_config *cfg, lfs_block_t block, lfs_off_t off, + const void* buffer, lfs_size_t size +); +static int flashmem_erase(const struct lfs_config *cfg, lfs_block_t block); +static int flashmem_sync(const struct lfs_config *cfg); + +static int errno_from_lfs_error(int lfs_error); +static int lfs_flags_from_flags(int flags); + +const extern AP_HAL::HAL& hal; + +int AP_Filesystem_FlashMemory_LittleFS::open(const char *pathname, int flags, bool allow_absolute_path) +{ + int fd, retval; + file_descriptor* fp; + + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + + ENSURE_MOUNTED(); + + fd = allocate_fd(); + if (fd < 0) { + return -1; + } + + fp = lfs_file_from_fd(fd); + if (fp == nullptr) { + return -1; + } + + // file is closed, see if we already have a modification time + if (lfs_getattr(&fs, pathname, LFS_ATTR_MTIME, &fp->mtime, sizeof(fp->mtime)) != sizeof(fp->mtime)) { + // no attribute, populate from RTC + uint64_t utc_usec = 0; + AP::rtc().get_utc_usec(utc_usec); + fp->mtime = utc_usec/(1000U*1000U); + } + + // populate the file config with the mtime attribute, will get written out on first sync or close + fp->cfg.attrs = fp->attrs; + fp->cfg.attr_count = 1; + fp->attrs[0] = { + .type = LFS_ATTR_MTIME, + .buffer = &fp->mtime, + .size = sizeof(fp->mtime) + }; + // this is a cheat, in theory it could be dynamically allocated but is only used to find the descriptor + // for mtime and we know that's called with a name that doesn't disappear + fp->filename = pathname; + + retval = lfs_file_opencfg(&fs, &fp->file, pathname, lfs_flags_from_flags(flags), &fp->cfg); + + if (retval < 0) { + errno = errno_from_lfs_error(retval); + free_fd(fd); + return -1; + } + + + return fd; +} + +int AP_Filesystem_FlashMemory_LittleFS::close(int fileno) +{ + file_descriptor* fp; + int retval; + + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + + fp = lfs_file_from_fd(fileno); + if (fp == nullptr) { + return -1; + } + + retval = lfs_file_close(&fs, &(fp->file)); + if (retval < 0) { + free_fd(fileno); // ignore error code, we have something else to report + errno = errno_from_lfs_error(retval); + return -1; + } + + if (free_fd(fileno) < 0) { + return -1; + } + + return 0; +} + +int32_t AP_Filesystem_FlashMemory_LittleFS::read(int fd, void *buf, uint32_t count) +{ + file_descriptor* fp; + lfs_ssize_t read; + + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED(); + + fp = lfs_file_from_fd(fd); + if (fp == nullptr) { + return -1; + } + + read = lfs_file_read(&fs, &(fp->file), buf, count); + if (read < 0) { + errno = errno_from_lfs_error(read); + return -1; + } + + return read; +} + +int32_t AP_Filesystem_FlashMemory_LittleFS::write(int fd, const void *buf, uint32_t count) +{ + file_descriptor* fp; + lfs_ssize_t written; + + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED(); + + fp = lfs_file_from_fd(fd); + if (fp == nullptr) { + return -1; + } + + written = lfs_file_write(&fs, &(fp->file), buf, count); + if (written < 0) { + errno = errno_from_lfs_error(written); + return -1; + } + + return written; +} + +int AP_Filesystem_FlashMemory_LittleFS::fsync(int fd) +{ + file_descriptor* fp; + + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED(); + + fp = lfs_file_from_fd(fd); + if (fp == nullptr) { + return -1; + } + + LFS_CHECK(lfs_file_sync(&fs, &(fp->file))); + return 0; +} + +int32_t AP_Filesystem_FlashMemory_LittleFS::lseek(int fd, int32_t position, int whence) +{ + file_descriptor* fp; + + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED(); + + fp = lfs_file_from_fd(fd); + if (fp == nullptr) { + return -1; + } + + LFS_CHECK(lfs_file_seek(&fs, &(fp->file), position, whence)); + return 0; +} + +int AP_Filesystem_FlashMemory_LittleFS::stat(const char *name, struct stat *buf) +{ + lfs_info info; + + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED(); + + LFS_CHECK(lfs_stat(&fs, name, &info)); + + memset(buf, 0, sizeof(*buf)); + uint32_t mtime; + if (lfs_getattr(&fs, name, LFS_ATTR_MTIME, &mtime, sizeof(mtime)) == sizeof(mtime)) { + buf->st_mtime = mtime; + buf->st_atime = mtime; + buf->st_ctime = mtime; + } + buf->st_mode = (info.type == LFS_TYPE_DIR ? S_IFDIR : S_IFREG) | 0666; + buf->st_nlink = 1; + buf->st_size = info.size; + buf->st_blksize = fs_cfg.read_size; + buf->st_uid=1000; + buf->st_gid=1000; + buf->st_blocks = (info.size >> 9) + ((info.size & 0x1FF) > 0 ? 1 : 0); + + return 0; +} + +// set modification time on a file +bool AP_Filesystem_FlashMemory_LittleFS::set_mtime(const char *filename, const uint32_t mtime_sec) +{ + // unfortunately lfs_setattr will not work while the file is open, instead + // we need to update the file config, but that means finding the file config + for (int fd = 0; fd < MAX_OPEN_FILES; fd++) { + if (open_files[fd] != nullptr && open_files[fd]->filename == filename) { + open_files[fd]->mtime = mtime_sec; + return true; + } + } + return false; +} + +int AP_Filesystem_FlashMemory_LittleFS::unlink(const char *pathname) +{ + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED(); + LFS_CHECK(lfs_remove(&fs, pathname)); + return 0; +} + +int AP_Filesystem_FlashMemory_LittleFS::mkdir(const char *pathname) +{ + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED(); + LFS_CHECK(lfs_mkdir(&fs, pathname)); + return 0; +} + +typedef struct { + lfs_dir_t dir; + struct dirent entry; +} lfs_dir_entry_pair; + +void *AP_Filesystem_FlashMemory_LittleFS::opendir(const char *pathdir) +{ + int retval; + + FS_CHECK_ALLOWED(nullptr); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED_NULL(); + + lfs_dir_entry_pair *result = new lfs_dir_entry_pair; + if (!result) { + errno = ENOMEM; + return nullptr; + } + + retval = lfs_dir_open(&fs, &result->dir, pathdir); + if (retval < 0) { + delete result; + errno = errno_from_lfs_error(retval); + return nullptr; + } + + memset(&result->entry, 0, sizeof(result->entry)); + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL + result->entry.d_reclen = sizeof(result->entry); +#endif + + return result; +} + +struct dirent *AP_Filesystem_FlashMemory_LittleFS::readdir(void *ptr) +{ + FS_CHECK_ALLOWED(nullptr); + WITH_SEMAPHORE(fs_sem); + + lfs_info info; + lfs_dir_entry_pair *pair = static_cast(ptr); + if (!pair) { + errno = EINVAL; + return nullptr; + } + + if (!lfs_dir_read(&fs, &pair->dir, &info)) { + /* no more entries */ + return nullptr; + } + + memset(&pair->entry, 0, sizeof(pair->entry)); + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL + pair->entry.d_ino = 0; + pair->entry.d_seekoff++; +#endif + + strncpy(pair->entry.d_name, info.name, MIN(strlen(info.name)+1, sizeof(pair->entry.d_name))); +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL + pair->entry.d_namlen = strlen(info.name); +#endif + + pair->entry.d_type = info.type == LFS_TYPE_DIR ? DT_DIR : DT_REG; + + return &pair->entry; +} + +int AP_Filesystem_FlashMemory_LittleFS::closedir(void *ptr) +{ + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + + lfs_dir_entry_pair *pair = static_cast(ptr); + if (!pair) { + errno = EINVAL; + return 0; + } + + LFS_CHECK(lfs_dir_close(&fs, &pair->dir)); + + delete pair; + + return 0; +} + +int64_t AP_Filesystem_FlashMemory_LittleFS::disk_free(const char *path) +{ + lfs_ssize_t alloc_size; + + FS_CHECK_ALLOWED(-1); + WITH_SEMAPHORE(fs_sem); + ENSURE_MOUNTED(); + + alloc_size = lfs_fs_size(&fs); + if (alloc_size < 0) { + errno = errno_from_lfs_error(alloc_size); + return -1; + } + + return disk_space(path) - alloc_size; +} + +int64_t AP_Filesystem_FlashMemory_LittleFS::disk_space(const char *path) +{ + return fs_cfg.block_count * fs_cfg.block_size; +} + +bool AP_Filesystem_FlashMemory_LittleFS::retry_mount(void) +{ + FS_CHECK_ALLOWED(false); + WITH_SEMAPHORE(fs_sem); + + if (!dead) { + if (!mounted && !mount_filesystem()) { + errno = EIO; + return false; + } + + return true; + } else { + return false; + } +} + +void AP_Filesystem_FlashMemory_LittleFS::unmount(void) +{ + WITH_SEMAPHORE(fs_sem); + + if (mounted && !dead) { + if (lfs_unmount(&fs) >= 0) { + mounted = false; + } + } +} + +/* ************************************************************************* */ +/* Private functions */ +/* ************************************************************************* */ + +int AP_Filesystem_FlashMemory_LittleFS::allocate_fd() +{ + int fd; + + for (fd = 0; fd < MAX_OPEN_FILES; fd++) { + if (open_files[fd] == nullptr) { + open_files[fd] = static_cast(calloc(1, sizeof(file_descriptor))); + if (open_files[fd] == nullptr) { + errno = ENOMEM; + return -1; + } + + return fd; + } + } + + errno = ENFILE; + return -1; +} + +int AP_Filesystem_FlashMemory_LittleFS::free_fd(int fd) +{ + file_descriptor* fp = lfs_file_from_fd(fd); + if (!fp) { + return -1; + } + + free(fp); + open_files[fd] = fp = nullptr; + + return 0; +} + +void AP_Filesystem_FlashMemory_LittleFS::free_all_fds() +{ + int fd; + + for (fd = 0; fd < MAX_OPEN_FILES; fd++) { + if (open_files[fd] == nullptr) { + free_fd(fd); + } + } +} + +AP_Filesystem_FlashMemory_LittleFS::file_descriptor* AP_Filesystem_FlashMemory_LittleFS::lfs_file_from_fd(int fd) const +{ + if (fd < 0 || fd >= MAX_OPEN_FILES || open_files[fd] == nullptr) { + errno = EBADF; + return nullptr; + } + + return open_files[fd]; +} + +void AP_Filesystem_FlashMemory_LittleFS::mark_dead() +{ + if (!dead) { + printf("FlashMemory_LittleFS: dead\n"); + free_all_fds(); + dead = true; + } +} + +/* ************************************************************************* */ +/* Low-level flash memory access */ +/* ************************************************************************* */ + +#define JEDEC_WRITE_ENABLE 0x06 +#define JEDEC_READ_STATUS 0x05 +#define JEDEC_WRITE_STATUS 0x01 +#define JEDEC_READ_DATA 0x03 +#define JEDEC_PAGE_DATA_READ 0x13 +#define JEDEC_DEVICE_ID 0x9F +#define JEDEC_PAGE_WRITE 0x02 +#define JEDEC_PROGRAM_EXECUTE 0x10 + +#define JEDEC_DEVICE_RESET 0xFF + +#define JEDEC_BULK_ERASE 0xC7 +#define JEDEC_SECTOR4_ERASE 0x20 // 4k erase +#define JEDEC_BLOCK_ERASE 0xD8 + +#define JEDEC_STATUS_BUSY 0x01 +#define JEDEC_STATUS_WRITEPROTECT 0x02 +#define JEDEC_STATUS_BP0 0x04 +#define JEDEC_STATUS_BP1 0x08 +#define JEDEC_STATUS_BP2 0x10 +#define JEDEC_STATUS_TP 0x20 +#define JEDEC_STATUS_SEC 0x40 +#define JEDEC_STATUS_SRP0 0x80 + +#define W25NXX_STATUS_EFAIL 0x04 +#define W25NXX_STATUS_PFAIL 0x08 + + +/* + flash device IDs taken from betaflight flash_m25p16.c + + Format is manufacturer, memory type, then capacity +*/ +#define JEDEC_ID_UNKNOWN 0x000000 +#define JEDEC_ID_MACRONIX_MX25L3206E 0xC22016 +#define JEDEC_ID_MACRONIX_MX25L6406E 0xC22017 +#define JEDEC_ID_MACRONIX_MX25L25635E 0xC22019 +#define JEDEC_ID_MICRON_M25P16 0x202015 +#define JEDEC_ID_MICRON_N25Q064 0x20BA17 +#define JEDEC_ID_MICRON_N25Q128 0x20BA18 +#define JEDEC_ID_WINBOND_W25Q16 0xEF4015 +#define JEDEC_ID_WINBOND_W25Q32 0xEF4016 +#define JEDEC_ID_WINBOND_W25X32 0xEF3016 +#define JEDEC_ID_WINBOND_W25Q64 0xEF4017 +#define JEDEC_ID_WINBOND_W25Q128 0xEF4018 +#define JEDEC_ID_WINBOND_W25Q256 0xEF4019 +#define JEDEC_ID_WINBOND_W25Q128_2 0xEF7018 +#define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21 +#define JEDEC_ID_WINBOND_W25N02KV 0xEFAA22 +#define JEDEC_ID_CYPRESS_S25FL128L 0x016018 + +/* Hardware-specific constants */ + +#define W25NXX_PROT_REG 0xA0 +#define W25NXX_CONF_REG 0xB0 +#define W25NXX_STATUS_REG 0xC0 +#define W25NXX_STATUS_EFAIL 0x04 +#define W25NXX_STATUS_PFAIL 0x08 + +#define W25N01G_NUM_BLOCKS 1024 +#define W25N02K_NUM_BLOCKS 2048 + +#define W25NXX_CONFIG_ECC_ENABLE (1 << 4) +#define W25NXX_CONFIG_BUFFER_READ_MODE (1 << 3) + +#define W25NXX_TIMEOUT_PAGE_READ_US 60 // tREmax = 60us (ECC enabled) +#define W25NXX_TIMEOUT_PAGE_PROGRAM_US 700 // tPPmax = 700us +#define W25NXX_TIMEOUT_BLOCK_ERASE_MS 10 // tBEmax = 10ms +#define W25NXX_TIMEOUT_RESET_MS 500 // tRSTmax = 500ms + +bool AP_Filesystem_FlashMemory_LittleFS::is_busy() +{ + WITH_SEMAPHORE(dev_sem); + uint8_t status; +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + uint8_t cmd[2] { JEDEC_READ_STATUS, W25NXX_STATUS_REG }; + dev->transfer(cmd, 2, &status, 1); + return (status & (JEDEC_STATUS_BUSY | W25NXX_STATUS_PFAIL | W25NXX_STATUS_EFAIL)) != 0; +#else + uint8_t cmd = JEDEC_READ_STATUS; + dev->transfer(&cmd, 1, &status, 1); + return (status & (JEDEC_STATUS_BUSY | JEDEC_STATUS_SRP0)) != 0; +#endif +} + +void AP_Filesystem_FlashMemory_LittleFS::send_command_addr(uint8_t command, uint32_t addr) +{ + uint8_t cmd[5]; + cmd[0] = command; + + if (use_32bit_address) { + cmd[1] = (addr >> 24) & 0xff; + cmd[2] = (addr >> 16) & 0xff; + cmd[3] = (addr >> 8) & 0xff; + cmd[4] = (addr >> 0) & 0xff; + } else { + cmd[1] = (addr >> 16) & 0xff; + cmd[2] = (addr >> 8) & 0xff; + cmd[3] = (addr >> 0) & 0xff; + cmd[4] = 0; + } + + dev->transfer(cmd, use_32bit_address ? 5 : 4, nullptr, 0); +} + +void AP_Filesystem_FlashMemory_LittleFS::send_command_page(uint8_t command, uint32_t page) +{ + uint8_t cmd[3]; + cmd[0] = command; + cmd[1] = (page >> 8) & 0xff; + cmd[2] = (page >> 0) & 0xff; + dev->transfer(cmd, 3, nullptr, 0); +} + +bool AP_Filesystem_FlashMemory_LittleFS::wait_until_device_is_ready() +{ + if (dead) { + return false; + } + + uint32_t t = AP_HAL::millis(); + while (is_busy()) { + hal.scheduler->delay_microseconds(100); + if (AP_HAL::millis() - t > 5000) { + mark_dead(); + return false; + } + } + + return true; +} + +void AP_Filesystem_FlashMemory_LittleFS::write_status_register(uint8_t reg, uint8_t bits) +{ + WITH_SEMAPHORE(dev_sem); + uint8_t cmd[3] = { JEDEC_WRITE_STATUS, reg, bits }; + dev->transfer(cmd, 3, nullptr, 0); +} + +uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { + if (!wait_until_device_is_ready()) { + return false; + } + + WITH_SEMAPHORE(dev_sem); + + // Read manufacturer ID + uint8_t cmd = JEDEC_DEVICE_ID; + uint8_t buf[4]; + dev->transfer(&cmd, 1, buf, 4); + +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + uint32_t id = buf[1] << 16 | buf[2] << 8 | buf[3]; + +#else + uint32_t id = buf[0] << 16 | buf[1] << 8 | buf[2]; +#endif + + // Let's specify the terminology here. + // + // 1 block = smallest unit that we can _erase_ in a single operation + // 1 page = smallest unit that we can read or program in a single operation + // + // So, for instance, if we have 4K sectors on the flash chip and we can + // always erase a single 4K sector, the LFS block size will be 4096 bytes, + + // irrespectively of what the flash chip documentation refers to as a "block" + /* Most flash chips are programmable in chunks of 256 bytes and erasable in + * blocks of 4K so we start with these defaults */ + uint32_t block_count = 0; + uint16_t page_size = 256; + uint32_t block_size = 4096; + + switch (id) { +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + case JEDEC_ID_WINBOND_W25N01GV: + /* 128M, programmable in chunks of 2048 bytes, erasable in blocks of 128K */ + page_size = 2048; + block_size = 131072; + block_count = 1024; + break; + case JEDEC_ID_WINBOND_W25N02KV: + /* 256M, programmable in chunks of 2048 bytes, erasable in blocks of 128K */ + page_size = 2048; + block_size = 131072; + block_count = 2048; + break; +#else + case JEDEC_ID_WINBOND_W25Q16: + case JEDEC_ID_MICRON_M25P16: + block_count = 32; /* 128K */ + break; + + case JEDEC_ID_WINBOND_W25Q32: + case JEDEC_ID_WINBOND_W25X32: + case JEDEC_ID_MACRONIX_MX25L3206E: + block_count = 64; /* 256K */ + break; + + case JEDEC_ID_MICRON_N25Q064: + case JEDEC_ID_WINBOND_W25Q64: + case JEDEC_ID_MACRONIX_MX25L6406E: + block_count = 128; /* 512K */ + break; + + case JEDEC_ID_MICRON_N25Q128: + case JEDEC_ID_WINBOND_W25Q128: + case JEDEC_ID_WINBOND_W25Q128_2: + case JEDEC_ID_CYPRESS_S25FL128L: + block_count = 256; /* 1M */ + break; + + case JEDEC_ID_WINBOND_W25Q256: + case JEDEC_ID_MACRONIX_MX25L25635E: + block_count = 512; /* 2M */ + use_32bit_address = true; + break; +#endif + default: + hal.scheduler->delay(2000); + printf("Unknown SPI Flash 0x%08x\n", id); + return 0; + } + + fs_cfg.read_size = page_size; + fs_cfg.prog_size = page_size; + fs_cfg.block_size = block_size; + fs_cfg.block_count = block_count; + + fs_cfg.block_cycles = 500; + fs_cfg.lookahead_size = 16; + + // cache_size has to be the same as the page_size, otherwise we would + // occasionally get read or prog requests in flashmem_read() and + // flashmem_prog() whose size is equal to the cache size, and we do not + // handle that right now + fs_cfg.cache_size = page_size; + + return id; +} + +bool AP_Filesystem_FlashMemory_LittleFS::mount_filesystem() { + if (dead) { + return false; + } + + if (mounted) { + return true; + } + + EXPECT_DELAY_MS(3000); + + fs_cfg.context = this; + + fs_cfg.read = flashmem_read; + fs_cfg.prog = flashmem_prog; + fs_cfg.erase = flashmem_erase; + fs_cfg.sync = flashmem_sync; + + dev = hal.spi->get_device("dataflash"); + if (!dev) { + mark_dead(); + return false; + } + + dev_sem = dev->get_semaphore(); + + uint32_t id = find_block_size_and_count(); + + if (!id) { + mark_dead(); + return false; + } + + if (!init_flash()) { + mark_dead(); + return false; + } + + if (lfs_mount(&fs, &fs_cfg) < 0) { + /* maybe not formatted? try formatting it */ + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Formatting flash"); +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + EXPECT_DELAY_MS(3000); +#else + EXPECT_DELAY_MS(30000); +#endif + + if (lfs_format(&fs, &fs_cfg) < 0) { + mark_dead(); + return false; + } + + /* try mounting again */ + if (lfs_mount(&fs, &fs_cfg) < 0) { + /* cannot mount after formatting */ + mark_dead(); + return false; + } + } + +#ifdef HAL_BOARD_STORAGE_DIRECTORY + // try to create the root storage folder. Ignore the error code in case + // the filesystem is corrupted or it already exists. + if (strlen(HAL_BOARD_STORAGE_DIRECTORY) > 0) { + lfs_mkdir(&fs, HAL_BOARD_STORAGE_DIRECTORY); + } +#endif + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Mounted flash 0x%x as littlefs", unsigned(id)); + mounted = true; + return true; +} + +/* + format sdcard +*/ +bool AP_Filesystem_FlashMemory_LittleFS::format(void) +{ + WITH_SEMAPHORE(fs_sem); + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Filesystem_FlashMemory_LittleFS::format_handler, void)); + // the format is handled asynchronously, we inform user of success + // via a text message. format_status can be polled for progress + format_status = FormatStatus::PENDING; + return true; +} + +/* + format sdcard +*/ +void AP_Filesystem_FlashMemory_LittleFS::format_handler(void) +{ + if (format_status != FormatStatus::PENDING) { + return; + } + WITH_SEMAPHORE(fs_sem); + format_status = FormatStatus::IN_PROGRESS; + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Formatting flash using littlefs"); + + int ret = lfs_format(&fs, &fs_cfg); + + /* try mounting */ + if (ret == LFS_ERR_OK) { + ret = lfs_mount(&fs, &fs_cfg); + } + +#ifdef HAL_BOARD_STORAGE_DIRECTORY + // try to create the root storage folder. Ignore the error code in case + // the filesystem is corrupted or it already exists. + if (strlen(HAL_BOARD_STORAGE_DIRECTORY) > 0) { + lfs_mkdir(&fs, HAL_BOARD_STORAGE_DIRECTORY); + } +#endif + + if (ret == LFS_ERR_OK) { + format_status = FormatStatus::SUCCESS; + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Format flash: OK"); + } else { + format_status = FormatStatus::FAILURE; + mark_dead(); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Format flash: Failed (%d)", int(ret)); + } +} + +// returns true if we are currently formatting the SD card: +AP_Filesystem_Backend::FormatStatus AP_Filesystem_FlashMemory_LittleFS::get_format_status(void) const +{ + // note that format_handler holds sem, so we can't take it here. + return format_status; +} + +uint32_t AP_Filesystem_FlashMemory_LittleFS::lfs_block_and_offset_to_raw_flash_address(lfs_block_t index, lfs_off_t off) +{ + return index * fs_cfg.block_size + off; +} + +uint32_t AP_Filesystem_FlashMemory_LittleFS::lfs_block_to_raw_flash_page_index(lfs_block_t index) +{ + return index * (fs_cfg.block_size / fs_cfg.prog_size); +} + +bool AP_Filesystem_FlashMemory_LittleFS::write_enable() +{ + uint8_t b = JEDEC_WRITE_ENABLE; + + if (!wait_until_device_is_ready()) { + return false; + } + + WITH_SEMAPHORE(dev_sem); + return dev->transfer(&b, 1, nullptr, 0); +} + +bool AP_Filesystem_FlashMemory_LittleFS::init_flash() +{ +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + // reset the device + if (!wait_until_device_is_ready()) { + return false; + } + { + WITH_SEMAPHORE(dev_sem); + uint8_t b = JEDEC_DEVICE_RESET; + dev->transfer(&b, 1, nullptr, 0); + } + hal.scheduler->delay(W25NXX_TIMEOUT_RESET_MS); + + // disable write protection + write_status_register(W25NXX_PROT_REG, 0); + + // enable ECC and buffer mode + write_status_register(W25NXX_CONF_REG, W25NXX_CONFIG_ECC_ENABLE | W25NXX_CONFIG_BUFFER_READ_MODE); +#else + if (use_32bit_address) { + WITH_SEMAPHORE(dev_sem); + + const uint8_t cmd = 0xB7; + dev->transfer(&cmd, 1, nullptr, 0); + } +#endif + + return true; +} + +int AP_Filesystem_FlashMemory_LittleFS::_flashmem_read( + lfs_block_t block, lfs_off_t off, void* buffer, lfs_size_t size +) { + uint32_t address; + + if (dead) { + return LFS_ERR_IO; + } + + address = lfs_block_and_offset_to_raw_flash_address(block, off); +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + /* We need to read an entire page into an internal buffer and then read + * that buffer with JEDEC_READ_DATA later */ + if (!wait_until_device_is_ready()) { + return LFS_ERR_IO; + } + + { + WITH_SEMAPHORE(dev_sem); + send_command_addr(JEDEC_PAGE_DATA_READ, address / fs_cfg.read_size); + address %= fs_cfg.read_size; + } +#endif + if (!wait_until_device_is_ready()) { + return LFS_ERR_IO; + } + + WITH_SEMAPHORE(dev_sem); + + dev->set_chip_select(true); + send_command_addr(JEDEC_READ_DATA, address); + dev->transfer(nullptr, 0, static_cast(buffer), size); + dev->set_chip_select(false); + + return LFS_ERR_OK; +} + +#ifdef AP_LFS_DEBUG +static uint32_t block_writes; +static uint32_t last_write_msg_ms; +#endif + +int AP_Filesystem_FlashMemory_LittleFS::_flashmem_prog( + lfs_block_t block, lfs_off_t off, const void* buffer, lfs_size_t size +) { + uint32_t address; + + if (dead) { + return LFS_ERR_IO; + } + + if (!write_enable()) { + return LFS_ERR_IO; + } + +#ifdef AP_LFS_DEBUG + block_writes++; + if (AP_HAL::millis() - last_write_msg_ms > 5000) { + debug("LFS: writes %lukB/s, pages %lu/s\n", (block_writes*fs_cfg.prog_size)/(5*1024), block_writes/5); + block_writes = 0; + last_write_msg_ms = AP_HAL::millis(); + } +#endif + address = lfs_block_and_offset_to_raw_flash_address(block, off); + + WITH_SEMAPHORE(dev_sem); +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + /* First we need to write into the data buffer at column address zero, + * then we need to issue PROGRAM_EXECUTE to commit the internal buffer */ + dev->set_chip_select(true); + send_command_page(JEDEC_PAGE_WRITE, address % fs_cfg.prog_size); + dev->transfer(static_cast(buffer), size, nullptr, 0); + dev->set_chip_select(false); + send_command_addr(JEDEC_PROGRAM_EXECUTE, address / fs_cfg.prog_size); + // this simply means the data is in the internal cache, it will take some period to + // propagate to the flash itself +#else + dev->set_chip_select(true); + send_command_addr(JEDEC_PAGE_WRITE, address); + dev->transfer(static_cast(buffer), size, nullptr, 0); + dev->set_chip_select(false); +#endif + return LFS_ERR_OK; +} + +int AP_Filesystem_FlashMemory_LittleFS::_flashmem_erase(lfs_block_t block) { + if (dead) { + return LFS_ERR_IO; + } + + if (!write_enable()) { + return LFS_ERR_IO; + } + + WITH_SEMAPHORE(dev_sem); +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + send_command_addr(JEDEC_BLOCK_ERASE, lfs_block_to_raw_flash_page_index(block)); +#else + send_command_addr(JEDEC_SECTOR4_ERASE, lfs_block_and_offset_to_raw_flash_address(block)); +#endif + + // sleep so that othher processes get the CPU cycles that the 4ms erase cycle needs. + hal.scheduler->delay(4); + + return LFS_ERR_OK; +} + +int AP_Filesystem_FlashMemory_LittleFS::_flashmem_sync() { + if (wait_until_device_is_ready()) { + return LFS_ERR_OK; + } else { + return LFS_ERR_IO; + } +} + +static int flashmem_read( + const struct lfs_config *cfg, lfs_block_t block, lfs_off_t off, + void* buffer, lfs_size_t size +) { + AP_Filesystem_FlashMemory_LittleFS* self = static_cast(cfg->context); + return self->_flashmem_read(block, off, buffer, size); +} + +static int flashmem_prog( + const struct lfs_config *cfg, lfs_block_t block, lfs_off_t off, + const void* buffer, lfs_size_t size +) { + AP_Filesystem_FlashMemory_LittleFS* self = static_cast(cfg->context); + return self->_flashmem_prog(block, off, buffer, size); +} + +static int flashmem_erase(const struct lfs_config *cfg, lfs_block_t block) { + AP_Filesystem_FlashMemory_LittleFS* self = static_cast(cfg->context); + return self->_flashmem_erase(block); +} + +static int flashmem_sync(const struct lfs_config *cfg) { + AP_Filesystem_FlashMemory_LittleFS* self = static_cast(cfg->context); + return self->_flashmem_sync(); +} + +/* ************************************************************************* */ +/* LittleFS to POSIX API conversion functions */ +/* ************************************************************************* */ + +static int errno_from_lfs_error(int lfs_error) +{ + switch (lfs_error) { + case LFS_ERR_OK: return 0; + case LFS_ERR_IO: return EIO; + case LFS_ERR_CORRUPT: return EIO; + case LFS_ERR_NOENT: return ENOENT; + case LFS_ERR_EXIST: return EEXIST; + case LFS_ERR_NOTDIR: return ENOTDIR; + case LFS_ERR_ISDIR: return EISDIR; + case LFS_ERR_NOTEMPTY: return ENOTEMPTY; + case LFS_ERR_BADF: return EBADF; + case LFS_ERR_FBIG: return EFBIG; + case LFS_ERR_INVAL: return EINVAL; + case LFS_ERR_NOSPC: return ENOSPC; + case LFS_ERR_NOMEM: return ENOMEM; +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL + case LFS_ERR_NOATTR: return ENOATTR; +#endif + case LFS_ERR_NAMETOOLONG: return ENAMETOOLONG; + default: return EIO; + } +} + +static int lfs_flags_from_flags(int flags) +{ + int outflags = 0; + + if (flags & O_WRONLY) { + outflags |= LFS_O_WRONLY; + } else if (flags & O_RDWR) { + outflags |= LFS_O_RDWR; + } else { + outflags |= LFS_O_RDONLY; + } + + if (flags & O_CREAT) { + outflags |= LFS_O_CREAT; + } + + if (flags & O_EXCL) { + outflags |= LFS_O_EXCL; + } + + if (flags & O_TRUNC) { + outflags |= LFS_O_TRUNC; + } + + if (flags & O_APPEND) { + outflags |= LFS_O_APPEND; + } + + return outflags; +} + +#endif // AP_FILESYSTEM_LITTLEFS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h new file mode 100644 index 0000000000000..351e5cca4c082 --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h @@ -0,0 +1,125 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include "AP_Filesystem_backend.h" + +#if AP_FILESYSTEM_LITTLEFS_ENABLED + +#include +#include +#include "lfs.h" + +class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend +{ +public: + // functions that closely match the equivalent posix calls + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; + int close(int fd) override; + int32_t read(int fd, void *buf, uint32_t count) override; + int32_t write(int fd, const void *buf, uint32_t count) override; + int fsync(int fd) override; + int32_t lseek(int fd, int32_t offset, int whence) override; + int stat(const char *pathname, struct stat *stbuf) override; + + int unlink(const char *pathname) override; + int mkdir(const char *pathname) override; + + void *opendir(const char *pathname) override; + struct dirent *readdir(void *dirp) override; + int closedir(void *dirp) override; + + int64_t disk_free(const char *path) override; + int64_t disk_space(const char *path) override; + + // set modification time on a file + bool set_mtime(const char *filename, const uint32_t mtime_sec) override; + + uint32_t get_sync_size() const override { return fs_cfg.block_size; } + + bool retry_mount(void) override; + void unmount(void) override; + // format flash. This is async, monitor get_format_status for progress + bool format(void) override; + AP_Filesystem_Backend::FormatStatus get_format_status() const override; + + int _flashmem_read(lfs_block_t block, lfs_off_t off, void* buffer, lfs_size_t size); + int _flashmem_prog(lfs_block_t block, lfs_off_t off, const void* buffer, lfs_size_t size); + int _flashmem_erase(lfs_block_t block); + int _flashmem_sync(); + +private: + // JEDEC ID of the flash memory, JEDEC_ID_UNKNOWN if not known or not supported + uint32_t jedec_id; + + // Semaphore to protect against concurrent accesses to fs + HAL_Semaphore fs_sem; + + // The filesystem object + lfs_t fs; + + // The configuration of the filesystem + struct lfs_config fs_cfg; + + // Maximum number of files that may be open at the same time + static constexpr int MAX_OPEN_FILES = 16; + + // Stores whether the filesystem is mounted + bool mounted; + + // Stores whether the filesystem has been marked as dead + bool dead; + + // Array of currently open file descriptors + struct file_descriptor { + lfs_file_t file; + lfs_file_config cfg; + lfs_attr attrs[1]; + uint32_t mtime; + const char* filename; + }; + file_descriptor* open_files[MAX_OPEN_FILES]; + + // SPI device that handles the raw flash memory + AP_HAL::OwnPtr dev; + + // Semaphore to protect access to the SPI device + AP_HAL::Semaphore *dev_sem; + + // Flag to denote that the underlying flash chip uses 32-bit addresses + bool use_32bit_address; + FormatStatus format_status; + + int allocate_fd(); + int free_fd(int fd); + void free_all_fds(); + file_descriptor* lfs_file_from_fd(int fd) const; + + uint32_t lfs_block_and_offset_to_raw_flash_address(lfs_block_t block, lfs_off_t off = 0); + uint32_t lfs_block_to_raw_flash_page_index(lfs_block_t block); + uint32_t find_block_size_and_count(); + bool init_flash() WARN_IF_UNUSED; + bool write_enable() WARN_IF_UNUSED; + bool is_busy(); + bool mount_filesystem(); + void send_command_addr(uint8_t command, uint32_t addr); + void send_command_page(uint8_t command, uint32_t page); + bool wait_until_device_is_ready() WARN_IF_UNUSED; + void write_status_register(uint8_t reg, uint8_t bits); + void format_handler(void); + void mark_dead(); +}; + +#endif // #if AP_FILESYSTEM_LITTLEFS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_backend.h b/libraries/AP_Filesystem/AP_Filesystem_backend.h index 8cf0fa655a1cf..d59fce4244bfc 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_backend.h +++ b/libraries/AP_Filesystem/AP_Filesystem_backend.h @@ -96,6 +96,9 @@ class AP_Filesystem_Backend { // unload data from load_file() virtual void unload_file(FileData *fd); + // if non-zero size at which syncs should be peformed, only used by flash fs + virtual uint32_t get_sync_size() const { return 0; } + protected: // return true if file operations are allowed bool file_op_allowed(void) const; diff --git a/libraries/AP_Filesystem/AP_Filesystem_config.h b/libraries/AP_Filesystem/AP_Filesystem_config.h index a23b1f2c1b543..49cc6ec3f8496 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_config.h +++ b/libraries/AP_Filesystem/AP_Filesystem_config.h @@ -2,6 +2,9 @@ #include +#define AP_FILESYSTEM_FLASH_W25QXX 1 +#define AP_FILESYSTEM_FLASH_W25NXX 2 + // backends: #ifndef AP_FILESYSTEM_ESP32_ENABLED @@ -12,6 +15,14 @@ #define AP_FILESYSTEM_FATFS_ENABLED HAL_OS_FATFS_IO #endif +#ifndef AP_FILESYSTEM_LITTLEFS_ENABLED +#define AP_FILESYSTEM_LITTLEFS_ENABLED HAL_OS_LITTLEFS_IO +#endif + +#ifndef AP_FILESYSTEM_LITTLEFS_FLASH_TYPE +#define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25QXX +#endif + #ifndef AP_FILESYSTEM_PARAM_ENABLED #define AP_FILESYSTEM_PARAM_ENABLED 1 #endif @@ -39,7 +50,7 @@ // upload targets, and also excludes ROMFS (where you can read but not // write!) #ifndef AP_FILESYSTEM_FILE_WRITING_ENABLED -#define AP_FILESYSTEM_FILE_WRITING_ENABLED (AP_FILESYSTEM_ESP32_ENABLED || AP_FILESYSTEM_FATFS_ENABLED || AP_FILESYSTEM_POSIX_ENABLED) +#define AP_FILESYSTEM_FILE_WRITING_ENABLED (AP_FILESYSTEM_ESP32_ENABLED || AP_FILESYSTEM_FATFS_ENABLED || AP_FILESYSTEM_LITTLEFS_ENABLED || AP_FILESYSTEM_POSIX_ENABLED) #endif // AP_FILESYSTEM_FILE_READING_ENABLED is true if you could expect to From acf1b40dca8547a5d1c7a85b86845574bffe5bce Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 7 Dec 2024 14:52:01 +0000 Subject: [PATCH 35/77] AP_Filesystem: reduce metadata_max on W25N flash --- .../AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp index b717d706bdcdd..10a16a3dd6c84 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp @@ -713,7 +713,9 @@ uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { fs_cfg.prog_size = page_size; fs_cfg.block_size = block_size; fs_cfg.block_count = block_count; - +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + fs_cfg.metadata_max = page_size; +#endif fs_cfg.block_cycles = 500; fs_cfg.lookahead_size = 16; From 5f8e339d476288a6f6da0bb1fc8721934e6ce3d3 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 7 Dec 2024 19:51:34 +0000 Subject: [PATCH 36/77] AP_Logger: add flash speed test --- libraries/AP_Logger/AP_Logger_Block.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Logger/AP_Logger_Block.cpp b/libraries/AP_Logger/AP_Logger_Block.cpp index 7ebf38c8c025a..07357c833a9d2 100644 --- a/libraries/AP_Logger/AP_Logger_Block.cpp +++ b/libraries/AP_Logger/AP_Logger_Block.cpp @@ -926,7 +926,7 @@ void AP_Logger_Block::write_log_page() void AP_Logger_Block::flash_test() { - const uint32_t pages_to_check = 128; + uint32_t pages_to_check = 128; for (uint32_t i=1; i<=pages_to_check; i++) { if ((i-1) % df_PagePerBlock == 0) { printf("Block erase %u\n", get_block(i)); @@ -962,6 +962,19 @@ void AP_Logger_Block::flash_test() i, bad_bytes, df_PageSize, buffer[first_bad_byte]); } } + + // speed test + pages_to_check = 4096; // 1mB / 8mB + for (uint32_t i=1; i<=pages_to_check; i++) { + if ((i-1) % df_PagePerBlock == 0) { + SectorErase(get_block(i)); + } + } + uint32_t now_ms = AP_HAL::millis(); + for (uint32_t i=1; i<=pages_to_check; i++) { + BufferToPage(i); + } + printf("Flash speed test: %ukB/s\n", unsigned((pages_to_check * df_PageSize * 1000) / (1024 * (AP_HAL::millis() - now_ms)))); } #endif // HAL_LOGGING_BLOCK_ENABLED From 6ac4bf3fc484a9cf0d3fe4db60cce1626697a575 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 7 Dec 2024 19:59:58 +0000 Subject: [PATCH 37/77] AP_Logger: add performance debug to W25NXX logger --- libraries/AP_Logger/AP_Logger_W25NXX.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/libraries/AP_Logger/AP_Logger_W25NXX.cpp b/libraries/AP_Logger/AP_Logger_W25NXX.cpp index df3e8bcea2d89..e2fd073d9b81b 100644 --- a/libraries/AP_Logger/AP_Logger_W25NXX.cpp +++ b/libraries/AP_Logger/AP_Logger_W25NXX.cpp @@ -247,6 +247,11 @@ void AP_Logger_W25NXX::PageToBuffer(uint32_t pageNum) } } +//#define AP_W25NXX_DEBUG +#ifdef AP_W25NXX_DEBUG +static uint32_t block_writes; +static uint32_t last_write_msg_ms; +#endif void AP_Logger_W25NXX::BufferToPage(uint32_t pageNum) { if (pageNum == 0 || pageNum > df_NumPages+1) { @@ -283,6 +288,15 @@ void AP_Logger_W25NXX::BufferToPage(uint32_t pageNum) WITH_SEMAPHORE(dev_sem); send_command_addr(JEDEC_PROGRAM_EXECUTE, PageAdr); } + +#ifdef AP_W25NXX_DEBUG + block_writes++; + if (AP_HAL::millis() - last_write_msg_ms > 5000) { + hal.console->printf("W25NXX: writes %lukB/s, pages %lu/s\n", (block_writes*df_PageSize)/(5*1024), block_writes/5); + block_writes = 0; + last_write_msg_ms = AP_HAL::millis(); + } +#endif } /* From 8a992740f5dd5b61ece13a9bd9c685779adbab23 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 11 Dec 2024 18:25:56 +0000 Subject: [PATCH 38/77] AP_Filesystem: add littlefs singleton for sync block use --- libraries/AP_Filesystem/AP_Filesystem.cpp | 6 --- libraries/AP_Filesystem/AP_Filesystem.h | 3 -- .../AP_Filesystem_FlashMemory_LittleFS.cpp | 52 +++++++++++++++++++ .../AP_Filesystem_FlashMemory_LittleFS.h | 12 ++++- .../AP_Filesystem/AP_Filesystem_backend.h | 3 -- 5 files changed, 63 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index b7240a6f97b5d..dac87e0d5ce28 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -322,12 +322,6 @@ void AP_Filesystem::unmount(void) return LOCAL_BACKEND.fs.unmount(); } -// if non-zero size at which syncs should be peformed, only used by flash fs -uint32_t AP_Filesystem::get_sync_size(void) const -{ - return LOCAL_BACKEND.fs.get_sync_size(); -} - /* Load a file's contents into memory. Returned object must be `delete`d to free the data. The data is guaranteed to be null-terminated such that it can be diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index fe5214fd55809..cc8619ca52152 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -128,9 +128,6 @@ class AP_Filesystem { // format filesystem. This is async, monitor get_format_status for progress bool format(void); - // if non-zero size at which syncs should be peformed, only used by flash fs - uint32_t get_sync_size() const; - // retrieve status of format process: AP_Filesystem_Backend::FormatStatus get_format_status() const; diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp index 10a16a3dd6c84..d375042f9b046 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp @@ -58,6 +58,16 @@ static int lfs_flags_from_flags(int flags); const extern AP_HAL::HAL& hal; +AP_Filesystem_FlashMemory_LittleFS* AP_Filesystem_FlashMemory_LittleFS::singleton; + +AP_Filesystem_FlashMemory_LittleFS::AP_Filesystem_FlashMemory_LittleFS() +{ + if (singleton) { + AP_HAL::panic("Too many AP_Filesystem_FlashMemory_LittleFS instances"); + } + singleton = this; +} + int AP_Filesystem_FlashMemory_LittleFS::open(const char *pathname, int flags, bool allow_absolute_path) { int fd, retval; @@ -555,6 +565,34 @@ void AP_Filesystem_FlashMemory_LittleFS::mark_dead() #define W25NXX_TIMEOUT_BLOCK_ERASE_MS 10 // tBEmax = 10ms #define W25NXX_TIMEOUT_RESET_MS 500 // tRSTmax = 500ms +bool AP_Filesystem_FlashMemory_LittleFS::sync_block(int _write_fd, uint32_t _write_offset, uint32_t& nbytes) +{ + // see https://github.com/littlefs-project/littlefs/issues/564#issuecomment-2363032827 + // n = (N − w/8 ( popcount( N/(B − 2w/8) − 1) + 2))/(B − 2w/8)) + // off = N − ( B − 2w/8 ) n − w/8popcount( n ) +#define BLOCK_INDEX(N, B) \ + (N - sizeof(uint32_t) * (__builtin_popcount(N/(B - 2 * sizeof(uint32_t)) -1) + 2))/(B - 2 * sizeof(uint32_t)) + +#define BLOCK_OFFSET(N, B, n) \ + (N - (B - 2*sizeof(uint32_t)) * n - sizeof(uint32_t) * __builtin_popcount(n)) + + uint32_t blocksize = fs_cfg.block_size; + uint32_t block_index = BLOCK_INDEX(_write_offset, blocksize); + uint32_t block_offset = BLOCK_OFFSET(_write_offset, blocksize, block_index); + if (blocksize - block_offset <= nbytes) { + if (blocksize == block_offset) { + // exactly at the end of the block, sync and then write all the data + AP::FS().fsync(_write_fd); + return false; + } else { + // near the end of the block, fill in the remaining gap + nbytes = blocksize - block_offset; + return true; + } + } + return false; +} + bool AP_Filesystem_FlashMemory_LittleFS::is_busy() { WITH_SEMAPHORE(dev_sem); @@ -1110,4 +1148,18 @@ static int lfs_flags_from_flags(int flags) return outflags; } +// get_singleton for scripting +AP_Filesystem_FlashMemory_LittleFS *AP_Filesystem_FlashMemory_LittleFS::get_singleton(void) +{ + return singleton; +} + +namespace AP +{ +AP_Filesystem_FlashMemory_LittleFS &littlefs() +{ + return *AP_Filesystem_FlashMemory_LittleFS::get_singleton(); +} +} + #endif // AP_FILESYSTEM_LITTLEFS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h index 351e5cca4c082..2f99a8a667719 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h @@ -25,6 +25,7 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend { public: + AP_Filesystem_FlashMemory_LittleFS(); // functions that closely match the equivalent posix calls int open(const char *fname, int flags, bool allow_absolute_paths = false) override; int close(int fd) override; @@ -47,7 +48,7 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend // set modification time on a file bool set_mtime(const char *filename, const uint32_t mtime_sec) override; - uint32_t get_sync_size() const override { return fs_cfg.block_size; } + bool sync_block(int _write_fd, uint32_t _write_offset, uint32_t& nbytes); bool retry_mount(void) override; void unmount(void) override; @@ -60,6 +61,9 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend int _flashmem_erase(lfs_block_t block); int _flashmem_sync(); + // get_singleton for scripting + static AP_Filesystem_FlashMemory_LittleFS *get_singleton(void); + private: // JEDEC ID of the flash memory, JEDEC_ID_UNKNOWN if not known or not supported uint32_t jedec_id; @@ -102,6 +106,8 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend bool use_32bit_address; FormatStatus format_status; + static AP_Filesystem_FlashMemory_LittleFS* singleton; + int allocate_fd(); int free_fd(int fd); void free_all_fds(); @@ -122,4 +128,8 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend void mark_dead(); }; +namespace AP { + AP_Filesystem_FlashMemory_LittleFS &littlefs(); +}; + #endif // #if AP_FILESYSTEM_LITTLEFS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_backend.h b/libraries/AP_Filesystem/AP_Filesystem_backend.h index d59fce4244bfc..8cf0fa655a1cf 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_backend.h +++ b/libraries/AP_Filesystem/AP_Filesystem_backend.h @@ -96,9 +96,6 @@ class AP_Filesystem_Backend { // unload data from load_file() virtual void unload_file(FileData *fd); - // if non-zero size at which syncs should be peformed, only used by flash fs - virtual uint32_t get_sync_size() const { return 0; } - protected: // return true if file operations are allowed bool file_op_allowed(void) const; From b556ffd1e03b500e67820d4cfb28fb12add82a6d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 11 Dec 2024 18:26:22 +0000 Subject: [PATCH 39/77] AP_Logger: user sync_block from littlefs to decide when to sync when using littlefs --- libraries/AP_Logger/AP_Logger_File.cpp | 29 +++++--------------------- 1 file changed, 5 insertions(+), 24 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 71d8e8d605a42..f87b6ed19e39b 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -16,6 +16,9 @@ #include #include +#if AP_FILESYSTEM_LITTLEFS_ENABLED +#include +#endif #include "AP_Logger.h" #include "AP_Logger_File.h" @@ -984,29 +987,7 @@ void AP_Logger_File::io_timer(void) } #if AP_FILESYSTEM_LITTLEFS_ENABLED - // see https://github.com/littlefs-project/littlefs/issues/564#issuecomment-2363032827 - // n = (N − w/8 ( popcount( N/(B − 2w/8) − 1) + 2))/(B − 2w/8)) - // off = N − ( B − 2w/8 ) n − w/8popcount( n ) -#define BLOCK_INDEX(N, B) \ - (N - sizeof(uint32_t) * (__builtin_popcount(N/(B - 2 * sizeof(uint32_t)) -1) + 2))/(B - 2 * sizeof(uint32_t)) - -#define BLOCK_OFFSET(N, B, n) \ - (N - (B - 2*sizeof(uint32_t)) * n - sizeof(uint32_t) * __builtin_popcount(n)) - - uint32_t blocksize = AP::FS().get_sync_size(); - uint32_t block_index = BLOCK_INDEX(_write_offset, blocksize); - uint32_t block_offset = BLOCK_OFFSET(_write_offset, blocksize, block_index); - bool end_of_block = false; - if (blocksize - block_offset <= nbytes) { - if (blocksize == block_offset) { - // exactly at the end of the block, sync and then write all the data - AP::FS().fsync(_write_fd); - } else { - // near the end of the block, fill in the remaining gap - nbytes = blocksize - block_offset; - end_of_block = true; - } - } + bool sync_block = AP::littlefs().sync_block(_write_fd, _write_offset, nbytes); #endif // AP_FILESYSTEM_LITTLEFS_ENABLED ssize_t nwritten = AP::FS().write(_write_fd, head, nbytes); @@ -1034,7 +1015,7 @@ void AP_Logger_File::io_timer(void) required since the whole point of the filesystem is to avoid corruption */ #if AP_FILESYSTEM_LITTLEFS_ENABLED - if (end_of_block) + if (sync_block) #endif // AP_FILESYSTEM_LITTLEFS_ENABLED #if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE { From 1abe46e9239967209d716cce21c74269695c0338 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 11 Dec 2024 20:10:03 +0000 Subject: [PATCH 40/77] AP_HAL_ChibiOS: terrain turned on on 2Mb boards with littlefs --- libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 1 - libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h | 7 ++++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index e9f227d6358b6..c4cdc3df85842 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -989,7 +989,6 @@ def write_mcu_config(self, f): f.write('#define HAL_USE_SDC FALSE\n') self.build_flags.append('USE_FATFS=no') self.env_vars['WITH_LITTLEFS'] = "1" - self.env_vars['DISABLE_SCRIPTING'] = True else: f.write('#define HAL_USE_SDC FALSE\n') self.build_flags.append('USE_FATFS=no') diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h index 82d108d9024bd..373c30497e280 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h @@ -11,13 +11,18 @@ #endif // a similar define is present in AP_HAL_Boards.h: +// needed to compile chibios #ifndef HAL_OS_FATFS_IO #define HAL_OS_FATFS_IO 0 #endif +#ifndef HAL_OS_LITTLEFS_IO +#define HAL_OS_LITTLEFS_IO 0 +#endif + #ifndef AP_TERRAIN_AVAILABLE // enable terrain only if there's an SD card available: -#define AP_TERRAIN_AVAILABLE HAL_OS_FATFS_IO +#define AP_TERRAIN_AVAILABLE (HAL_OS_FATFS_IO || (HAL_OS_LITTLEFS_IO && (BOARD_FLASH_SIZE>1024))) #endif #if AP_TERRAIN_AVAILABLE From 955448704b5b997ca232953e52cce60a7738b953 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 11 Dec 2024 20:10:26 +0000 Subject: [PATCH 41/77] AP_HAL_ChibiOS: turn off storage backup on anything other than FATFS --- libraries/AP_HAL_ChibiOS/Storage.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 2663403227878..e1ffb337f1ccc 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -42,12 +42,8 @@ extern const AP_HAL::HAL& hal; #endif #ifndef HAL_STORAGE_BACKUP_COUNT -#if AP_FILESYSTEM_LITTLEFS_ENABLED -#define HAL_STORAGE_BACKUP_COUNT 0 -#else #define HAL_STORAGE_BACKUP_COUNT 100 #endif -#endif #define STORAGE_FLASH_RETRIES 5 @@ -131,7 +127,7 @@ void Storage::_storage_open(void) */ void Storage::_save_backup(void) { -#ifdef USE_POSIX +#ifdef USE_POSIX_FATFS // allow for fallback to microSD or dataflash based storage // create the backup directory if need be int ret; From 3330c1b11377f93f95d2b36bea3e402b5df82885 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 11 Dec 2024 20:15:05 +0000 Subject: [PATCH 42/77] AP_HAL_ChibiOS: littlefs support for Aocoda-RC-H743Dual --- libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat index f31999bc43395..045040643c427 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat @@ -218,7 +218,5 @@ define HAL_OSD_TYPE_DEFAULT 1 ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin # enable logging to dataflash -define HAL_LOGGING_DATAFLASH_ENABLED 1 -define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_W25NXX - - +define HAL_OS_LITTLEFS_IO 1 +define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX From cf2ebfacc9a9be2171ba1b331a4224380ee8f4b4 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 11 Dec 2024 20:15:33 +0000 Subject: [PATCH 43/77] AP_HAL_ChibiOS: littlefs support for JHEMCU-H743HD --- libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat index 5d7c65b52855e..f2c5276091a62 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat @@ -145,8 +145,8 @@ define HAL_GPIO_B_LED_PIN 91 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ -define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_W25NXX -define HAL_LOGGING_DATAFLASH_ENABLED 1 +define HAL_OS_LITTLEFS_IO 1 +define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX # OSD setup SPIDEV osd SPI2 DEVID1 OSD1_CS MODE0 10*MHZ 10*MHZ From 85f4b563e0457f5682fb96aebeb3e96ab81b58d6 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 12 Dec 2024 09:50:41 +0000 Subject: [PATCH 44/77] AP_HAL_ChibiOS: littlefs support for KakuteH7v2 --- libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat index 2a40cc91f5912..6e4077aec32fd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat @@ -64,7 +64,5 @@ DMA_NOSHARE *UP SPI1* # Motor order implies Betaflight/X for standard ESCs define HAL_FRAME_TYPE_DEFAULT 12 -define HAL_LOGGING_DATAFLASH_ENABLED 1 -define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_W25NXX - -define AP_SCRIPTING_ENABLED 0 +define HAL_OS_LITTLEFS_IO 1 +define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX From 0c541b72fd8d5bade3c1af5698b8df42523fb308 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 12 Dec 2024 20:56:10 +0000 Subject: [PATCH 45/77] AP_Filesystem: provide SITL implementation for littlefs --- libraries/AP_Filesystem/AP_Filesystem.cpp | 6 +- libraries/AP_Filesystem/AP_Filesystem.h | 6 ++ .../AP_Filesystem_FlashMemory_LittleFS.cpp | 57 ++++++++++++++++--- 3 files changed, 57 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index dac87e0d5ce28..edcbd1e0e3782 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -32,12 +32,12 @@ static AP_Filesystem_FATFS fs_local; #elif AP_FILESYSTEM_ESP32_ENABLED #include "AP_Filesystem_ESP32.h" static AP_Filesystem_ESP32 fs_local; -#elif AP_FILESYSTEM_POSIX_ENABLED -#include "AP_Filesystem_posix.h" -static AP_Filesystem_Posix fs_local; #elif AP_FILESYSTEM_LITTLEFS_ENABLED #include "AP_Filesystem_FlashMemory_LittleFS.h" static AP_Filesystem_FlashMemory_LittleFS fs_local; +#elif AP_FILESYSTEM_POSIX_ENABLED +#include "AP_Filesystem_posix.h" +static AP_Filesystem_Posix fs_local; #else static AP_Filesystem_Backend fs_local; int errno; diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index cc8619ca52152..be4147a450f9a 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -38,6 +38,9 @@ #if AP_FILESYSTEM_FATFS_ENABLED #include "AP_Filesystem_FATFS.h" #endif +#if AP_FILESYSTEM_LITTLEFS_ENABLED +#include "AP_Filesystem_FlashMemory_LittleFS.h" +#endif struct dirent { char d_name[MAX_NAME_LEN]; /* filename */ @@ -56,6 +59,9 @@ struct dirent { #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_QURT #include "AP_Filesystem_posix.h" +#if AP_FILESYSTEM_LITTLEFS_ENABLED +#include "AP_Filesystem_FlashMemory_LittleFS.h" +#endif #endif #if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp index d375042f9b046..b008cf0cf452b 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp @@ -16,7 +16,8 @@ ArduPilot filesystem interface for systems using the LittleFS filesystem in flash memory - littlefs integration by Tamas Nepusz + Original littlefs integration by Tamas Nepusz + Further development by Andy Piper */ #include "AP_Filesystem_config.h" @@ -28,6 +29,10 @@ #include #include "lfs.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include "bd/lfs_filebd.h" +#include +#endif //#define AP_LFS_DEBUG #ifdef AP_LFS_DEBUG @@ -42,6 +47,7 @@ #define LFS_CHECK_NULL(func) do { int __retval = func; if (__retval < 0) { errno = errno_from_lfs_error(__retval); return nullptr; }} while (0) #define LFS_ATTR_MTIME 'M' +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL static int flashmem_read( const struct lfs_config *cfg, lfs_block_t block, lfs_off_t off, void* buffer, lfs_size_t size @@ -52,7 +58,7 @@ static int flashmem_prog( ); static int flashmem_erase(const struct lfs_config *cfg, lfs_block_t block); static int flashmem_sync(const struct lfs_config *cfg); - +#endif static int errno_from_lfs_error(int lfs_error); static int lfs_flags_from_flags(int flags); @@ -341,13 +347,13 @@ struct dirent *AP_Filesystem_FlashMemory_LittleFS::readdir(void *ptr) memset(&pair->entry, 0, sizeof(pair->entry)); -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX pair->entry.d_ino = 0; pair->entry.d_seekoff++; #endif strncpy(pair->entry.d_name, info.name, MIN(strlen(info.name)+1, sizeof(pair->entry.d_name))); -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX pair->entry.d_namlen = strlen(info.name); #endif @@ -662,7 +668,17 @@ void AP_Filesystem_FlashMemory_LittleFS::write_status_register(uint8_t reg, uint dev->transfer(cmd, 3, nullptr, 0); } +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +const static struct lfs_filebd_config fbd_config { + .read_size = 2048, + .prog_size = 2048, + .erase_size = 131072, + .erase_count = 256 +}; +#endif + uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL if (!wait_until_device_is_ready()) { return false; } @@ -754,6 +770,20 @@ uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { #if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX fs_cfg.metadata_max = page_size; #endif +#else + fs_cfg.read_size = 2048; + fs_cfg.prog_size = 2048; + fs_cfg.block_size = 131072; + fs_cfg.block_count = 256; + fs_cfg.metadata_max = 2048; + + char lfsname[L_tmpnam]; + uint32_t id = 0; + if (std::tmpnam(lfsname)) { + lfs_filebd_create(&fs_cfg, lfsname, &fbd_config); + id = 0xFAFF; + } +#endif // CONFIG_HAL_BOARD != HAL_BOARD_SITL fs_cfg.block_cycles = 500; fs_cfg.lookahead_size = 16; @@ -761,7 +791,7 @@ uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { // occasionally get read or prog requests in flashmem_read() and // flashmem_prog() whose size is equal to the cache size, and we do not // handle that right now - fs_cfg.cache_size = page_size; + fs_cfg.cache_size = fs_cfg.prog_size; return id; } @@ -778,19 +808,26 @@ bool AP_Filesystem_FlashMemory_LittleFS::mount_filesystem() { EXPECT_DELAY_MS(3000); fs_cfg.context = this; - +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL fs_cfg.read = flashmem_read; fs_cfg.prog = flashmem_prog; fs_cfg.erase = flashmem_erase; fs_cfg.sync = flashmem_sync; dev = hal.spi->get_device("dataflash"); + if (!dev) { mark_dead(); return false; } dev_sem = dev->get_semaphore(); +#else + fs_cfg.read = lfs_filebd_read; + fs_cfg.prog = lfs_filebd_prog; + fs_cfg.erase = lfs_filebd_erase; + fs_cfg.sync = lfs_filebd_sync; +#endif uint32_t id = find_block_size_and_count(); @@ -798,12 +835,12 @@ bool AP_Filesystem_FlashMemory_LittleFS::mount_filesystem() { mark_dead(); return false; } - +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL if (!init_flash()) { mark_dead(); return false; } - +#endif if (lfs_mount(&fs, &fs_cfg) < 0) { /* maybe not formatted? try formatting it */ GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Formatting flash"); @@ -1063,6 +1100,7 @@ int AP_Filesystem_FlashMemory_LittleFS::_flashmem_sync() { } } +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL static int flashmem_read( const struct lfs_config *cfg, lfs_block_t block, lfs_off_t off, void* buffer, lfs_size_t size @@ -1088,6 +1126,7 @@ static int flashmem_sync(const struct lfs_config *cfg) { AP_Filesystem_FlashMemory_LittleFS* self = static_cast(cfg->context); return self->_flashmem_sync(); } +#endif /* ************************************************************************* */ /* LittleFS to POSIX API conversion functions */ @@ -1109,7 +1148,7 @@ static int errno_from_lfs_error(int lfs_error) case LFS_ERR_INVAL: return EINVAL; case LFS_ERR_NOSPC: return ENOSPC; case LFS_ERR_NOMEM: return ENOMEM; -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX case LFS_ERR_NOATTR: return ENOATTR; #endif case LFS_ERR_NAMETOOLONG: return ENAMETOOLONG; From 49c61bc729e1be3f3af53b88ae6f34142f27d72c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 12 Dec 2024 20:56:47 +0000 Subject: [PATCH 46/77] AP_Logger: support SITL implementation of littlefs --- libraries/AP_Logger/AP_Logger_File.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index f87b6ed19e39b..aab5a15585295 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -986,7 +986,7 @@ void AP_Logger_File::io_timer(void) return; } -#if AP_FILESYSTEM_LITTLEFS_ENABLED +#if AP_FILESYSTEM_LITTLEFS_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL bool sync_block = AP::littlefs().sync_block(_write_fd, _write_offset, nbytes); #endif // AP_FILESYSTEM_LITTLEFS_ENABLED @@ -1014,10 +1014,10 @@ void AP_Logger_File::io_timer(void) fsync on littlefs is extremely expensive (20% CPU on an H7) and not required since the whole point of the filesystem is to avoid corruption */ +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE #if AP_FILESYSTEM_LITTLEFS_ENABLED if (sync_block) #endif // AP_FILESYSTEM_LITTLEFS_ENABLED -#if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE { last_io_operation = "fsync"; AP::FS().fsync(_write_fd); From d1dd9229157d94c0f5a0945244f74c3566c5c3a5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 12 Dec 2024 20:57:05 +0000 Subject: [PATCH 47/77] waf: build littlefs filebd --- Tools/ardupilotwaf/littlefs.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/littlefs.py b/Tools/ardupilotwaf/littlefs.py index 864983e20ffab..c4f488db5bc57 100644 --- a/Tools/ardupilotwaf/littlefs.py +++ b/Tools/ardupilotwaf/littlefs.py @@ -17,7 +17,7 @@ def configure(cfg): def littlefs(bld, **kw): kw.update( name='littlefs', - source=['modules/littlefs/lfs.c', 'modules/littlefs/lfs_util.c'], + source=['modules/littlefs/lfs.c', 'modules/littlefs/lfs_util.c', 'modules/littlefs/bd/lfs_filebd.c'], target='littlefs', defines=['LFS_NO_DEBUG'], cflags=['-Wno-format', '-Wno-format-extra-args', '-Wno-shadow', '-Wno-unused-function', '-Wno-missing-declarations'] From 8a3e53c1634337a0af914b872280bb3895376e3f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 13 Dec 2024 10:13:52 +0000 Subject: [PATCH 48/77] AP_Logger: nuance minspace for W25NXX and W25QXX --- libraries/AP_Logger/AP_Logger.cpp | 8 ++++++-- libraries/AP_Logger/AP_Logger_File.cpp | 12 ++++++++++-- libraries/AP_Logger/AP_Logger_File.h | 6 +++++- 3 files changed, 21 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 3d834946edf5c..f19a3636f0cd5 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -58,7 +58,11 @@ extern const AP_HAL::HAL& hal; #ifndef HAL_LOGGER_MIN_MB_FREE #if AP_FILESYSTEM_LITTLEFS_ENABLED -#define HAL_LOGGER_MIN_MB_FREE 1 +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX +#define HAL_LOGGER_MIN_MB_FREE 10 +#else +#define HAL_LOGGER_MIN_MB_FREE 2 +#endif #else #define HAL_LOGGER_MIN_MB_FREE 500 #endif @@ -143,7 +147,7 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = { // @DisplayName: Old logs on the SD card will be deleted to maintain this amount of free space // @Description: Set this such that the free space is larger than your largest typical flight log // @Units: MB - // @Range: 10 1000 + // @Range: 2 1000 // @User: Standard AP_GROUPINFO("_FILE_MB_FREE", 7, AP_Logger, _params.min_MB_free, HAL_LOGGER_MIN_MB_FREE), diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index aab5a15585295..30a53b7a8fad6 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -1011,14 +1011,22 @@ void AP_Logger_File::io_timer(void) _writebuf.advance(nwritten); /* - fsync on littlefs is extremely expensive (20% CPU on an H7) and not - required since the whole point of the filesystem is to avoid corruption + fsync on littlefs is extremely expensive (20% CPU on an H7) particularly because the + COW architecture can mean you end up copying a load of blocks. instead try and only sync + at the end of a block to avoid copying and minimise CPU. fsync is needed for the file + metadata (including size) to be updated */ #if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE #if AP_FILESYSTEM_LITTLEFS_ENABLED if (sync_block) #endif // AP_FILESYSTEM_LITTLEFS_ENABLED { + /* + the best strategy for minimizing corruption on microSD cards + seems to be to write in 4k chunks and fsync the file on each + chunk, ensuring the directory entry is updated after each + write. + */ last_io_operation = "fsync"; AP::FS().fsync(_write_fd); last_io_operation = ""; diff --git a/libraries/AP_Logger/AP_Logger_File.h b/libraries/AP_Logger/AP_Logger_File.h index c465b96b3457a..77732b1860008 100644 --- a/libraries/AP_Logger/AP_Logger_File.h +++ b/libraries/AP_Logger/AP_Logger_File.h @@ -130,7 +130,11 @@ class AP_Logger_File : public AP_Logger_Backend uint32_t _free_space_last_check_time; // milliseconds const uint32_t _free_space_check_interval = 1000UL; // milliseconds #if AP_FILESYSTEM_LITTLEFS_ENABLED - const uint32_t _free_space_min_avail = 4096; // bytes +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + const uint32_t _free_space_min_avail = 1024 * 1024; // bytes +#else + const uint32_t _free_space_min_avail = 1024 * 256; // bytes +#endif #else const uint32_t _free_space_min_avail = 8388608; // bytes #endif From ce7f4db7ee85eab6598e6c9a4f62a1a514827720 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 13 Dec 2024 19:50:30 +0000 Subject: [PATCH 49/77] waf: optionally add littlefs to sitl build options --- Tools/ardupilotwaf/boards.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index b3382ee88370c..6ee1292332311 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -826,6 +826,9 @@ def configure_env(self, cfg, env): cfg.fatal("Failed to find SFML Audio libraries") env.CXXFLAGS += ['-DWITH_SITL_TONEALARM'] + if cfg.options.sitl_littlefs: + env.CXXFLAGS += ['-DHAL_OS_LITTLEFS_IO=1'] + if cfg.env.DEST_OS == 'cygwin': env.LIB += [ 'winmm', From a2de66b8d215b101fed4420703426cc5eb557d76 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 13 Dec 2024 19:50:59 +0000 Subject: [PATCH 50/77] wscript: add littlefs build option for sitl --- wscript | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/wscript b/wscript index 803f7dbaf57f9..45ad476dfc77c 100644 --- a/wscript +++ b/wscript @@ -303,6 +303,11 @@ submodules at specific revisions. g.add_option('--enable-dronecan-tests', action='store_true', default=False, help="Enables DroneCAN tests in sitl") + + g.add_option('--sitl-littlefs', action='store_true', + default=False, + help="Enable littlefs for filesystem accesson SITL") + g = opt.ap_groups['linux'] linux_options = ('--prefix', '--destdir', '--bindir', '--libdir') @@ -565,6 +570,15 @@ def configure(cfg): if cfg.options.enable_benchmarks: cfg.load('gbenchmark') cfg.load('gtest') + + if cfg.env.BOARD == "sitl": + cfg.start_msg('Littlefs') + + if cfg.options.sitl_littlefs: + cfg.end_msg('enabled') + else: + cfg.end_msg('disabled', color='YELLOW') + cfg.load('littlefs') cfg.load('static_linking') cfg.load('build_summary') From f22e1ff38b83e272a015ff3a5a2d35e7b78a1d74 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 18 Dec 2024 13:41:09 +0000 Subject: [PATCH 51/77] AP_Filesystem: support mutiple reads/writes in littlefs improve performance by avoiding block validation on writes --- .../AP_Filesystem_FlashMemory_LittleFS.cpp | 131 ++++++++++-------- 1 file changed, 76 insertions(+), 55 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp index b008cf0cf452b..60ac04530dc6b 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp @@ -785,13 +785,8 @@ uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { } #endif // CONFIG_HAL_BOARD != HAL_BOARD_SITL fs_cfg.block_cycles = 500; - fs_cfg.lookahead_size = 16; - - // cache_size has to be the same as the page_size, otherwise we would - // occasionally get read or prog requests in flashmem_read() and - // flashmem_prog() whose size is equal to the cache size, and we do not - // handle that right now - fs_cfg.cache_size = fs_cfg.prog_size; + fs_cfg.lookahead_size = 128; + fs_cfg.cache_size = 2*fs_cfg.prog_size; return id; } @@ -985,88 +980,112 @@ bool AP_Filesystem_FlashMemory_LittleFS::init_flash() return true; } +#ifdef AP_LFS_DEBUG +static uint32_t block_writes; +static uint32_t last_write_msg_ms; +uint32_t page_reads; +#endif int AP_Filesystem_FlashMemory_LittleFS::_flashmem_read( lfs_block_t block, lfs_off_t off, void* buffer, lfs_size_t size ) { uint32_t address; + lfs_off_t read_off = 0; if (dead) { return LFS_ERR_IO; } address = lfs_block_and_offset_to_raw_flash_address(block, off); -#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX - /* We need to read an entire page into an internal buffer and then read - * that buffer with JEDEC_READ_DATA later */ - if (!wait_until_device_is_ready()) { - return LFS_ERR_IO; - } - { - WITH_SEMAPHORE(dev_sem); - send_command_addr(JEDEC_PAGE_DATA_READ, address / fs_cfg.read_size); - address %= fs_cfg.read_size; - } + EXPECT_DELAY_MS((25*size)/(fs_cfg.read_size*1000)); + + while (size > 0) { + uint32_t read_size = MIN(size, fs_cfg.read_size); +#ifdef AP_LFS_DEBUG + page_reads++; #endif - if (!wait_until_device_is_ready()) { - return LFS_ERR_IO; - } +#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX + /* We need to read an entire page into an internal buffer and then read + * that buffer with JEDEC_READ_DATA later */ + if (!wait_until_device_is_ready()) { + return LFS_ERR_IO; + } + { + WITH_SEMAPHORE(dev_sem); + send_command_addr(JEDEC_PAGE_DATA_READ, address / fs_cfg.read_size); + } +#endif + if (!wait_until_device_is_ready()) { + return LFS_ERR_IO; + } - WITH_SEMAPHORE(dev_sem); + WITH_SEMAPHORE(dev_sem); - dev->set_chip_select(true); - send_command_addr(JEDEC_READ_DATA, address); - dev->transfer(nullptr, 0, static_cast(buffer), size); - dev->set_chip_select(false); + dev->set_chip_select(true); + send_command_addr(JEDEC_READ_DATA, address % fs_cfg.read_size); + dev->transfer(nullptr, 0, static_cast(buffer)+read_off, size); + dev->set_chip_select(false); + size -= read_size; + address += read_size; + read_off += read_size; + } return LFS_ERR_OK; } -#ifdef AP_LFS_DEBUG -static uint32_t block_writes; -static uint32_t last_write_msg_ms; -#endif int AP_Filesystem_FlashMemory_LittleFS::_flashmem_prog( lfs_block_t block, lfs_off_t off, const void* buffer, lfs_size_t size ) { uint32_t address; + lfs_off_t prog_off = 0; if (dead) { return LFS_ERR_IO; } - if (!write_enable()) { - return LFS_ERR_IO; - } + EXPECT_DELAY_MS((250*size)/(fs_cfg.read_size*1000)); + + address = lfs_block_and_offset_to_raw_flash_address(block, off); + + while (size > 0) { + uint32_t prog_size = MIN(size, fs_cfg.prog_size); + if (!write_enable()) { + return LFS_ERR_IO; + } #ifdef AP_LFS_DEBUG - block_writes++; - if (AP_HAL::millis() - last_write_msg_ms > 5000) { - debug("LFS: writes %lukB/s, pages %lu/s\n", (block_writes*fs_cfg.prog_size)/(5*1024), block_writes/5); - block_writes = 0; - last_write_msg_ms = AP_HAL::millis(); - } + block_writes++; + if (AP_HAL::millis() - last_write_msg_ms > 5000) { + debug("LFS: writes %lukB/s, pages %lu/s (reads %lu/s)", + (block_writes*fs_cfg.prog_size)/(5*1024), block_writes/5, page_reads/5); + block_writes = 0; + page_reads = 0; + last_write_msg_ms = AP_HAL::millis(); + } #endif - address = lfs_block_and_offset_to_raw_flash_address(block, off); - WITH_SEMAPHORE(dev_sem); + WITH_SEMAPHORE(dev_sem); #if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX - /* First we need to write into the data buffer at column address zero, - * then we need to issue PROGRAM_EXECUTE to commit the internal buffer */ - dev->set_chip_select(true); - send_command_page(JEDEC_PAGE_WRITE, address % fs_cfg.prog_size); - dev->transfer(static_cast(buffer), size, nullptr, 0); - dev->set_chip_select(false); - send_command_addr(JEDEC_PROGRAM_EXECUTE, address / fs_cfg.prog_size); - // this simply means the data is in the internal cache, it will take some period to - // propagate to the flash itself + /* First we need to write into the data buffer at column address zero, + * then we need to issue PROGRAM_EXECUTE to commit the internal buffer */ + dev->set_chip_select(true); + send_command_page(JEDEC_PAGE_WRITE, address % fs_cfg.prog_size); + dev->transfer(static_cast(buffer)+prog_off, prog_size, nullptr, 0); + dev->set_chip_select(false); + send_command_addr(JEDEC_PROGRAM_EXECUTE, address / fs_cfg.prog_size); + // this simply means the data is in the internal cache, it will take some period to + // propagate to the flash itself #else - dev->set_chip_select(true); - send_command_addr(JEDEC_PAGE_WRITE, address); - dev->transfer(static_cast(buffer), size, nullptr, 0); - dev->set_chip_select(false); + dev->set_chip_select(true); + send_command_addr(JEDEC_PAGE_WRITE, address); + dev->transfer(static_cast(buffer)+prog_off, prog_size, nullptr, 0); + dev->set_chip_select(false); #endif + size -= prog_size; + address += prog_size; + prog_off += prog_size; + } return LFS_ERR_OK; } @@ -1075,6 +1094,8 @@ int AP_Filesystem_FlashMemory_LittleFS::_flashmem_erase(lfs_block_t block) { return LFS_ERR_IO; } + EXPECT_DELAY_MS(2); + if (!write_enable()) { return LFS_ERR_IO; } @@ -1161,7 +1182,7 @@ static int lfs_flags_from_flags(int flags) int outflags = 0; if (flags & O_WRONLY) { - outflags |= LFS_O_WRONLY; + outflags |= LFS_O_WRONLY | LFS_F_WRUNCHECKED; } else if (flags & O_RDWR) { outflags |= LFS_O_RDWR; } else { From 36b9bf673653e062165316e097458912865f9b85 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 18 Dec 2024 13:41:55 +0000 Subject: [PATCH 52/77] AP_Logger: avoid disk_free() checks on littlefs provide more meaningful feedback if write() results in ENOSPC address review comments --- libraries/AP_Logger/AP_Logger_File.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 30a53b7a8fad6..e32d293fa7621 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -945,6 +945,7 @@ void AP_Logger_File::io_timer(void) return; } +#if !AP_FILESYSTEM_LITTLEFS_ENABLED // too expensive on littlefs, rely on ENOSPC below if (tnow - _free_space_last_check_time > _free_space_check_interval) { _free_space_last_check_time = tnow; last_io_operation = "disk_space_avail"; @@ -957,7 +958,7 @@ void AP_Logger_File::io_timer(void) } last_io_operation = ""; } - +#endif _last_write_time = tnow; if (nbytes > _writebuf_chunk) { // be kind to the filesystem layer @@ -993,7 +994,12 @@ void AP_Logger_File::io_timer(void) ssize_t nwritten = AP::FS().write(_write_fd, head, nbytes); last_io_operation = ""; if (nwritten <= 0) { - if ((tnow - _last_write_ms)/1000U > unsigned(_front._params.file_timeout)) { + if (errno == ENOSPC) { + DEV_PRINTF("Out of space for logging\n"); + stop_logging(); + _open_error_ms = AP_HAL::millis(); // prevent logging starting again for 5s + last_io_operation = ""; + } else if ((tnow - _last_write_ms)/1000U > unsigned(_front._params.file_timeout)) { // if we can't write for LOG_FILE_TIMEOUT seconds we give up and close // the file. This allows us to cope with temporary write // failures caused by directory listing From 90ac6bdc9ec25e5c4b74608487aac01c0b58ef90 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Thu, 9 Jan 2025 10:22:18 -0600 Subject: [PATCH 53/77] AP_Filesystem: lock littlefs between opendir and closedir Avoids issues with (alleged) corruption if writes happen between. --- .../AP_Filesystem_FlashMemory_LittleFS.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp index 60ac04530dc6b..5f0f8187aafcb 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp @@ -325,6 +325,16 @@ void *AP_Filesystem_FlashMemory_LittleFS::opendir(const char *pathdir) result->entry.d_reclen = sizeof(result->entry); #endif + // LittleFS has issues with opendir where filesystem operations that trigger + // writes while a directory is open can break the iteration and cause + // LittleFS to report filesystem corruption. We hope readdir loops don't do + // writes (none do in ArduPilot at the time of writing), but also take the + // lock again so other threads can't write until the corresponding release + // in closedir. This is safe here since the lock is recursive; recursion + // also lets the thread with the directory open do reads. Hopefully this + // will be fixed upstream so we can remove this quirk. + fs_sem.take_blocking(); + return result; } @@ -373,6 +383,11 @@ int AP_Filesystem_FlashMemory_LittleFS::closedir(void *ptr) return 0; } + // Before the close, undo the lock we did in opendir so it's released even + // if the close fails. We don't undo it above, as the input being nullptr + // means we didn't successfully open the directory and lock. + fs_sem.give(); + LFS_CHECK(lfs_dir_close(&fs, &pair->dir)); delete pair; From c971bb685003fb88cf32aa8535a245aa966b2036 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 20 Dec 2024 10:55:26 +0000 Subject: [PATCH 54/77] AP_Filesystem: allow for logical blocks bigger than physical blocks in littlefs. optimize configured defaults on littlefs and address review comments support lseek() in littlefs in a way that enables terrain to work rename file and directory structures in littlefs code in littlefs glue should be C++ rather than C check for strdup failure on littlefs --- .../AP_Filesystem_FlashMemory_LittleFS.cpp | 224 +++++++++--------- .../AP_Filesystem_FlashMemory_LittleFS.h | 18 +- 2 files changed, 127 insertions(+), 115 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp index 5f0f8187aafcb..e7cc3b870a419 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp @@ -28,7 +28,6 @@ #include #include -#include "lfs.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include "bd/lfs_filebd.h" #include @@ -46,6 +45,7 @@ #define LFS_CHECK(func) do { int __retval = func; if (__retval < 0) { errno = errno_from_lfs_error(__retval); return -1; }} while (0) #define LFS_CHECK_NULL(func) do { int __retval = func; if (__retval < 0) { errno = errno_from_lfs_error(__retval); return nullptr; }} while (0) #define LFS_ATTR_MTIME 'M' +#define LFS_FLASH_BLOCKS_PER_BLOCK 1 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL static int flashmem_read( @@ -76,20 +76,17 @@ AP_Filesystem_FlashMemory_LittleFS::AP_Filesystem_FlashMemory_LittleFS() int AP_Filesystem_FlashMemory_LittleFS::open(const char *pathname, int flags, bool allow_absolute_path) { - int fd, retval; - file_descriptor* fp; - FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); ENSURE_MOUNTED(); - fd = allocate_fd(); + int fd = allocate_fd(); if (fd < 0) { return -1; } - fp = lfs_file_from_fd(fd); + FileDescriptor* fp = lfs_file_from_fd(fd); if (fp == nullptr) { return -1; } @@ -110,11 +107,14 @@ int AP_Filesystem_FlashMemory_LittleFS::open(const char *pathname, int flags, bo .buffer = &fp->mtime, .size = sizeof(fp->mtime) }; - // this is a cheat, in theory it could be dynamically allocated but is only used to find the descriptor - // for mtime and we know that's called with a name that doesn't disappear - fp->filename = pathname; + fp->filename = strdup(pathname); + if (fp->filename == nullptr) { + errno = ENOMEM; + free_fd(fd); + return -1; + } - retval = lfs_file_opencfg(&fs, &fp->file, pathname, lfs_flags_from_flags(flags), &fp->cfg); + int retval = lfs_file_opencfg(&fs, &fp->file, pathname, lfs_flags_from_flags(flags), &fp->cfg); if (retval < 0) { errno = errno_from_lfs_error(retval); @@ -128,18 +128,15 @@ int AP_Filesystem_FlashMemory_LittleFS::open(const char *pathname, int flags, bo int AP_Filesystem_FlashMemory_LittleFS::close(int fileno) { - file_descriptor* fp; - int retval; - FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); - fp = lfs_file_from_fd(fileno); + FileDescriptor* fp = lfs_file_from_fd(fileno); if (fp == nullptr) { return -1; } - retval = lfs_file_close(&fs, &(fp->file)); + int retval = lfs_file_close(&fs, &(fp->file)); if (retval < 0) { free_fd(fileno); // ignore error code, we have something else to report errno = errno_from_lfs_error(retval); @@ -155,19 +152,16 @@ int AP_Filesystem_FlashMemory_LittleFS::close(int fileno) int32_t AP_Filesystem_FlashMemory_LittleFS::read(int fd, void *buf, uint32_t count) { - file_descriptor* fp; - lfs_ssize_t read; - FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); ENSURE_MOUNTED(); - fp = lfs_file_from_fd(fd); + FileDescriptor* fp = lfs_file_from_fd(fd); if (fp == nullptr) { return -1; } - read = lfs_file_read(&fs, &(fp->file), buf, count); + lfs_ssize_t read = lfs_file_read(&fs, &(fp->file), buf, count); if (read < 0) { errno = errno_from_lfs_error(read); return -1; @@ -178,19 +172,16 @@ int32_t AP_Filesystem_FlashMemory_LittleFS::read(int fd, void *buf, uint32_t cou int32_t AP_Filesystem_FlashMemory_LittleFS::write(int fd, const void *buf, uint32_t count) { - file_descriptor* fp; - lfs_ssize_t written; - FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); ENSURE_MOUNTED(); - fp = lfs_file_from_fd(fd); + FileDescriptor* fp = lfs_file_from_fd(fd); if (fp == nullptr) { return -1; } - written = lfs_file_write(&fs, &(fp->file), buf, count); + lfs_ssize_t written = lfs_file_write(&fs, &(fp->file), buf, count); if (written < 0) { errno = errno_from_lfs_error(written); return -1; @@ -201,13 +192,11 @@ int32_t AP_Filesystem_FlashMemory_LittleFS::write(int fd, const void *buf, uint3 int AP_Filesystem_FlashMemory_LittleFS::fsync(int fd) { - file_descriptor* fp; - FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); ENSURE_MOUNTED(); - fp = lfs_file_from_fd(fd); + FileDescriptor* fp = lfs_file_from_fd(fd); if (fp == nullptr) { return -1; } @@ -218,29 +207,46 @@ int AP_Filesystem_FlashMemory_LittleFS::fsync(int fd) int32_t AP_Filesystem_FlashMemory_LittleFS::lseek(int fd, int32_t position, int whence) { - file_descriptor* fp; - FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); ENSURE_MOUNTED(); - fp = lfs_file_from_fd(fd); + FileDescriptor* fp = lfs_file_from_fd(fd); if (fp == nullptr) { return -1; } - LFS_CHECK(lfs_file_seek(&fs, &(fp->file), position, whence)); + int lfs_whence; + switch (whence) { + case SEEK_END: + lfs_whence = LFS_SEEK_SET; + break; + case SEEK_CUR: + lfs_whence = LFS_SEEK_CUR; + break; + case SEEK_SET: + default: + lfs_whence = LFS_SEEK_SET; + break; + } + + lfs_soff_t size = lfs_file_size(&fs, &(fp->file)); + // emulate SEEK_SET past the end by truncating and filling with zeros + if (position > size && whence == SEEK_SET) { + LFS_CHECK(lfs_file_truncate(&fs, &(fp->file), position)); + } + + LFS_CHECK(lfs_file_seek(&fs, &(fp->file), position, lfs_whence)); return 0; } int AP_Filesystem_FlashMemory_LittleFS::stat(const char *name, struct stat *buf) { - lfs_info info; - FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); ENSURE_MOUNTED(); + lfs_info info; LFS_CHECK(lfs_stat(&fs, name, &info)); memset(buf, 0, sizeof(*buf)); @@ -267,7 +273,7 @@ bool AP_Filesystem_FlashMemory_LittleFS::set_mtime(const char *filename, const u // unfortunately lfs_setattr will not work while the file is open, instead // we need to update the file config, but that means finding the file config for (int fd = 0; fd < MAX_OPEN_FILES; fd++) { - if (open_files[fd] != nullptr && open_files[fd]->filename == filename) { + if (open_files[fd] != nullptr && strcmp(open_files[fd]->filename, filename) == 0) { open_files[fd]->mtime = mtime_sec; return true; } @@ -293,26 +299,25 @@ int AP_Filesystem_FlashMemory_LittleFS::mkdir(const char *pathname) return 0; } -typedef struct { + +struct DirEntry { lfs_dir_t dir; struct dirent entry; -} lfs_dir_entry_pair; +}; void *AP_Filesystem_FlashMemory_LittleFS::opendir(const char *pathdir) { - int retval; - FS_CHECK_ALLOWED(nullptr); WITH_SEMAPHORE(fs_sem); ENSURE_MOUNTED_NULL(); - lfs_dir_entry_pair *result = new lfs_dir_entry_pair; + DirEntry *result = new DirEntry; if (!result) { errno = ENOMEM; return nullptr; } - retval = lfs_dir_open(&fs, &result->dir, pathdir); + int retval = lfs_dir_open(&fs, &result->dir, pathdir); if (retval < 0) { delete result; errno = errno_from_lfs_error(retval); @@ -338,22 +343,30 @@ void *AP_Filesystem_FlashMemory_LittleFS::opendir(const char *pathdir) return result; } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstringop-truncation" struct dirent *AP_Filesystem_FlashMemory_LittleFS::readdir(void *ptr) { FS_CHECK_ALLOWED(nullptr); WITH_SEMAPHORE(fs_sem); - lfs_info info; - lfs_dir_entry_pair *pair = static_cast(ptr); + DirEntry *pair = static_cast(ptr); if (!pair) { errno = EINVAL; return nullptr; } - if (!lfs_dir_read(&fs, &pair->dir, &info)) { + lfs_info info; + int retval = lfs_dir_read(&fs, &pair->dir, &info); + if (retval == 0) { /* no more entries */ return nullptr; } + if (retval < 0) { + // failure + errno = errno_from_lfs_error(retval); + return nullptr; + } memset(&pair->entry, 0, sizeof(pair->entry)); @@ -371,13 +384,14 @@ struct dirent *AP_Filesystem_FlashMemory_LittleFS::readdir(void *ptr) return &pair->entry; } +#pragma GCC diagnostic pop int AP_Filesystem_FlashMemory_LittleFS::closedir(void *ptr) { FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); - lfs_dir_entry_pair *pair = static_cast(ptr); + DirEntry *pair = static_cast(ptr); if (!pair) { errno = EINVAL; return 0; @@ -397,13 +411,11 @@ int AP_Filesystem_FlashMemory_LittleFS::closedir(void *ptr) int64_t AP_Filesystem_FlashMemory_LittleFS::disk_free(const char *path) { - lfs_ssize_t alloc_size; - FS_CHECK_ALLOWED(-1); WITH_SEMAPHORE(fs_sem); ENSURE_MOUNTED(); - alloc_size = lfs_fs_size(&fs); + lfs_ssize_t alloc_size = lfs_fs_size(&fs); if (alloc_size < 0) { errno = errno_from_lfs_error(alloc_size); return -1; @@ -439,9 +451,9 @@ void AP_Filesystem_FlashMemory_LittleFS::unmount(void) WITH_SEMAPHORE(fs_sem); if (mounted && !dead) { - if (lfs_unmount(&fs) >= 0) { - mounted = false; - } + free_all_fds(); + lfs_unmount(&fs); + mounted = false; } } @@ -451,11 +463,9 @@ void AP_Filesystem_FlashMemory_LittleFS::unmount(void) int AP_Filesystem_FlashMemory_LittleFS::allocate_fd() { - int fd; - - for (fd = 0; fd < MAX_OPEN_FILES; fd++) { + for (int fd = 0; fd < MAX_OPEN_FILES; fd++) { if (open_files[fd] == nullptr) { - open_files[fd] = static_cast(calloc(1, sizeof(file_descriptor))); + open_files[fd] = static_cast(calloc(1, sizeof(FileDescriptor))); if (open_files[fd] == nullptr) { errno = ENOMEM; return -1; @@ -471,29 +481,28 @@ int AP_Filesystem_FlashMemory_LittleFS::allocate_fd() int AP_Filesystem_FlashMemory_LittleFS::free_fd(int fd) { - file_descriptor* fp = lfs_file_from_fd(fd); + FileDescriptor* fp = lfs_file_from_fd(fd); if (!fp) { return -1; } + free(fp->filename); free(fp); - open_files[fd] = fp = nullptr; + open_files[fd] = nullptr; return 0; } void AP_Filesystem_FlashMemory_LittleFS::free_all_fds() { - int fd; - - for (fd = 0; fd < MAX_OPEN_FILES; fd++) { - if (open_files[fd] == nullptr) { + for (int fd = 0; fd < MAX_OPEN_FILES; fd++) { + if (open_files[fd] != nullptr) { free_fd(fd); } } } -AP_Filesystem_FlashMemory_LittleFS::file_descriptor* AP_Filesystem_FlashMemory_LittleFS::lfs_file_from_fd(int fd) const +AP_Filesystem_FlashMemory_LittleFS::FileDescriptor* AP_Filesystem_FlashMemory_LittleFS::lfs_file_from_fd(int fd) const { if (fd < 0 || fd >= MAX_OPEN_FILES || open_files[fd] == nullptr) { errno = EBADF; @@ -723,52 +732,52 @@ uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { // irrespectively of what the flash chip documentation refers to as a "block" /* Most flash chips are programmable in chunks of 256 bytes and erasable in * blocks of 4K so we start with these defaults */ - uint32_t block_count = 0; uint16_t page_size = 256; - uint32_t block_size = 4096; + flash_block_size = 4096; + flash_block_count = 0; switch (id) { #if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX case JEDEC_ID_WINBOND_W25N01GV: /* 128M, programmable in chunks of 2048 bytes, erasable in blocks of 128K */ page_size = 2048; - block_size = 131072; - block_count = 1024; + flash_block_size = 131072; + flash_block_count = 1024; break; case JEDEC_ID_WINBOND_W25N02KV: /* 256M, programmable in chunks of 2048 bytes, erasable in blocks of 128K */ page_size = 2048; - block_size = 131072; - block_count = 2048; + flash_block_size = 131072; + flash_block_count = 2048; break; #else case JEDEC_ID_WINBOND_W25Q16: case JEDEC_ID_MICRON_M25P16: - block_count = 32; /* 128K */ + flash_block_count = 32; /* 128K */ break; case JEDEC_ID_WINBOND_W25Q32: case JEDEC_ID_WINBOND_W25X32: case JEDEC_ID_MACRONIX_MX25L3206E: - block_count = 64; /* 256K */ + flash_block_count = 64; /* 256K */ break; case JEDEC_ID_MICRON_N25Q064: case JEDEC_ID_WINBOND_W25Q64: case JEDEC_ID_MACRONIX_MX25L6406E: - block_count = 128; /* 512K */ + flash_block_count = 128; /* 512K */ break; case JEDEC_ID_MICRON_N25Q128: case JEDEC_ID_WINBOND_W25Q128: case JEDEC_ID_WINBOND_W25Q128_2: case JEDEC_ID_CYPRESS_S25FL128L: - block_count = 256; /* 1M */ + flash_block_count = 256; /* 1M */ break; case JEDEC_ID_WINBOND_W25Q256: case JEDEC_ID_MACRONIX_MX25L25635E: - block_count = 512; /* 2M */ + flash_block_count = 512; /* 2M */ use_32bit_address = true; break; #endif @@ -780,12 +789,14 @@ uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { fs_cfg.read_size = page_size; fs_cfg.prog_size = page_size; - fs_cfg.block_size = block_size; - fs_cfg.block_count = block_count; + fs_cfg.block_size = flash_block_size * LFS_FLASH_BLOCKS_PER_BLOCK; + fs_cfg.block_count = flash_block_count / LFS_FLASH_BLOCKS_PER_BLOCK; #if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX - fs_cfg.metadata_max = page_size; + fs_cfg.metadata_max = page_size * 2; + fs_cfg.compact_thresh = fs_cfg.metadata_max * 0.88f; #endif #else + // SITL config fs_cfg.read_size = 2048; fs_cfg.prog_size = 2048; fs_cfg.block_size = 131072; @@ -799,9 +810,14 @@ uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { id = 0xFAFF; } #endif // CONFIG_HAL_BOARD != HAL_BOARD_SITL - fs_cfg.block_cycles = 500; - fs_cfg.lookahead_size = 128; - fs_cfg.cache_size = 2*fs_cfg.prog_size; + fs_cfg.block_cycles = 75000; + // cache entire flash state in RAM (8 blocks = 1 byte of storage) to + // avoid scanning while logging + fs_cfg.lookahead_size = fs_cfg.block_count/8; + // non-inlined files require their own block, but must be copie. Generally we have requirements for tiny files + // (scripting) and very large files (e.g. logging), but not much in-between. Setting the cache size will also + // limit the inline size. + fs_cfg.cache_size = fs_cfg.prog_size; return id; } @@ -854,11 +870,6 @@ bool AP_Filesystem_FlashMemory_LittleFS::mount_filesystem() { if (lfs_mount(&fs, &fs_cfg) < 0) { /* maybe not formatted? try formatting it */ GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Formatting flash"); -#if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX - EXPECT_DELAY_MS(3000); -#else - EXPECT_DELAY_MS(30000); -#endif if (lfs_format(&fs, &fs_cfg) < 0) { mark_dead(); @@ -873,13 +884,14 @@ bool AP_Filesystem_FlashMemory_LittleFS::mount_filesystem() { } } -#ifdef HAL_BOARD_STORAGE_DIRECTORY // try to create the root storage folder. Ignore the error code in case // the filesystem is corrupted or it already exists. if (strlen(HAL_BOARD_STORAGE_DIRECTORY) > 0) { lfs_mkdir(&fs, HAL_BOARD_STORAGE_DIRECTORY); } -#endif + + // Force garbage collection to avoid expensive operations after boot + lfs_fs_gc(&fs); GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Mounted flash 0x%x as littlefs", unsigned(id)); mounted = true; return true; @@ -917,13 +929,11 @@ void AP_Filesystem_FlashMemory_LittleFS::format_handler(void) ret = lfs_mount(&fs, &fs_cfg); } -#ifdef HAL_BOARD_STORAGE_DIRECTORY // try to create the root storage folder. Ignore the error code in case // the filesystem is corrupted or it already exists. - if (strlen(HAL_BOARD_STORAGE_DIRECTORY) > 0) { - lfs_mkdir(&fs, HAL_BOARD_STORAGE_DIRECTORY); + if (ret == LFS_ERR_OK && strlen(HAL_BOARD_STORAGE_DIRECTORY) > 0) { + ret = lfs_mkdir(&fs, HAL_BOARD_STORAGE_DIRECTORY); } -#endif if (ret == LFS_ERR_OK) { format_status = FormatStatus::SUCCESS; @@ -942,14 +952,14 @@ AP_Filesystem_Backend::FormatStatus AP_Filesystem_FlashMemory_LittleFS::get_form return format_status; } -uint32_t AP_Filesystem_FlashMemory_LittleFS::lfs_block_and_offset_to_raw_flash_address(lfs_block_t index, lfs_off_t off) +inline uint32_t AP_Filesystem_FlashMemory_LittleFS::lfs_block_and_offset_to_raw_flash_address(lfs_block_t index, lfs_off_t off, lfs_off_t flash_block) { - return index * fs_cfg.block_size + off; + return index * fs_cfg.block_size + off + flash_block * flash_block_size; } -uint32_t AP_Filesystem_FlashMemory_LittleFS::lfs_block_to_raw_flash_page_index(lfs_block_t index) +inline uint32_t AP_Filesystem_FlashMemory_LittleFS::lfs_block_to_raw_flash_page_index(lfs_block_t index, lfs_off_t flash_block) { - return index * (fs_cfg.block_size / fs_cfg.prog_size); + return index * (fs_cfg.block_size / fs_cfg.prog_size) + flash_block * (flash_block_size / fs_cfg.prog_size); } bool AP_Filesystem_FlashMemory_LittleFS::write_enable() @@ -986,7 +996,7 @@ bool AP_Filesystem_FlashMemory_LittleFS::init_flash() #else if (use_32bit_address) { WITH_SEMAPHORE(dev_sem); - + // enter 4-byte address mode const uint8_t cmd = 0xB7; dev->transfer(&cmd, 1, nullptr, 0); } @@ -998,7 +1008,7 @@ bool AP_Filesystem_FlashMemory_LittleFS::init_flash() #ifdef AP_LFS_DEBUG static uint32_t block_writes; static uint32_t last_write_msg_ms; -uint32_t page_reads; +static uint32_t page_reads; #endif int AP_Filesystem_FlashMemory_LittleFS::_flashmem_read( lfs_block_t block, lfs_off_t off, void* buffer, lfs_size_t size @@ -1109,21 +1119,23 @@ int AP_Filesystem_FlashMemory_LittleFS::_flashmem_erase(lfs_block_t block) { return LFS_ERR_IO; } - EXPECT_DELAY_MS(2); + for (lfs_off_t fblock = 0; fblock < LFS_FLASH_BLOCKS_PER_BLOCK; fblock++) { - if (!write_enable()) { - return LFS_ERR_IO; - } + if (!write_enable()) { + return LFS_ERR_IO; + } + + WITH_SEMAPHORE(dev_sem); - WITH_SEMAPHORE(dev_sem); #if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX - send_command_addr(JEDEC_BLOCK_ERASE, lfs_block_to_raw_flash_page_index(block)); + send_command_addr(JEDEC_BLOCK_ERASE, lfs_block_to_raw_flash_page_index(block, fblock)); #else - send_command_addr(JEDEC_SECTOR4_ERASE, lfs_block_and_offset_to_raw_flash_address(block)); + send_command_addr(JEDEC_SECTOR4_ERASE, lfs_block_and_offset_to_raw_flash_address(block, 0, fblock)); #endif - // sleep so that othher processes get the CPU cycles that the 4ms erase cycle needs. - hal.scheduler->delay(4); + // sleep so that other processes get the CPU cycles that the 4ms erase cycle needs. + hal.scheduler->delay(4); + } return LFS_ERR_OK; } @@ -1223,7 +1235,7 @@ static int lfs_flags_from_flags(int flags) return outflags; } -// get_singleton for scripting +// get_singleton for access from logging layer AP_Filesystem_FlashMemory_LittleFS *AP_Filesystem_FlashMemory_LittleFS::get_singleton(void) { return singleton; diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h index 2f99a8a667719..16790440dad38 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.h @@ -65,9 +65,6 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend static AP_Filesystem_FlashMemory_LittleFS *get_singleton(void); private: - // JEDEC ID of the flash memory, JEDEC_ID_UNKNOWN if not known or not supported - uint32_t jedec_id; - // Semaphore to protect against concurrent accesses to fs HAL_Semaphore fs_sem; @@ -76,6 +73,8 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend // The configuration of the filesystem struct lfs_config fs_cfg; + lfs_size_t flash_block_size; + lfs_size_t flash_block_count; // Maximum number of files that may be open at the same time static constexpr int MAX_OPEN_FILES = 16; @@ -87,14 +86,15 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend bool dead; // Array of currently open file descriptors - struct file_descriptor { + struct FileDescriptor { lfs_file_t file; lfs_file_config cfg; lfs_attr attrs[1]; uint32_t mtime; - const char* filename; + char* filename; }; - file_descriptor* open_files[MAX_OPEN_FILES]; + + FileDescriptor* open_files[MAX_OPEN_FILES]; // SPI device that handles the raw flash memory AP_HAL::OwnPtr dev; @@ -111,10 +111,10 @@ class AP_Filesystem_FlashMemory_LittleFS : public AP_Filesystem_Backend int allocate_fd(); int free_fd(int fd); void free_all_fds(); - file_descriptor* lfs_file_from_fd(int fd) const; + FileDescriptor* lfs_file_from_fd(int fd) const; - uint32_t lfs_block_and_offset_to_raw_flash_address(lfs_block_t block, lfs_off_t off = 0); - uint32_t lfs_block_to_raw_flash_page_index(lfs_block_t block); + uint32_t lfs_block_and_offset_to_raw_flash_address(lfs_block_t block, lfs_off_t off = 0, lfs_off_t flash_block = 0); + uint32_t lfs_block_to_raw_flash_page_index(lfs_block_t block, lfs_off_t flash_block = 0); uint32_t find_block_size_and_count(); bool init_flash() WARN_IF_UNUSED; bool write_enable() WARN_IF_UNUSED; From f6f546c538df17a80d311cf67789663d086b1532 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 31 Dec 2024 16:32:09 +0000 Subject: [PATCH 55/77] AP_HAL_ChibiOS: introduce new DATAFLASH directive and use it add HAL_OS_POSIX_IO to defaults ensure explicitly configured DATAFLASH is prioritized correct inclusion of fat malloc only create storage backups on fatfs --- libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h | 2 +- libraries/AP_HAL_ChibiOS/Storage.cpp | 4 +- .../hwdef/Aocoda-RC-H743Dual/hwdef.dat | 3 +- .../hwdef/JHEMCU-H743HD/hwdef.dat | 3 +- .../hwdef/KakuteH7Mini-Nand/hwdef.dat | 3 +- .../AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat | 3 +- .../hwdef/MambaH743v4/hwdef.dat | 3 +- .../AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat | 3 +- .../AP_HAL_ChibiOS/hwdef/common/malloc.c | 4 +- .../hwdef/scripts/chibios_hwdef.py | 60 +++++++++++++++---- .../hwdef/scripts/defaults_bootloader.h | 3 + .../hwdef/scripts/defaults_iofirmware.h | 4 ++ .../hwdef/scripts/defaults_normal.h | 4 ++ .../hwdef/scripts/defaults_periph.h | 4 ++ libraries/AP_HAL_ChibiOS/sdcard.cpp | 10 ++-- 15 files changed, 79 insertions(+), 34 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h index 4f0ff4ca8c1f2..848e86e048d5c 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h @@ -21,7 +21,7 @@ #include #include #include "hwdef/common/halconf.h" -#ifdef USE_POSIX_FATFS +#if HAL_USE_FATFS #include #endif #include diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index e1ffb337f1ccc..8244eaf22bfcf 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -127,8 +127,8 @@ void Storage::_storage_open(void) */ void Storage::_save_backup(void) { -#ifdef USE_POSIX_FATFS - // allow for fallback to microSD or dataflash based storage +#if HAL_USE_FATFS + // allow for fallback to microSD or littlefs flash based storage // create the backup directory if need be int ret; const char* _storage_bak_directory = HAL_STORAGE_BACKUP_FOLDER; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat index 045040643c427..821e5c7b3e374 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/hwdef.dat @@ -218,5 +218,4 @@ define HAL_OSD_TYPE_DEFAULT 1 ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin # enable logging to dataflash -define HAL_OS_LITTLEFS_IO 1 -define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX +DATAFLASH littlefs:w25nxx diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat index f2c5276091a62..43196ab07dfa9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-H743HD/hwdef.dat @@ -145,8 +145,7 @@ define HAL_GPIO_B_LED_PIN 91 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ -define HAL_OS_LITTLEFS_IO 1 -define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX +DATAFLASH littlefs:w25nxx # OSD setup SPIDEV osd SPI2 DEVID1 OSD1_CS MODE0 10*MHZ 10*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat index fd5eb4879da6b..0ca2b13bf2536 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat @@ -10,5 +10,4 @@ SPIDEV bmi270 SPI4 DEVID1 MPU6000_CS MODE3 1*MHZ 4*MHZ IMU BMI270 SPI:bmi270 ROTATION_PITCH_180_YAW_90 undef HAL_LOGGING_DATAFLASH_ENABLED -define HAL_OS_LITTLEFS_IO 1 -define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX +DATAFLASH littlefs:w25nxx diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat index 6e4077aec32fd..90f8c250a5fb2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7v2/hwdef.dat @@ -64,5 +64,4 @@ DMA_NOSHARE *UP SPI1* # Motor order implies Betaflight/X for standard ESCs define HAL_FRAME_TYPE_DEFAULT 12 -define HAL_OS_LITTLEFS_IO 1 -define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX +DATAFLASH littlefs:w25nxx diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat index 9a221ff10a7cd..a28f83b8eecc8 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat @@ -180,8 +180,7 @@ IMU BMI270 SPI:bmi270 ROTATION_PITCH_180 IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180 IMU Invensensev3 SPI:icm42688_2 ROTATION_ROLL_180_YAW_270 -define HAL_OS_LITTLEFS_IO 1 -define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX +DATAFLASH littlefs:w25nxx # DPS280 or SPL06 integrated on I2C bus BARO DPS280 I2C:0:0x76 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat index 006b6bc7fe612..c8c30df603558 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat @@ -162,8 +162,7 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin SPIDEV sdcard SPI2 DEVID3 SD_CS MODE0 400*KHZ 25*MHZ SPIDEV dataflash SPI2 DEVID1 FLASH_CS MODE3 104*MHZ 104*MHZ -define HAL_OS_LITTLEFS_IO 1 -define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX +DATAFLASH littlefs:w25nxx # ----------------- I2C compass & Baro ----------------- # no built-in compass, but probe the i2c bus for all possible diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c index b733b20e71f9c..8a6e0a8ea0ce9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -585,7 +585,7 @@ void __wrap__free_r(void *rptr, void *ptr) return free(ptr); } -#ifdef USE_POSIX_FATFS +#if HAL_USE_FATFS /* allocation functions for FATFS */ @@ -615,7 +615,7 @@ void ff_memfree(void* mblock) { free(mblock); } -#endif // USE_POSIX_FATFS +#endif // HAL_USE_FATFS /* return true if a memory region is safe for a DMA operation diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index c4cdc3df85842..c35031a42d936 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -118,6 +118,9 @@ def __init__(self, quiet=False, bootloader=False, signed_fw=False, outdir=None, self.baro_list = [] self.airspeed_list = [] + # dataflash config + self.dataflash_list = [] + # output lines: self.all_lines = [] @@ -948,17 +951,24 @@ def write_mcu_config(self, f): f.write('#define HAL_STDOUT_SERIAL %s\n\n' % self.get_config('STDOUT_SERIAL')) f.write('// baudrate used for stdout (printf)\n') f.write('#define HAL_STDOUT_BAUDRATE %u\n\n' % self.get_config('STDOUT_BAUDRATE', type=int)) - if self.have_type_prefix('SDIO'): + if len(self.dataflash_list) > 0: + # we only support dataflash OR sdcard, so prioritize dataflash if its been explicitly configured + f.write('#define HAL_USE_FATFS FALSE\n\n') + f.write('#define HAL_USE_SDC FALSE\n') + self.build_flags.append('USE_FATFS=no') + elif self.have_type_prefix('SDIO'): f.write('// SDIO available, enable POSIX filesystem support\n') f.write('#define USE_POSIX\n') - f.write('#define USE_POSIX_FATFS\n\n') + f.write('#define HAL_OS_POSIX_IO TRUE\n\n') + f.write('#define HAL_USE_FATFS TRUE\n\n') f.write('#define HAL_USE_SDC TRUE\n') self.build_flags.append('USE_FATFS=yes') self.env_vars['WITH_FATFS'] = "1" elif self.have_type_prefix('SDMMC2'): f.write('// SDMMC2 available, enable POSIX filesystem support\n') f.write('#define USE_POSIX\n') - f.write('#define USE_POSIX_FATFS\n\n') + f.write('#define HAL_OS_POSIX_IO TRUE\n\n') + f.write('#define HAL_USE_FATFS TRUE\n\n') f.write('#define HAL_USE_SDC TRUE\n') f.write('#define STM32_SDC_USE_SDMMC2 TRUE\n') f.write('#define HAL_USE_SDMMC 1\n') @@ -967,7 +977,8 @@ def write_mcu_config(self, f): elif self.have_type_prefix('SDMMC'): f.write('// SDMMC available, enable POSIX filesystem support\n') f.write('#define USE_POSIX\n') - f.write('#define USE_POSIX_FATFS\n\n') + f.write('#define HAL_USE_FATFS TRUE\n\n') + f.write('#define HAL_OS_POSIX_IO TRUE\n\n') f.write('#define HAL_USE_SDC TRUE\n') f.write('#define STM32_SDC_USE_SDMMC1 TRUE\n') f.write('#define HAL_USE_SDMMC 1\n') @@ -976,20 +987,15 @@ def write_mcu_config(self, f): elif self.has_sdcard_spi(): f.write('// MMC via SPI available, enable POSIX filesystem support\n') f.write('#define USE_POSIX\n') - f.write('#define USE_POSIX_FATFS\n\n') + f.write('#define HAL_USE_FATFS TRUE\n\n') + f.write('#define HAL_OS_POSIX_IO TRUE\n\n') f.write('#define HAL_USE_MMC_SPI TRUE\n') f.write('#define HAL_USE_SDC FALSE\n') f.write('#define HAL_SDCARD_SPI_HOOK TRUE\n') self.build_flags.append('USE_FATFS=yes') self.env_vars['WITH_FATFS'] = "1" - elif self.has_dataflash_spi(): - f.write('// Dataflash memory via SPI available, enable POSIX filesystem support\n') - f.write('#define USE_POSIX\n') - f.write('#define USE_POSIX_LITTLEFS\n\n') - f.write('#define HAL_USE_SDC FALSE\n') - self.build_flags.append('USE_FATFS=no') - self.env_vars['WITH_LITTLEFS'] = "1" else: + f.write('#define HAL_USE_FATFS FALSE\n\n') f.write('#define HAL_USE_SDC FALSE\n') self.build_flags.append('USE_FATFS=no') if 'OTG1' in self.bytype: @@ -1797,6 +1803,31 @@ def write_AIRSPEED_config(self, f): if len(devlist) > 0: f.write('#define HAL_AIRSPEED_PROBE_LIST %s\n\n' % ';'.join(devlist)) + def write_DATAFLASH_config(self, f): + '''write dataflash config defines''' + # DATAFLASH block|littlefs: + devlist = [] + seen = set() + for dev in self.dataflash_list: + if not self.has_dataflash_spi(): + self.error("Missing DATAFLASH device: %s" % self.seen_str(dev)) + if self.seen_str(dev) in seen: + self.error("Duplicate DATAFLASH: %s" % self.seen_str(dev)) + seen.add(self.seen_str(dev)) + a = dev[0].split(':') + if a[0].startswith('block'): + if len(a) > 1 and a[1].startswith('w25nxx'): + f.write('#define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_W25NXX') + f.write('#define HAL_LOGGING_DATAFLASH_ENABLED TRUE') + elif a[0].startswith('littlefs'): + f.write('#define USE_POSIX\n') + f.write('#define HAL_OS_LITTLEFS_IO TRUE\n') + f.write('#define HAL_OS_POSIX_IO TRUE\n') + if len(a) > 1 and a[1].startswith('w25nxx'): + f.write('#define AP_FILESYSTEM_LITTLEFS_FLASH_TYPE AP_FILESYSTEM_FLASH_W25NXX') + self.build_flags.append('USE_FATFS=no') + self.env_vars['WITH_LITTLEFS'] = "1" + def write_board_validate_macro(self, f): '''write board validation macro''' validate_string = '' @@ -2661,6 +2692,7 @@ def write_hwdef_header(self, outfilename): self.write_MAG_config(f) self.write_BARO_config(f) self.write_AIRSPEED_config(f) + self.write_DATAFLASH_config(f) self.write_board_validate_macro(f) self.write_check_firmware(f) @@ -3088,6 +3120,8 @@ def process_line(self, line, depth): self.compass_list.append(a[1:]) elif a[0] == 'BARO': self.baro_list.append(a[1:]) + elif a[0] == 'DATAFLASH': + self.dataflash_list.append(a[1:]) elif a[0] == 'AIRSPEED': self.airspeed_list.append(a[1:]) elif a[0] == 'ROMFS': @@ -3127,6 +3161,8 @@ def process_line(self, line, depth): self.compass_list = [] if u == 'BARO': self.baro_list = [] + if u == 'DATAFLASH': + self.dataflash_list = [] if u == 'AIRSPEED': self.airspeed_list = [] if u == 'ROMFS': diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h index 91f9de9ae3e5c..c6bb94d097d91 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h @@ -62,3 +62,6 @@ #define STM32_DMA_REQUIRED 1 #endif +#ifndef HAL_OS_POSIX_IO +#define HAL_OS_POSIX_IO 0 +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h index 0092424ed766f..89c6424e7bd4c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h @@ -77,3 +77,7 @@ #ifndef HAL_GCS_ENABLED #define HAL_GCS_ENABLED 0 #endif + +#ifndef HAL_OS_POSIX_IO +#define HAL_OS_POSIX_IO 0 +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h index 373c30497e280..df241a3bd5d58 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h @@ -20,6 +20,10 @@ #define HAL_OS_LITTLEFS_IO 0 #endif +#ifndef HAL_OS_POSIX_IO +#define HAL_OS_POSIX_IO 0 +#endif + #ifndef AP_TERRAIN_AVAILABLE // enable terrain only if there's an SD card available: #define AP_TERRAIN_AVAILABLE (HAL_OS_FATFS_IO || (HAL_OS_LITTLEFS_IO && (BOARD_FLASH_SIZE>1024))) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index a262f9fa13bb3..46aadd32d022e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -472,3 +472,7 @@ #ifndef AP_QUICKTUNE_ENABLED #define AP_QUICKTUNE_ENABLED 0 #endif + +#ifndef HAL_OS_POSIX_IO +#define HAL_OS_POSIX_IO 0 +#endif diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index e2750960572be..0706d3dec8bcd 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal; -#ifdef USE_POSIX_FATFS +#if HAL_USE_FATFS static FATFS SDC_FS; // FATFS object #ifndef HAL_BOOTLOADER_BUILD static HAL_Semaphore sem; @@ -54,7 +54,7 @@ static SPIConfig highspeed; */ bool sdcard_init() { -#ifdef USE_POSIX_FATFS +#if HAL_USE_FATFS #ifndef HAL_BOOTLOADER_BUILD WITH_SEMAPHORE(sem); @@ -150,7 +150,7 @@ bool sdcard_init() } #endif sdcard_running = false; -#endif // USE_POSIX_FATFS +#endif // HAL_USE_FATFS return false; } @@ -159,7 +159,7 @@ bool sdcard_init() */ void sdcard_stop(void) { -#ifdef USE_POSIX_FATFS +#if HAL_USE_FATFS // unmount f_mount(nullptr, "/", 1); #endif @@ -185,7 +185,7 @@ void sdcard_stop(void) bool sdcard_retry(void) { -#ifdef USE_POSIX_FATFS +#if HAL_USE_FATFS if (!sdcard_running) { if (sdcard_init()) { #if AP_FILESYSTEM_FILE_WRITING_ENABLED From 24ab3f8f20c99ab262f4906534a9b89995df2d15 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 31 Dec 2024 16:32:44 +0000 Subject: [PATCH 56/77] AP_HAL: tidy meanings of FATFS, POSIX_IO and friends --- libraries/AP_HAL/AP_HAL_Boards.h | 4 ++++ libraries/AP_HAL/board/linux.h | 4 ++++ libraries/AP_HAL/board/sitl.h | 4 ++++ 3 files changed, 12 insertions(+) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index fb85427706ac7..2c97c1ebff46c 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -156,6 +156,10 @@ #define HAL_OS_SOCKETS 0 #endif +#ifndef HAL_OS_POSIX_IO +#define HAL_OS_POSIX_IO 0 +#endif + #ifndef HAL_PARAM_DEFAULTS_PATH #define HAL_PARAM_DEFAULTS_PATH nullptr #endif diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index c55fec67e24ae..705728aa68039 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -423,6 +423,10 @@ #define HAL_LINUX_USE_VIRTUAL_CAN 0 #endif +#ifndef HAL_OS_POSIX_IO +#define HAL_OS_POSIX_IO 1 +#endif + #ifndef HAL_INS_RATE_LOOP #define HAL_INS_RATE_LOOP 1 #endif diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index e4d1afff91c5c..4e8c6d961d6f5 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -97,6 +97,10 @@ #define HAL_SOLO_GIMBAL_ENABLED 1 +#ifndef HAL_OS_POSIX_IO +#define HAL_OS_POSIX_IO 1 +#endif + #ifndef HAL_INS_RATE_LOOP #define HAL_INS_RATE_LOOP 1 #endif From da74370fddd1fb5d9392a732bca3002d8d9201de Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 31 Dec 2024 16:33:33 +0000 Subject: [PATCH 57/77] wscript: address review comments --- wscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wscript b/wscript index 45ad476dfc77c..8062c6bb34bc9 100644 --- a/wscript +++ b/wscript @@ -306,7 +306,7 @@ submodules at specific revisions. g.add_option('--sitl-littlefs', action='store_true', default=False, - help="Enable littlefs for filesystem accesson SITL") + help="Enable littlefs for filesystem access on SITL (under construction)") g = opt.ap_groups['linux'] From 628b000ace31d0ee43226958d87b86e39fe298cb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 2 Jan 2025 15:49:11 +0000 Subject: [PATCH 58/77] AP_Scripting: scripting directory configuration for posix IO ensure SITL uses ./scripts --- libraries/AP_Scripting/lua_common_defs.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scripting/lua_common_defs.h b/libraries/AP_Scripting/lua_common_defs.h index f48a79c19d60b..7d2e07262db7b 100644 --- a/libraries/AP_Scripting/lua_common_defs.h +++ b/libraries/AP_Scripting/lua_common_defs.h @@ -3,11 +3,12 @@ #include #ifndef SCRIPTING_DIRECTORY - #if HAL_OS_FATFS_IO + // can't use HAL_OS_POSIX_IO here ebcause SITL assumes no APM prefix + #if HAL_OS_FATFS_IO || HAL_OS_LITTLEFS_IO #define SCRIPTING_DIRECTORY "/APM/scripts" #else #define SCRIPTING_DIRECTORY "./scripts" - #endif //HAL_OS_FATFS_IO + #endif // HAL_OS_FATFS_IO || HAL_OS_LITTLEFS_IO #endif // SCRIPTING_DIRECTORY int lua_get_current_env_ref(); From 7f51f84fc6fdd95607ec093ea064c4abdfba7e82 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 12 Jan 2025 09:50:43 +0000 Subject: [PATCH 59/77] waf: disable littlefs debug and asserts --- Tools/ardupilotwaf/littlefs.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/littlefs.py b/Tools/ardupilotwaf/littlefs.py index c4f488db5bc57..a07386022f600 100644 --- a/Tools/ardupilotwaf/littlefs.py +++ b/Tools/ardupilotwaf/littlefs.py @@ -19,7 +19,7 @@ def littlefs(bld, **kw): name='littlefs', source=['modules/littlefs/lfs.c', 'modules/littlefs/lfs_util.c', 'modules/littlefs/bd/lfs_filebd.c'], target='littlefs', - defines=['LFS_NO_DEBUG'], + defines=['LFS_NO_DEBUG', 'LFS_NO_WARN', 'LFS_NO_ERROR', 'LFS_NO_ASSERT'], cflags=['-Wno-format', '-Wno-format-extra-args', '-Wno-shadow', '-Wno-unused-function', '-Wno-missing-declarations'] ) return bld.stlib(**kw) From 0f159b8d83e8136bf7ee9f9cacd2865a0ac10388 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 12 Jan 2025 10:16:56 +0000 Subject: [PATCH 60/77] AP_HAL: no littlefs on QURT --- libraries/AP_HAL/board/qurt.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 6f48b5fcbd572..a964f12a7c349 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -73,6 +73,12 @@ #define HAL_WITH_ESC_TELEM 1 +#ifndef HAL_OS_POSIX_IO +#define HAL_OS_POSIX_IO 1 +#endif + +#define HAL_OS_LITTLEFS_IO 0 + /* battery monitoring setup, comes in via ESCs */ From 949e6bcbba8d1f93c8d640b48752fcf5b19432c2 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 12 Jan 2025 11:10:50 -0600 Subject: [PATCH 61/77] Tools: waf: have boards call superclass initializer Allows future variables to propagate down to board subclasses. only build littlefs when hwdef or board calls for it Avoids issues with build on QURT due to QURT's poor POSIX implementation. --- Tools/ardupilotwaf/ardupilotwaf.py | 4 ---- Tools/ardupilotwaf/boards.py | 20 ++++++++++++++++++++ Tools/ardupilotwaf/chibios.py | 2 ++ 3 files changed, 22 insertions(+), 4 deletions(-) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 1695be2465cdc..c8634444135ec 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -445,10 +445,6 @@ def ap_stlib(bld, **kw): kw['target'] = kw['name'] kw['source'] = [] - if 'use' not in kw: - kw['use'] = [] - kw['use'].append('littlefs') - bld.stlib(**kw) _created_program_dirs = set() diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 6ee1292332311..b1aea6bc244cc 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -40,6 +40,7 @@ class Board: def __init__(self): self.with_can = False + self.with_littlefs = False def configure(self, cfg): cfg.env.TOOLCHAIN = cfg.options.toolchain or self.toolchain @@ -677,7 +678,10 @@ def get_board(ctx): class sitl(Board): def __init__(self): + super().__init__() + self.with_can = True + self.with_littlefs = True def configure_env(self, cfg, env): super(sitl, self).configure_env(cfg, env) @@ -1355,6 +1359,8 @@ def get_name(self): class linux(Board): def __init__(self): + super().__init__() + if self.toolchain == 'native': self.with_can = True else: @@ -1501,6 +1507,8 @@ class edge(linux): toolchain = 'arm-linux-gnueabihf' def __init__(self): + super().__init__() + self.with_can = True def configure_env(self, cfg, env): @@ -1534,6 +1542,8 @@ class bbbmini(linux): toolchain = 'arm-linux-gnueabihf' def __init__(self): + super().__init__() + self.with_can = True def configure_env(self, cfg, env): @@ -1547,6 +1557,8 @@ class blue(linux): toolchain = 'arm-linux-gnueabihf' def __init__(self): + super().__init__() + self.with_can = True def configure_env(self, cfg, env): @@ -1561,6 +1573,8 @@ class pocket(linux): toolchain = 'arm-linux-gnueabihf' def __init__(self): + super().__init__() + self.with_can = True def configure_env(self, cfg, env): @@ -1653,6 +1667,8 @@ def configure_env(self, cfg, env): class aero(linux): def __init__(self): + super().__init__() + self.with_can = True def configure_env(self, cfg, env): @@ -1686,6 +1702,8 @@ class canzero(linux): toolchain = 'arm-linux-gnueabihf' def __init__(self): + super().__init__() + self.with_can = True def configure_env(self, cfg, env): @@ -1711,6 +1729,8 @@ class QURT(Board): toolchain = 'custom' def __init__(self): + super().__init__() + self.with_can = False def configure_toolchain(self, cfg): diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 6c998c14f204d..46a723d9006fe 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -655,6 +655,8 @@ def pre_build(bld): load_env_vars(bld.env) if bld.env.HAL_NUM_CAN_IFACES: bld.get_board().with_can = True + if bld.env.WITH_LITTLEFS: + bld.get_board().with_littlefs = True hwdef_h = os.path.join(bld.env.BUILDROOT, 'hwdef.h') if not os.path.exists(hwdef_h): print("Generating hwdef.h") From a93f51d631397f9d0a5708f58340ed3330e9b3e0 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 12 Jan 2025 12:09:02 -0600 Subject: [PATCH 62/77] wscript: only build littlefs if called for --- wscript | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/wscript b/wscript index 8062c6bb34bc9..ca175e4b02398 100644 --- a/wscript +++ b/wscript @@ -810,8 +810,6 @@ def _build_common_taskgens(bld): ap_libraries=bld.ap_get_all_libraries(), ) - bld.littlefs() - if bld.env.HAS_GTEST: bld.libgtest(cxxflags=['-include', 'ap_config.h']) @@ -911,6 +909,10 @@ def build(bld): if bld.get_board().with_can: bld.env.AP_LIBRARIES_OBJECTS_KW['use'] += ['dronecan'] + if bld.get_board().with_littlefs: + bld.env.AP_LIBRARIES_OBJECTS_KW['use'] += ['littlefs'] + bld.littlefs() + _build_cmd_tweaks(bld) if bld.env.SUBMODULE_UPDATE: From 03cdab003be1718f61173e4b0b2d15d481132cd7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 14 Jan 2025 22:40:58 +0000 Subject: [PATCH 63/77] GCS_MAVLink: enable HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED on littlefs --- libraries/GCS_MAVLink/GCS_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 95bc5d7d1384a..9111a7c4e4da0 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -43,7 +43,7 @@ #endif #ifndef HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED -#define HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED ((AP_FILESYSTEM_FATFS_ENABLED || AP_FILESYSTEM_POSIX_ENABLED) && BOARD_FLASH_SIZE > 1024) +#define HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED ((AP_FILESYSTEM_FATFS_ENABLED || AP_FILESYSTEM_LITTLEFS_ENABLED || AP_FILESYSTEM_POSIX_ENABLED) && BOARD_FLASH_SIZE > 1024) #endif #ifndef AP_MAVLINK_MSG_RELAY_STATUS_ENABLED From 9beb09f80fc76a839a23e61b66ec8963d96ec3ac Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 14 Jan 2025 22:41:28 +0000 Subject: [PATCH 64/77] github: add MambaH743v4 to size workflow --- .github/workflows/test_size.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index fe83756a3a759..c6be5ef7cf8ef 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -72,6 +72,7 @@ jobs: MatekF405, KakuteF7, MatekH743-bdshot, + MambaH743v4, # for littlefs support Pixhawk1-1M, MatekF405-CAN, # see special "build bootloader" code below DrotekP3Pro, # see special "build bootloader" code below From 22dacf617872d3e1bc086d9a2aa99a96141ad9cd Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 12 Jun 2023 11:28:54 -0400 Subject: [PATCH 65/77] modules: add littlefs v2.10.1-ardupilot --- .gitmodules | 3 +++ modules/littlefs | 1 + 2 files changed, 4 insertions(+) create mode 160000 modules/littlefs diff --git a/.gitmodules b/.gitmodules index 6a900f5fbaad6..3c1434efc720f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -42,3 +42,6 @@ [submodule "modules/lwip"] path = modules/lwip url = https://github.com/ArduPilot/lwip.git +[submodule "modules/littlefs"] + path = modules/littlefs + url = https://github.com/ArduPilot/littlefs.git diff --git a/modules/littlefs b/modules/littlefs new file mode 160000 index 0000000000000..34be692aafaaf --- /dev/null +++ b/modules/littlefs @@ -0,0 +1 @@ +Subproject commit 34be692aafaaf4319a18e415b08aa4332697408b From b21fce129047f7510824e07a582a984f8bf9b6c5 Mon Sep 17 00:00:00 2001 From: Yury MonZon Date: Mon, 9 Dec 2024 16:11:39 +0100 Subject: [PATCH 66/77] AP_Tuning: Show current parameter value on OSD and GCS --- libraries/AP_Tuning/AP_Tuning.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp index 490416fdab840..8b0c6f7fde922 100644 --- a/libraries/AP_Tuning/AP_Tuning.cpp +++ b/libraries/AP_Tuning/AP_Tuning.cpp @@ -202,10 +202,10 @@ void AP_Tuning::check_input(uint8_t flightmode) //hal.console->printf("chan_value %.2f last_channel_value %.2f\n", chan_value, last_channel_value); + const float dead_zone = 0.02; if (mid_point_wait) { // see if we have crossed the mid-point. We use a small deadzone to make it easier // to move to the "indent" portion of a slider to start tuning - const float dead_zone = 0.02; if ((chan_value > dead_zone && last_channel_value > 0) || (chan_value < -dead_zone && last_channel_value < 0)) { // still waiting @@ -213,7 +213,6 @@ void AP_Tuning::check_input(uint8_t flightmode) } // starting tuning mid_point_wait = false; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Tuning: mid-point %s", get_tuning_name(current_parm)); AP_Notify::events.tune_started = 1; } last_channel_value = chan_value; @@ -228,6 +227,12 @@ void AP_Tuning::check_input(uint8_t flightmode) need_revert |= (1U << current_parm_index); set_value(current_parm, new_value); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, + "Tuning %s%s%0.5f", + get_tuning_name(current_parm), + ((chan_value < dead_zone) && (chan_value > -dead_zone)) ? "> " : ": ", + (double)(new_value)); + #if HAL_LOGGING_ENABLED Log_Write_Parameter_Tuning(new_value); #endif From ada4afa88f09b534eca6dc7a1d609b887e4ebde3 Mon Sep 17 00:00:00 2001 From: Yury MonZon Date: Mon, 9 Dec 2024 15:02:02 +0100 Subject: [PATCH 67/77] AP_Tuning_Plane: Added PIDFF tuning sets --- ArduPlane/tuning.cpp | 6 ++++++ ArduPlane/tuning.h | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp index be860264fd339..c93c3d862a8ec 100644 --- a/ArduPlane/tuning.cpp +++ b/ArduPlane/tuning.cpp @@ -37,6 +37,9 @@ const uint8_t AP_Tuning_Plane::tuning_set_az[] = { TUNING_AZ_P, TU const uint8_t AP_Tuning_Plane::tuning_set_rate_pitchDP[]= { TUNING_RATE_PITCH_D, TUNING_RATE_PITCH_P }; const uint8_t AP_Tuning_Plane::tuning_set_rate_rollDP[]= { TUNING_RATE_ROLL_D, TUNING_RATE_ROLL_P }; const uint8_t AP_Tuning_Plane::tuning_set_rate_yawDP[]= { TUNING_RATE_YAW_D, TUNING_RATE_YAW_P }; +const uint8_t AP_Tuning_Plane::tuning_set_dp_roll_pitch[] = { TUNING_RLL_D, TUNING_RLL_P, TUNING_PIT_D, TUNING_PIT_P }; +const uint8_t AP_Tuning_Plane::tuning_set_pidff_roll[] = { TUNING_RLL_P, TUNING_RLL_I, TUNING_RLL_D, TUNING_RLL_FF }; +const uint8_t AP_Tuning_Plane::tuning_set_pidff_pitch[] = { TUNING_PIT_P, TUNING_PIT_I, TUNING_PIT_D, TUNING_PIT_FF }; // macro to prevent getting the array length wrong #define TUNING_ARRAY(v) ARRAY_SIZE(v), v @@ -53,6 +56,9 @@ const AP_Tuning_Plane::tuning_set AP_Tuning_Plane::tuning_sets[] = { { TUNING_SET_RATE_PITCHDP, TUNING_ARRAY(tuning_set_rate_pitchDP) }, { TUNING_SET_RATE_ROLLDP, TUNING_ARRAY(tuning_set_rate_rollDP) }, { TUNING_SET_RATE_YAWDP, TUNING_ARRAY(tuning_set_rate_yawDP) }, + { TUNING_SET_DP_ROLL_PITCH, TUNING_ARRAY(tuning_set_dp_roll_pitch) }, + { TUNING_SET_PIDFF_ROLL, TUNING_ARRAY(tuning_set_pidff_roll) }, + { TUNING_SET_PIDFF_PITCH, TUNING_ARRAY(tuning_set_pidff_pitch) }, { 0, 0, nullptr } }; diff --git a/ArduPlane/tuning.h b/ArduPlane/tuning.h index 4334455a09657..9e63bf729d704 100644 --- a/ArduPlane/tuning.h +++ b/ArduPlane/tuning.h @@ -94,6 +94,9 @@ class AP_Tuning_Plane : public AP_Tuning TUNING_SET_RATE_PITCHDP = 8, TUNING_SET_RATE_ROLLDP = 9, TUNING_SET_RATE_YAWDP = 10, + TUNING_SET_DP_ROLL_PITCH = 11, + TUNING_SET_PIDFF_ROLL = 12, + TUNING_SET_PIDFF_PITCH = 13, }; AP_Float *get_param_pointer(uint8_t parm) override; @@ -112,6 +115,9 @@ class AP_Tuning_Plane : public AP_Tuning static const uint8_t tuning_set_rate_pitchDP[]; static const uint8_t tuning_set_rate_rollDP[]; static const uint8_t tuning_set_rate_yawDP[]; + static const uint8_t tuning_set_dp_roll_pitch[]; + static const uint8_t tuning_set_pidff_roll[]; + static const uint8_t tuning_set_pidff_pitch[]; // mask of what params have been set uint64_t have_set; From 4d3f5b53ef6e2d88913a75a8657ceda0a508b60c Mon Sep 17 00:00:00 2001 From: Yury MonZon Date: Mon, 9 Dec 2024 21:42:57 +0100 Subject: [PATCH 68/77] AP_Tuning: Update OSD on 5 percent change --- libraries/AP_Tuning/AP_Tuning.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp index 8b0c6f7fde922..87bbf15a13e8c 100644 --- a/libraries/AP_Tuning/AP_Tuning.cpp +++ b/libraries/AP_Tuning/AP_Tuning.cpp @@ -218,6 +218,7 @@ void AP_Tuning::check_input(uint8_t flightmode) last_channel_value = chan_value; float new_value; + static float old_value; if (chan_value > 0) { new_value = linear_interpolate(center_value, range*center_value, chan_value, 0, 1); } else { @@ -227,11 +228,14 @@ void AP_Tuning::check_input(uint8_t flightmode) need_revert |= (1U << current_parm_index); set_value(current_parm, new_value); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, - "Tuning %s%s%0.5f", - get_tuning_name(current_parm), - ((chan_value < dead_zone) && (chan_value > -dead_zone)) ? "> " : ": ", - (double)(new_value)); + if ( fabsf(new_value-old_value) > (0.05 * old_value) ) { + old_value = new_value; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, + "Tuning %s%s%0.4f", + get_tuning_name(current_parm), + ((chan_value < dead_zone) && (chan_value > -dead_zone)) ? "> " : ": ", + (double)(new_value)); + } #if HAL_LOGGING_ENABLED Log_Write_Parameter_Tuning(new_value); From ca0d50df7324556f0b2560f285741cc2b12fba8e Mon Sep 17 00:00:00 2001 From: Yury MonZon Date: Tue, 31 Dec 2024 13:08:12 +0100 Subject: [PATCH 69/77] AP_Tuning: Bugfix --- libraries/AP_Tuning/AP_Tuning.cpp | 4 ++-- libraries/AP_Tuning/AP_Tuning.h | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp index 87bbf15a13e8c..c10e5b1253e53 100644 --- a/libraries/AP_Tuning/AP_Tuning.cpp +++ b/libraries/AP_Tuning/AP_Tuning.cpp @@ -123,6 +123,7 @@ void AP_Tuning::re_center(void) AP_Float *f = get_param_pointer(current_parm); if (f != nullptr) { center_value = f->get(); + old_value = 0.0; } mid_point_wait = true; } @@ -218,7 +219,6 @@ void AP_Tuning::check_input(uint8_t flightmode) last_channel_value = chan_value; float new_value; - static float old_value; if (chan_value > 0) { new_value = linear_interpolate(center_value, range*center_value, chan_value, 0, 1); } else { @@ -231,7 +231,7 @@ void AP_Tuning::check_input(uint8_t flightmode) if ( fabsf(new_value-old_value) > (0.05 * old_value) ) { old_value = new_value; GCS_SEND_TEXT(MAV_SEVERITY_INFO, - "Tuning %s%s%0.4f", + "Tuning: %s%s%0.4f", get_tuning_name(current_parm), ((chan_value < dead_zone) && (chan_value > -dead_zone)) ? "> " : ": ", (double)(new_value)); diff --git a/libraries/AP_Tuning/AP_Tuning.h b/libraries/AP_Tuning/AP_Tuning.h index 88ffda0c4b08a..5aad12ea9b019 100644 --- a/libraries/AP_Tuning/AP_Tuning.h +++ b/libraries/AP_Tuning/AP_Tuning.h @@ -64,6 +64,9 @@ class AP_Tuning uint32_t last_check_ms; + // last tuning value scaled + float old_value; + void Log_Write_Parameter_Tuning(float value); // the parameter we are tuning From 4f39a842b22001dff14e4eb4d7aff9c32462166f Mon Sep 17 00:00:00 2001 From: MattKear Date: Thu, 5 Dec 2024 04:47:20 +0000 Subject: [PATCH 70/77] AP_InertialSensor: Add config error for helis trying to use throttle notch --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 0d8e6029e81c9..24f2c51287c99 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1006,6 +1006,16 @@ AP_InertialSensor::init(uint16_t loop_rate) #endif #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED + +#if APM_BUILD_TYPE(APM_BUILD_Heli) + // Throttle tracking does not make sense with heli because "throttle" is actually collective position in AP_MotorsHeli + for (auto ¬ch : harmonic_notches) { + if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateThrottle) { + AP_BoardConfig::config_error("Throttle notch unavailable with heli"); + } + } +#endif + // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the // configured value From 634cb02dbde3083862fc2f4921856ff4b8f05f9b Mon Sep 17 00:00:00 2001 From: MattKear Date: Thu, 5 Dec 2024 04:49:20 +0000 Subject: [PATCH 71/77] Filter: Make heli default mode for harmonic notch tracking = fixed --- libraries/Filter/HarmonicNotchFilter.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index c98a50eeac4ea..36788d50b971f 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -21,6 +21,7 @@ #include "HarmonicNotchFilter.h" #include #include +#include #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters @@ -48,6 +49,13 @@ */ #define NOTCHFILTER_ATTENUATION_CUTOFF 0.25 +#if APM_BUILD_TYPE(APM_BUILD_Heli) + // We cannot use throttle based notch on helis + #define NOTCHFILTER_DEFAULT_MODE float(HarmonicNotchDynamicMode::Fixed) // fixed +#else + #define NOTCHFILTER_DEFAULT_MODE float(HarmonicNotchDynamicMode::UpdateThrottle) // throttle based +#endif + // table of user settable parameters const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { @@ -120,7 +128,7 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Range: 0 5 // @Values: 0:Fixed,1:Throttle,2:RPM Sensor,3:ESC Telemetry,4:Dynamic FFT,5:Second RPM Sensor // @User: Advanced - AP_GROUPINFO("MODE", 7, HarmonicNotchFilterParams, _tracking_mode, int8_t(HarmonicNotchDynamicMode::UpdateThrottle)), + AP_GROUPINFO("MODE", 7, HarmonicNotchFilterParams, _tracking_mode, NOTCHFILTER_DEFAULT_MODE), // @Param: OPTS // @DisplayName: Harmonic Notch Filter options From 125c64074c3785191caa856e9fbb95e3be4f42ab Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Mon, 20 Jan 2025 11:27:49 -0600 Subject: [PATCH 72/77] ardupilotwaf: fix ESP32 memory zero comment Followup to PR #29005 (d9e5f2d8a7581b79430568bc7a8487df6b4f3a85). --- Tools/ardupilotwaf/boards.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index b1aea6bc244cc..a52bb7814e841 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1068,6 +1068,7 @@ def expand_path(p): env.CXXFLAGS.remove('-Werror=shadow') # wrap malloc to ensure memory is zeroed + # note that this also needs to be done in the CMakeLists.txt files env.LINKFLAGS += ['-Wl,--wrap,malloc'] # TODO: remove once hwdef.dat support is in place @@ -1399,7 +1400,6 @@ def configure_env(self, cfg, env): ] # wrap malloc to ensure memory is zeroed - # note that this also needs to be done in the CMakeLists.txt files env.LINKFLAGS += ['-Wl,--wrap,malloc'] if cfg.options.force_32bit: From 2bc167d2ff84e40e77ed9235c17e98bfd73d7f51 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 15 Jan 2025 16:05:26 -0600 Subject: [PATCH 73/77] AP_Landing: mode AUTOLAND enhancements Co-authored-by: Andrew Tridgell Co-authored-by: Pete Hall --- libraries/AP_Landing/AP_Landing.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index bd85696c961fc..e9bc8e37f7d86 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -99,7 +99,10 @@ class AP_Landing { // accessor functions for the params and states static const struct AP_Param::GroupInfo var_info[]; - + + // Return the landing type + Landing_Type get_type() const { return (Landing_Type)type.get(); } + int16_t get_pitch_cd(void) const { return pitch_deg*100; } float get_flare_sec(void) const { return flare_sec; } int8_t get_disarm_delay(void) const { return disarm_delay; } From 010e6ba0b0f777d53999c62bbaa19c019a07ea1e Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 15 Jan 2025 16:05:26 -0600 Subject: [PATCH 74/77] ArduPlane: mode AUTOLAND enhancements Co-authored-by: Andrew Tridgell Co-authored-by: Pete Hall --- ArduPlane/AP_Arming.cpp | 4 + ArduPlane/Plane.cpp | 4 + ArduPlane/Plane.h | 5 + ArduPlane/commands_logic.cpp | 32 +++--- ArduPlane/mode.h | 73 +++++++++++- ArduPlane/mode_autoland.cpp | 211 ++++++++++++++++++++++++----------- ArduPlane/mode_takeoff.cpp | 13 --- 7 files changed, 248 insertions(+), 94 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 25a43be25cab3..f52fd6df10042 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -321,6 +321,10 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c // rising edge of delay_arming oneshot delay_arming = true; +#if MODE_AUTOLAND_ENABLED + plane.mode_autoland.arm_check(); +#endif + send_arm_disarm_statustext("Throttle armed"); return true; diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 2fb6cb1eebdc5..ed451a403dbf8 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -514,6 +514,10 @@ void Plane::update_control_mode(void) update_fly_forward(); control_mode->update(); + +#if MODE_AUTOLAND_ENABLED + mode_autoland.check_takeoff_direction(); +#endif } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b272a13b11ff1..1dda8aa741f40 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1333,6 +1333,11 @@ class Plane : public AP_Vehicle { #endif // AP_SCRIPTING_ENABLED + bool tkoff_option_is_set(AP_FixedWing::TakeoffOption option) const { + return (aparm.takeoff_options & int32_t(option)) != 0; + } + + }; extern Plane plane; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index af99693635b98..b5f91f89d5b42 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -26,15 +26,18 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) // reset loiter start time. New command is a new loiter loiter.start_time_ms = 0; - AP_Mission::Mission_Command next_nav_cmd; - const uint16_t next_index = mission.get_current_nav_index() + 1; - const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd); - auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND); + // Mission lookahead is only valid in auto + if (control_mode == &mode_auto) { + AP_Mission::Mission_Command next_nav_cmd; + const uint16_t next_index = mission.get_current_nav_index() + 1; + const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd); + auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND); #if HAL_QUADPLANE_ENABLED - if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) { - auto_state.wp_is_land_approach = false; - } + if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) { + auto_state.wp_is_land_approach = false; + } #endif + } } switch(cmd.id) { @@ -382,9 +385,6 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) // zero locked course steer_state.locked_course_err = 0; steer_state.hold_course_cd = -1; -#if MODE_AUTOLAND_ENABLED - takeoff_state.initial_direction.initialized = false; -#endif auto_state.baro_takeoff_alt = barometer.get_altitude(); } @@ -559,11 +559,6 @@ bool Plane::verify_takeoff() gps.ground_speed() > min_gps_speed && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { auto_state.takeoff_speed_time_ms = millis(); -#if MODE_AUTOLAND_ENABLED - plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off); - takeoff_state.initial_direction.initialized = true; - gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.initial_direction.heading)); -#endif } if (auto_state.takeoff_speed_time_ms != 0 && millis() - auto_state.takeoff_speed_time_ms >= 2000) { @@ -1173,6 +1168,13 @@ bool Plane::verify_loiter_heading(bool init) } #endif +#if MODE_AUTOLAND_ENABLED + if (control_mode == &mode_autoland) { + // autoland mode has its own lineup criterion + return mode_autoland.landing_lined_up(); + } +#endif + //Get the lat/lon of next Nav waypoint after this one: AP_Mission::Mission_Command next_nav_cmd; if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 6e5d0b56327f0..c5cf09fb4988a 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -156,6 +156,11 @@ class Mode // true if voltage correction should be applied to throttle virtual bool use_battery_compensation() const; + +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + virtual bool allows_autoland_direction_capture() const { return false; } +#endif #if AP_QUICKTUNE_ENABLED // does this mode support VTOL quicktune? @@ -212,6 +217,11 @@ friend class ModeQAcro; void stabilize_quaternion(); +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + protected: // ACRO controller state @@ -262,6 +272,11 @@ class ModeAuto : public Mode void run() override; +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + #if AP_PLANE_GLIDER_PULLUP_ENABLED bool in_pullup() const { return pullup.in_pullup(); } #endif @@ -308,6 +323,11 @@ class ModeAutoTune : public Mode void run() override; +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + protected: bool _enter() override; @@ -449,6 +469,11 @@ class ModeManual : public Mode // true if voltage correction should be applied to throttle bool use_battery_compensation() const override { return false; } +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + }; @@ -495,6 +520,11 @@ class ModeStabilize : public Mode void run() override; +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + private: void stabilize_stick_mixing_direct(); @@ -513,6 +543,10 @@ class ModeTraining : public Mode void run() override; +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif }; class ModeInitializing : public Mode @@ -552,6 +586,11 @@ class ModeFBWA : public Mode void run() override; +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + }; class ModeFBWB : public Mode @@ -844,6 +883,11 @@ class ModeTakeoff: public Mode bool does_auto_throttle() const override { return true; } +#if MODE_AUTOLAND_ENABLED + // true if mode allows landing direction to be set on first takeoff after arm in this mode + bool allows_autoland_direction_capture() const override { return true; } +#endif + // var_info for holding parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -887,6 +931,13 @@ class ModeAutoLand: public Mode bool does_auto_throttle() const override { return true; } + void check_takeoff_direction(void); + + // return true when lined up correctly from the LOITER_TO_ALT + bool landing_lined_up(void); + + // see if we should capture the direction + void arm_check(void); // var_info for holding parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -894,11 +945,29 @@ class ModeAutoLand: public Mode AP_Int16 final_wp_alt; AP_Int16 final_wp_dist; AP_Int16 landing_dir_off; + AP_Int8 options; + + // Bitfields of AUTOLAND_OPTIONS + enum class AutoLandOption { + AUTOLAND_DIR_ON_ARM = (1U << 0), // set dir for autoland on arm if compass in use. + }; + + enum class AutoLandStage { + LOITER, + LANDING + }; + + bool autoland_option_is_set(AutoLandOption option) const { + return (options & int8_t(option)) != 0; + } protected: bool _enter() override; - AP_Mission::Mission_Command cmd[3]; - uint8_t stage; + AP_Mission::Mission_Command cmd_loiter; + AP_Mission::Mission_Command cmd_land; + Location land_start; + AutoLandStage stage; + void set_autoland_direction(const float heading); }; #endif #if HAL_SOARING_ENABLED diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index e9ec9e8e96442..4727cc585015a 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -28,13 +28,20 @@ const AP_Param::GroupInfo ModeAutoLand::var_info[] = { // @Param: DIR_OFF // @DisplayName: Landing direction offset from takeoff - // @Description: The captured takeoff direction (at arming,if TKOFF_OPTION bit1 is set, or after ground course is established in autotakeoffs)is offset by this amount to create a different landing direction and approach. + // @Description: The captured takeoff direction after ground course is established in autotakeoffsis offset by this amount to create a different landing direction and approach.However,if TKOFF_OPTION bit1 is set, the takeoff(landing) direction is captured immediately via compass heading upon arming, then this offset is NOT applied. // @Range: -360 360 // @Increment: 1 // @Units: deg // @User: Standard AP_GROUPINFO("DIR_OFF", 3, ModeAutoLand, landing_dir_off, 0), + // @Param: OPTIONS + // @DisplayName: Autoland mode options + // @Description: Enables optional autoland mode behaviors + // @Bitmask: 1: When set if there is a healthy compass in use the compass heading will be captured at arming and used for the AUTOLAND mode's initial takeoff direction instead of capturing ground course in NAV_TAKEOFF or Mode TAKEOFF or other modes. + // @User: Standard + AP_GROUPINFO("OPTIONS", 4, ModeAutoLand, options, 0), + AP_GROUPEND }; @@ -45,81 +52,100 @@ ModeAutoLand::ModeAutoLand() : } bool ModeAutoLand::_enter() -{ +{ //must be flying to enter - if (!plane.is_flying()) { - gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!"); + if (!plane.is_flying()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!"); return false; } - - //takeoff direction must be set and must not be a quadplane, otherwise since flying switch to RTL so this can be used as FS action + + // autoland not available for quadplanes as capture of takeoff direction + // doesn't make sense #if HAL_QUADPLANE_ENABLED - if (quadplane.available() && !quadplane.option_is_set(QuadPlane::OPTION::ALLOW_FW_LAND)) { - gcs().send_text(MAV_SEVERITY_WARNING, "Option not set to allow fixed wing autoland"); - return false; + if (quadplane.available()) { + gcs().send_text(MAV_SEVERITY_WARNING, "autoland not available"); + return false; } #endif + if (!plane.takeoff_state.initial_direction.initialized) { - gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set,must use autotakeoff"); + gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set"); return false; } - plane.prev_WP_loc = plane.current_loc; plane.set_target_altitude_current(); + plane.auto_state.next_wp_crosstrack = true; + + plane.prev_WP_loc = plane.current_loc; + + // In flight stage normal for approach + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + + const Location &home = ahrs.get_home(); + +#ifndef HAL_LANDING_DEEPSTALL_ENABLED + if (plane.landing.get_type() == AP_Landing::TYPE_DEEPSTALL) { + // Deep stall landings require only a landing location, they do there own loiter to alt and approach + cmd_land.id = MAV_CMD_NAV_LAND; + cmd_land.content.location = home; + + // p1 gives the altitude from which to start the deep stall above the location alt + cmd_land.p1 = final_wp_alt; + plane.start_command(cmd_land); + + stage = AutoLandStage::LANDING; + return true; + } +#endif // HAL_LANDING_DEEPSTALL_ENABLED /* - landing is in 3 steps: - 1) a base leg waypoint - 2) a land start WP, with crosstrack - 3) the landing WP, with crosstrack + Glide slope landing is in 3 steps: + 1) a loitering to alt waypoint centered on base leg + 2) exiting and proceeeing to a final approach land start WP, with crosstrack + 3) the landing WP at home, with crosstrack the base leg point is 90 degrees off from the landing leg */ - const Location &home = ahrs.get_home(); /* first calculate the starting waypoint we will use when doing the NAV_LAND. This is offset by final_wp_dist from home, in a direction 180 degrees from takeoff direction */ - Location land_start_WP = home; - land_start_WP.offset_bearing(wrap_360(plane.takeoff_state.initial_direction.heading + 180), final_wp_dist); - land_start_WP.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME); - land_start_WP.change_alt_frame(Location::AltFrame::ABSOLUTE); + land_start = home; + land_start.offset_bearing(plane.takeoff_state.initial_direction.heading, -final_wp_dist); + land_start.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME); + land_start.change_alt_frame(Location::AltFrame::ABSOLUTE); /* - now create the initial target waypoint for the base leg. We + now create the initial target waypoint for the loitering to alt centered on base leg waypoint. We choose if we will do a right or left turn onto the landing based on where we are when we enter the landing mode */ - const float bearing_to_current_deg = wrap_180(degrees(land_start_WP.get_bearing(plane.current_loc))); - const float bearing_err_deg = wrap_180(wrap_180(plane.takeoff_state.initial_direction.heading) - bearing_to_current_deg); - const float bearing_offset_deg = bearing_err_deg > 0? -90 : 90; - const float base_leg_length = final_wp_dist * 0.333; - cmd[0].id = MAV_CMD_NAV_WAYPOINT; - cmd[0].content.location = land_start_WP; - cmd[0].content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, base_leg_length); - // set a 1m acceptance radius, we want to fly all the way to this waypoint - cmd[0].p1 = 1; - - /* - create the WP for the start of the landing - */ - cmd[1].content.location = land_start_WP; - cmd[1].id = MAV_CMD_NAV_WAYPOINT; - - // and the land WP - cmd[2].id = MAV_CMD_NAV_LAND; - cmd[2].content.location = home; - - // start first leg - stage = 0; - plane.start_command(cmd[0]); - - // don't crosstrack initially - plane.auto_state.crosstrack = false; - plane.auto_state.next_wp_crosstrack = true; - + const float bearing_to_current_deg = degrees(land_start.get_bearing(plane.current_loc)); + const float bearing_err_deg = wrap_180(plane.takeoff_state.initial_direction.heading - bearing_to_current_deg); + const float bearing_offset_deg = (bearing_err_deg > 0) ? -90 : 90; + + // Try and minimize loiter radius by using the smaller of the waypoint loiter radius or 1/3 of the final WP distance + const float loiter_radius = MIN(final_wp_dist * 0.333, fabsf(plane.aparm.loiter_radius)); + + // corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases. + // Strictly this gets the loiter radius at the current altitude, really we want the loiter radius at final_wp_alt. + const float corrected_loiter_radius = plane.nav_controller->loiter_radius(loiter_radius); + + cmd_loiter.id = MAV_CMD_NAV_LOITER_TO_ALT; + cmd_loiter.p1 = loiter_radius; + cmd_loiter.content.location = land_start; + cmd_loiter.content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, corrected_loiter_radius); + cmd_loiter.content.location.loiter_ccw = bearing_err_deg>0? 1 :0; + + // land WP at home + cmd_land.id = MAV_CMD_NAV_LAND; + cmd_land.content.location = home; + + // start first leg toward the base leg loiter to alt point + stage = AutoLandStage::LOITER; + plane.start_command(cmd_loiter); return true; } @@ -129,30 +155,87 @@ void ModeAutoLand::update() plane.calc_nav_pitch(); plane.set_offset_altitude_location(plane.prev_WP_loc, plane.next_WP_loc); if (plane.landing.is_throttle_suppressed()) { - // if landing is considered complete throttle is never allowed, regardless of landing type - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + // if landing is considered complete throttle is never allowed, regardless of landing type + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); } else { - plane.calc_throttle(); + plane.calc_throttle(); } } void ModeAutoLand::navigate() { - if (stage == 2) { - // we are on final landing leg + switch (stage) { + case AutoLandStage::LOITER: + // check if we have arrived and completed loiter at base leg waypoint + if (plane.verify_loiter_to_alt(cmd_loiter)) { + stage = AutoLandStage::LANDING; + plane.start_command(cmd_land); + // Crosstrack from the land start location + plane.prev_WP_loc = land_start; + + } + break; + + case AutoLandStage::LANDING: plane.set_flight_stage(AP_FixedWing::FlightStage::LAND); - plane.verify_command(cmd[stage]); + plane.verify_command(cmd_land); + // make sure we line up + plane.auto_state.crosstrack = true; + break; + } +} + +/* + Takeoff direction is initialized after arm when sufficient altitude + and ground speed is obtained, then captured takeoff direction + + offset used as landing direction in AUTOLAND +*/ +void ModeAutoLand::check_takeoff_direction() +{ +#if HAL_QUADPLANE_ENABLED + // we don't allow fixed wing autoland for quadplanes + if (quadplane.available()) { return; } +#endif - // see if we have completed the leg - if (plane.verify_nav_wp(cmd[stage])) { - stage++; - plane.prev_WP_loc = plane.next_WP_loc; - plane.auto_state.next_turn_angle = 90; - plane.start_command(cmd[stage]); - plane.auto_state.crosstrack = true; - plane.auto_state.next_wp_crosstrack = true; + if (plane.takeoff_state.initial_direction.initialized) { + return; + } + //set autoland direction to GPS course over ground + if (plane.control_mode->allows_autoland_direction_capture() && + plane.is_flying() && + hal.util->get_soft_armed() && + plane.gps.ground_speed() > GPS_GND_CRS_MIN_SPD) { + set_autoland_direction(plane.gps.ground_course() + landing_dir_off); } } -#endif + +// Sets autoland direction using ground course + offest parameter +void ModeAutoLand::set_autoland_direction(const float heading) +{ + plane.takeoff_state.initial_direction.heading = wrap_360(heading); + plane.takeoff_state.initial_direction.initialized = true; + gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading)); +} + +/* + return true when the LOITER_TO_ALT is lined up ready for the landing, used in commands_logic verify loiter to alt + */ +bool ModeAutoLand::landing_lined_up(void) +{ + // use the line between the center of the LOITER_TO_ALT on the base leg and the + // start of the landing leg (land_start_WP) + return plane.mode_loiter.isHeadingLinedUp(cmd_loiter.content.location, cmd_land.content.location); +} + +// possibly capture heading on arming for autoland mode if option is set and using a compass +void ModeAutoLand::arm_check(void) +{ + if (plane.ahrs.use_compass() && autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_DIR_ON_ARM)) { + set_autoland_direction(plane.ahrs.yaw_sensor * 0.01); + } +} + +#endif // MODE_AUTOLAND_ENABLED + diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 8f18f6d1b2eeb..9eebcd1c119ad 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -134,9 +134,6 @@ void ModeTakeoff::update() plane.takeoff_state.throttle_max_timer_ms = millis(); takeoff_mode_setup = true; plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function. -#if MODE_AUTOLAND_ENABLED - plane.takeoff_state.initial_direction.initialized = false; -#endif } } } @@ -145,16 +142,6 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); takeoff_mode_setup = false; } -#if MODE_AUTOLAND_ENABLED - // set initial_direction.heading - const float min_gps_speed = GPS_GND_CRS_MIN_SPD; - if (!(plane.takeoff_state.initial_direction.initialized) && (plane.gps.ground_speed() > min_gps_speed) - && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { - plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off); - plane.takeoff_state.initial_direction.initialized = true; - gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading)); - } -#endif // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we // pass the target location. The check for target location prevents us // flying towards a wrong location if we can't climb. From d59b5a5b40f010cf680c1e2d0fed366fb8d158b6 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 15 Jan 2025 16:05:26 -0600 Subject: [PATCH 75/77] Tools: mode AUTOLAND enhancements Co-authored-by: Andrew Tridgell Co-authored-by: Pete Hall --- Tools/autotest/arduplane.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 3dd362bed181b..172a4a1c904d6 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4241,17 +4241,28 @@ def FlyEachFrame(self): def AutoLandMode(self): '''Test AUTOLAND mode''' + self.set_parameters({ + "AUTOLAND_DIR_OFF": 45, + }) self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"]) self.context_collect('STATUSTEXT') self.takeoff(alt=80, mode='TAKEOFF') self.wait_text("Autoland direction", check_context=True) self.change_mode(26) self.wait_disarmed(120) - self.progress("Check the landed heading matches takeoff") - self.wait_heading(173, accuracy=5, timeout=1) - loc = mavutil.location(-35.362938, 149.165085, 585, 173) + self.progress("Check the landed heading matches takeoff plus offset") + self.wait_heading(218, accuracy=5, timeout=1) + loc = mavutil.location(-35.362938, 149.165085, 585, 218) if self.get_distance(loc, self.mav.location()) > 35: raise NotAchievedException("Did not land close to home") + self.set_parameters({ + "TKOFF_OPTIONS": 2, + }) + self.wait_ready_to_arm() + self.set_autodisarm_delay(0) + self.arm_vehicle() + self.progress("Check the set dir on arm option") + self.wait_text("Autoland direction", check_context=True) def RCDisableAirspeedUse(self): '''Test RC DisableAirspeedUse option''' From 781b6bd8f869641240d65065ef6100e97e698327 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Mon, 20 Jan 2025 19:24:37 -0600 Subject: [PATCH 76/77] AP_HAL_ChibiOS: enable scripting on KakuteH7Mini-Nand It has LittleFS now so it can easily use scripting. --- libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat index 0ca2b13bf2536..d3853b9805049 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini-Nand/hwdef.dat @@ -11,3 +11,6 @@ IMU BMI270 SPI:bmi270 ROTATION_PITCH_180_YAW_90 undef HAL_LOGGING_DATAFLASH_ENABLED DATAFLASH littlefs:w25nxx + +# KakuteH7Mini disables due to smaller flash chip; allow the enabled default +undef AP_SCRIPTING_ENABLED From 429ed5cdd26e857491a4c2c26c885bd5da70df46 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Mon, 20 Jan 2025 18:47:34 -0600 Subject: [PATCH 77/77] AP_Scripting: only log aerobatic trick file when supported LittleFS does not have this function implemented due to logging bandwidth, so `logger:log_file_content` is nil. Don't call it unless it exists, assuming this logging is not critical. --- .../applets/Aerobatics/FixedWing/plane_aerobatics.lua | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua index 9a644a3d5fbcc..670e64349acc9 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua @@ -3035,7 +3035,9 @@ function load_trick(id) local pc = path_composer(name, paths) gcs:send_text(MAV_SEVERITY.INFO, string.format("Loaded trick%u '%s'", id, name)) command_table[id] = PathFunction(pc, name) - logger:log_file_content(filename) + if logger.log_file_content then + logger:log_file_content(filename) + end calculate_timestamps(command_table[id]) end