terrain + blackbox at the same time

Author: error414

src/main/blackbox/blackbox.c

@@ -65,6 +65,8 @@
 
 #include "io/beeper.h"
 #include "io/gps.h"
+#include "io/asyncfatfs/asyncfatfs.h"
+
 
 #include "navigation/navigation.h"
 
@@ -198,6 +200,54 @@ typedef struct blackboxDeltaFieldDefinition_s {
     uint8_t condition; // Decide whether this field should appear in the log
 } blackboxDeltaFieldDefinition_t;
 
+static bool canUseBlackboxWithCurrentConfiguration(void)
+{
+    return feature(FEATURE_BLACKBOX);
+}
+
+#ifdef USE_TERRAIN
+//state machine to get access to other device, it's designed only for one other device, in this case for terrain
+//if you would like to have more devices, some queue must be implemented
+static struct blackboxSDCardAccessStatus_s {
+    bool blackboxAccessToSDGrantedToOtherDevice;
+    bool requestToSdCardAccessState;
+
+} blackboxSDCardAccessStatus = {
+        .blackboxAccessToSDGrantedToOtherDevice = false,
+        .requestToSdCardAccessState = false
+};
+
+
+/**
+ * request access from terrain subsystem
+ * @return
+ */
+bool requestToSdCardAccess(void)
+{
+    if(blackboxConfig()->device != BLACKBOX_DEVICE_SDCARD || !canUseBlackboxWithCurrentConfiguration()){
+        return true;
+    }
+
+    if(blackboxSDCardAccessStatus.blackboxAccessToSDGrantedToOtherDevice){
+        return true;
+    }
+
+    blackboxSDCardAccessStatus.requestToSdCardAccessState = true;
+    return false;
+}
+
+/**
+ * release access from terrain subsystem
+ * @return
+ */
+void releaseSdCardAccess(void)
+{
+    blackboxSDCardAccessStatus.blackboxAccessToSDGrantedToOtherDevice = false;
+    blackboxSDCardAccessStatus.requestToSdCardAccessState = false;
+}
+#endif
+
+
 /**
  * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
  * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
@@ -2178,6 +2228,24 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
  */
 void blackboxUpdate(timeUs_t currentTimeUs)
 {
+#ifdef USE_TERRAIN
+    if(blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD){
+        //access to SD card is given to other device
+        if(blackboxSDCardAccessStatus.blackboxAccessToSDGrantedToOtherDevice){
+            return;
+        }
+
+        //incooming request to get access to SD card
+        if(blackboxSDCardAccessStatus.requestToSdCardAccessState && (blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_STOPPED)){
+            //we have to be sure that all writes are already processed and SD card is in idle
+            if(afatfs_isIdle()){
+                blackboxSDCardAccessStatus.requestToSdCardAccessState = false;
+                blackboxSDCardAccessStatus.blackboxAccessToSDGrantedToOtherDevice = true;
+            }
+        }
+    }
+#endif
+
     if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) {
         blackboxReplenishHeaderBudget();
     }
@@ -2308,15 +2376,6 @@ void blackboxUpdate(timeUs_t currentTimeUs)
     }
 }
 
-static bool canUseBlackboxWithCurrentConfiguration(void)
-{
-    return feature(FEATURE_BLACKBOX)
-#ifdef USE_TERRAIN
-    && terrainConfig()->terrainEnabled == false && blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD
-#endif
-    ;
-}
-
 BlackboxState getBlackboxState(void)
 {
     return blackboxState;

src/main/blackbox/blackbox.h

@@ -75,3 +75,6 @@ void blackboxIncludeFlagSet(uint32_t mask);
 void blackboxIncludeFlagClear(uint32_t mask);
 bool blackboxIncludeFlag(uint32_t mask);
 BlackboxState getBlackboxState(void);
+
+bool requestToSdCardAccess(void);
+void releaseSdCardAccess(void);

src/main/blackbox/blackbox_io.c

@@ -435,7 +435,16 @@ static bool blackboxSDCardBeginLog(void)
         if (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY) {
             blackboxSDCard.state = BLACKBOX_SDCARD_WAITING;
 
-            afatfs_mkdir("logs", blackboxLogDirCreated);
+            if(afatfs_isCurrentDirRoot()){
+                //we are in root of SD card, we have to create or move to log directory
+                afatfs_mkdir("logs", blackboxLogDirCreated);
+            }
+            else
+            {
+                //we are already in log directory
+                blackboxSDCard.logDirectory = NULL;
+                blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG;
+            }
         }
         break;
 
@@ -444,6 +453,12 @@ static bool blackboxSDCardBeginLog(void)
         break;
 
     case BLACKBOX_SDCARD_ENUMERATE_FILES:
+
+        if (blackboxSDCard.logDirectory == NULL) {
+            blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG;
+            break;
+        }
+
         while (afatfs_findNext(blackboxSDCard.logDirectory, &blackboxSDCard.logDirectoryFinder, &directoryEntry) == AFATFS_OPERATION_SUCCESS) {
             if (directoryEntry && !fat_isDirectoryEntryTerminator(directoryEntry)) {
                 // If this is a log file, parse the log number from the filename
@@ -469,6 +484,12 @@ static bool blackboxSDCardBeginLog(void)
         break;
 
     case BLACKBOX_SDCARD_CHANGE_INTO_LOG_DIRECTORY:
+        //if logDirectory is NULL, it would mean change directory to ROOT, we don't want to do that
+        if (blackboxSDCard.logDirectory == NULL) {
+            blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG;
+            break;
+        }
+
         // Change into the log directory:
         if (afatfs_chdir(blackboxSDCard.logDirectory)) {
             // We no longer need our open handle on the log directory
@@ -535,7 +556,7 @@ bool blackboxDeviceEndLog(bool retainLog)
         ) {
             // Don't bother waiting the for the close to complete, it's queued now and will complete eventually
             blackboxSDCard.logFile = NULL;
-            blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG;
+            blackboxSDCard.state = BLACKBOX_SDCARD_INITIAL;
             return true;
         }
         return false;

src/main/io/asyncfatfs/asyncfatfs.c

@@ -495,6 +495,50 @@ static void afatfs_fileOperationContinue(afatfsFile_t *file);
 static uint8_t* afatfs_fileLockCursorSectorForWrite(afatfsFilePtr_t file);
 static uint8_t* afatfs_fileRetainCursorSectorForRead(afatfsFilePtr_t file);
 
+bool afatfs_isIdle(void)
+{
+    // 1. Check if any sector is being actively flushed or read at the hardware level
+    if (afatfs.cacheFlushInProgress) {
+        return false;
+    }
+
+    // 2. Check all cache descriptors for active I/O states (waiting for SD card)
+    for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) {
+        if (afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_READING ||
+            afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_WRITING) {
+            return false;
+        }
+    }
+
+    // 3. Check if current directory state machine is performing an async task (like chdir or findNext)
+    if (afatfs.currentDirectory.operation.operation != AFATFS_FILE_OPERATION_NONE) {
+        return false;
+    }
+
+#ifdef AFATFS_USE_FREEFILE
+    // 4. Check freeFile state (ignore LOCKED as it's a static permission marker, not an operation)
+    if (afatfs.freeFile.operation.operation != AFATFS_FILE_OPERATION_NONE &&
+        afatfs.freeFile.operation.operation != AFATFS_FILE_OPERATION_LOCKED) {
+        return false;
+    }
+#endif
+
+    // 5. Check all potentially open file handles for pending background operations
+    for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) {
+        if (afatfs.openFiles[i].type != AFATFS_FILE_TYPE_NONE &&
+            afatfs.openFiles[i].operation.operation != AFATFS_FILE_OPERATION_NONE) {
+            return false;
+        }
+    }
+
+    // 6. During initialization, we are never "idle" in the operational sense
+    if (afatfs.filesystemState == AFATFS_FILESYSTEM_STATE_INITIALIZATION) {
+        return false;
+    }
+
+    return true;
+}
+
 static uint32_t roundUpTo(uint32_t value, uint32_t rounding)
 {
     uint32_t remainder = value % rounding;
@@ -3622,6 +3666,11 @@ afatfsError_e afatfs_getLastError(void)
     return afatfs.lastError;
 }
 
+bool afatfs_isCurrentDirRoot(void)
+{
+    return afatfs.currentDirectory.directoryEntryPos.sectorNumberPhysical == 0;
+}
+
 void afatfs_init(void)
 {
 #ifdef STM32H7

src/main/io/asyncfatfs/asyncfatfs.h

@@ -95,3 +95,6 @@ bool afatfs_isFull(void);
 
 afatfsFilesystemState_e afatfs_getFilesystemState(void);
 afatfsError_e afatfs_getLastError(void);
+
+bool afatfs_isIdle(void);
+bool afatfs_isCurrentDirRoot(void);

src/main/terrain/terrain.c

@@ -170,9 +170,8 @@ void terrainUpdateTask(timeUs_t currentTimeUs)
     }
 
     /////////////////////////////////////////////////////////////////////////////////////////////////////
-    //try to find home altitude to safe offset
-    //save reference only once when gps origin is valid and different than last saved
-    if(terrainHomePos.homeAltitudeFound == false && posControl.gpsOrigin.valid && (posControl.gpsOrigin.lat != terrainHomePos.homeLocation.lat || posControl.gpsOrigin.lon != terrainHomePos.homeLocation.lon)){
+    //try to find home altitude to home offset
+    if(terrainHomePos.homeAltitudeFound == false && posControl.gpsOrigin.valid && (posControl.gpsOrigin.lat != terrainHomePos.homeLocation.lat || posControl.gpsOrigin.lon != terrainHomePos.homeLocation.lon)) {
 
         gpsLocation_t homeLoc = {
                 .lat = posControl.gpsOrigin.lat,
@@ -214,7 +213,7 @@ int32_t terrainGetLastDistanceCm(void)
         terrainSystemStatus.isStarted = true;
     }
 
-    return terrainHeight.lastUpdate + 500 < millis() ? TERRAIN_STATUS_NO_DATA : terrainHeight.terrainAGL;
+    return terrainHeight.lastUpdate + 200 < millis() ? TERRAIN_STATUS_NO_DATA : terrainHeight.terrainAGL;
 }
 
 #endif

src/main/terrain/terrain.h

@@ -30,6 +30,7 @@
 #include "navigation/navigation.h"
 
 #define TERRAIN_TASK_RATE_HZ 50
+#define TERRAIN_TASK_ 200
 
 #define TERRAIN_STATUS_FAILURE (-1)
 #define TERRAIN_STATUS_WRONG_BLOC_SIZE (-2)

src/main/terrain/terrain_io.c

@@ -33,6 +33,7 @@
 #include "drivers/time.h"
 #include "terrain_utils.h"
 #include "drivers/sdcard/sdcard.h"
+#include "blackbox/blackbox.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 
@@ -47,6 +48,7 @@ static void hardFailure(void)
     terrainIoState.status = TERRAIN_IO_FAILURE;
     terrainIoState.gridBlock = NULL;
     //LOG_DEBUG(SYSTEM, "TERRAIN RELEASE FAILURE");
+    releaseSdCardAccess();
 }
 
 /**
@@ -62,6 +64,7 @@ static void resetStateMachine(void)
     terrainIoState.openFileStartTimeMs = 0;
 
     //LOG_DEBUG(SYSTEM, "TERRAIN RELEASE RESET STATE");
+    releaseSdCardAccess();
 }
 
 /**
@@ -182,6 +185,13 @@ void loadGridToCacheTask(void)
             return;
         }
 
+        ///////////////////////////////////////////////////////////////////
+        //request access from blackbox
+        if(requestToSdCardAccess() == false){
+            return;
+        }
+        ////////////////////////////////////////////////////////////////
+
         //check if we had too many errors opening of this file
         terrainIoFileOpenStatus_t* fileOpenStatus = getFileOpenStatusIndex(gridBlock->latDegrees, gridBlock->lonDegrees);
         if(fileOpenStatus->errorOpenCount >= 3){
@@ -200,6 +210,12 @@ void loadGridToCacheTask(void)
     ///////////////////////////////////////////////////////////////////
     /////// CHANGE DIR   /////////////////////////////////////////////
     if(terrainIoState.status == TERRAIN_IO_CHANGE_DIR){
+        //already in the root directory, no need to change the directory
+        if(afatfs_isCurrentDirRoot()){
+            terrainIoState.status = TERRAIN_IO_OPEN_FILE;
+            return;
+        }
+
         //LOG_DEBUG(SYSTEM, "TERRAIN CHANGING DIR");
         //change dir to ROOT (null)
         if(!afatfs_chdir(NULL)){