//#DOC Main, program entry and main loop
#include "main.h"
#include "otRetinaMinimal.h"
#include "otStatusLed.h"
#include "otRetinaTelemetry.h"
#include "otRetinaAcquisitionStage.h"
#include "otRetinaRunClock.h"
#include "retinaConfig.h"
#include <otSerial.h>
#include <otTimer.h>
#include <otPrintf.h>
#include <otCamera.h>
#include <otImageSensor.h>
#include <otImage.h>
#include <otI2cBus.h>
#include <otFILE.h>
#include <otImageFile.h>
#include <otMPU9250.h>
#include <otMath.h>
#include <otMemory.h>
#include <otMalloc.h>
#include <otPWM.h>
#include <otPort.h>
#include <otFPGA.h>
#include <otDMA.h>
#include <otRTC.h>
#include <mt9v032.h>
// -----------------------------------------------------------------------------
// Analog Devices Blackfin full-search motion estimator (motionvect.asm)
// Output vectors are signed bytes in HALF-PIXEL units (x2).
// The assembly ABI expects: target, reference, vertical MV, horizontal MV,
// rows, columns, search_factor.
// -----------------------------------------------------------------------------
extern "C" void _motionvect(const U8 *targetFrame,
const U8 *referenceFrame,
S8 *mvVertical,
S8 *mvHorizontal,
U32 rows,
U32 columns,
U32 searchFactor);
// -----------------------------------------------------------------------------
// Devices
// -----------------------------------------------------------------------------
otCamera camera;
otImageSensor sensor;
otImage image;
otImage previousImage;
otImage debugImage; // U8 image used only for STOP snapshot diagnostics
otFILE file;
otImageFile imgFile;
otMPU9250 imu;
otPWM pwm;
otRetinaMinimal retina;
otStatusLed statusLed;
// V2.3C PF15 shared asynchronous input. The ISR records only the falling-edge
// sequence and clears the hardware request. Meaning is assigned by state:
// RUN -> physical last-line defence; post-timeout IDLE -> next-run trigger.
#define RETINA_PF15_PIN 15U
#define RETINA_PF15_IDLE_RELEASE_STABLE_MS 80U
static FAST_DATA_2 volatile U32 _retinaPf15FallingSeq = 0U;
static FAST_DATA_2 U32 _retinaFrontContactCountRun = 0U;
static FAST_DATA_2 bool _retinaLastRunTimedOut = false;
static FAST_DATA_2 bool _retinaRunStartedByPf15 = false;
static FAST_DATA_2 U32 _retinaPf15RunTriggerCount = 0U;
static FAST_DATA_2 bool _retinaPf15IrqArmed = false;
FAST_CODE_2 void retinaPf15InterruptService()
{
++_retinaPf15FallingSeq;
PortFClearInterrupt();
}
// IMPORTANT: on BF51x PORTFIO is not a raw pin register once EDGE/POLAR are
// configured for an edge interrupt. With falling-edge sensitivity it becomes
// the sticky falling-edge event latch. Therefore PortGet(PP_F,15) is used only
// in plain level mode; while the IRQ is armed, contact assertion is detected
// exclusively through _retinaPf15FallingSeq.
static void retinaPf15LevelMode()
{
const U16 bit = (U16)BIT(RETINA_PF15_PIN);
PortFInterruptEnable(bit, false);
PPF_EDG &= (U16)(~bit);
PPF_RSE &= (U16)(~bit);
PPF_POL &= (U16)(~bit);
PPF_CLR = bit;
PortConfig(PP_F, RETINA_PF15_PIN, P_IN);
_retinaPf15IrqArmed = false;
}
static bool retinaPf15PressedLevel()
{
// Valid ONLY after retinaPf15LevelMode().
return !PortGet(PP_F, RETINA_PF15_PIN);
}
static void retinaPf15ArmFalling()
{
const U16 bit = (U16)BIT(RETINA_PF15_PIN);
PortFInterruptEnable(bit, false);
PortFSetInterruptBothEdges(bit, false);
PortFSetInterruptService(bit, retinaPf15InterruptService, false);
PortFClearInterrupt();
PortFInterruptEnable(bit, true);
_retinaPf15IrqArmed = true;
}
static bool retinaPf15WaitReleased(bool allowUartAbort)
{
retinaPf15LevelMode();
U32 stableMs = 0U;
while(stableMs < RETINA_PF15_IDLE_RELEASE_STABLE_MS)
{
if(allowUartAbort && UartCheckChar(UART0) && UartGetChar(UART0) == '.')
return false;
if(retinaPf15PressedLevel())
stableMs = 0U;
else
stableMs += 10U;
DelayMs(10);
}
return true;
}
// Field-run diagnostic logging. V2.0D keeps the complete text in SDRAM for
// the entire locomotion interval. Filesystem writes occur only after motors/DMA
// are stopped; there are no realtime checkpoints.
#define RETINA_SDLOG_BUFFER_BYTES (512UL * 1024UL)
#define RETINA_SDLOG_NAME_TRIES 10000U
#define RETINA_SDLOG_CHECKPOINT_PERF 0U
// V1.6 telemetry. Buffers live in SDRAM; minimal-drive decisions remain inside otRetinaMinimal:
// no event/path/flow record can alter a motor command or a navigation decision.
// V2.6A 600 s field profile: RUN0000 produced 2286 EVT and 2593 PATH records in
// 300 s, so the old 4096 rings would wrap at ~538 s and ~474 s respectively.
// 8192 preserves the complete 10-minute history with comfortable margin.
#define RETINA_EVENT_LOG_CAPACITY 8192U
#define RETINA_PATH_HISTORY_CAPACITY 8192U
#define RETINA_PATH_SAMPLE_FRAMES 4U
// 16384 complete 23x15 maps = about 12.2 MiB. At the measured V2.6A field
// rate (~16.5 FLOW records/s, ~17.5 camera fps) this covers about 16.5 minutes.
// It therefore has ample margin for the 600 s field run without consuming another
// 12 MiB merely to double an already-safe ring. Raw half-pixel vectors are preserved.
#define RETINA_FLOW_LOG_CAPACITY 16384U
#define RETINA_IMU_LOG_CAPACITY 16384U
#define RETINA_ODO_LOG_CAPACITY 16384U
#define RETINA_PROBE_LOG_CAPACITY 16384U
#define RETINA_PROBE_MAG_SAMPLE_FRAMES 4U // ~5 Hz at field frame rate
#define RETINA_PROBE_TEMP_SAMPLE_FRAMES 20U // ~1 Hz
#define RETINA_PROBE_MPU9250_ADDR 0x68U
#define RETINA_PROBE_MPU9250_TEMP_REG 0x41U
// -----------------------------------------------------------------------------
// Field-run logger
// -----------------------------------------------------------------------------
// V2.0D realtime strategy:
// * keep the complete diagnostic log in SDRAM while the robot is moving;
// * perform zero filesystem writes in the autonomic loop;
// * after motors/DMA are stopped, write the complete log to the A/B pair.
// A power loss during a run may lose that run's log; it cannot stall locomotion.
static FAST_DATA_2 U8 *_retinaLogBuffer = 0;
static FAST_DATA_2 U32 _retinaLogUsed = 0U;
static FAST_DATA_2 U32 _retinaLogCheckpointCount = 0U;
static FAST_DATA_2 U32 _retinaLogLastFlushUs = 0U;
static FAST_DATA_2 U32 _retinaLogMaxFlushUs = 0U;
static FAST_DATA_2 U32 _retinaLogTotalSdBytes = 0U;
static FAST_DATA_2 U32 _retinaLogErrorCount = 0U;
static FAST_DATA_2 U32 _retinaLogSlot = 0U;
static FAST_DATA_2 U32 _retinaLogPerfSinceCheckpoint = 0U;
static FAST_DATA_2 bool _retinaLogOverflow = false;
static FAST_DATA_2 char _retinaLogNameA[16];
static FAST_DATA_2 char _retinaLogNameB[16];
static FAST_DATA_2 char _retinaStopAviName[16];
static FAST_DATA_2 char _retinaEventName[16];
static FAST_DATA_2 char _retinaPathName[16];
static FAST_DATA_2 char _retinaFlowName[16];
static FAST_DATA_2 char _retinaImuName[16];
static FAST_DATA_2 char _retinaOdoName[16];
static FAST_DATA_2 char _retinaProbeName[16];
static FAST_DATA_2 U32 _retinaRunIndex = 0U;
static FAST_DATA_2 bool _retinaRunNamesValid = false;
static otRetinaEventLog _retinaEventLog;
static otPathHistory _retinaPathHistory;
static otRetinaFlowLog _retinaFlowLog;
static otRetinaImuLog _retinaImuLog;
static otRetinaOdometer _retinaOdometer;
static otRetinaProbeLog _retinaProbeLog;
static FAST_DATA_2 RetinaEventRecord *_retinaEventStorage = 0;
static FAST_DATA_2 RetinaPathNode *_retinaPathStorage = 0;
static FAST_DATA_2 RetinaFlowRecord *_retinaFlowStorage = 0;
static FAST_DATA_2 RetinaImuRecord *_retinaImuStorage = 0;
static FAST_DATA_2 RetinaOdomRecord *_retinaOdoStorage = 0;
static FAST_DATA_2 RetinaProbeRecord *_retinaProbeStorage = 0;
static FAST_DATA_2 bool _retinaProbeMagPresent = false;
static FAST_DATA_2 U32 _retinaProbeImuSequence = 0U;
static FAST_DATA_2 U32 _retinaProbeLastMagMs = 0U;
static FAST_DATA_2 F32 _retinaProbeMx = 0.0f, _retinaProbeMy = 0.0f, _retinaProbeMz = 0.0f;
static FAST_DATA_2 bool _retinaProbeMagValid = false;
static FAST_DATA_2 S32 _retinaProbeTempCentiC = 0;
static FAST_DATA_2 bool _retinaProbeTempValid = false;
static otRetinaAcquisitionStage _retinaAcquisitionStage;
static bool retinaFileExists(const char *name)
{
if(!name || !name[0])
return false;
FILE probe = file.fopen(name, otFILE::A_READ);
if(!probe)
return false;
file.fclose(probe);
return true;
}
// Select one persistent 8.3-compatible run id shared by the redundant LOG pair
// and the STOP AVI. Existing files are never overwritten. RTC supplies the FAT
// creation/modification timestamps; the numeric id only guarantees uniqueness
// across repeated resets and field locations.
static bool retinaSelectRunNames()
{
_retinaRunNamesValid = false;
for(U32 i = 0U; i < RETINA_SDLOG_NAME_TRIES; ++i)
{
sprintf(_retinaLogNameA, "RUN%04luA.LOG", (unsigned long)i);
sprintf(_retinaLogNameB, "RUN%04luB.LOG", (unsigned long)i);
sprintf(_retinaStopAviName, "STOP%04lu.AVI", (unsigned long)i);
sprintf(_retinaEventName, "EVT%04lu.BIN", (unsigned long)i);
sprintf(_retinaPathName, "PATH%04lu.BIN", (unsigned long)i);
sprintf(_retinaFlowName, "FLOW%04lu.BIN", (unsigned long)i);
sprintf(_retinaImuName, "IMU%04lu.BIN", (unsigned long)i);
sprintf(_retinaOdoName, "ODO%04lu.BIN", (unsigned long)i);
sprintf(_retinaProbeName, "PROB%04lu.BIN", (unsigned long)i);
if(!retinaFileExists(_retinaLogNameA) &&
!retinaFileExists(_retinaLogNameB) &&
!retinaFileExists(_retinaStopAviName) &&
!retinaFileExists(_retinaEventName) &&
!retinaFileExists(_retinaPathName) &&
!retinaFileExists(_retinaFlowName) &&
!retinaFileExists(_retinaImuName) &&
!retinaFileExists(_retinaOdoName) &&
!retinaFileExists(_retinaProbeName))
{
_retinaRunIndex = i;
_retinaRunNamesValid = true;
return true;
}
}
RETINA_EPRINTF("RETINA FILE ERROR: no free RUN/STOP/EVT/PATH/FLOW/IMU/ODO/PROB id in 0000..9999\n");
return false;
}
static const char *retinaRunLogCurrentName()
{
return (_retinaLogSlot == 0U) ? _retinaLogNameA : _retinaLogNameB;
}
static bool retinaRunLogFlush()
{
if(!_retinaLogBuffer || !_retinaLogUsed)
return true;
const char *name = retinaRunLogCurrentName();
U32 written = 0U;
U32 t0 = TimerGetCounts(TIMER0);
FILE fd = file.fopen(name, otFILE::A_CREATE_ALWAYS | otFILE::A_WRITE);
if(!fd)
{
U32 t1 = TimerGetCounts(TIMER0);
_retinaLogLastFlushUs = ElapsedTimeUs(t0, t1);
if(_retinaLogLastFlushUs > _retinaLogMaxFlushUs)
_retinaLogMaxFlushUs = _retinaLogLastFlushUs;
++_retinaLogErrorCount;
RETINA_EPRINTF("RETINA SDLOG ERROR: can't create checkpoint %s\n", name);
// Keep the same slot: the other file remains the last known-good copy.
return false;
}
file.fwrite(fd, _retinaLogBuffer, _retinaLogUsed, written);
file.fclose(fd);
U32 t1 = TimerGetCounts(TIMER0);
_retinaLogLastFlushUs = ElapsedTimeUs(t0, t1);
if(_retinaLogLastFlushUs > _retinaLogMaxFlushUs)
_retinaLogMaxFlushUs = _retinaLogLastFlushUs;
if(written != _retinaLogUsed)
{
++_retinaLogErrorCount;
RETINA_EPRINTF("RETINA SDLOG ERROR: wrote %lu/%lu bytes to %s\n",
(unsigned long)written, (unsigned long)_retinaLogUsed, name);
// Do not advance the slot. The opposite slot is still the known-good one.
return false;
}
_retinaLogTotalSdBytes += written;
++_retinaLogCheckpointCount;
_retinaLogPerfSinceCheckpoint = 0U;
// Next checkpoint goes to the other closed file.
_retinaLogSlot ^= 1U;
return true;
}
static void retinaRunLogAppend(const char *text)
{
if(!_retinaLogBuffer || !text || _retinaLogOverflow)
return;
U32 len = 0U;
while(text[len])
++len;
// Keep the complete history in SDRAM. Do not discard old records to make
// room: if the field run ever exhausts 512 KB, preserve what we already have
// and report it explicitly.
if(len > (RETINA_SDLOG_BUFFER_BYTES - _retinaLogUsed))
{
_retinaLogOverflow = true;
RETINA_VPRINTF("RETINA SDLOG WARNING: SDRAM log full at %lu bytes; realtime SD remains disabled\n",
(unsigned long)_retinaLogUsed);
return;
}
for(U32 i = 0U; i < len; ++i)
_retinaLogBuffer[_retinaLogUsed++] = (U8)text[i];
}
static bool retinaRunLogOpen()
{
_retinaLogUsed = 0U;
_retinaLogCheckpointCount = 0U;
_retinaLogLastFlushUs = 0U;
_retinaLogMaxFlushUs = 0U;
_retinaLogTotalSdBytes = 0U;
_retinaLogErrorCount = 0U;
_retinaLogSlot = 0U;
_retinaLogPerfSinceCheckpoint = 0U;
_retinaLogOverflow = false;
// Pick the persistent run-id file set before allocating the logger buffer so STOP AVI
// remains unique even if text logging has to continue in degraded mode.
if(!retinaSelectRunNames())
return false;
_retinaLogBuffer = (U8 *)_malloc(RETINA_SDLOG_BUFFER_BYTES);
if(!_retinaLogBuffer)
{
RETINA_EPRINTF("RETINA SDLOG: can't allocate %lu byte SDRAM buffer; AVI name remains %s\n",
(unsigned long)RETINA_SDLOG_BUFFER_BYTES, _retinaStopAviName);
return false;
}
char line[384];
sprintf(line,
"RETINA SDLOG BEGIN run=%04lu pair=%s/%s avi=%s evt=%s path=%s flow=%s imu=%s odo=%s probe=%s buffer=%lu bytes\n",
(unsigned long)_retinaRunIndex,
_retinaLogNameA, _retinaLogNameB, _retinaStopAviName,
_retinaEventName, _retinaPathName, _retinaFlowName,
_retinaImuName, _retinaOdoName, _retinaProbeName,
(unsigned long)RETINA_SDLOG_BUFFER_BYTES);
retinaRunLogAppend(line);
RETINA_VPRINTF("RETINA SDLOG: run=%04lu pair=%s/%s avi=%s SDRAM=%lu bytes realtime-SD=OFF\n",
(unsigned long)_retinaRunIndex,
_retinaLogNameA, _retinaLogNameB, _retinaStopAviName,
(unsigned long)RETINA_SDLOG_BUFFER_BYTES);
return true;
}
static void retinaRunLogClose()
{
if(_retinaLogBuffer)
{
retinaRunLogFlush();
_free(_retinaLogBuffer);
_retinaLogBuffer = 0;
}
}
// -----------------------------------------------------------------------------
// Binary telemetry + passive science streams (V3.1A)
// -----------------------------------------------------------------------------
typedef struct RetinaTelemetryFileHeader
{
U32 magic;
U32 version;
U32 recordSize;
U32 count;
U32 total;
U32 overwritten;
U32 capacity;
U32 reserved;
} RetinaTelemetryFileHeader;
#define RETINA_TELEM_VERSION RETINA_ARCH_VERSION
#define RETINA_EVENT_MAGIC 0x45564E54UL // "EVNT"
#define RETINA_PATH_MAGIC 0x50415448UL // "PATH"
#define RETINA_FLOW_MAGIC 0x464C4F57UL // "FLOW"
#define RETINA_IMU_MAGIC 0x494D5539UL // "IMU9"
#define RETINA_ODO_MAGIC 0x4F444F4DUL // "ODOM"
#define RETINA_PROBE_MAGIC 0x50524F42UL // "PROB"
static bool retinaTelemetryWriteRing(const char *name, U32 magic,
const U8 *storage, U32 recordSize,
U32 capacity, U32 count, U32 head,
U32 total, U32 overwritten)
{
if(!name || !storage || !recordSize || !capacity || !count)
return true;
FILE fd = file.fopen(name, otFILE::A_CREATE_ALWAYS | otFILE::A_WRITE);
if(!fd)
{
RETINA_EPRINTF("RETINA TELEMETRY ERROR: can't create %s\n", name);
return false;
}
RetinaTelemetryFileHeader hdr;
hdr.magic = magic;
hdr.version = RETINA_TELEM_VERSION;
hdr.recordSize = recordSize;
hdr.count = count;
hdr.total = total;
hdr.overwritten = overwritten;
hdr.capacity = capacity;
hdr.reserved = 0U;
U32 written = 0U;
file.fwrite(fd, (U8 *)&hdr, (U32)sizeof(hdr), written);
if(written != (U32)sizeof(hdr))
{
file.fclose(fd);
RETINA_EPRINTF("RETINA TELEMETRY ERROR: header write %s\n", name);
return false;
}
// Before the first wrap records are [0,count). After wrap the oldest record
// starts at head and the chronology is [head,capacity) + [0,head).
U32 first = (count < capacity) ? 0U : head;
U32 n1 = (count < capacity) ? count : (capacity - first);
U32 n2 = (count < capacity) ? 0U : first;
if(n1)
{
U32 bytes = n1 * recordSize;
written = 0U;
file.fwrite(fd, (U8 *)(storage + first * recordSize), bytes, written);
if(written != bytes)
{
file.fclose(fd);
RETINA_EPRINTF("RETINA TELEMETRY ERROR: data write %s\n", name);
return false;
}
}
if(n2)
{
U32 bytes = n2 * recordSize;
written = 0U;
file.fwrite(fd, (U8 *)storage, bytes, written);
if(written != bytes)
{
file.fclose(fd);
RETINA_EPRINTF("RETINA TELEMETRY ERROR: wrap write %s\n", name);
return false;
}
}
file.fclose(fd);
return true;
}
static void retinaTelemetryOpen()
{
_retinaEventStorage = (RetinaEventRecord *)_malloc(
RETINA_EVENT_LOG_CAPACITY * (U32)sizeof(RetinaEventRecord));
_retinaPathStorage = (RetinaPathNode *)_malloc(
RETINA_PATH_HISTORY_CAPACITY * (U32)sizeof(RetinaPathNode));
_retinaFlowStorage = (RetinaFlowRecord *)_malloc(
RETINA_FLOW_LOG_CAPACITY * (U32)sizeof(RetinaFlowRecord));
_retinaImuStorage = (RetinaImuRecord *)_malloc(
RETINA_IMU_LOG_CAPACITY * (U32)sizeof(RetinaImuRecord));
_retinaOdoStorage = (RetinaOdomRecord *)_malloc(
RETINA_ODO_LOG_CAPACITY * (U32)sizeof(RetinaOdomRecord));
_retinaProbeStorage = (RetinaProbeRecord *)_malloc(
RETINA_PROBE_LOG_CAPACITY * (U32)sizeof(RetinaProbeRecord));
_retinaEventLog.init(_retinaEventStorage,
_retinaEventStorage ? RETINA_EVENT_LOG_CAPACITY : 0U);
_retinaPathHistory.init(_retinaPathStorage,
_retinaPathStorage ? RETINA_PATH_HISTORY_CAPACITY : 0U);
_retinaFlowLog.init(_retinaFlowStorage,
_retinaFlowStorage ? RETINA_FLOW_LOG_CAPACITY : 0U);
_retinaImuLog.init(_retinaImuStorage, _retinaImuStorage ? RETINA_IMU_LOG_CAPACITY : 0U);
_retinaOdometer.init(_retinaOdoStorage, _retinaOdoStorage ? RETINA_ODO_LOG_CAPACITY : 0U);
_retinaProbeLog.init(_retinaProbeStorage, _retinaProbeStorage ? RETINA_PROBE_LOG_CAPACITY : 0U);
if(!_retinaEventStorage)
RETINA_EPRINTF("RETINA TELEMETRY ERROR: event SDRAM allocation failed\n");
if(!_retinaPathStorage)
RETINA_EPRINTF("RETINA TELEMETRY ERROR: path SDRAM allocation failed\n");
if(!_retinaFlowStorage) RETINA_EPRINTF("RETINA TELEMETRY ERROR: flow SDRAM allocation failed\n");
if(!_retinaImuStorage) RETINA_EPRINTF("RETINA PROBE ERROR: IMU SDRAM allocation failed\n");
if(!_retinaOdoStorage) RETINA_EPRINTF("RETINA PROBE ERROR: ODO SDRAM allocation failed\n");
if(!_retinaProbeStorage) RETINA_EPRINTF("RETINA PROBE ERROR: PROB SDRAM allocation failed\n");
}
static void retinaTelemetryDump()
{
if(_retinaEventStorage)
retinaTelemetryWriteRing(_retinaEventName, RETINA_EVENT_MAGIC,
(const U8 *)_retinaEventLog.storage(), (U32)sizeof(RetinaEventRecord),
_retinaEventLog.capacity(), _retinaEventLog.count(), _retinaEventLog.head(),
_retinaEventLog.total(), _retinaEventLog.overwritten());
if(_retinaPathStorage)
retinaTelemetryWriteRing(_retinaPathName, RETINA_PATH_MAGIC,
(const U8 *)_retinaPathHistory.storage(), (U32)sizeof(RetinaPathNode),
_retinaPathHistory.capacity(), _retinaPathHistory.count(), _retinaPathHistory.head(),
_retinaPathHistory.total(), _retinaPathHistory.overwritten());
if(_retinaFlowStorage)
retinaTelemetryWriteRing(_retinaFlowName, RETINA_FLOW_MAGIC,
(const U8 *)_retinaFlowLog.storage(), (U32)sizeof(RetinaFlowRecord),
_retinaFlowLog.capacity(), _retinaFlowLog.count(), _retinaFlowLog.head(),
_retinaFlowLog.total(), _retinaFlowLog.overwritten());
if(_retinaImuStorage)
retinaTelemetryWriteRing(_retinaImuName, RETINA_IMU_MAGIC,
(const U8 *)_retinaImuLog.storage(), (U32)sizeof(RetinaImuRecord),
_retinaImuLog.capacity(), _retinaImuLog.count(), _retinaImuLog.head(),
_retinaImuLog.total(), _retinaImuLog.overwritten());
if(_retinaOdoStorage)
retinaTelemetryWriteRing(_retinaOdoName, RETINA_ODO_MAGIC,
(const U8 *)_retinaOdometer.storage(), (U32)sizeof(RetinaOdomRecord),
_retinaOdometer.capacity(), _retinaOdometer.count(), _retinaOdometer.head(),
_retinaOdometer.total(), _retinaOdometer.overwritten());
if(_retinaProbeStorage)
retinaTelemetryWriteRing(_retinaProbeName, RETINA_PROBE_MAGIC,
(const U8 *)_retinaProbeLog.storage(), (U32)sizeof(RetinaProbeRecord),
_retinaProbeLog.capacity(), _retinaProbeLog.count(), _retinaProbeLog.head(),
_retinaProbeLog.total(), _retinaProbeLog.overwritten());
}
static void retinaTelemetryClose()
{
if(_retinaEventStorage) { _free(_retinaEventStorage); _retinaEventStorage = 0; }
if(_retinaPathStorage) { _free(_retinaPathStorage); _retinaPathStorage = 0; }
if(_retinaFlowStorage) { _free(_retinaFlowStorage); _retinaFlowStorage = 0; }
if(_retinaImuStorage) { _free(_retinaImuStorage); _retinaImuStorage = 0; }
if(_retinaOdoStorage) { _free(_retinaOdoStorage); _retinaOdoStorage = 0; }
if(_retinaProbeStorage) { _free(_retinaProbeStorage); _retinaProbeStorage = 0; }
_retinaEventLog.init(0, 0U); _retinaPathHistory.init(0, 0U); _retinaFlowLog.init(0, 0U);
_retinaImuLog.init(0, 0U); _retinaOdometer.init(0, 0U); _retinaProbeLog.init(0, 0U);
}
static U16 retinaClampU16(U32 v)
{
return (v > 65535UL) ? 65535U : (U16)v;
}
static S32 retinaScale10(F32 v)
{
return (S32)(v * 10.0f);
}
static S32 retinaScale1000(F32 v){return (S32)(v*1000.0f);}
static S32 retinaScaleMag10(F32 v){return (S32)(v*10.0f);}
static bool retinaProbeReadTemperature(S32 ¢iC)
{
U16 u=0U;
if(!I2cBusReadReg8_16(RETINA_PROBE_MPU9250_ADDR, RETINA_PROBE_MPU9250_TEMP_REG, &u))
return false;
const S16 rawA=(S16)u;
const S16 rawB=(S16)((U16)(u>>8)|(U16)(u<<8));
const F32 ta=((F32)rawA/333.87f)+21.0f;
const F32 tb=((F32)rawB/333.87f)+21.0f;
F32 t=ta;
// I2C helpers differ in byte-order handling across old library revisions.
// Pick the physically plausible interpretation nearest ordinary electronics temperatures.
const bool pa=(ta>-40.0f&&ta<125.0f), pb=(tb>-40.0f&&tb<125.0f);
if(!pa&&pb)t=tb; else if(pa&&pb){F32 da=fabs(ta-30.0f),db=fabs(tb-30.0f);if(db<da)t=tb;} else if(!pa&&!pb)return false;
centiC=(S32)(t*100.0f);return true;
}
static void retinaProbeRecordImu(U32 timeMs,U32 frame,Retina3State state)
{
if(!_retinaImuStorage)return;
RetinaImuRecord r; _memset(&r,0,(U32)sizeof(r));
r.timeMs=timeMs;r.frame=frame;r.sequence=++_retinaProbeImuSequence;r.avoidState=(U8)state;
F32 ax=0,ay=0,az=0,gx=0,gy=0,gz=0; bool ag=false; bool reused=false;
if(state==RETINA3_ESCAPE_TURN && retina.gyroSampleValid())
{ax=retina.accelX();ay=retina.accelY();az=retina.accelZ();gx=retina.gyroX();gy=retina.gyroY();gz=retina.gyroZ();ag=true;reused=true;}
else if(imu.ready()) ag=imu.accelerometerGyroscopeChecked(ax,ay,az,gx,gy,gz);
if(ag){r.axMg=retinaScale1000(ax);r.ayMg=retinaScale1000(ay);r.azMg=retinaScale1000(az);r.gxMdps=retinaScale1000(gx);r.gyMdps=retinaScale1000(gy);r.gzMdps=retinaScale1000(gz);r.flags|=RETINA_IMU_FLAG_AG_VALID;if(reused)r.flags|=RETINA_IMU_FLAG_REUSED_CTL;}
if(_retinaProbeMagPresent && ((frame%RETINA_PROBE_MAG_SAMPLE_FRAMES)==0U || !_retinaProbeLastMagMs))
{F32 mx=0,my=0,mz=0;imu.magnetometer(mx,my,mz);if(fabs(mx)<100000.0f&&fabs(my)<100000.0f&&fabs(mz)<100000.0f){_retinaProbeMx=mx;_retinaProbeMy=my;_retinaProbeMz=mz;_retinaProbeMagValid=true;_retinaProbeLastMagMs=timeMs;}}
if(_retinaProbeMagValid){r.mxX10mG=retinaScaleMag10(_retinaProbeMx);r.myX10mG=retinaScaleMag10(_retinaProbeMy);r.mzX10mG=retinaScaleMag10(_retinaProbeMz);r.flags|=RETINA_IMU_FLAG_MAG_VALID;r.magAgeMs=timeMs-_retinaProbeLastMagMs;}
if((frame%RETINA_PROBE_TEMP_SAMPLE_FRAMES)==0U || !_retinaProbeTempValid){S32 t=0;if(retinaProbeReadTemperature(t)){_retinaProbeTempCentiC=t;_retinaProbeTempValid=true;}else _retinaProbeTempValid=false;}
if(_retinaProbeTempValid){r.tempCentiC=_retinaProbeTempCentiC;r.flags|=RETINA_IMU_FLAG_TEMP_VALID;}
r.headingX10=retinaScale10(retina.headingDeg());_retinaImuLog.record(r);
}
// -----------------------------------------------------------------------------
// Controller diagnostics are printed by otRetinaMinimal itself; this callback only
// mirrors the same line into the SDRAM/SD field logger.
static void retinaControlLogSink(const char *text)
{
if(_retinaLogBuffer && text)
retinaRunLogAppend(text);
}
// -----------------------------------------------------------------------------
// Retina V0 configuration
// MT9V032 hardware binning 2x2 -> expected 376 x 240 output samples.
// Acquisition remains native 10-bit through the U16/PPI-10 path. The camera/FPGA
// U8 path is intentionally NOT used because it exposes D7..D0 rather than D9..D2.
// RETINA converts only the samples it needs to U8 as (pixel10 >> 2).
// Hardware binning now produces the logical retina directly: no software 2x2
// decimation remains in the hot path. One logical 8x8 retinal cell is therefore
// one 8x8 block in the binned sensor output, preserving the 47 x 30 retinal map.
// -----------------------------------------------------------------------------
#define RETINA_SW_DECIMATION 1
#define RETINA_CELL_W 8
#define RETINA_CELL_H 8
#define RETINA_MAX_COLS 64
#define RETINA_MAX_ROWS 48
#define RETINA_MIN_ACTIVITY 3U
#define RETINA_DECISION_MIN_ACTIVITY 6U
#define RETINA_STEER_PERCENT 25U
#define RETINA_CENTER_STOP_PERCENT 80U
#define RETINA_GROUND_STOP_PERCENT 90U
#define RETINA_GYRO_WARN_DPS 60.0f
#define RETINA_PERF_EVERY 100U
// Maximum navigation activity time. 0 = unlimited. The timer starts only
// when the RETINA navigation loop begins, after the initial CAMCAL.
#define RETINA_RUN_DURATION_SECONDS 600U
// Persistent field SD policy. Existing runs are preserved. The filesystem is
// formatted only after repeated mount/readability failures; a normal reset simply
// allocates the next RUNnnnnA/B.LOG + STOPnnnn.AVI + EVT/PATH/FLOWnnnn.BIN set.
#define RETINA_SD_FORMAT_IF_UNREADABLE 1U
#define RETINA_SD_INIT_RETRIES 3U
#define RETINA_PHOTO_LOGICAL_STEP 4U // sparse PHOTO grid: 94x60 ~= 5640 samples
#define RETINA_TILE_MAX_W 768U
#define RETINA_TILE_ROWS 8U
#define RETINA_FLOW_STEP 2 // binned-output pixels: same angular step as +/-4 native pixels
#define RETINA_FLOW_MIN_TEXTURE 3U
#define RETINA_FLOW_MIN_VALID_CELLS 32U
// ADI motionvect.asm path. Its output is retained as the field-proven
// motion sensor used by the minimal controller.
#define RETINA_MVASM_ENABLE 1
#define RETINA_MVASM_SEARCH_FACTOR 1U
#define RETINA_MVASM_MB_SIZE 16U
// Logical MV map: use 368x240, i.e. 23x15 macroblocks.
// Logical retina itself remains 376x240; the rightmost 8 logical columns are
// intentionally outside the motion-vector map.
#define RETINA_MVASM_WIDTH 368U
#define RETINA_MVASM_HEIGHT 240U
#define RETINA_MVASM_COLS (RETINA_MVASM_WIDTH / RETINA_MVASM_MB_SIZE)
#define RETINA_MVASM_ROWS (RETINA_MVASM_HEIGHT / RETINA_MVASM_MB_SIZE)
#define RETINA_MVASM_VECTORS (RETINA_MVASM_COLS * RETINA_MVASM_ROWS)
#if (RETINA_MVASM_COLS != RETINA_FLOW_COLS) || (RETINA_MVASM_ROWS != RETINA_FLOW_ROWS)
#error RETINA_FLOW binary format does not match the motion-vector map
#endif
// FAST_DATA tile size. 80x96 = 5x6 macroblocks = 30 vectors.
// V0.18e uses one U16 DMA tile in each 32-KB L1 data bank. After MDMA,
// each tile is packed in-place to U8(D9..D2) in the first half of the same
// buffer; _motionvect then reads those packed bytes directly.
#define RETINA_MVASM_TILE_W 80U
#define RETINA_MVASM_TILE_H 96U
#define RETINA_MVASM_TILE_PIXELS (RETINA_MVASM_TILE_W * RETINA_MVASM_TILE_H)
#define RETINA_MVASM_TILE_MAX_VECTORS ((RETINA_MVASM_TILE_W/16U) * (RETINA_MVASM_TILE_H/16U))
#define RETINA_DMA_TILE_TIMEOUT_US 10000U
#define RETINA_DMA_LOG_FAULT_LIMIT 4U
// ASM-retina thresholds. Motion-vector components are signed half-pixel units.
#define RETINA_MVASM_MIN_MAG2 2U
#define RETINA_MVASM_LOW_MOTION_COUNT 12U
#define RETINA_MVASM_MIN_VALID_ZONE 6U
#define RETINA_MVASM_CENTER_STOP_MAG2 3U
#define RETINA_MVASM_GROUND_STOP_MAG2 3U
#define RETINA_MVASM_STEER_MARGIN_PCT 25U
// Directional coherence: random block matches tend to cancel, real translation
// tends to produce a non-zero resultant. Keep these provisional until PAN/TILT
// tests are available.
#define RETINA_MVASM_ZONE_COH_MIN_PCT 25U
#define RETINA_MVASM_COH_MEAN_MIN_X2 1U
#define RETINA_MVASM_MIN_PHOTO_ACTIVITY 5U // provisional confidence gate from suspended-run diagnostics
#define RETINA_MVASM_INPUT_DECIMATED 1U // 376x240 U8 input to _motionvect
#define RETINA_MVASM_MAX_VECTORS 1536U // > 47*30 = 1410
// V0.18n camera-control policy:
// - AEC and AGC are permanently OFF during navigation.
// - IR is part of the navigation sensor and is strobed with each exposure.
// - Calibration changes only exposure and analog gain while stopped.
// - The selected shutter/gain are then held fixed during motion.
// Field tests showed LED=OFF was effectively blind even at EXP=8 ms / GAIN=64.
// MT9V03x analog gain register: 16=1x, 64=4x. Values >31 are quantized to even
// register codes by the sensor, so the clamp helper below mirrors that behavior.
#define RETINA_CONTROL_GAIN_DELTA 1U // fixed gain: any register step is unexpected
#define RETINA_CONTROL_EXPOSURE_PCT 2U // fixed exposure: catches accidental changes/quantization drift
// V2.0D: intermittent I2C corruption must not evade recovery merely because
// valid samples occur between bad ones. Keep an 8-frame history and recover
// after any 3 impossible readbacks in that window (~0.4 s at 20 fps).
#define RETINA_CONTROL_BUS_WINDOW_FRAMES 8U
#define RETINA_CONTROL_BUS_BAD_TRIGGER 3U
// STOP snapshots are buffered as raw U8 frames in SDRAM. No AVI/JPEG/SD work
// occurs while locomotion is active; the AVI is assembled only after motors and
// DMA are stopped. 128 x 376 x 240 is about 11 MiB.
#define RETINA_STOP_SNAPSHOT_CAPACITY 128U
#define RETINA_MANUAL_GAIN_MIN 16U
#define RETINA_MANUAL_GAIN_INITIAL 16U
#define RETINA_MANUAL_GAIN_MAX 64U
#define RETINA_EXPOSURE_INITIAL_MS 2.0f
#define RETINA_EXPOSURE_MIN_MS 0.05f
#define RETINA_EXPOSURE_FAST_MAX_MS 2.0f // prefer gain before exceeding this shutter time
#define RETINA_EXPOSURE_MAX_MS 8.0f
#define RETINA_CAMERA_CAL_MAX_ITER 10U
// V2.6A-BUS1: runtime camera-control access deliberately bypasses the legacy
// otImageSensor::exposure()/gain() readback path. Those functions do not
// propagate I2C read failure and exposure() also uses uninitialized temporaries
// if a transaction fails. The RETINA geometry is fixed after MT9V032 2x2
// binning, so shutter conversion can use the programmed default window timing
// without rereading WINDOW_WIDTH/HORIZONTAL_BLANKING every frame.
#define RETINA_MT9V032_TB_US 0.04
static U16 retinaExposureRegisterFromMs(F32 exposureMs)
{
const U32 sw = (U32)MT9V032_WINDOW_WIDTH_DEF +
(U32)MT9V032_HORIZONTAL_BLANKING_DEF;
const U32 so = (sw > 255U) ? (sw - 255U) : 0U;
const F64 exposureUs = (F64)exposureMs * 1000.0;
F64 reg = (exposureUs - (F64)so * RETINA_MT9V032_TB_US) /
((F64)sw * RETINA_MT9V032_TB_US);
// Preserve the legacy MT9V032 conversion exactly at the low end: the
// historical exposeNumber() can produce register 0 for very short
// exposures (the sensor then exposes for its fixed overhead/minimum).
if(reg < 0.0) reg = 0.0;
if(reg > 32767.0) reg = 32767.0;
return (U16)reg; // preserve the legacy truncation semantics
}
static F64 retinaExposureUsFromRegister(U16 shutter)
{
const U32 sw = (U32)MT9V032_WINDOW_WIDTH_DEF +
(U32)MT9V032_HORIZONTAL_BLANKING_DEF;
const U32 so = (sw > 255U) ? (sw - 255U) : 0U;
F64 et = (F64)((U32)shutter * sw);
et = et * RETINA_MT9V032_TB_US + (F64)so * RETINA_MT9V032_TB_US;
return et;
}
static void retinaSetExposureMsBusSafe(F32 exposureMs)
{
// Use the U16 overload: it performs exactly one register write and, unlike
// setExposure(F32), does not reread geometry from the control bus.
sensor.setExposure(retinaExposureRegisterFromMs(exposureMs));
}
static bool retinaReadCameraControlBus(U16 &gain, F64 &exposureUs,
bool &gainReadOk, bool &shutterReadOk,
U16 &rawShutter)
{
U16 g = 0U;
U16 sh = 0U;
gainReadOk = I2cBusReadReg8_16(otImageSensor::MT9V03x_00,
MT9V032_ANALOG_GAIN, &g);
shutterReadOk = I2cBusReadReg8_16(otImageSensor::MT9V03x_00,
MT9V032_TOTAL_SHUTTER_WIDTH, &sh);
gain = gainReadOk ? g : 0U;
rawShutter = shutterReadOk ? sh : 0U;
exposureUs = shutterReadOk ? retinaExposureUsFromRegister(sh) : 0.0;
return gainReadOk && shutterReadOk;
}
#define RETINA_EXPOSURE_TARGET_P95 220U
#define RETINA_EXPOSURE_P95_LOW 200U
#define RETINA_EXPOSURE_P95_HIGH 235U
#define RETINA_EXPOSURE_MAX_SAT_PCT 1U
#define RETINA_CAL_SCORE_SAT_WEIGHT 20U // penalty per saturated-percent when neither light state is ideal
// Deferred post-STOP exposure calibration. The STOP frame itself is often a
// close bright obstacle and must never become the exposure reference. Calibration
// is attempted only inside the normal motor STOP-hold window; if the scene does
// not become suitable before that hold expires, calibration is skipped and RETINA
// resumes with the previous exposure. This prevents a photometric state from
// latching the tracks at PWM HOLD indefinitely.
// Photometric immunity: normalize the PREVIOUS frame to the CURRENT
// global brightness before temporal differencing and block matching.
#define RETINA_PHOTO_Q 12U
#define RETINA_PHOTO_ONE_Q (1U << RETINA_PHOTO_Q)
#define RETINA_PHOTO_SCALE_MIN_Q (RETINA_PHOTO_ONE_Q / 4U) // 0.25x
#define RETINA_PHOTO_SCALE_MAX_Q (RETINA_PHOTO_ONE_Q * 4U) // 4.00x
#define RETINA_ILLUM_COHERENCE_PCT 70U
#define RETINA_ILLUM_SCALE_DELTA_PCT 12U
// Exposure-range diagnostics used by the runtime confidence gate.
// Manual exposure calibration uses exact saturation (pixel==255) plus P95.
#define RETINA_CLIP_HIGH 250U
#define RETINA_CLIP_LOW 4U
#define RETINA_CLIP_HIGH_PERCENT 5U
#define RETINA_CLIP_LOW_PERCENT 40U
// Normal forward motion naturally produces strong coherent optical flow in
// the lower third of a downward-looking camera. Do NOT call that a cliff.
// GROUND becomes an emergency only when it is both much stronger than CENTER
// and very radially coherent. The RUN00 terrain log had ordinary ground means
// of 3..9 half-pixels, so 10 is intentionally above that observed background.
#define RETINA_GROUND_EMERGENCY_MEAN_X2 10U
#define RETINA_GROUND_EMERGENCY_RCOH_PCT 75U
#define RETINA_GROUND_EMERGENCY_MARGIN_X2 3U
// Prospective swept BODY guard. The geometry itself is owned by
// otRetinaMinimal and is calculated once in init() from the 3.6mm / 12um binned camera,
// 135mm height and a conservative 200mm vehicle corridor at 5deg down.
// These thresholds deliberately look for LOCAL excess over ordinary ground
// perspective flow so a normal tiled floor does not become an obstacle again.
#define RETINA_TRACK_GUARD_MIN_VALID 4U
#define RETINA_TRACK_GUARD_COH_MIN_PCT 45U
#define RETINA_TRACK_GUARD_RADIAL_COH_MIN_PCT 60U
#define RETINA_TRACK_GUARD_MIN_MEAN_X2 4U
#define RETINA_TRACK_GUARD_GROUND_MARGIN_X2 2U
#define RETINA_TRACK_GUARD_BOTH_MARGIN_X2 3U
#define RETINA_TRACK_GUARD_SIDE_MARGIN_X2 2U
// V2.2C: ordinary optical hazards must persist across two consecutive frames
// before they can become a physical STOP. GROUND/HEIGHT remain immediate.
// TRACK continuity is geometric: any L/R overlap with the previous raw mask
// counts as persistence, so L->BOTH and BOTH->L are not discarded.
#define RETINA_TRACK_CONFIRM_FRAMES 2U
#define RETINA_CENTER_CONFIRM_FRAMES 2U
#define RETINA_COG_RECENT_TURN_MS 5000UL
// Upper BODY/HEIGHT guard diagnostic. V1.4 keeps this geometry MONITOR-ONLY:
// a monocular image row does not by itself determine the physical height of an
// observed feature. Keep collecting HG statistics, but do not let HG steer.
#define RETINA_HEIGHT_GUARD_ACTIVE 0U
#define RETINA_HEIGHT_GUARD_MIN_VALID 3U
#define RETINA_HEIGHT_GUARD_COH_MIN_PCT 50U
#define RETINA_HEIGHT_GUARD_RADIAL_COH_MIN_PCT 70U
#define RETINA_HEIGHT_GUARD_MIN_MEAN_X2 4U
#define RETINA_HEIGHT_GUARD_SIDE_MARGIN_X2 2U
// V0.18e/V2.3C memory layout:
// FAST_DATA_1 / bank A: CURRENT U16 DMA tile (15360 bytes max)
// FAST_DATA_2 / bank B: PREVIOUS U16 DMA tile (15360 bytes max), small
// vector outputs/maps and POD runtime/telemetry state.
// C++ controller objects with constructors remain in their normal section.
// After DMA completion each U16 tile is packed in-place to U8 in its first
// RETINA_MVASM_TILE_PIXELS bytes. This replaces the old pair of U8 tile arrays.
static FAST_DATA_2 bool retinaPreviousValid = false;
static FAST_DATA_2 U32 retinaFrameCounter = 0;
static FAST_DATA_2 U16 retinaPrevGain = 0;
static FAST_DATA_2 F64 retinaPrevExposure = 0.0;
static bool retinaGrabToImageA = true;
// PF15 safety service outside the ISR. The falling edge is latched asynchronously,
// but all PWM/FPGA/log/topology work stays in normal code. The BF51x PORTFIO read
// changes meaning in edge-sensitive mode, so interrupt mode and physical-level
// polling are never used at the same time.
static bool retinaHandlePf15RunSafety(U32 timeMs, U32 frame, U32 &seenSeq)
{
const U32 seq = _retinaPf15FallingSeq;
const bool newEdge = (seq != seenSeq);
// During a physical retreat PF15 is a true GPIO level. The minimal FSM
// refuses FORWARD/TURN while it remains LOW.
if(retina.frontContactActive())
{
if(seq != seenSeq) seenSeq = seq;
if(_retinaPf15IrqArmed) retinaPf15LevelMode();
retina.setFrontContactLevel(retinaPf15PressedLevel());
return false;
}
bool assertContact = newEdge;
bool levelLow = false;
if(newEdge)
{
seenSeq = seq;
retinaPf15LevelMode();
levelLow = retinaPf15PressedLevel();
}
else if(!_retinaPf15IrqArmed)
{
levelLow = retinaPf15PressedLevel();
if(levelLow) assertContact = true;
else
{
retinaPf15ArmFalling();
seenSeq = _retinaPf15FallingSeq;
return false;
}
}
else return false;
if(!assertContact) return false;
retina.setFrontContactLevel(levelLow);
if(!retina.triggerFrontContact())
return false;
// Apply the first reverse update immediately: PF15 never waits for the next
// camera frame before beginning physical separation.
retina.process(DRIVE_REQ_REFERENCE);
++_retinaFrontContactCountRun;
_retinaEventLog.record(RETINA_EVENT_FRONT_CONTACT, 0U, timeMs, frame,
1, levelLow ? 1 : 0, (S32)seq);
statusLed.setHazard(STATUS_HAZARD_BOTH);
statusLed.setState(STATUS_LED_CAUTION);
statusLed.showNow();
if(_retinaLogBuffer)
{
char line[224];
sprintf(line,
"FRONT CONTACT #%lu time=%lu ms irqSeq=%lu level=%u -> MINIMAL ESCAPE state=%s turn=%s target=%.0f taboo=0x%02x\n",
(unsigned long)frame, (unsigned long)timeMs,
(unsigned long)seq, levelLow ? 1U : 0U,
retina.stateName(), retina.escapeTurnCw() ? "CW" : "CCW",
(double)retina.escapeTurnTargetDeg(), (unsigned int)retina.tabooMask());
retinaRunLogAppend(line);
}
// Physical contact invalidates the temporal optical reference immediately.
retinaPreviousValid = false;
retinaGrabToImageA = true;
retinaPrevGain = 0U;
retinaPrevExposure = 0.0;
return true;
}
static FAST_DATA_1 U16 retinaMvDmaTileCur[RETINA_MVASM_TILE_PIXELS] __attribute__((aligned(32)));
static FAST_DATA_2 U16 retinaMvDmaTilePrev[RETINA_MVASM_TILE_PIXELS] __attribute__((aligned(32)));
// Per-tile outputs are tiny and hot.
static FAST_DATA_2 S8 retinaMvAsmTileV[RETINA_MVASM_TILE_MAX_VECTORS] __attribute__((aligned(32)));
static FAST_DATA_2 S8 retinaMvAsmTileH[RETINA_MVASM_TILE_MAX_VECTORS] __attribute__((aligned(32)));
// Complete 23x15 motion map assembled tile by tile.
// 345 bytes per component.
static FAST_DATA_2 S8 retinaMvMapV[RETINA_MVASM_VECTORS] __attribute__((aligned(32)));
static FAST_DATA_2 S8 retinaMvMapH[RETINA_MVASM_VECTORS] __attribute__((aligned(32)));
static F32 absF32(F32 v)
{
return (v < 0.0f) ? -v : v;
}
static F64 absF64(F64 v)
{
return (v < 0.0) ? -v : v;
}
static U16 retinaBitCount8(U8 v)
{
U16 n = 0U;
for(U16 i = 0U; i < 8U; ++i)
{
if(v & (U8)(1U << i))
++n;
}
return n;
}
// MT9V032 native samples are 10-bit values acquired through the U16/PPI-10 path.
// RETINA and motionvect.asm deliberately use the eight most-significant sensor bits.
static U8 retinaPixel10To8(U16 v)
{
return (U8)((v & 0x03ffU) >> 2);
}
static U8 retinaScalePrevious(U8 v, U32 scaleQ)
{
U32 x = ((U32)v * scaleQ + (RETINA_PHOTO_ONE_Q >> 1)) >> RETINA_PHOTO_Q;
if(x > 255U) x = 255U;
return (U8)x;
}
struct Retina3PhotometricResult
{
S32 signedSum;
U32 absSum;
U32 coherencePct;
U32 scaleQ;
U32 rawActivityAvg;
U32 clipHighCount;
U32 clipLowCount;
U32 satExactCount;
U32 pixelCount;
U32 sumCur;
U32 sumSqCur;
U16 hist[RETINA_PROBE_HIST_BINS];
U8 minValue;
U8 maxValue;
bool illuminationTransient;
};
// V3.0C keeps the field-proven photometric normalization but removes the
// PerceptionStage object entirely. This function is read-only and has no state.
static void retinaPhotometricAnalyze(const U16 *current, const U16 *previous,
U16 width, U16 height, U16 sampleStep,
Retina3PhotometricResult &o)
{
U32 sumCur = 0U, sumPrev = 0U;
o.signedSum = 0; o.absSum = 0U; o.coherencePct = 0U;
o.scaleQ = RETINA_PHOTO_ONE_Q; o.rawActivityAvg = 0U;
o.clipHighCount = o.clipLowCount = o.satExactCount = o.pixelCount = 0U;
o.sumCur = o.sumSqCur = 0U; o.minValue = 255U; o.maxValue = 0U;
for(U16 hi=0U; hi<RETINA_PROBE_HIST_BINS; ++hi) o.hist[hi]=0U;
o.illuminationTransient = false;
if(!current || !previous || !width || !height || !sampleStep)
return;
for(U16 y = 0U; y < height; y = (U16)(y + sampleStep))
{
const U16 *pc = current + (U32)y * (U32)width;
const U16 *pp = previous + (U32)y * (U32)width;
const U16 *pcEnd = pc + width;
while(pc < pcEnd)
{
const U8 a = (U8)((*pc & 0x03ffU) >> 2);
const U8 b = (U8)((*pp & 0x03ffU) >> 2);
const S32 sd = (S32)a - (S32)b;
const U32 ad = (sd >= 0) ? (U32)sd : (U32)(-sd);
sumCur += a; sumPrev += b;
o.sumCur += a; o.sumSqCur += (U32)a * (U32)a;
++o.hist[(U16)(a >> 4)];
if(a < o.minValue) o.minValue = a; if(a > o.maxValue) o.maxValue = a;
o.signedSum += sd; o.absSum += ad; ++o.pixelCount;
if(a >= RETINA_CLIP_HIGH) ++o.clipHighCount;
if(a == 255U) ++o.satExactCount;
if(a <= RETINA_CLIP_LOW) ++o.clipLowCount;
pc += sampleStep; pp += sampleStep;
}
}
if(o.pixelCount) o.rawActivityAvg = o.absSum / o.pixelCount;
if(sumPrev)
{
unsigned long long q = ((unsigned long long)sumCur *
(unsigned long long)RETINA_PHOTO_ONE_Q) / sumPrev;
if(q < RETINA_PHOTO_SCALE_MIN_Q) q = RETINA_PHOTO_SCALE_MIN_Q;
if(q > RETINA_PHOTO_SCALE_MAX_Q) q = RETINA_PHOTO_SCALE_MAX_Q;
o.scaleQ = (U32)q;
}
if(o.absSum)
{
const U32 sa = (o.signedSum >= 0) ? (U32)o.signedSum : (U32)(-o.signedSum);
o.coherencePct = (U32)(((unsigned long long)sa * 100ULL) /
(unsigned long long)o.absSum);
}
{
const U32 scalePct = (U32)(((unsigned long long)o.scaleQ * 100ULL) /
RETINA_PHOTO_ONE_Q);
const U32 deltaPct = (scalePct >= 100U) ? scalePct - 100U : 100U - scalePct;
o.illuminationTransient =
(o.coherencePct >= RETINA_ILLUM_COHERENCE_PCT &&
deltaPct >= RETINA_ILLUM_SCALE_DELTA_PCT);
}
}
// V0.18e DMA tile packer.
// MDMA has already copied a contiguous U16 tile into L1. Pack the right-aligned
// MT9V032 10-bit samples to D9..D2 U8 in-place. Forward packing is safe because
// the byte destination always trails the unread U16 source.
extern "C" void __attribute__((section(".l1.text"), noinline, optimize("O3"), aligned(4)))
retinaPack10To8InPlace(U16 *buf, U32 count)
{
U16 *src = buf;
U8 *dst = (U8 *)buf;
while(count >= 4U)
{
// Load all four source words before writing any overlapping packed bytes.
const U16 v0 = src[0];
const U16 v1 = src[1];
const U16 v2 = src[2];
const U16 v3 = src[3];
dst[0] = (U8)((v0 & 0x03ffU) >> 2);
dst[1] = (U8)((v1 & 0x03ffU) >> 2);
dst[2] = (U8)((v2 & 0x03ffU) >> 2);
dst[3] = (U8)((v3 & 0x03ffU) >> 2);
src += 4;
dst += 4;
count -= 4U;
}
while(count--)
{
const U16 v = *src++;
*dst++ = (U8)((v & 0x03ffU) >> 2);
}
}
// CPU fallback used only if a DMA channel cannot start, reports an error, or
// exceeds the conservative tile timeout. It writes directly into the packed
// byte halves of the same L1 DMA buffers, preserving navigation.
extern "C" void __attribute__((section(".l1.text"), noinline, optimize("O3"), aligned(4)))
retinaPrepareMvTileCpuFallback(const U16 *__restrict__ cur,
const U16 *__restrict__ prev,
U8 *__restrict__ dstCur,
U8 *__restrict__ dstPrev,
U32 frameStride,
U32 tileX0,
U32 tileY0,
U32 tileW,
U32 tileH)
{
U32 ty;
for(ty = 0U; ty < tileH; ++ty)
{
const U16 *pc = cur + (tileY0 + ty) * frameStride + tileX0;
const U16 *pp = prev + (tileY0 + ty) * frameStride + tileX0;
U32 n = tileW;
while(n >= 4U)
{
dstCur[0] = (U8)((pc[0] & 0x03ffU) >> 2);
dstPrev[0] = (U8)((pp[0] & 0x03ffU) >> 2);
dstCur[1] = (U8)((pc[1] & 0x03ffU) >> 2);
dstPrev[1] = (U8)((pp[1] & 0x03ffU) >> 2);
dstCur[2] = (U8)((pc[2] & 0x03ffU) >> 2);
dstPrev[2] = (U8)((pp[2] & 0x03ffU) >> 2);
dstCur[3] = (U8)((pc[3] & 0x03ffU) >> 2);
dstPrev[3] = (U8)((pp[3] & 0x03ffU) >> 2);
pc += 4;
pp += 4;
dstCur += 4;
dstPrev += 4;
n -= 4U;
}
while(n--)
{
*dstCur++ = (U8)((*pc++ & 0x03ffU) >> 2);
*dstPrev++ = (U8)((*pp++ & 0x03ffU) >> 2);
}
}
}
struct RetinaExposureStats
{
U32 count;
U32 mean;
U32 satCount;
U32 satPct;
U8 p95;
U8 minValue;
U8 maxValue;
};
static FAST_DATA_2 U16 retinaExposureHist[256];
static void retinaExposureStats(const U16 *src, U16 w, U16 h, RetinaExposureStats &st)
{
U32 i;
U32 sum = 0U;
U32 target95;
U32 accum = 0U;
const U16 step = (U16)RETINA_PHOTO_LOGICAL_STEP;
st.count = 0U;
st.mean = 0U;
st.satCount = 0U;
st.satPct = 0U;
st.p95 = 0U;
st.minValue = 255U;
st.maxValue = 0U;
for(i = 0U; i < 256U; ++i)
retinaExposureHist[i] = 0U;
for(U16 y = 0U; y < h; y += step)
{
const U16 *p = src + (U32)y * (U32)w;
const U16 *end = p + w;
while(p < end)
{
U8 v = retinaPixel10To8(*p);
sum += v;
++st.count;
++retinaExposureHist[v];
if(v == 255U) ++st.satCount;
if(v < st.minValue) st.minValue = v;
if(v > st.maxValue) st.maxValue = v;
p += step;
}
}
if(!st.count)
return;
st.mean = sum / st.count;
st.satPct = (st.satCount * 100U) / st.count;
target95 = (st.count * 95U + 99U) / 100U;
for(i = 0U; i < 256U; ++i)
{
accum += retinaExposureHist[i];
if(accum >= target95)
{
st.p95 = (U8)i;
break;
}
}
}
static F32 retinaClampExposureMs(F32 exposureMs)
{
if(exposureMs < RETINA_EXPOSURE_MIN_MS) exposureMs = RETINA_EXPOSURE_MIN_MS;
if(exposureMs > RETINA_EXPOSURE_MAX_MS) exposureMs = RETINA_EXPOSURE_MAX_MS;
return exposureMs;
}
static U16 retinaClampManualGain(U32 gain)
{
if(gain < RETINA_MANUAL_GAIN_MIN) gain = RETINA_MANUAL_GAIN_MIN;
if(gain > RETINA_MANUAL_GAIN_MAX) gain = RETINA_MANUAL_GAIN_MAX;
// MT9V03x uses only even effective steps above code 31. Round upward when
// increasing through this range so the requested change is not lost.
if(gain > 31U && (gain & 1U))
{
++gain;
if(gain > RETINA_MANUAL_GAIN_MAX) gain = RETINA_MANUAL_GAIN_MAX;
}
return (U16)gain;
}
// Calibrate shutter + analog gain for the CURRENT light state only.
// AEC/AGC stay OFF before, during and after this routine. The policy deliberately
// keeps exposure in the fast <=2 ms region where possible: in low light it first
// uses gain up to 4x, and exceeds 2 ms only as a last resort. In bright scenes it
// removes gain first, then shortens exposure.
static bool retinaCameraStatsGood(const RetinaExposureStats &st)
{
return (st.satPct <= RETINA_EXPOSURE_MAX_SAT_PCT) &&
(st.p95 >= RETINA_EXPOSURE_P95_LOW) &&
(st.p95 <= RETINA_EXPOSURE_P95_HIGH);
}
static U32 retinaCameraCalibrationScore(const RetinaExposureStats &st)
{
U32 p95err = (st.p95 >= RETINA_EXPOSURE_TARGET_P95) ?
(U32)(st.p95 - RETINA_EXPOSURE_TARGET_P95) :
(U32)(RETINA_EXPOSURE_TARGET_P95 - st.p95);
return p95err + st.satPct * RETINA_CAL_SCORE_SAT_WEIGHT;
}
// Compute ONE bounded exposure/gain adjustment from statistics that were
// obtained by the normal camera frame. This is intentionally side-effect free:
// the caller decides when to write the new controls. V1.6 uses the same policy
// both for startup CAMCAL and for the frame-stepped post-STOP recalibration.
static bool retinaCameraCalibrationAdjust(const RetinaExposureStats &st,
F32 &exposureMs, U16 &gain)
{
F32 ratio;
bool changed = false;
if(st.p95)
ratio = (F32)RETINA_EXPOSURE_TARGET_P95 / (F32)st.p95;
else
ratio = 1.50f;
if(st.satPct >= 20U)
ratio = 0.50f;
else if(st.satPct >= 5U && ratio > 0.65f)
ratio = 0.65f;
else if(st.satPct > RETINA_EXPOSURE_MAX_SAT_PCT && ratio > 0.80f)
ratio = 0.80f;
if(ratio > 1.50f) ratio = 1.50f;
if(ratio < 0.50f) ratio = 0.50f;
if(ratio < 1.0f)
{
// Too bright: remove analog gain first. At unity gain shorten shutter.
if(gain > RETINA_MANUAL_GAIN_MIN)
{
U32 ng = (U32)((F32)gain * ratio + 0.5f);
U16 nextGain = retinaClampManualGain(ng);
if(nextGain >= gain)
nextGain = retinaClampManualGain((U32)gain - ((gain > 32U) ? 2U : 1U));
if(nextGain != gain)
{
gain = nextGain;
changed = true;
}
}
else
{
F32 nextExposureMs = retinaClampExposureMs(exposureMs * ratio);
if(nextExposureMs != exposureMs)
{
exposureMs = nextExposureMs;
changed = true;
}
}
}
else
{
// Too dark. Keep the shutter in the fast <=2 ms region if possible:
// gain is the preferred actuator there. Only after 4x gain is reached
// may exposure grow toward the absolute 8 ms limit.
if(exposureMs < RETINA_EXPOSURE_FAST_MAX_MS)
{
F32 nextExposureMs = exposureMs * ratio;
if(nextExposureMs > RETINA_EXPOSURE_FAST_MAX_MS)
nextExposureMs = RETINA_EXPOSURE_FAST_MAX_MS;
nextExposureMs = retinaClampExposureMs(nextExposureMs);
if(nextExposureMs != exposureMs)
{
exposureMs = nextExposureMs;
changed = true;
}
}
else if(gain < RETINA_MANUAL_GAIN_MAX)
{
U32 ng = (U32)((F32)gain * ratio + 0.5f);
U16 nextGain = retinaClampManualGain(ng);
if(nextGain <= gain)
nextGain = retinaClampManualGain((U32)gain + ((gain >= 32U) ? 2U : 1U));
if(nextGain != gain)
{
gain = nextGain;
changed = true;
}
}
else
{
F32 nextExposureMs = retinaClampExposureMs(exposureMs * ratio);
if(nextExposureMs != exposureMs)
{
exposureMs = nextExposureMs;
changed = true;
}
}
}
return changed;
}
static F32 retinaCalibrateExposureGainForCurrentLight(F32 startExposureMs,
U16 &manualGain,
const char *reason,
RetinaExposureStats &finalStats,
bool &cameraGood)
{
F32 exposureMs = retinaClampExposureMs(startExposureMs);
U16 gain = retinaClampManualGain(manualGain);
U32 iter = 0U;
sensor.setAec(false);
sensor.setAgc(false);
for(iter = 0U; iter < RETINA_CAMERA_CAL_MAX_ITER; ++iter)
{
retinaSetExposureMsBusSafe(exposureMs);
sensor.setGain(gain);
camera.grab(image);
retinaExposureStats((const U16 *)image.data(),
sensor.horPixel(), sensor.verPixel(),
finalStats);
cameraGood = retinaCameraStatsGood(finalStats);
if(cameraGood || (iter + 1U) >= RETINA_CAMERA_CAL_MAX_ITER)
break;
if(!retinaCameraCalibrationAdjust(finalStats, exposureMs, gain))
break;
}
manualGain = gain;
sensor.setAec(false);
sensor.setAgc(false);
retinaSetExposureMsBusSafe(exposureMs);
sensor.setGain(manualGain);
cameraGood = retinaCameraStatsGood(finalStats);
RETINA_VPRINTF("CAMCAL TRY %s LED=%s iter=%lu exp=%.3f ms gain=%u mean=%lu P95=%u SAT=%lu%% range=%u..%u %s\n",
reason ? reason : "?", retina.lightName(),
(unsigned long)(iter + 1U), exposureMs, manualGain,
(unsigned long)finalStats.mean, finalStats.p95,
(unsigned long)finalStats.satPct,
finalStats.minValue, finalStats.maxValue,
cameraGood ? "OK" : "OUT");
if(_retinaLogBuffer)
{
char line[256];
sprintf(line,
"CAMCAL TRY %s LED=%s iter=%lu exp=%.3f ms gain=%u mean=%lu P95=%u SAT=%lu%% range=%u..%u %s\n",
reason ? reason : "?", retina.lightName(),
(unsigned long)(iter + 1U), exposureMs, manualGain,
(unsigned long)finalStats.mean, finalStats.p95,
(unsigned long)finalStats.satPct,
finalStats.minValue, finalStats.maxValue,
cameraGood ? "OK" : "OUT");
retinaRunLogAppend(line);
}
return exposureMs;
}
// Full camera calibration. Field tests showed that LED=OFF leaves the navigation
// camera effectively blind (EXP=8 ms / GAIN=64 still only P95~36). The IR
// emitter is therefore a permanent part of the navigation sensor. It is driven
// synchronously with MT9V032 exposure, not continuously; CAMCAL adjusts only
// shutter and analog gain and never disables the selected navigation light.
static F32 retinaCalibrateExposureGain(F32 startExposureMs,
U16 &manualGain,
const char *reason,
RetinaExposureStats &finalStats)
{
bool cameraGood = false;
F32 exposureMs = retinaClampExposureMs(startExposureMs);
manualGain = retinaClampManualGain(manualGain);
retina.setLight(2U);
exposureMs = retinaCalibrateExposureGainForCurrentLight(exposureMs, manualGain,
reason, finalStats,
cameraGood);
RETINA_VPRINTF("CAMCAL SELECT %s LED=IR exp=%.3f ms gain=%u P95=%u SAT=%lu%% status=%s\n",
reason ? reason : "?", exposureMs, manualGain,
finalStats.p95, (unsigned long)finalStats.satPct,
cameraGood ? "OK" : "BEST");
if(_retinaLogBuffer)
{
char line[192];
sprintf(line,
"CAMCAL SELECT %s LED=IR exp=%.3f ms gain=%u P95=%u SAT=%lu%% status=%s\n",
reason ? reason : "?", exposureMs, manualGain,
finalStats.p95, (unsigned long)finalStats.satPct,
cameraGood ? "OK" : "BEST");
retinaRunLogAppend(line);
}
return exposureMs;
}
// -----------------------------------------------------------------------------
// IMU calibration and selftest
// -----------------------------------------------------------------------------
void imuCalibrate()
{
F32 SelfTest[6];
imu.reset();
RETINA_VPRINTF("IMU Self test started ...\n");
imu.selfTest(SelfTest);
RETINA_VPRINTF("Acceleration trim within = X:%6.1f, Y:%6.1f, Z:%6.1f %% of factory value\n", SelfTest[0], SelfTest[1], SelfTest[2]);
RETINA_VPRINTF("Gyroscope trim within = X:%6.1f, Y:%6.1f, Z:%6.1f %% of factory value\n", SelfTest[3], SelfTest[4], SelfTest[5]);
RETINA_VPRINTF("Calibrating gyroscope and accelerometer ...\n");
imu.accelGyroCalibration();
RETINA_VPRINTF("Accelerometer biases (mg) = X:%6.1f, Y:%6.1f, Z:%6.1f\n", 1000. * imu.d->accelBias[0], 1000. * imu.d->accelBias[1], 1000. * imu.d->accelBias[2]);
RETINA_VPRINTF("Gyroscope biases (dps)= X:%6.1f, Y:%6.1f, Z:%6.1f\n", imu.d->gyroBias[0], imu.d->gyroBias[1], imu.d->gyroBias[2]);
imu.setRange(otMPU9250::AFS_2G, otMPU9250::GFS_250DPS, otMPU9250::MFS_16BITS);
imu.imuInit();
_retinaProbeMagPresent = imu.magnetometerCheck();
if(_retinaProbeMagPresent)
{
imu.magnetometerInit();
RETINA_VPRINTF("Magnetometer initialized for active data mode ...\n");
imu.magnetometerCalibration();
RETINA_VPRINTF("Magnetometer biases (mG) = X:%6.1f, Y:%6.1f, Z:%6.1f\n", imu.d->magBias[0], imu.d->magBias[1], imu.d->magBias[2]);
RETINA_VPRINTF("Magnetometer scale (mG) = X:%6.1f, Y:%6.1f, Z:%6.1f\n", imu.d->magScale[0], imu.d->magScale[1], imu.d->magScale[2]);
}
}
// V3.0C deterministic run-start conditioning: fixed neutral camera, then a
// bounded discard window before the minimal FSM is allowed to move.
#define RETINA_RUN_INIT_TILT_SETTLE_FRAMES 8U
// -----------------------------------------------------------------------------
// Retina V0.4
//
// PURPOSE OF THIS FIRST VERSION:
// 1) verify U16 acquisition and actual MT9V032 value packing;
// 2) measure frame-to-frame activity in 8x8 cells;
// 3) produce coarse LEFT/CENTER/RIGHT/GROUND observables;
// 4) show gyro and fixed camera-control diagnostics alongside visual activity;
// 5) produce a deliberately simple preliminary decision on the console.
//
// V0.4 adds a coarse local directional flow estimate using 8x8 block matching.
// It is intentionally readable and diagnostic; optimization comes later.
// -----------------------------------------------------------------------------
static void retinaRefreshGuardLut(const otRetinaMinimal &retina,
U16 *trackLeft, U16 *trackRight, U8 *trackValid,
U16 *heightLeft, U16 *heightRight, U8 *heightValid)
{
if(!trackLeft || !trackRight || !trackValid ||
!heightLeft || !heightRight || !heightValid)
return;
for(U16 row = 0U; row < RETINA3_TRACK_GUARD_ROWS; ++row)
{
U16 distanceMm = 0U;
trackValid[row] = retina.trackGuardBounds(row, &trackLeft[row],
&trackRight[row], &distanceMm) ? 1U : 0U;
if(!trackValid[row])
trackLeft[row] = trackRight[row] = 0U;
heightValid[row] = retina.heightGuardBounds(row, &heightLeft[row],
&heightRight[row], &distanceMm) ? 1U : 0U;
if(!heightValid[row])
heightLeft[row] = heightRight[row] = 0U;
}
}
static void retinaAutonomicCoreRun()
{
RETINA_EPRINTF("RETINA BOOT: RUN ENTER %s\n", RETINA_BUILD_TAG);
RETINA_EPRINTF("RETINA Scout - Robotic Environmental Telemetry, Imaging and Navigation Agent\n");
U8 charLoop = 0;
_retinaLastRunTimedOut = false;
_retinaFrontContactCountRun = 0U;
_retinaProbeImuSequence=0U;_retinaProbeLastMagMs=0U;_retinaProbeMagValid=false;_retinaProbeTempValid=false;
U16 *cur = 0;
U16 *prev = 0;
const U16 w = sensor.horPixel();
const U16 h = sensor.verPixel();
// sensor.horPixel()/verPixel() report the current binned output size.
// With hardware binning 2x2 this is already the 376x240 logical retina,
// therefore software decimation is 1x1 and an 8x8 cell is directly 8x8.
const U16 logicalW = w / RETINA_SW_DECIMATION;
const U16 logicalH = h / RETINA_SW_DECIMATION;
const U16 cols = logicalW >> 3;
const U16 rows = logicalH >> 3;
// V3.0C deterministic run start: force the field-tuned neutral before
// CAMCAL and before any temporal reference is established.
const U16 previousRunTilt = retina.cameraTilt();
retina.abort();
retina.setCameraTilt(RETINA3_DEFAULT_CAMERA_TILT);
for(U8 initSettle = 0U; initSettle < RETINA_RUN_INIT_TILT_SETTLE_FRAMES; ++initSettle)
camera.grab(image);
// Every run starts with clean actuator and temporal state.
retinaFrameCounter = 0U;
retinaPreviousValid = false;
retinaGrabToImageA = true;
retinaPrevGain = 0U;
retinaPrevExposure = 0.0;
retina.reset();
// RETINA owns shutter and analog gain. AEC/AGC are never enabled.
// IR is the navigation illuminator and is strobed only during MT9V032 exposure.
F32 manualExposureMs = RETINA_EXPOSURE_INITIAL_MS;
U16 manualGain = RETINA_MANUAL_GAIN_INITIAL;
RetinaExposureStats exposureStats;
U16 controlBusBadFrames = 0U;
U8 controlBusBadWindow = 0U;
U16 controlBusRecoverCooldown = 0U;
U32 cameraGainReadFail = 0U;
U32 cameraShutterReadFail = 0U;
U32 cameraReadbackOutOfRange = 0U;
statusLed.setHazard(STATUS_HAZARD_NONE);
statusLed.setState(STATUS_LED_CALIBRATE);
statusLed.showNow();
// Select persistent run filenames and start the field log before CAMCAL so
// the SD contains the complete camera decision that precedes the first terrain
// frame. SDRAM logger allocation failure is non-fatal, but failure to reserve
// a unique run id is fatal: normal operation must never overwrite old runs.
retinaRunLogOpen();
if(!_retinaRunNamesValid)
{
RETINA_EPRINTF("RETINA FILE ERROR: cannot reserve a unique persistent run id; navigation aborted\n");
statusLed.setState(STATUS_LED_FAULT);
statusLed.showNow();
retina.stop();
return;
}
if(_retinaLogBuffer)
{
char line[256];
sprintf(line,
"RUN INIT previousTilt=%u requestedTilt=%u actualTilt=%u settleFrames=%u stateReset=OK\n",
(unsigned int)previousRunTilt,
(unsigned int)RETINA3_DEFAULT_CAMERA_TILT,
(unsigned int)retina.cameraTilt(),
(unsigned int)RETINA_RUN_INIT_TILT_SETTLE_FRAMES);
retinaRunLogAppend(line);
}
RETINA_VPRINTF("RUN INIT: previousTilt=%u requestedTilt=%u actualTilt=%u settleFrames=%u stateReset=OK\n",
(unsigned int)previousRunTilt,
(unsigned int)RETINA3_DEFAULT_CAMERA_TILT,
(unsigned int)retina.cameraTilt(),
(unsigned int)RETINA_RUN_INIT_TILT_SETTLE_FRAMES);
RETINA_EPRINTF("RETINA BOOT: TELEMETRY OPEN\n");
retinaTelemetryOpen();
RETINA_EPRINTF("RETINA BOOT: TELEMETRY OK\n");
retina.stop();
RETINA_EPRINTF("RETINA BOOT: CAMCAL START\n");
manualExposureMs = retinaCalibrateExposureGain(manualExposureMs, manualGain,
"START", exposureStats);
RETINA_EPRINTF("RETINA BOOT: CAMCAL OK EXP=%.3f G=%u\n", (double)manualExposureMs, (unsigned int)manualGain);
retinaPrevGain = 0;
retinaPrevExposure = 0.0;
retinaPreviousValid = false;
retinaGrabToImageA = true;
statusLed.setState(STATUS_LED_FORWARD);
statusLed.showNow();
if(_retinaLogBuffer)
{
char line[320];
sprintf(line,
"RUN START build=%s arch=%s geom=%ux%u logical=%ux%u EXP=%.3f ms G=%u LED=%s tilt=%u perfEvery=%u duration=%lu s\n",
RETINA_BUILD_TAG, RETINA_ARCH_TAG, w, h, logicalW, logicalH,
manualExposureMs, manualGain, retina.lightName(),
(unsigned int)retina.cameraTilt(), RETINA_PERF_EVERY, (unsigned long)RETINA_RUN_DURATION_SECONDS);
retinaRunLogAppend(line);
}
_retinaEventLog.record(RETINA_EVENT_RUN_START, _retinaRunStartedByPf15 ? 1U : 0U, 0U, 0U,
(S32)manualGain,
(S32)(manualExposureMs * 1000.0f + 0.5f),
(S32)retina.cameraTilt());
if(_retinaRunStartedByPf15)
_retinaEventLog.record(RETINA_EVENT_RUN_TRIGGER, 0U, 0U, 0U,
(S32)_retinaPf15RunTriggerCount, 0, 0);
_retinaRunStartedByPf15 = false;
RETINA_VPRINTF("RETINA %s directional/photo-normalized started\n", RETINA_BUILD_TAG);
RETINA_VPRINTF("RETINA LIGHT=%s (forced for navigation), timing controlled by MT9V032\n", retina.lightName());
RETINA_VPRINTF("RETINA STATUS: 10x WS2812B on PF14, state/heartbeat display\n");
RETINA_VPRINTF("PF15: active-low parallel contact array, falling-edge IRQ, no safety debounce; after TIMEOUT release+press starts next run\n");
if(RETINA_RUN_DURATION_SECONDS)
RETINA_VPRINTF("RETINA RUN TIMER: automatic clean stop after %lu s of navigation\n",
(unsigned long)RETINA_RUN_DURATION_SECONDS);
else
RETINA_VPRINTF("RETINA RUN TIMER: unlimited (manual '.' stop only)\n");
RETINA_VPRINTF("RETINA GEOMETRY physical=%ux%u logical=%ux%u cells=%ux%u total=%lu (8x8 logical / %ux%u physical)\n",
w, h, logicalW, logicalH, cols, rows,
(unsigned long)((U32)cols * (U32)rows), RETINA_CELL_W, RETINA_CELL_H);
// Hard geometry guard for MT9V032 hardware binning 2x2.
// The binned camera output must already be the 376x240 logical retina.
if(w != 376U || h != 240U || logicalW != 376U || logicalH != 240U ||
cols != 47U || rows != 30U || ((U32)cols * (U32)rows) != 1410U)
{
RETINA_EPRINTF("RETINA ERROR: invalid geometry for %s! Expected HW-bin2 output=376x240 logical=376x240 cells=47x30 total=1410\n",
RETINA_BUILD_TAG);
retina.setLight(0U);
IDLE;
}
RETINA_VPRINTF("RETINA GEOMETRY CHECK: OK\n");
RETINA_VPRINTF("RETINA SOURCE CHECK: %s HW binning 2x2 + 3.6mm/300px + 200x160mm prospective body guard + IR exposure-strobe + STATUS10 + gyro closed-loop escape + raw FLOW23x15 + minimal monotonic escape FSM + PROBE science recorder, U16/10-bit -> U8 D9..D2\n",
RETINA_BUILD_TAG);
#if RETINA_MVASM_ENABLE
if((RETINA_MVASM_WIDTH % 16U) || (RETINA_MVASM_HEIGHT % 16U))
{
RETINA_EPRINTF("RETINA MVASM ERROR: map geometry invalid!\n");
retina.setLight(0U);
IDLE;
}
if(RETINA_MVASM_WIDTH != RETINA3_TRACK_GUARD_MAP_WIDTH_PX ||
RETINA_MVASM_ROWS != RETINA3_TRACK_GUARD_ROWS ||
RETINA_MVASM_MB_SIZE != RETINA3_TRACK_GUARD_MB_SIZE_PX)
{
RETINA_EPRINTF("RETINA BODY_GUARD ERROR: map=%ux%u mb=%u guard=%ux%u mb=%u\n",
RETINA_MVASM_WIDTH, RETINA_MVASM_ROWS, RETINA_MVASM_MB_SIZE,
RETINA3_TRACK_GUARD_MAP_WIDTH_PX, RETINA3_TRACK_GUARD_ROWS, RETINA3_TRACK_GUARD_MB_SIZE_PX);
retina.setLight(0U);
IDLE;
}
RETINA_VPRINTF("RETINA MVASM TILED: map=%ux%u px, macroblocks=%ux%u (%u vectors), tile max=%ux%u\n",
RETINA_MVASM_WIDTH, RETINA_MVASM_HEIGHT,
RETINA_MVASM_COLS, RETINA_MVASM_ROWS, RETINA_MVASM_VECTORS,
RETINA_MVASM_TILE_W, RETINA_MVASM_TILE_H);
RETINA_VPRINTF("RETINA MVZONE non-overlap maxima: L=%u C=%u R=%u G=%u (upper 2/3 vs lower 1/3)\n",
(RETINA_MVASM_COLS / 3U) * ((RETINA_MVASM_ROWS * 2U) / 3U),
(((RETINA_MVASM_COLS * 2U) / 3U) - (RETINA_MVASM_COLS / 3U)) * ((RETINA_MVASM_ROWS * 2U) / 3U),
(RETINA_MVASM_COLS - ((RETINA_MVASM_COLS * 2U) / 3U)) * ((RETINA_MVASM_ROWS * 2U) / 3U),
RETINA_MVASM_COLS * (RETINA_MVASM_ROWS - ((RETINA_MVASM_ROWS * 2U) / 3U)));
RETINA_VPRINTF("RETINA COHERENCE: zone >= %u%% and coherent mean >= %u half-pixel\n",
RETINA_MVASM_ZONE_COH_MIN_PCT, RETINA_MVASM_COH_MEAN_MIN_X2);
RETINA_VPRINTF("RETINA MV confidence: raw temporal activity >= %u before LEFT/RIGHT/STOP\n",
RETINA_MVASM_MIN_PHOTO_ACTIVITY);
#endif
RETINA_VPRINTF("CAMERA: AEC=OFF AGC=OFF, manual shutter %.3f ms, manual analog gain=%u during RUN\n",
manualExposureMs, manualGain);
RETINA_VPRINTF("Camera-control gate: fixed gain delta >= %u or fixed exposure delta >= %u%% -> HOLD (unexpected register change)\n",
RETINA_CONTROL_GAIN_DELTA, RETINA_CONTROL_EXPOSURE_PCT);
RETINA_VPRINTF("Photometric immunity: global Q%u normalization; flicker coherence>=%u%%, scale delta>=%u%%\n",
RETINA_PHOTO_Q, RETINA_ILLUM_COHERENCE_PCT, RETINA_ILLUM_SCALE_DELTA_PCT);
RETINA_VPRINTF("Camera calibration target: P95=%u (%u..%u), exact SAT<=%u%%, shutter %.2f..%.2f ms, gain=%u..%u, LED=IR EXPOSURE-STROBED\n",
RETINA_EXPOSURE_TARGET_P95, RETINA_EXPOSURE_P95_LOW, RETINA_EXPOSURE_P95_HIGH,
RETINA_EXPOSURE_MAX_SAT_PCT, RETINA_EXPOSURE_MIN_MS, RETINA_EXPOSURE_MAX_MS,
RETINA_MANUAL_GAIN_MIN, RETINA_MANUAL_GAIN_MAX);
RETINA_VPRINTF("Decision gates: PHOTO transient -> HOLD; directional flow valid >= %u cells\n",
RETINA_FLOW_MIN_VALID_CELLS);
RETINA_VPRINTF("CAMERA TILT: PWM=%u (approximately horizontal)\n", (unsigned int)retina.cameraTilt());
RETINA_VPRINTF("BODY GUARD: width body=180mm safe=200mm, height safe=160mm, camera h=135mm f=3.6mm/300px bin2 pitch=3+/-2deg (guard uses 5deg)\n");
{
U16 gr;
for(gr = 0U; gr < RETINA_MVASM_ROWS; ++gr)
{
U16 gl, rr, gz;
if(retina.heightGuardBounds(gr, &gl, &rr, &gz))
RETINA_VPRINTF(" HG row=%u z=%u mm x=%u..%u\n",
(unsigned int)gr, (unsigned int)gz,
(unsigned int)gl, (unsigned int)rr);
if(retina.trackGuardBounds(gr, &gl, &rr, &gz))
RETINA_VPRINTF(" TG row=%u z=%u mm x=%u..%u\n",
(unsigned int)gr, (unsigned int)gz,
(unsigned int)gl, (unsigned int)rr);
}
}
RETINA_VPRINTF("MINIMAL FSM: reverse=%u..%u frames, turn=%.0f deg, contact=%.0f deg, escalated=%.0f deg, clear=%u..%u frames, TABOO active only during CLEAR\n",
(unsigned int)RETINA3_ESCAPE_REVERSE_MIN_FRAMES,
(unsigned int)RETINA3_ESCAPE_REVERSE_MAX_FRAMES,
(double)RETINA3_TURN_NORMAL_DEG,
(double)RETINA3_TURN_CONTACT_DEG,
(double)RETINA3_TURN_ESCALATED_DEG,
(unsigned int)RETINA3_CLEAR_MIN_FRAMES,
(unsigned int)RETINA3_CLEAR_MAX_FRAMES);
RETINA_VPRINTF("GROUND emergency only: mean>=%u x2, RCOH>=%u%%, margin over CENTER>=%u x2\n",
RETINA_GROUND_EMERGENCY_MEAN_X2, RETINA_GROUND_EMERGENCY_RCOH_PCT,
RETINA_GROUND_EMERGENCY_MARGIN_X2);
if(w > RETINA_TILE_MAX_W)
{
RETINA_EPRINTF("Retina tile width too small for %u px\n", w);
retina.setLight(0U);
IDLE;
}
// V2.0D: STOP frames stay entirely in SDRAM while the robot is alive.
// JPEG compression and AVI/SD creation are deferred until after motors and
// DMA are stopped. This removes filesystem latency from the autonomic loop.
const U32 stopFrameBytes = debugImage.size();
U8 *stopJpeg = (U8 *)_malloc(stopFrameBytes);
U8 *stopFrameStorage = (U8 *)_malloc(stopFrameBytes * RETINA_STOP_SNAPSHOT_CAPACITY);
struct avi_t *stopAvi = 0;
const char *stopAviName = _retinaStopAviName;
U32 stopSnapshotCount = 0U;
U32 stopSnapshotDropped = 0U;
U32 stopAviWritten = 0U;
// V3.0C: one CAMCAL at run start only. Exposure and gain remain fixed
// throughout the 600 s field run; there is no post-STOP recalibration state.
// Tiled ADI motion-vector diagnostics.
U32 usMvTileCopy = 0; // total tile preparation = DMA + pack or fallback
U32 usMvTileDma = 0;
U32 usMvTilePack = 0;
U32 usMvTileAsm = 0;
U32 usMvTileAggregate = 0;
U32 mvAsmNonZero = 0;
U32 mvLeftCount = 0, mvCenterCount = 0, mvRightCount = 0, mvGroundCount = 0;
U32 mvLeftMagSum = 0, mvCenterMagSum = 0, mvRightMagSum = 0, mvGroundMagSum = 0;
S32 mvLeftX2Sum = 0, mvCenterX2Sum = 0, mvRightX2Sum = 0, mvGroundY2Sum = 0;
U32 mvGlobalMagSum = 0;
S32 mvGlobalX2Sum = 0, mvGlobalY2Sum = 0;
U32 mvGlobalCohPct = 0, mvGlobalCohMeanX2 = 0;
unsigned long long mvRadialAbsSum = 0ULL;
S32 mvRadialSignedSum = 0;
U32 mvRadialCohPct = 0;
U32 mvLeftCohPct = 0, mvCenterCohPct = 0, mvRightCohPct = 0, mvGroundCohPct = 0;
U32 mvLeftCohMeanX2 = 0, mvCenterCohMeanX2 = 0, mvRightCohMeanX2 = 0, mvGroundCohMeanX2 = 0;
S32 mvLeftY2Sum = 0, mvCenterY2Sum = 0, mvRightY2Sum = 0, mvGroundX2Sum = 0;
// Prospective swept-corridor halves. These intentionally overlap the legacy
// L/C/R/G zones: they answer a different question, namely whether motion
// lies inside the physical 200mm path that the tracks must clear.
U32 mvTrackLeftCount = 0, mvTrackRightCount = 0;
U32 mvTrackLeftMagSum = 0, mvTrackRightMagSum = 0;
S32 mvTrackLeftX2Sum = 0, mvTrackLeftY2Sum = 0;
S32 mvTrackRightX2Sum = 0, mvTrackRightY2Sum = 0;
unsigned long long mvTrackLeftRadialAbsSum = 0ULL, mvTrackRightRadialAbsSum = 0ULL;
S32 mvTrackLeftRadialSignedSum = 0, mvTrackRightRadialSignedSum = 0;
U32 mvTrackLeftCohPct = 0, mvTrackRightCohPct = 0;
U32 mvTrackLeftCohMeanX2 = 0, mvTrackRightCohMeanX2 = 0;
U32 mvTrackLeftRadialCohPct = 0, mvTrackRightRadialCohPct = 0;
// Upper BODY/HEIGHT guard. The camera sits inside the 160mm
// safety height, so the roof-plane projection naturally occupies only
// the upper motion-vector rows. Split left/right for directed escape.
U32 mvHeightLeftCount = 0, mvHeightRightCount = 0;
U32 mvHeightLeftMagSum = 0, mvHeightRightMagSum = 0;
S32 mvHeightLeftX2Sum = 0, mvHeightLeftY2Sum = 0;
S32 mvHeightRightX2Sum = 0, mvHeightRightY2Sum = 0;
unsigned long long mvHeightLeftRadialAbsSum = 0ULL, mvHeightRightRadialAbsSum = 0ULL;
S32 mvHeightLeftRadialSignedSum = 0, mvHeightRightRadialSignedSum = 0;
U32 mvHeightLeftCohPct = 0, mvHeightRightCohPct = 0;
U32 mvHeightLeftCohMeanX2 = 0, mvHeightRightCohMeanX2 = 0;
U32 mvHeightLeftRadialCohPct = 0, mvHeightRightRadialCohPct = 0;
U16 trackGuardLeftRow[RETINA3_TRACK_GUARD_ROWS];
U16 trackGuardRightRow[RETINA3_TRACK_GUARD_ROWS];
U8 trackGuardValidRow[RETINA3_TRACK_GUARD_ROWS];
U16 heightGuardLeftRow[RETINA3_TRACK_GUARD_ROWS];
U16 heightGuardRightRow[RETINA3_TRACK_GUARD_ROWS];
U8 heightGuardValidRow[RETINA3_TRACK_GUARD_ROWS];
// Performance accumulators. One compact printf every RETINA_PERF_EVERY frames.
unsigned long long perfTotalUs = 0ULL;
unsigned long long perfGrabUs = 0ULL;
unsigned long long perfControlUs = 0ULL;
unsigned long long perfPhotoUs = 0ULL;
unsigned long long perfTileCopyUs = 0ULL;
unsigned long long perfTileDmaUs = 0ULL;
unsigned long long perfTilePackUs = 0ULL;
unsigned long long perfAsmUs = 0ULL;
U32 dmaFallbackCount = 0U;
U32 dmaFaultLogCount = 0U;
unsigned long long perfScatterUs = 0ULL;
unsigned long long perfDriveUs = 0ULL;
U32 perfFrames = 0U;
if(!stopJpeg)
{
RETINA_EPRINTF("RETINA SNAPSHOT: can't allocate %lu byte JPEG buffer\n", (unsigned long)debugImage.size());
retina.setLight(0U);
IDLE;
}
// Keep a stack-local cache for the 345-vector hot loop. Camera geometry is
// fixed in V3.0C, so this is initialized once before the realtime loop.
retinaRefreshGuardLut(retina,
trackGuardLeftRow, trackGuardRightRow, trackGuardValidRow,
heightGuardLeftRow, heightGuardRightRow, heightGuardValidRow);
// RETINA owns both memory-DMA streams while active. Use polling only:
// completion interrupts are explicitly masked to keep this hot path deterministic.
DmaEnableInterrupt(DMA_CH_0, false);
DmaEnableInterrupt(DMA_CH_1, false);
DmaStop(DMA_CH_0);
DmaStop(DMA_CH_1);
RETINA_VPRINTF("RETINA %s LENS36 IR PERSIST-SD STATUS10 HEIGHT-MONITOR FLOW23x15 GYRO-ESCAPE MINFSM running: MDMA0=current MDMA1=previous, fixed exposure + fixed gain, HW bin2, STOP frames -> %s, PERF every %u, max=%lu s - press '.' to stop\n",
RETINA_BUILD_TAG, stopAviName, RETINA_PERF_EVERY, (unsigned long)RETINA_RUN_DURATION_SECONDS);
// PF15 must be read as an ordinary GPIO BEFORE edge sensitivity is armed.
// Once PortFSetInterruptService(..., falling) is active, PortGet reads the
// sticky edge latch rather than the physical pin level on BF51x.
if(!retinaPf15WaitReleased(true))
{
retina.stop();
return;
}
retina.setFrontContactLevel(false);
U32 pf15SeenSeq = _retinaPf15FallingSeq;
retinaPf15ArmFalling();
RETINA_EPRINTF("PF15 safety armed: idle HIGH confirmed, falling-edge IRQ active\n");
// Wall-clock navigation timer. Accumulate short TIMER0 deltas rather than
// measuring one 60/120-second interval, so a 32-bit hardware-counter wrap
// cannot corrupt the timeout. SD checkpoints, STOP snapshots and any other
// work performed while the navigation loop is active are therefore included.
otRetinaRunClock runClock;
U32 runClockPrev = TimerGetCounts(TIMER0);
runClock.start(runClockPrev, RETINA_RUN_DURATION_SECONDS);
bool runTimedOut = false;
bool runControlFault = false;
// V3.0C telemetry transition memory. The persisted numeric state is
// Retina3State: FORWARD, REVERSE, TURN, CLEAR, REFERENCE, FAULT.
DriveRequest telemetryPrevDriveRequest = DRIVE_REQ_REFERENCE;
Retina3State telemetryPrevState = retina.state();
U16 telemetryPrevForwardPwm = retina.forwardPwm();
U32 telemetryPrevBusRecovery = retina.busRecoveryCount();
U8 telemetryPrevTaboo = retina.tabooMask();
U32 telemetryPrevEscapeCount = retina.escapeCount();
U32 telemetryPrevClearRehit = retina.clearRehitCount();
U32 telemetryPrevActionDone = retina.actionDoneSequence();
U32 telemetryPrevTurnTimeout = retina.turnTimeoutSequence();
U32 telemetryPrevStallCount = retina.stallCount();
U8 trackPendingMask = 0U;
U8 trackPendingFrames = 0U;
U8 centerPendingFrames = 0U;
while(charLoop != '.')
{
{
// Always accumulate short deltas, even when the timeout is disabled,
// so the final elapsed time remains valid across TIMER0 wraps.
U32 runClockNow = TimerGetCounts(TIMER0);
U32 runDeltaUs = ElapsedTimeUs(runClockPrev, runClockNow);
runClockPrev = runClockNow;
if(runClock.update(runClockNow, runDeltaUs))
{
runTimedOut = true;
break;
}
}
if(retinaHandlePf15RunSafety((U32)(runClock.elapsedUs() / 1000ULL),
retinaFrameCounter + 1U, pf15SeenSeq))
continue;
retina.setFrameNumber(retinaFrameCounter + 1U);
U32 tTotal0, tTotal1;
U32 t0, t1;
U32 usGrab = 0, usControl = 0, usPhoto = 0, usDrive = 0, usScene = 0;
U16 currentGain = retinaPrevGain;
F64 currentExposure = retinaPrevExposure;
bool referenceOnlyFrame = !retinaPreviousValid;
// Photometric diagnostics and global normalization.
S32 photoSignedSum = 0;
U32 photoAbsSum = 0;
U32 photoCoherencePct = 0;
U32 photoScaleQ = RETINA_PHOTO_ONE_Q;
U32 rawActivityAvg = 0;
U32 clipHighCount = 0, clipLowCount = 0, satExactCount = 0, pixelCount = 0;
U32 photoSum = 0U, photoSumSq = 0U; U16 photoHist[RETINA_PROBE_HIST_BINS];
U8 photoMin = 0U, photoMax = 0U; for(U16 phi=0U;phi<RETINA_PROBE_HIST_BINS;++phi) photoHist[phi]=0U;
bool illuminationTransient = false;
bool cameraControlChanged = false;
U32 controlGainDelta = 0;
U32 controlExposureDeltaPct = 0;
DriveRequest driveRequest = DRIVE_REQ_REFERENCE;
const char *stopCause = "NONE";
U32 stopCohPct = 0U;
U32 stopCohMeanX2 = 0U;
U8 trackRawHazardMask = 0U;
U8 trackConfirmedHazardMask = 0U;
bool boostObstacleEvidence = false;
tTotal0 = TimerGetCounts(TIMER0);
// -----------------------------------------------------------------
// CAMERA ACQUISITION + FIXED MANUAL EXPOSURE/GAIN MONITOR
// -----------------------------------------------------------------
// AEC and AGC are OFF. Both registers stay fixed during navigation.
// After any stopped calibration, one frame is accepted only as the new
// temporal reference.
_retinaAcquisitionStage.acquire(camera, image, previousImage,
retinaGrabToImageA, cur, prev, usGrab);
if(retinaHandlePf15RunSafety((U32)(runClock.elapsedUs() / 1000ULL),
retinaFrameCounter + 1U, pf15SeenSeq))
continue;
// otRetinaMinimal is the only runtime owner of IMU samples.
// Read back actual gain/exposure after the grab. Both should remain fixed.
// Any material register change is unexpected and invalidates this temporal
// comparison instead of silently allowing the sensor to alter the retina.
t0 = TimerGetCounts(TIMER0);
bool cameraGainReadOk = false;
bool cameraShutterReadOk = false;
U16 cameraRawShutter = 0U;
retinaReadCameraControlBus(currentGain, currentExposure,
cameraGainReadOk, cameraShutterReadOk,
cameraRawShutter);
if(!cameraGainReadOk)
++cameraGainReadFail;
if(!cameraShutterReadOk)
++cameraShutterReadFail;
// V2.6A-BUS1 control-bus health monitor. A failed I2C transaction is
// explicitly different from a valid-but-out-of-range sensor value.
// Failed read data is never converted into exposure and never reaches
// downstream camera-change logic.
if(controlBusRecoverCooldown)
--controlBusRecoverCooldown;
const bool cameraReadFailed = !cameraGainReadOk || !cameraShutterReadOk;
const bool cameraValueOutOfRange =
!cameraReadFailed && !retina.cameraReadbackSane(currentGain, currentExposure);
if(cameraValueOutOfRange)
++cameraReadbackOutOfRange;
const bool controlBusSampleBad = cameraReadFailed || cameraValueOutOfRange;
controlBusBadWindow = (U8)(((U16)controlBusBadWindow << 1) & 0xffU);
if(controlBusSampleBad)
controlBusBadWindow |= 0x01U;
controlBusBadFrames = retinaBitCount8(controlBusBadWindow);
if(controlBusSampleBad)
{
RETINA_VPRINTF("CONTROL-BUS BAD #%lu window=%u/%u gainRead=%s shutterRead=%s gain=%u shutter=%u exp=%.0f us expected gain=%u exp=%.3f ms\n",
(unsigned long)(retinaFrameCounter + 1U),
(unsigned int)controlBusBadFrames,
(unsigned int)RETINA_CONTROL_BUS_WINDOW_FRAMES,
cameraGainReadOk ? "OK" : "FAIL",
cameraShutterReadOk ? "OK" : "FAIL",
currentGain, cameraRawShutter, currentExposure,
manualGain, manualExposureMs);
if(_retinaLogBuffer)
{
char line[320];
sprintf(line,
"CONTROL-BUS BAD #%lu window=%u/%u gainRead=%s shutterRead=%s gain=%u shutter=%u exp=%.0f us expected gain=%u exp=%.3f ms\n",
(unsigned long)(retinaFrameCounter + 1U),
(unsigned int)controlBusBadFrames,
(unsigned int)RETINA_CONTROL_BUS_WINDOW_FRAMES,
cameraGainReadOk ? "OK" : "FAIL",
cameraShutterReadOk ? "OK" : "FAIL",
currentGain, cameraRawShutter, currentExposure,
manualGain, manualExposureMs);
retinaRunLogAppend(line);
}
// Treat the readback as unknown, not as a real control change.
currentGain = manualGain;
currentExposure = (F64)manualExposureMs * 1000.0;
}
if(controlBusBadFrames >= RETINA_CONTROL_BUS_BAD_TRIGGER &&
controlBusRecoverCooldown == 0U)
{
bool busOk;
RETINA_VPRINTF("CONTROL-BUS RECOVERY #%lu requested window=%u/%u\n",
(unsigned long)(retinaFrameCounter + 1U),
(unsigned int)controlBusBadFrames,
(unsigned int)RETINA_CONTROL_BUS_WINDOW_FRAMES);
if(_retinaLogBuffer)
{
char line[160];
sprintf(line, "CONTROL-BUS RECOVERY #%lu requested window=%u/%u\n",
(unsigned long)(retinaFrameCounter + 1U),
(unsigned int)controlBusBadFrames,
(unsigned int)RETINA_CONTROL_BUS_WINDOW_FRAMES);
retinaRunLogAppend(line);
}
busOk = retina.recoverControlBus();
controlBusRecoverCooldown = 20U;
cameraControlChanged = true;
referenceOnlyFrame = true;
retinaPreviousValid = false;
retinaPrevGain = 0U;
retinaPrevExposure = 0.0;
if(busOk)
{
sensor.setAec(false);
sensor.setAgc(false);
retinaSetExposureMsBusSafe(manualExposureMs);
sensor.setGain(manualGain);
retina.setLight(2U);
currentGain = manualGain;
currentExposure = (F64)manualExposureMs * 1000.0;
controlBusBadWindow = 0U;
controlBusBadFrames = 0U;
RETINA_VPRINTF("CONTROL-BUS RESTORED #%lu gain=%u exp=%.3f ms recoveries=%lu\n",
(unsigned long)(retinaFrameCounter + 1U),
manualGain, manualExposureMs,
(unsigned long)retina.busRecoveryCount());
if(_retinaLogBuffer)
{
char line[192];
sprintf(line,
"CONTROL-BUS RESTORED #%lu gain=%u exp=%.3f ms recoveries=%lu\n",
(unsigned long)(retinaFrameCounter + 1U),
manualGain, manualExposureMs,
(unsigned long)retina.busRecoveryCount());
retinaRunLogAppend(line);
}
}
else
{
RETINA_VPRINTF("CONTROL-BUS RECOVERY FAILED #%lu; keep HOLD/REFERENCE and retry later\n",
(unsigned long)(retinaFrameCounter + 1U));
if(_retinaLogBuffer)
{
char line[160];
sprintf(line,
"CONTROL-BUS RECOVERY FAILED #%lu; retry after cooldown\n",
(unsigned long)(retinaFrameCounter + 1U));
retinaRunLogAppend(line);
}
}
}
if(retinaPreviousValid && retinaPrevGain)
{
controlGainDelta = (currentGain >= retinaPrevGain) ?
(U32)(currentGain - retinaPrevGain) :
(U32)(retinaPrevGain - currentGain);
if(retinaPrevExposure > 0.0)
{
F64 ed = absF64(currentExposure - retinaPrevExposure);
controlExposureDeltaPct = (U32)((ed * 100.0) / retinaPrevExposure);
}
cameraControlChanged =
(controlGainDelta >= RETINA_CONTROL_GAIN_DELTA) ||
(controlExposureDeltaPct >= RETINA_CONTROL_EXPOSURE_PCT);
}
t1 = TimerGetCounts(TIMER0);
usControl = ElapsedTimeUs(t0, t1);
if(referenceOnlyFrame)
{
// First frame after START/recalibration: establish a clean reference.
// Do not compare across a shutter/gain calibration boundary.
retinaPreviousValid = true;
driveRequest = DRIVE_REQ_REFERENCE;
}
// -----------------------------------------------------------------
// ACTIVE TEMPORAL RETINA + DIRECTIONAL FLOW
// -----------------------------------------------------------------
if(!referenceOnlyFrame && retinaFrameCounter > 0U)
{
// V2.0D extracted temporal photometric pre-pass. The exact V1.6
// constants are passed in from the Core, so this is a refactor rather
// than a retune of illumination/transient behaviour.
t0 = TimerGetCounts(TIMER0);
{
Retina3PhotometricResult photo;
retinaPhotometricAnalyze(cur, prev, w, h,
(U16)(RETINA_SW_DECIMATION * RETINA_PHOTO_LOGICAL_STEP), photo);
photoSignedSum = photo.signedSum;
photoAbsSum = photo.absSum;
photoCoherencePct = photo.coherencePct;
photoScaleQ = photo.scaleQ;
rawActivityAvg = photo.rawActivityAvg;
clipHighCount = photo.clipHighCount;
clipLowCount = photo.clipLowCount;
satExactCount = photo.satExactCount;
pixelCount = photo.pixelCount; photoSum = photo.sumCur; photoSumSq = photo.sumSqCur;
photoMin = photo.minValue; photoMax = photo.maxValue;
for(U16 phi=0U;phi<RETINA_PROBE_HIST_BINS;++phi) photoHist[phi]=photo.hist[phi];
illuminationTransient = photo.illuminationTransient;
}
t1 = TimerGetCounts(TIMER0);
usPhoto = ElapsedTimeUs(t0, t1);
if(retinaHandlePf15RunSafety((U32)(runClock.elapsedUs() / 1000ULL),
retinaFrameCounter + 1U, pf15SeenSeq))
continue;
#if RETINA_MVASM_ENABLE
// -------------------------------------------------------------
// ADI motionvect.asm - tiled FAST_DATA engine
// -------------------------------------------------------------
// Input frames are native 10-bit samples stored in U16 SDRAM buffers.
// Each sampled pixel is converted to D9..D2 while copying the active tile
// into FAST_DATA_1. No full-frame U8 conversion/copy is performed.
{
U32 tileY0, tileX0;
U32 dmaAccum = 0;
U32 packAccum = 0;
U32 asmAccum = 0;
U32 aggAccum = 0;
// Tile decomposition remains unchanged:
// X: 80 + 80 + 80 + 80 + 48 = 368
// Y: 96 + 96 + 48 = 240
for(tileY0 = 0; tileY0 < RETINA_MVASM_HEIGHT; )
{
U32 tileH = RETINA_MVASM_HEIGHT - tileY0;
if(tileH > RETINA_MVASM_TILE_H) tileH = RETINA_MVASM_TILE_H;
tileH &= ~15U;
for(tileX0 = 0; tileX0 < RETINA_MVASM_WIDTH; )
{
U32 tileW = RETINA_MVASM_WIDTH - tileX0;
if(tileW > RETINA_MVASM_TILE_W) tileW = RETINA_MVASM_TILE_W;
tileW &= ~15U;
U32 mbColsTile = tileW / 16U;
U32 mbRowsTile = tileH / 16U;
U32 tilePixels = tileW * tileH;
U32 tt0, tt1;
bool dmaOk = true;
// V0.18e: two 2-D MDMA streams fetch CURRENT and PREVIOUS
// U16 tiles from strided SDRAM rows into separate L1 banks.
// Destination stride == tileW, hence it is contiguous.
tt0 = TimerGetCounts(TIMER0);
{
const U16 *srcCur = cur + tileY0 * (U32)w + tileX0;
const U16 *srcPrev = prev + tileY0 * (U32)w + tileX0;
bool started0 = DmaMove2DChecked(DMA_CH_0,
(void *)srcCur,
(void *)retinaMvDmaTileCur,
tileW, tileH,
(U32)w, tileW,
DMA_WORD);
bool started1 = DmaMove2DChecked(DMA_CH_1,
(void *)srcPrev,
(void *)retinaMvDmaTilePrev,
tileW, tileH,
(U32)w, tileW,
DMA_WORD);
if(!started0 || !started1)
{
dmaOk = false;
}
else
{
U32 waitStart = TimerGetCounts(TIMER0);
U32 pollSpin = 0U;
while(!(DmaIsDone(DMA_CH_0) && DmaIsDone(DMA_CH_1)))
{
if(DmaHasError(DMA_CH_0) || DmaHasError(DMA_CH_1))
{
dmaOk = false;
break;
}
// Check the timer only occasionally: normal tile DMA
// completion should be much faster than this timeout.
if(((++pollSpin) & 0xffU) == 0U)
{
U32 now = TimerGetCounts(TIMER0);
if(ElapsedTimeUs(waitStart, now) > RETINA_DMA_TILE_TIMEOUT_US)
{
dmaOk = false;
break;
}
}
}
// A completion and an error can become visible together;
// do one final status check after both DONE bits are seen.
if(DmaHasError(DMA_CH_0) || DmaHasError(DMA_CH_1))
dmaOk = false;
}
if(!dmaOk)
{
DmaStop(DMA_CH_0);
DmaStop(DMA_CH_1);
}
}
tt1 = TimerGetCounts(TIMER0);
dmaAccum += ElapsedTimeUs(tt0, tt1);
tt0 = TimerGetCounts(TIMER0);
if(dmaOk)
{
// Pack U16 -> U8 in-place entirely inside L1.
retinaPack10To8InPlace(retinaMvDmaTileCur, tilePixels);
retinaPack10To8InPlace(retinaMvDmaTilePrev, tilePixels);
}
else
{
++dmaFallbackCount;
if(dmaFaultLogCount < RETINA_DMA_LOG_FAULT_LIMIT)
{
++dmaFaultLogCount;
RETINA_VPRINTF("RETINA DMA TILE fallback #%lu at x=%lu y=%lu w=%lu h=%lu\n",
(unsigned long)dmaFallbackCount,
(unsigned long)tileX0, (unsigned long)tileY0,
(unsigned long)tileW, (unsigned long)tileH);
if(_retinaLogBuffer)
{
char line[160];
sprintf(line,
"DMA FALLBACK #%lu frame=%lu x=%lu y=%lu w=%lu h=%lu\n",
(unsigned long)dmaFallbackCount,
(unsigned long)(retinaFrameCounter + 1U),
(unsigned long)tileX0, (unsigned long)tileY0,
(unsigned long)tileW, (unsigned long)tileH);
retinaRunLogAppend(line);
}
}
retinaPrepareMvTileCpuFallback(cur, prev,
(U8 *)retinaMvDmaTileCur,
(U8 *)retinaMvDmaTilePrev,
(U32)w,
tileX0, tileY0, tileW, tileH);
}
tt1 = TimerGetCounts(TIMER0);
packAccum += ElapsedTimeUs(tt0, tt1);
tt0 = TimerGetCounts(TIMER0);
_motionvect((const U8 *)retinaMvDmaTileCur,
(const U8 *)retinaMvDmaTilePrev,
retinaMvAsmTileV,
retinaMvAsmTileH,
tileH,
tileW,
RETINA_MVASM_SEARCH_FACTOR);
tt1 = TimerGetCounts(TIMER0);
asmAccum += ElapsedTimeUs(tt0, tt1);
// Scatter this tile's vectors into the complete 23x15 map.
tt0 = TimerGetCounts(TIMER0);
{
U32 my, mx;
for(my = 0; my < mbRowsTile; ++my)
{
U32 globalMy = tileY0 / 16U + my;
U32 globalBase = globalMy * RETINA_MVASM_COLS + tileX0 / 16U;
U32 localBase = my * mbColsTile;
for(mx = 0; mx < mbColsTile; ++mx)
{
retinaMvMapV[globalBase + mx] = retinaMvAsmTileV[localBase + mx];
retinaMvMapH[globalBase + mx] = retinaMvAsmTileH[localBase + mx];
}
}
}
tt1 = TimerGetCounts(TIMER0);
aggAccum += ElapsedTimeUs(tt0, tt1);
tileX0 += tileW;
}
tileY0 += tileH;
}
usMvTileCopy = dmaAccum + packAccum;
usMvTileDma = dmaAccum;
usMvTilePack = packAccum;
usMvTileAsm = asmAccum;
usMvTileAggregate = aggAccum;
// Summarize the complete motion map. Keep half-pixel units (x2).
{
U32 vi;
// IMPORTANT: these are frame-local statistics.
mvAsmNonZero = 0;
mvLeftCount = mvCenterCount = mvRightCount = mvGroundCount = 0;
mvLeftMagSum = mvCenterMagSum = mvRightMagSum = mvGroundMagSum = 0;
mvLeftX2Sum = mvCenterX2Sum = mvRightX2Sum = mvGroundY2Sum = 0;
mvGlobalMagSum = 0;
mvGlobalX2Sum = mvGlobalY2Sum = 0;
mvGlobalCohPct = mvGlobalCohMeanX2 = 0;
mvRadialAbsSum = 0ULL;
mvRadialSignedSum = 0;
mvRadialCohPct = 0;
mvLeftCohPct = mvCenterCohPct = mvRightCohPct = mvGroundCohPct = 0;
mvLeftCohMeanX2 = mvCenterCohMeanX2 = mvRightCohMeanX2 = mvGroundCohMeanX2 = 0;
mvLeftY2Sum = mvCenterY2Sum = mvRightY2Sum = mvGroundX2Sum = 0;
mvTrackLeftCount = mvTrackRightCount = 0U;
mvTrackLeftMagSum = mvTrackRightMagSum = 0U;
mvTrackLeftX2Sum = mvTrackLeftY2Sum = 0;
mvTrackRightX2Sum = mvTrackRightY2Sum = 0;
mvTrackLeftRadialAbsSum = mvTrackRightRadialAbsSum = 0ULL;
mvTrackLeftRadialSignedSum = mvTrackRightRadialSignedSum = 0;
mvTrackLeftCohPct = mvTrackRightCohPct = 0U;
mvTrackLeftCohMeanX2 = mvTrackRightCohMeanX2 = 0U;
mvTrackLeftRadialCohPct = mvTrackRightRadialCohPct = 0U;
mvHeightLeftCount = mvHeightRightCount = 0U;
mvHeightLeftMagSum = mvHeightRightMagSum = 0U;
mvHeightLeftX2Sum = mvHeightLeftY2Sum = 0;
mvHeightRightX2Sum = mvHeightRightY2Sum = 0;
mvHeightLeftRadialAbsSum = mvHeightRightRadialAbsSum = 0ULL;
mvHeightLeftRadialSignedSum = mvHeightRightRadialSignedSum = 0;
mvHeightLeftCohPct = mvHeightRightCohPct = 0U;
mvHeightLeftCohMeanX2 = mvHeightRightCohMeanX2 = 0U;
mvHeightLeftRadialCohPct = mvHeightRightRadialCohPct = 0U;
for(vi = 0; vi < RETINA_MVASM_VECTORS; ++vi)
{
S16 vx2 = (S16)retinaMvMapH[vi];
S16 vy2 = (S16)retinaMvMapV[vi];
U32 mx = vi % RETINA_MVASM_COLS;
U32 my = vi / RETINA_MVASM_COLS;
S16 cxp = (S16)(mx * 16U + 8U);
S16 cyp = (S16)(my * 16U + 8U);
S16 rx = (S16)(cxp - (S16)(RETINA_MVASM_WIDTH / 2U));
S16 ry = (S16)(cyp - (S16)(RETINA_MVASM_HEIGHT / 2U));
{
U32 mag2 = (U32)((vx2 < 0 ? -vx2 : vx2) +
(vy2 < 0 ? -vy2 : vy2));
if(mag2 >= RETINA_MVASM_MIN_MAG2)
{
S32 radial = (S32)vx2 * (S32)rx + (S32)vy2 * (S32)ry;
unsigned long long radialAbs =
(radial >= 0) ? (unsigned long long)radial :
(unsigned long long)(-radial);
++mvAsmNonZero;
mvGlobalMagSum += mag2;
mvGlobalX2Sum += vx2;
mvGlobalY2Sum += vy2;
mvRadialSignedSum += radial;
mvRadialAbsSum += radialAbs;
// Prospective vehicle footprint. For rows
// below the conservative ground horizon, otRetinaMinimal
// gives the image-space 200mm swept corridor at the
// distance represented by this MV row. Split it at
// the optical centre so a left/right track hazard can
// force the escape turn away from the occupied side.
{
const U16 guardLeft = trackGuardLeftRow[my];
const U16 guardRight = trackGuardRightRow[my];
if(trackGuardValidRow[my] &&
cxp >= (S16)guardLeft && cxp <= (S16)guardRight)
{
// TRACK_GUARD uses the true optical centre of
// the 376-pixel binned image (x=188). The MV
// map merely omits x=368..375; cropping does
// not move the camera principal point.
const S16 trackRx = (S16)(cxp - (S16)RETINA3_TRACK_GUARD_CENTER_X_PX);
const S16 trackRy = (S16)(cyp - (S16)RETINA3_TRACK_GUARD_CENTER_Y_PX);
const S32 trackRadial = (S32)vx2 * (S32)trackRx +
(S32)vy2 * (S32)trackRy;
const unsigned long long trackRadialAbs =
(trackRadial >= 0) ? (unsigned long long)trackRadial :
(unsigned long long)(-trackRadial);
if(cxp < (S16)RETINA3_TRACK_GUARD_CENTER_X_PX)
{
++mvTrackLeftCount;
mvTrackLeftMagSum += mag2;
mvTrackLeftX2Sum += vx2;
mvTrackLeftY2Sum += vy2;
mvTrackLeftRadialSignedSum += trackRadial;
mvTrackLeftRadialAbsSum += trackRadialAbs;
}
else
{
++mvTrackRightCount;
mvTrackRightMagSum += mag2;
mvTrackRightX2Sum += vx2;
mvTrackRightY2Sum += vy2;
mvTrackRightRadialSignedSum += trackRadial;
mvTrackRightRadialAbsSum += trackRadialAbs;
}
}
}
// Upper BODY/HEIGHT guard. This is the
// projection of the 160mm safety roof plane using
// the same 200mm corridor. It lives in the upper
// rows and catches suspended crossbars/shelves that
// need not touch the ground or TRACK_GUARD.
{
const U16 guardLeft = heightGuardLeftRow[my];
const U16 guardRight = heightGuardRightRow[my];
if(heightGuardValidRow[my] &&
cxp >= (S16)guardLeft && cxp <= (S16)guardRight)
{
const S16 bodyRx = (S16)(cxp - (S16)RETINA3_TRACK_GUARD_CENTER_X_PX);
const S16 bodyRy = (S16)(cyp - (S16)RETINA3_TRACK_GUARD_CENTER_Y_PX);
const S32 bodyRadial = (S32)vx2 * (S32)bodyRx +
(S32)vy2 * (S32)bodyRy;
const unsigned long long bodyRadialAbs =
(bodyRadial >= 0) ? (unsigned long long)bodyRadial :
(unsigned long long)(-bodyRadial);
if(cxp < (S16)RETINA3_TRACK_GUARD_CENTER_X_PX)
{
++mvHeightLeftCount;
mvHeightLeftMagSum += mag2;
mvHeightLeftX2Sum += vx2;
mvHeightLeftY2Sum += vy2;
mvHeightLeftRadialSignedSum += bodyRadial;
mvHeightLeftRadialAbsSum += bodyRadialAbs;
}
else
{
++mvHeightRightCount;
mvHeightRightMagSum += mag2;
mvHeightRightX2Sum += vx2;
mvHeightRightY2Sum += vy2;
mvHeightRightRadialSignedSum += bodyRadial;
mvHeightRightRadialAbsSum += bodyRadialAbs;
}
}
}
// V0.18k: obstacle zones and ground zone are
// intentionally NON-OVERLAPPING. The old code
// counted bottom-center floor flow as both CENTER
// and GROUND, so disabling false GROUND stops still
// allowed the same floor vectors to trigger CENTER.
if(my < ((RETINA_MVASM_ROWS * 2U) / 3U))
{
if(mx < (RETINA_MVASM_COLS / 3U))
{
++mvLeftCount;
mvLeftMagSum += mag2;
mvLeftX2Sum += vx2;
mvLeftY2Sum += vy2;
}
else if(mx < ((RETINA_MVASM_COLS * 2U) / 3U))
{
++mvCenterCount;
mvCenterMagSum += mag2;
mvCenterX2Sum += vx2;
mvCenterY2Sum += vy2;
}
else
{
++mvRightCount;
mvRightMagSum += mag2;
mvRightX2Sum += vx2;
mvRightY2Sum += vy2;
}
}
else
{
++mvGroundCount;
mvGroundMagSum += mag2;
mvGroundY2Sum += vy2;
mvGroundX2Sum += vx2;
}
}
}
}
// L1 vector coherence:
// C = (|sum vx| + |sum vy|) / sum(|vx|+|vy|)
// Random best-matches cancel; organized translation survives.
if(mvGlobalMagSum && mvAsmNonZero)
{
U32 result = (U32)((mvGlobalX2Sum < 0 ? -mvGlobalX2Sum : mvGlobalX2Sum) +
(mvGlobalY2Sum < 0 ? -mvGlobalY2Sum : mvGlobalY2Sum));
mvGlobalCohPct = (U32)(((unsigned long long)result * 100ULL) / mvGlobalMagSum);
mvGlobalCohMeanX2 = result / mvAsmNonZero;
}
if(mvLeftMagSum && mvLeftCount)
{
U32 r = (U32)((mvLeftX2Sum < 0 ? -mvLeftX2Sum : mvLeftX2Sum) +
(mvLeftY2Sum < 0 ? -mvLeftY2Sum : mvLeftY2Sum));
mvLeftCohPct = (U32)(((unsigned long long)r * 100ULL) / mvLeftMagSum);
mvLeftCohMeanX2 = r / mvLeftCount;
}
if(mvCenterMagSum && mvCenterCount)
{
U32 r = (U32)((mvCenterX2Sum < 0 ? -mvCenterX2Sum : mvCenterX2Sum) +
(mvCenterY2Sum < 0 ? -mvCenterY2Sum : mvCenterY2Sum));
mvCenterCohPct = (U32)(((unsigned long long)r * 100ULL) / mvCenterMagSum);
mvCenterCohMeanX2 = r / mvCenterCount;
}
if(mvRightMagSum && mvRightCount)
{
U32 r = (U32)((mvRightX2Sum < 0 ? -mvRightX2Sum : mvRightX2Sum) +
(mvRightY2Sum < 0 ? -mvRightY2Sum : mvRightY2Sum));
mvRightCohPct = (U32)(((unsigned long long)r * 100ULL) / mvRightMagSum);
mvRightCohMeanX2 = r / mvRightCount;
}
if(mvGroundMagSum && mvGroundCount)
{
U32 r = (U32)((mvGroundX2Sum < 0 ? -mvGroundX2Sum : mvGroundX2Sum) +
(mvGroundY2Sum < 0 ? -mvGroundY2Sum : mvGroundY2Sum));
mvGroundCohPct = (U32)(((unsigned long long)r * 100ULL) / mvGroundMagSum);
mvGroundCohMeanX2 = r / mvGroundCount;
}
if(mvTrackLeftMagSum && mvTrackLeftCount)
{
U32 r = (U32)((mvTrackLeftX2Sum < 0 ? -mvTrackLeftX2Sum : mvTrackLeftX2Sum) +
(mvTrackLeftY2Sum < 0 ? -mvTrackLeftY2Sum : mvTrackLeftY2Sum));
mvTrackLeftCohPct = (U32)(((unsigned long long)r * 100ULL) / mvTrackLeftMagSum);
mvTrackLeftCohMeanX2 = r / mvTrackLeftCount;
}
if(mvTrackRightMagSum && mvTrackRightCount)
{
U32 r = (U32)((mvTrackRightX2Sum < 0 ? -mvTrackRightX2Sum : mvTrackRightX2Sum) +
(mvTrackRightY2Sum < 0 ? -mvTrackRightY2Sum : mvTrackRightY2Sum));
mvTrackRightCohPct = (U32)(((unsigned long long)r * 100ULL) / mvTrackRightMagSum);
mvTrackRightCohMeanX2 = r / mvTrackRightCount;
}
if(mvTrackLeftRadialAbsSum)
{
unsigned long long ra = (mvTrackLeftRadialSignedSum >= 0) ?
(unsigned long long)mvTrackLeftRadialSignedSum :
(unsigned long long)(-mvTrackLeftRadialSignedSum);
mvTrackLeftRadialCohPct = (U32)((ra * 100ULL) / mvTrackLeftRadialAbsSum);
}
if(mvTrackRightRadialAbsSum)
{
unsigned long long ra = (mvTrackRightRadialSignedSum >= 0) ?
(unsigned long long)mvTrackRightRadialSignedSum :
(unsigned long long)(-mvTrackRightRadialSignedSum);
mvTrackRightRadialCohPct = (U32)((ra * 100ULL) / mvTrackRightRadialAbsSum);
}
if(mvHeightLeftMagSum && mvHeightLeftCount)
{
U32 r = (U32)((mvHeightLeftX2Sum < 0 ? -mvHeightLeftX2Sum : mvHeightLeftX2Sum) +
(mvHeightLeftY2Sum < 0 ? -mvHeightLeftY2Sum : mvHeightLeftY2Sum));
mvHeightLeftCohPct = (U32)(((unsigned long long)r * 100ULL) / mvHeightLeftMagSum);
mvHeightLeftCohMeanX2 = r / mvHeightLeftCount;
}
if(mvHeightRightMagSum && mvHeightRightCount)
{
U32 r = (U32)((mvHeightRightX2Sum < 0 ? -mvHeightRightX2Sum : mvHeightRightX2Sum) +
(mvHeightRightY2Sum < 0 ? -mvHeightRightY2Sum : mvHeightRightY2Sum));
mvHeightRightCohPct = (U32)(((unsigned long long)r * 100ULL) / mvHeightRightMagSum);
mvHeightRightCohMeanX2 = r / mvHeightRightCount;
}
if(mvHeightLeftRadialAbsSum)
{
unsigned long long ra = (mvHeightLeftRadialSignedSum >= 0) ?
(unsigned long long)mvHeightLeftRadialSignedSum :
(unsigned long long)(-mvHeightLeftRadialSignedSum);
mvHeightLeftRadialCohPct = (U32)((ra * 100ULL) / mvHeightLeftRadialAbsSum);
}
if(mvHeightRightRadialAbsSum)
{
unsigned long long ra = (mvHeightRightRadialSignedSum >= 0) ?
(unsigned long long)mvHeightRightRadialSignedSum :
(unsigned long long)(-mvHeightRightRadialSignedSum);
mvHeightRightRadialCohPct = (U32)((ra * 100ULL) / mvHeightRightRadialAbsSum);
}
// Radial coherence is kept separate because looming/expansion can
// have near-zero translational coherence by construction.
if(mvRadialAbsSum)
{
unsigned long long ra =
(mvRadialSignedSum >= 0) ?
(unsigned long long)mvRadialSignedSum :
(unsigned long long)(-mvRadialSignedSum);
mvRadialCohPct = (U32)((ra * 100ULL) / mvRadialAbsSum);
}
// Sanity invariants for 23x15 map.
if((mvLeftCount + mvCenterCount + mvRightCount + mvGroundCount) != mvAsmNonZero)
{
RETINA_EPRINTF("RETINA MVZONE ERROR nz=%lu L=%lu C=%lu R=%lu G=%lu\n",
(unsigned long)mvAsmNonZero, (unsigned long)mvLeftCount,
(unsigned long)mvCenterCount, (unsigned long)mvRightCount,
(unsigned long)mvGroundCount);
if(_retinaLogBuffer)
{
char line[160];
sprintf(line, "MVZONE ERROR frame=%lu nz=%lu L=%lu C=%lu R=%lu G=%lu\n",
(unsigned long)(retinaFrameCounter + 1U),
(unsigned long)mvAsmNonZero, (unsigned long)mvLeftCount,
(unsigned long)mvCenterCount, (unsigned long)mvRightCount,
(unsigned long)mvGroundCount);
retinaRunLogAppend(line);
}
}
}
}
#endif
// Primary navigation decision from ASM vector organization.
// Raw nz/magnitude alone is intentionally NOT enough: low-light
// full-search can produce many random best-vectors.
if(cameraControlChanged)
{
trackPendingMask = 0U;
trackPendingFrames = 0U;
centerPendingFrames = 0U;
driveRequest = DRIVE_REQ_CAUTION;
}
else if(illuminationTransient)
{
trackPendingMask = 0U;
trackPendingFrames = 0U;
centerPendingFrames = 0U;
driveRequest = DRIVE_REQ_CAUTION;
}
else if(rawActivityAvg < RETINA_MVASM_MIN_PHOTO_ACTIVITY)
{
trackPendingMask = 0U;
trackPendingFrames = 0U;
centerPendingFrames = 0U;
// Do not trust an organized _motionvect result unless the frames
// also contain enough real temporal activity. This rejects the
// coherent best-match noise observed in the suspended run.
driveRequest = DRIVE_REQ_FORWARD;
}
else if(mvAsmNonZero < RETINA_MVASM_LOW_MOTION_COUNT)
{
trackPendingMask = 0U;
trackPendingFrames = 0U;
centerPendingFrames = 0U;
// Optical flow disappears when the robot is stationary. LOW MOTION
// must therefore not latch the tracked vehicle at rest forever.
driveRequest = DRIVE_REQ_FORWARD;
}
else
{
const bool leftCoherent =
(mvLeftCount >= RETINA_MVASM_MIN_VALID_ZONE) &&
(mvLeftCohPct >= RETINA_MVASM_ZONE_COH_MIN_PCT) &&
(mvLeftCohMeanX2 >= RETINA_MVASM_COH_MEAN_MIN_X2);
const bool centerCoherent =
(mvCenterCount >= RETINA_MVASM_MIN_VALID_ZONE) &&
(mvCenterCohPct >= RETINA_MVASM_ZONE_COH_MIN_PCT) &&
(mvCenterCohMeanX2 >= RETINA_MVASM_COH_MEAN_MIN_X2);
const bool rightCoherent =
(mvRightCount >= RETINA_MVASM_MIN_VALID_ZONE) &&
(mvRightCohPct >= RETINA_MVASM_ZONE_COH_MIN_PCT) &&
(mvRightCohMeanX2 >= RETINA_MVASM_COH_MEAN_MIN_X2);
const bool groundCoherent =
(mvGroundCount >= RETINA_MVASM_MIN_VALID_ZONE) &&
(mvGroundCohPct >= RETINA_MVASM_ZONE_COH_MIN_PCT) &&
(mvGroundCohMeanX2 >= RETINA_MVASM_COH_MEAN_MIN_X2);
const bool groundEmergency =
groundCoherent &&
(mvGroundCohMeanX2 >= RETINA_GROUND_EMERGENCY_MEAN_X2) &&
(mvRadialCohPct >= RETINA_GROUND_EMERGENCY_RCOH_PCT) &&
(mvGroundCohMeanX2 >= mvCenterCohMeanX2 + RETINA_GROUND_EMERGENCY_MARGIN_X2);
const bool leftHeightCandidate =
(mvHeightLeftCount >= RETINA_HEIGHT_GUARD_MIN_VALID) &&
(mvHeightLeftCohPct >= RETINA_HEIGHT_GUARD_COH_MIN_PCT) &&
(mvHeightLeftRadialCohPct >= RETINA_HEIGHT_GUARD_RADIAL_COH_MIN_PCT) &&
(mvHeightLeftCohMeanX2 >= RETINA_HEIGHT_GUARD_MIN_MEAN_X2);
const bool rightHeightCandidate =
(mvHeightRightCount >= RETINA_HEIGHT_GUARD_MIN_VALID) &&
(mvHeightRightCohPct >= RETINA_HEIGHT_GUARD_COH_MIN_PCT) &&
(mvHeightRightRadialCohPct >= RETINA_HEIGHT_GUARD_RADIAL_COH_MIN_PCT) &&
(mvHeightRightCohMeanX2 >= RETINA_HEIGHT_GUARD_MIN_MEAN_X2);
const bool bothHeightHazard =
(RETINA_HEIGHT_GUARD_ACTIVE != 0U) &&
leftHeightCandidate && rightHeightCandidate;
const bool leftHeightHazard =
(RETINA_HEIGHT_GUARD_ACTIVE != 0U) &&
leftHeightCandidate &&
(!rightHeightCandidate ||
mvHeightLeftCohMeanX2 >= mvHeightRightCohMeanX2 + RETINA_HEIGHT_GUARD_SIDE_MARGIN_X2);
const bool rightHeightHazard =
(RETINA_HEIGHT_GUARD_ACTIVE != 0U) &&
rightHeightCandidate &&
(!leftHeightCandidate ||
mvHeightRightCohMeanX2 >= mvHeightLeftCohMeanX2 + RETINA_HEIGHT_GUARD_SIDE_MARGIN_X2);
const bool leftTrackCandidate =
(mvTrackLeftCount >= RETINA_TRACK_GUARD_MIN_VALID) &&
(mvTrackLeftCohPct >= RETINA_TRACK_GUARD_COH_MIN_PCT) &&
(mvTrackLeftRadialCohPct >= RETINA_TRACK_GUARD_RADIAL_COH_MIN_PCT) &&
(mvTrackLeftCohMeanX2 >= RETINA_TRACK_GUARD_MIN_MEAN_X2);
const bool rightTrackCandidate =
(mvTrackRightCount >= RETINA_TRACK_GUARD_MIN_VALID) &&
(mvTrackRightCohPct >= RETINA_TRACK_GUARD_COH_MIN_PCT) &&
(mvTrackRightRadialCohPct >= RETINA_TRACK_GUARD_RADIAL_COH_MIN_PCT) &&
(mvTrackRightCohMeanX2 >= RETINA_TRACK_GUARD_MIN_MEAN_X2);
// Normal floor flow is present in these same pixels, so a guard is
// active only when it is measurably stronger than the ordinary
// GROUND baseline. A one-sided excess catches the typical case
// observed in the field: the centre clears but one track clips.
const bool bothTrackHazardRaw =
leftTrackCandidate && rightTrackCandidate &&
(mvTrackLeftCohMeanX2 >= mvGroundCohMeanX2 + RETINA_TRACK_GUARD_BOTH_MARGIN_X2) &&
(mvTrackRightCohMeanX2 >= mvGroundCohMeanX2 + RETINA_TRACK_GUARD_BOTH_MARGIN_X2);
const bool leftTrackHazardRaw =
leftTrackCandidate &&
(mvTrackLeftCohMeanX2 >= mvGroundCohMeanX2 + RETINA_TRACK_GUARD_GROUND_MARGIN_X2) &&
(!rightTrackCandidate ||
mvTrackLeftCohMeanX2 >= mvTrackRightCohMeanX2 + RETINA_TRACK_GUARD_SIDE_MARGIN_X2);
const bool rightTrackHazardRaw =
rightTrackCandidate &&
(mvTrackRightCohMeanX2 >= mvGroundCohMeanX2 + RETINA_TRACK_GUARD_GROUND_MARGIN_X2) &&
(!leftTrackCandidate ||
mvTrackRightCohMeanX2 >= mvTrackLeftCohMeanX2 + RETINA_TRACK_GUARD_SIDE_MARGIN_X2);
trackRawHazardMask = bothTrackHazardRaw ? 3U :
(leftTrackHazardRaw ? 1U :
(rightTrackHazardRaw ? 2U : 0U));
trackConfirmedHazardMask = trackRawHazardMask;
// V2.2C: every TRACK hazard, not only the photometric floor case,
// must persist for two frames. Continuity is based on spatial
// overlap of the L/R mask, so a real obstacle can legitimately
// evolve L<->BOTH or R<->BOTH without losing confirmation.
if(trackRawHazardMask != 0U)
{
if(trackPendingMask != 0U &&
(trackPendingMask & trackRawHazardMask) != 0U)
{
if(trackPendingFrames < 255U)
++trackPendingFrames;
}
else
{
trackPendingFrames = 1U;
}
trackPendingMask = trackRawHazardMask;
if(trackPendingFrames < RETINA_TRACK_CONFIRM_FRAMES)
trackConfirmedHazardMask = 0U;
}
else
{
trackPendingMask = 0U;
trackPendingFrames = 0U;
}
const bool centerHazardRaw =
centerCoherent &&
mvCenterCohMeanX2 > ((mvLeftCohMeanX2 + mvRightCohMeanX2) >> 1) + 1U;
if(centerHazardRaw)
{
if(centerPendingFrames < 255U)
++centerPendingFrames;
}
else
{
centerPendingFrames = 0U;
}
const bool centerHazard = centerPendingFrames >= RETINA_CENTER_CONFIRM_FRAMES;
const bool pendingOpticalHazard =
(trackRawHazardMask != 0U && trackConfirmedHazardMask == 0U) ||
(centerHazardRaw && !centerHazard);
// V3.0C: BOOST is traction assistance, never obstacle pushing.
// Candidate guard evidence is enough to inhibit a future boost
// step even before the normal two-frame STOP confirmation.
boostObstacleEvidence =
groundEmergency || leftHeightCandidate || rightHeightCandidate ||
(trackRawHazardMask != 0U) || centerHazardRaw;
const bool bothTrackHazard = (trackConfirmedHazardMask == 3U);
const bool leftTrackHazard = (trackConfirmedHazardMask == 1U);
const bool rightTrackHazard = (trackConfirmedHazardMask == 2U);
if(groundEmergency)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "GROUND";
stopCohPct = mvGroundCohPct;
stopCohMeanX2 = mvGroundCohMeanX2;
}
else if(bothHeightHazard)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "HEIGHT";
stopCohPct = (mvHeightLeftCohPct + mvHeightRightCohPct) >> 1;
stopCohMeanX2 = (mvHeightLeftCohMeanX2 + mvHeightRightCohMeanX2) >> 1;
}
else if(leftHeightHazard)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "HLEFT";
stopCohPct = mvHeightLeftCohPct;
stopCohMeanX2 = mvHeightLeftCohMeanX2;
}
else if(rightHeightHazard)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "HRIGHT";
stopCohPct = mvHeightRightCohPct;
stopCohMeanX2 = mvHeightRightCohMeanX2;
}
else if(bothTrackHazard)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "TRACK";
stopCohPct = (mvTrackLeftCohPct + mvTrackRightCohPct) >> 1;
stopCohMeanX2 = (mvTrackLeftCohMeanX2 + mvTrackRightCohMeanX2) >> 1;
}
else if(leftTrackHazard)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "LTRACK";
stopCohPct = mvTrackLeftCohPct;
stopCohMeanX2 = mvTrackLeftCohMeanX2;
}
else if(rightTrackHazard)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "RTRACK";
stopCohPct = mvTrackRightCohPct;
stopCohMeanX2 = mvTrackRightCohMeanX2;
}
else if(centerHazard)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "CENTER";
stopCohPct = mvCenterCohPct;
stopCohMeanX2 = mvCenterCohMeanX2;
}
else if(pendingOpticalHazard)
{
// First observation of an ordinary optical obstacle: reduce
// authority for one frame, but do not arm STOP/avoidance until
// the evidence persists. Emergency guards above stay immediate.
driveRequest = DRIVE_REQ_CAUTION;
}
else if(leftCoherent &&
mvLeftCohMeanX2 >
(mvRightCohMeanX2 * (100U + RETINA_MVASM_STEER_MARGIN_PCT)) / 100U)
{
driveRequest = DRIVE_REQ_RIGHT;
}
else if(rightCoherent &&
mvRightCohMeanX2 >
(mvLeftCohMeanX2 * (100U + RETINA_MVASM_STEER_MARGIN_PCT)) / 100U)
{
driveRequest = DRIVE_REQ_LEFT;
}
else
{
driveRequest = DRIVE_REQ_FORWARD;
}
}
}
// -----------------------------------------------------------------
// RETINA 3.0C LOCAL MOTION FEEDBACK
// -----------------------------------------------------------------
// No navigation policy lives here. This only verifies local action:
// FORWARD may BOOST if translation is not observed; CLEAR may finish
// early only after verified separation. Every path remains bounded.
bool forwardStallEvent = retina.observeForwardFeedback(
driveRequest,
boostObstacleEvidence,
rawActivityAvg,
mvAsmNonZero,
mvGlobalCohPct,
mvGlobalCohMeanX2,
mvGroundCount,
mvGroundCohPct,
mvGroundCohMeanX2,
mvGroundY2Sum,
mvRadialCohPct,
mvRadialSignedSum);
if(forwardStallEvent && driveRequest != DRIVE_REQ_STOP)
{
driveRequest = DRIVE_REQ_STOP;
stopCause = "STALL";
stopCohPct = mvRadialCohPct;
stopCohMeanX2 = mvGlobalCohMeanX2;
}
// -----------------------------------------------------------------
// RETINA 3.0C MINIMAL DECISION / ESCAPE FSM
// -----------------------------------------------------------------
// Perception above may request STOP, but it never chooses an escape
// policy. A STOP becomes one immutable hazard episode here. Active
// REVERSE/TURN/REFERENCE states merge fresh evidence without restarting;
// a re-hit during CLEAR escalates the same monotonic escape.
bool newStopEvent = false;
U8 hazardReason = RETINA3_REASON_NONE;
U8 hazardMask = RETINA3_HAZARD_BOTH;
U32 hazardLeftScore = mvLeftCohMeanX2 * (mvLeftCohPct + 1U);
U32 hazardRightScore = mvRightCohMeanX2 * (mvRightCohPct + 1U);
if(driveRequest == DRIVE_REQ_STOP)
{
if(stopCause[0] == 'L')
{
hazardMask = RETINA3_HAZARD_LEFT;
hazardReason = RETINA3_REASON_TRACK;
hazardLeftScore += 100000UL;
}
else if(stopCause[0] == 'R')
{
hazardMask = RETINA3_HAZARD_RIGHT;
hazardReason = RETINA3_REASON_TRACK;
hazardRightScore += 100000UL;
}
else if(stopCause[0] == 'H' && stopCause[1] == 'L')
{
hazardMask = RETINA3_HAZARD_LEFT;
hazardReason = RETINA3_REASON_HEIGHT;
hazardLeftScore += 100000UL;
}
else if(stopCause[0] == 'H' && stopCause[1] == 'R')
{
hazardMask = RETINA3_HAZARD_RIGHT;
hazardReason = RETINA3_REASON_HEIGHT;
hazardRightScore += 100000UL;
}
else if(stopCause[0] == 'T')
hazardReason = RETINA3_REASON_TRACK;
else if(stopCause[0] == 'G')
hazardReason = RETINA3_REASON_GROUND;
else if(stopCause[0] == 'H')
hazardReason = RETINA3_REASON_HEIGHT;
else if(stopCause[0] == 'S')
hazardReason = RETINA3_REASON_STALL;
else
hazardReason = RETINA3_REASON_CENTER;
newStopEvent = retina.triggerHazard(hazardReason, hazardMask,
hazardLeftScore, hazardRightScore);
if(newStopEvent)
{
if(hazardMask == RETINA3_HAZARD_LEFT)
statusLed.setHazard(STATUS_HAZARD_LEFT);
else if(hazardMask == RETINA3_HAZARD_RIGHT)
statusLed.setHazard(STATUS_HAZARD_RIGHT);
else
statusLed.setHazard(STATUS_HAZARD_BOTH);
}
}
// Optical flow is used directly to verify that reverse motion really
// occurred. It cannot choose CW/CCW and cannot cancel an escape.
retina.observeMotion(mvAsmNonZero, mvRadialCohPct, mvRadialSignedSum);
t0 = TimerGetCounts(TIMER0);
retina.process(driveRequest);
t1 = TimerGetCounts(TIMER0);
usDrive = ElapsedTimeUs(t0, t1);
const bool referenceResetEvent = retina.takeReferenceReset();
if(referenceResetEvent)
{
trackPendingMask = 0U;
trackPendingFrames = 0U;
centerPendingFrames = 0U;
retinaPreviousValid = false;
retinaGrabToImageA = true;
retinaPrevGain = 0U;
retinaPrevExposure = 0.0;
}
// -------------------------------------------------------------
// V3.1A EVENT / PATH / RAW FLOW + PROBE SCIENCE TELEMETRY
// -------------------------------------------------------------
{
const U32 telemTimeMs = (U32)(runClock.elapsedUs() / 1000ULL);
const U32 telemFrame = retinaFrameCounter + 1U;
const Retina3State telemState = retina.state();
const U16 telemForwardPwm = retina.forwardPwm();
const U32 telemBusRecovery = retina.busRecoveryCount();
bool forcePathNode = false;
if(driveRequest != telemetryPrevDriveRequest)
{
_retinaEventLog.record(RETINA_EVENT_DRIVE_REQUEST, 0U, telemTimeMs, telemFrame,
(S32)driveRequest, (S32)rawActivityAvg, (S32)mvRadialSignedSum);
telemetryPrevDriveRequest = driveRequest;
forcePathNode = true;
}
if(newStopEvent)
{
U16 causeCode = (U16)(U8)stopCause[0];
if(stopCause[1]) causeCode |= (U16)((U16)(U8)stopCause[1] << 8);
_retinaEventLog.record(RETINA_EVENT_STOP, causeCode, telemTimeMs, telemFrame,
(S32)stopCohPct, (S32)stopCohMeanX2, (S32)mvRadialSignedSum);
forcePathNode = true;
}
if(telemState != telemetryPrevState)
{
_retinaEventLog.record(RETINA_EVENT_AVOID_STATE, 0U, telemTimeMs, telemFrame,
(S32)telemetryPrevState, (S32)telemState, (S32)retina.stateFrames());
telemetryPrevState = telemState;
forcePathNode = true;
}
if(retina.tabooMask() != telemetryPrevTaboo)
{
_retinaEventLog.record(RETINA_EVENT_MIN_TABOO, 0U, telemTimeMs, telemFrame,
(S32)telemetryPrevTaboo, (S32)retina.tabooMask(),
(S32)retina.escapeLevel());
telemetryPrevTaboo = retina.tabooMask();
forcePathNode = true;
}
if(retina.escapeCount() != telemetryPrevEscapeCount)
{
_retinaEventLog.record(RETINA_EVENT_MIN_ESCAPE, (U16)retina.lastHazardMask(),
telemTimeMs, telemFrame, (S32)retina.escapeCount(),
retina.escapeTurnCw() ? 1 : -1,
(S32)(retina.escapeTurnTargetDeg() * 10.0f));
telemetryPrevEscapeCount = retina.escapeCount();
forcePathNode = true;
}
if(retina.clearRehitCount() != telemetryPrevClearRehit)
{
_retinaEventLog.record(RETINA_EVENT_MIN_CLEAR_REHIT, 0U,
telemTimeMs, telemFrame, (S32)retina.clearRehitCount(),
(S32)retina.tabooMask(), (S32)retina.escapeLevel());
telemetryPrevClearRehit = retina.clearRehitCount();
forcePathNode = true;
}
if(retina.actionDoneSequence() != telemetryPrevActionDone)
{
_retinaEventLog.record(RETINA_EVENT_MIN_ACTION_DONE,
(U16)retina.lastCompletionReason(),
telemTimeMs, telemFrame,
(S32)retina.lastActionDone(),
retina.lastCompletionValue(),
(S32)telemState);
telemetryPrevActionDone = retina.actionDoneSequence();
forcePathNode = true;
}
if(retina.turnTimeoutSequence() != telemetryPrevTurnTimeout)
{
_retinaEventLog.record(RETINA_EVENT_MIN_TURN_TIMEOUT,
retina.escapeTurnCw() ? 1U : 0U,
telemTimeMs, telemFrame,
retina.lastTurnTimeoutActualX10(),
retina.lastTurnTimeoutTargetX10(),
retina.lastTurnTimeoutResidualX10());
telemetryPrevTurnTimeout = retina.turnTimeoutSequence();
forcePathNode = true;
}
if(retina.stallCount() != telemetryPrevStallCount)
{
_retinaEventLog.record(RETINA_EVENT_STALL, 0U,
telemTimeMs, telemFrame,
(S32)retina.stallCount(),
(S32)RETINA3_PWM_FORWARD_MAX,
(S32)mvRadialSignedSum);
telemetryPrevStallCount = retina.stallCount();
forcePathNode = true;
}
if(telemForwardPwm != telemetryPrevForwardPwm)
{
_retinaEventLog.record(RETINA_EVENT_FORWARD_PWM, 0U, telemTimeMs, telemFrame,
(S32)telemetryPrevForwardPwm, (S32)telemForwardPwm, (S32)rawActivityAvg);
telemetryPrevForwardPwm = telemForwardPwm;
}
if(telemBusRecovery != telemetryPrevBusRecovery)
{
_retinaEventLog.record(RETINA_EVENT_BUS_RECOVERY, 0U, telemTimeMs, telemFrame,
(S32)telemetryPrevBusRecovery, (S32)telemBusRecovery, 0);
telemetryPrevBusRecovery = telemBusRecovery;
forcePathNode = true;
}
if(referenceResetEvent)
{
_retinaEventLog.record(RETINA_EVENT_REFERENCE_RESET, 0U, telemTimeMs, telemFrame,
(S32)telemState, 0, 0);
forcePathNode = true;
}
if(!referenceOnlyFrame && retinaFrameCounter > 0U)
{
RetinaFlowMeta fm;
fm.timeMs = telemTimeMs; fm.frame = telemFrame;
fm.globalX2Sum = mvGlobalX2Sum; fm.globalY2Sum = mvGlobalY2Sum;
fm.groundX2Sum = mvGroundX2Sum; fm.groundY2Sum = mvGroundY2Sum;
fm.radialSignedSum = mvRadialSignedSum;
fm.rawActivity = retinaClampU16(rawActivityAvg);
fm.nonZero = retinaClampU16(mvAsmNonZero);
fm.globalCohPct = retinaClampU16(mvGlobalCohPct);
fm.groundCohPct = retinaClampU16(mvGroundCohPct);
fm.radialCohPct = retinaClampU16(mvRadialCohPct);
fm.leftPwm = retina.driveLeftPwm(); fm.rightPwm = retina.driveRightPwm();
fm.forwardPwm = telemForwardPwm; fm.gain = currentGain;
fm.exposureUs = retinaClampU16((U32)(currentExposure + 0.5));
fm.flags = RETINA_FLOW_FLAG_NONE;
fm.driveRequest = (U8)driveRequest; fm.avoidState = (U8)telemState;
fm.gyroX10 = retinaScale10(retina.gyroX());
fm.gyroY10 = retinaScale10(retina.gyroY());
fm.gyroZ10 = retinaScale10(retina.gyroZ());
fm.yawRateX10 = retinaScale10(retina.gyroRateDps());
fm.headingX10 = retinaScale10(retina.headingDeg());
fm.balanceTrim = 0; fm.yawAxis = -1; fm.yawCwSign = 0;
fm.gyroValid = retina.gyroSampleValid() ? 1U : 0U; fm.reserved = 0U;
fm.cameraTiltPwm = retina.cameraTilt();
fm.guardPitchX100 = retina.trackGuardPitchX100();
fm.trackRawMask = trackRawHazardMask;
fm.trackConfirmedMask = trackConfirmedHazardMask;
fm.reserved2 = 0U;
if(cameraControlChanged) fm.flags |= RETINA_FLOW_FLAG_CAMERA_CONTROL;
if(illuminationTransient) fm.flags |= RETINA_FLOW_FLAG_ILLUM_TRANSIENT;
if(newStopEvent) fm.flags |= RETINA_FLOW_FLAG_STOP;
if(telemState != RETINA3_FORWARD) fm.flags |= RETINA_FLOW_FLAG_AVOID;
if(rawActivityAvg < RETINA_MVASM_MIN_PHOTO_ACTIVITY)
fm.flags |= RETINA_FLOW_FLAG_LOW_ACTIVITY;
const bool flowUsable = !cameraControlChanged && !illuminationTransient &&
rawActivityAvg >= RETINA_MVASM_MIN_PHOTO_ACTIVITY &&
mvAsmNonZero >= RETINA_MVASM_LOW_MOTION_COUNT;
if(flowUsable) fm.flags |= RETINA_FLOW_FLAG_USABLE;
if(flowUsable && driveRequest == DRIVE_REQ_FORWARD &&
telemState == RETINA3_FORWARD && !newStopEvent)
fm.flags |= RETINA_FLOW_FLAG_SAFE_CANDIDATE;
_retinaFlowLog.record(fm, retinaMvMapH, retinaMvMapV);
}
if(forcePathNode || ((telemFrame % RETINA_PATH_SAMPLE_FRAMES) == 0U))
{
RetinaPathNode node;
node.timeMs = telemTimeMs; node.frame = telemFrame;
node.radialSignedSum = mvRadialSignedSum;
node.rawActivity = retinaClampU16(rawActivityAvg);
node.globalCohPct = retinaClampU16(mvGlobalCohPct);
node.groundCohPct = retinaClampU16(mvGroundCohPct);
node.radialCohPct = retinaClampU16(mvRadialCohPct);
node.forwardPwm = telemForwardPwm; node.flags = RETINA_PATH_FLAG_NONE;
node.driveRequest = (U8)driveRequest; node.avoidState = (U8)telemState;
node.headingX10 = retinaScale10(retina.headingDeg());
node.yawRateX10 = retinaScale10(retina.gyroRateDps());
node.balanceTrim = 0; node.yawAxis = -1; node.yawCwSign = 0;
node.gyroValid = retina.gyroSampleValid() ? 1U : 0U; node.reserved = 0U;
if(newStopEvent) node.flags |= RETINA_PATH_FLAG_STOP;
if(telemState != RETINA3_FORWARD) node.flags |= RETINA_PATH_FLAG_AVOID;
if(driveRequest == DRIVE_REQ_REFERENCE) node.flags |= RETINA_PATH_FLAG_REFERENCE;
if(driveRequest == DRIVE_REQ_CAUTION) node.flags |= RETINA_PATH_FLAG_CAUTION;
if(driveRequest == DRIVE_REQ_FORWARD && telemState == RETINA3_FORWARD &&
!newStopEvent && !cameraControlChanged && !illuminationTransient)
node.flags |= RETINA_PATH_FLAG_SAFE_CANDIDATE;
_retinaPathHistory.record(node);
}
// PROBE science capture: synchronized but never authoritative.
retinaProbeRecordImu(telemTimeMs, telemFrame, telemState);
{
const bool flowUsableProbe=!cameraControlChanged&&!illuminationTransient&&rawActivityAvg>=RETINA_MVASM_MIN_PHOTO_ACTIVITY&&mvAsmNonZero>=RETINA_MVASM_LOW_MOTION_COUNT;
const S16 progressMeanX2=mvAsmNonZero?(S16)(mvRadialSignedSum/(S32)mvAsmNonZero):0;
S8 odDir=0;if(telemState==RETINA3_FORWARD||telemState==RETINA3_ESCAPE_CLEAR)odDir=1;else if(telemState==RETINA3_ESCAPE_REVERSE)odDir=-1;
_retinaOdometer.update(telemTimeMs,telemFrame,odDir,telemState==RETINA3_ESCAPE_CLEAR,flowUsableProbe,retinaClampU16(mvAsmNonZero),progressMeanX2,retinaScale10(retina.headingDeg()),telemForwardPwm,(U8)telemState,(U8)driveRequest);
}
}
// Output-only status: a direct rendering of the minimal FSM.
switch(retina.state())
{
case RETINA3_ESCAPE_REVERSE: statusLed.setState(STATUS_LED_REVERSE); break;
case RETINA3_ESCAPE_TURN:
statusLed.setState(retina.escapeTurnCw() ? STATUS_LED_TURN_CW : STATUS_LED_TURN_CCW); break;
case RETINA3_ESCAPE_CLEAR: statusLed.setState(STATUS_LED_FORWARD); break;
case RETINA3_REFERENCE: statusLed.setState(STATUS_LED_REFERENCE); break;
case RETINA3_FAULT: statusLed.setState(STATUS_LED_FAULT); break;
case RETINA3_FORWARD:
default:
if(driveRequest == DRIVE_REQ_LEFT) statusLed.setState(STATUS_LED_STEER_LEFT);
else if(driveRequest == DRIVE_REQ_RIGHT) statusLed.setState(STATUS_LED_STEER_RIGHT);
else if(driveRequest == DRIVE_REQ_CAUTION) statusLed.setState(STATUS_LED_CAUTION);
else if(retina.forwardPwm() > RETINA3_PWM_CRUISE) statusLed.setState(STATUS_LED_BOOST);
else statusLed.setState(STATUS_LED_FORWARD);
break;
}
if(retina.state() == RETINA3_FORWARD) statusLed.setHazard(STATUS_HAZARD_NONE);
statusLed.tick(retinaFrameCounter + 1U);
if(retina.faulted())
{
runControlFault = true;
charLoop = '.';
}
// STOP image capture stays diagnostic-only and entirely in SDRAM.
if(newStopEvent && cur)
{
const U32 eventFrame = retinaFrameCounter + 1U;
U8 *dd = (U8 *)debugImage.data();
U16 ly;
U32 snap0 = TimerGetCounts(TIMER0);
for(ly = 0; ly < logicalH; ++ly)
{
const U16 *ps = cur + ((U32)ly * RETINA_SW_DECIMATION) * (U32)w;
const U16 *psEnd = ps + (U32)logicalW * RETINA_SW_DECIMATION;
while(ps < psEnd)
{
*dd++ = (U8)((*ps & 0x03ffU) >> 2);
ps += RETINA_SW_DECIMATION;
}
}
bool saved = false;
if(stopFrameStorage && stopSnapshotCount < RETINA_STOP_SNAPSHOT_CAPACITY)
{
U8 *dst = stopFrameStorage + stopSnapshotCount * stopFrameBytes;
U8 *src = (U8 *)debugImage.data();
for(U32 si = 0U; si < stopFrameBytes; ++si) dst[si] = src[si];
saved = true; ++stopSnapshotCount;
}
else ++stopSnapshotDropped;
U32 snap1 = TimerGetCounts(TIMER0);
U32 snapUs = ElapsedTimeUs(snap0, snap1);
U32 currentSatPct = pixelCount ? (satExactCount * 100U) / pixelCount : 0U;
U32 clipHiPct = pixelCount ? (clipHighCount * 100U) / pixelCount : 0U;
if(_retinaLogBuffer)
{
char line[512];
sprintf(line,
"STOP #%lu %s C=%lu%% mean=%lu GCOH=%lu%% RCOH=%lu%% raw=%lu clipH=%lu%% SAT=%lu%% EXP=%.0f G=%u avi=%s frame=%lu save=%lu ms FSM=%s turn=%s target=%.0f taboo=0x%02x\n",
(unsigned long)eventFrame, stopCause, (unsigned long)stopCohPct,
(unsigned long)stopCohMeanX2, (unsigned long)mvGlobalCohPct,
(unsigned long)mvRadialCohPct, (unsigned long)rawActivityAvg,
(unsigned long)clipHiPct, (unsigned long)currentSatPct,
currentExposure, currentGain, saved ? "SDRAM" : "DROP",
(unsigned long)stopSnapshotCount, (unsigned long)(snapUs / 1000U),
retina.stateName(), retina.escapeTurnCw() ? "CW" : "CCW",
(double)retina.escapeTurnTargetDeg(), (unsigned int)retina.tabooMask());
retinaRunLogAppend(line);
}
}
// Camera controls are immutable after START CAMCAL.
retinaPrevGain = currentGain;
retinaPrevExposure = currentExposure;
tTotal1 = TimerGetCounts(TIMER0);
++retinaFrameCounter;
{
U32 et = ElapsedTimeUs(tTotal0, tTotal1);
if(_retinaProbeStorage)
{
RetinaProbeRecord pr; _memset(&pr,0,(U32)sizeof(pr));
pr.timeMs=(U32)(runClock.elapsedUs()/1000ULL);pr.frame=retinaFrameCounter;
pr.photoSum=photoSum;pr.photoSumSq=photoSumSq;pr.photoAbsSum=photoAbsSum;pr.photoSignedSum=photoSignedSum;pr.photoScaleQ=photoScaleQ;
pr.globalX2Sum=mvGlobalX2Sum;pr.globalY2Sum=mvGlobalY2Sum;pr.groundX2Sum=mvGroundX2Sum;pr.groundY2Sum=mvGroundY2Sum;pr.radialSignedSum=mvRadialSignedSum;pr.globalMagSum=mvGlobalMagSum;
pr.leftX2Sum=mvLeftX2Sum;pr.leftY2Sum=mvLeftY2Sum;pr.centerX2Sum=mvCenterX2Sum;pr.centerY2Sum=mvCenterY2Sum;pr.rightX2Sum=mvRightX2Sum;pr.rightY2Sum=mvRightY2Sum;pr.groundZoneX2Sum=mvGroundX2Sum;pr.groundZoneY2Sum=mvGroundY2Sum;pr.radialAbsSum=(U32)((mvRadialAbsSum>0xffffffffULL)?0xffffffffUL:mvRadialAbsSum);
pr.rawActivity=retinaClampU16(rawActivityAvg);pr.nonZero=retinaClampU16(mvAsmNonZero);pr.globalCohPct=retinaClampU16(mvGlobalCohPct);pr.globalCohMeanX2=retinaClampU16(mvGlobalCohMeanX2);pr.radialCohPct=retinaClampU16(mvRadialCohPct);
pr.leftCount=retinaClampU16(mvLeftCount);pr.centerCount=retinaClampU16(mvCenterCount);pr.rightCount=retinaClampU16(mvRightCount);pr.groundCount=retinaClampU16(mvGroundCount);pr.leftCohPct=retinaClampU16(mvLeftCohPct);pr.centerCohPct=retinaClampU16(mvCenterCohPct);pr.rightCohPct=retinaClampU16(mvRightCohPct);pr.groundCohPct=retinaClampU16(mvGroundCohPct);
pr.leftCohMeanX2=retinaClampU16(mvLeftCohMeanX2);pr.centerCohMeanX2=retinaClampU16(mvCenterCohMeanX2);pr.rightCohMeanX2=retinaClampU16(mvRightCohMeanX2);pr.groundCohMeanX2=retinaClampU16(mvGroundCohMeanX2);
pr.trackLeftCount=retinaClampU16(mvTrackLeftCount);pr.trackRightCount=retinaClampU16(mvTrackRightCount);pr.heightLeftCount=retinaClampU16(mvHeightLeftCount);pr.heightRightCount=retinaClampU16(mvHeightRightCount);pr.trackLeftCohPct=retinaClampU16(mvTrackLeftCohPct);pr.trackRightCohPct=retinaClampU16(mvTrackRightCohPct);pr.trackLeftRadialCohPct=retinaClampU16(mvTrackLeftRadialCohPct);pr.trackRightRadialCohPct=retinaClampU16(mvTrackRightRadialCohPct);pr.heightLeftCohPct=retinaClampU16(mvHeightLeftCohPct);pr.heightRightCohPct=retinaClampU16(mvHeightRightCohPct);pr.heightLeftRadialCohPct=retinaClampU16(mvHeightLeftRadialCohPct);pr.heightRightRadialCohPct=retinaClampU16(mvHeightRightRadialCohPct);
pr.leftPwm=retina.driveLeftPwm();pr.rightPwm=retina.driveRightPwm();pr.forwardPwm=retina.forwardPwm();pr.gain=currentGain;pr.exposureUs=retinaClampU16((U32)(currentExposure+0.5));pr.photoPixels=retinaClampU16(pixelCount);pr.clipHighCount=retinaClampU16(clipHighCount);pr.clipLowCount=retinaClampU16(clipLowCount);pr.satExactCount=retinaClampU16(satExactCount);pr.photoCohPct=retinaClampU16(photoCoherencePct);
pr.usGrab=retinaClampU16(usGrab);pr.usControl=retinaClampU16(usControl);pr.usPhoto=retinaClampU16(usPhoto);pr.usTileCopy=retinaClampU16(usMvTileCopy);pr.usTileDma=retinaClampU16(usMvTileDma);pr.usTilePack=retinaClampU16(usMvTilePack);pr.usAsm=retinaClampU16(usMvTileAsm);pr.usAggregate=retinaClampU16(usMvTileAggregate);pr.usDrive=retinaClampU16(usDrive);pr.usTotal=retinaClampU16(et);
pr.controlBusBadFrames=retinaClampU16(controlBusBadFrames);pr.busRecoveryCount=retinaClampU16(retina.busRecoveryCount());pr.dmaFallbackCount=retinaClampU16(dmaFallbackCount);pr.stopSnapshots=retinaClampU16(stopSnapshotCount);pr.stopSnapshotDropped=retinaClampU16(stopSnapshotDropped);pr.boostCount=retinaClampU16(retina.boostCount());pr.boostInhibitCount=retinaClampU16(retina.boostInhibitCount());pr.clearVerifiedCount=retinaClampU16(retina.clearVerifiedCount());pr.clearTimeoutCount=retinaClampU16(retina.clearTimeoutCount());pr.reverseVerifiedCount=retinaClampU16(retina.reverseVerifiedCount());pr.reverseTimeoutCount=retinaClampU16(retina.reverseTimeoutCount());
for(U16 phi=0U;phi<RETINA_PROBE_HIST_BINS;++phi)pr.photoHist[phi]=photoHist[phi];pr.driveRequest=(U8)driveRequest;pr.avoidState=(U8)retina.state();pr.trackRawMask=trackRawHazardMask;pr.trackConfirmedMask=trackConfirmedHazardMask;pr.contactActive=retina.frontContactActive()?1U:0U;pr.photoMin=photoMin;pr.photoMax=photoMax;pr.reserved=0U;_retinaProbeLog.record(pr);
}
perfTotalUs += et;
perfGrabUs += usGrab;
perfControlUs += usControl;
perfPhotoUs += usPhoto;
perfTileCopyUs += usMvTileCopy;
perfTileDmaUs += usMvTileDma;
perfTilePackUs += usMvTilePack;
perfAsmUs += usMvTileAsm;
perfScatterUs += usMvTileAggregate;
perfDriveUs += usDrive;
++perfFrames;
if(perfFrames >= RETINA_PERF_EVERY)
{
U32 avgUs = (U32)(perfTotalUs / perfFrames);
F32 fps = avgUs ? (1000000.0f / (F32)avgUs) : 0.0f;
RETINA_VPRINTF("PERF #%lu fps=%.2f avg=%lu us grab=%lu ctrl=%lu photo=%lu tile=%lu dma=%lu pack=%lu asm=%lu scat=%lu drive=%lu fb=%lu busrec=%lu snapshots=%lu sdlast=%lu sdmax=%lu sdbytes=%lu\n",
(unsigned long)retinaFrameCounter, fps, (unsigned long)avgUs,
(unsigned long)(perfGrabUs / perfFrames),
(unsigned long)(perfControlUs / perfFrames),
(unsigned long)(perfPhotoUs / perfFrames),
(unsigned long)(perfTileCopyUs / perfFrames),
(unsigned long)(perfTileDmaUs / perfFrames),
(unsigned long)(perfTilePackUs / perfFrames),
(unsigned long)(perfAsmUs / perfFrames),
(unsigned long)(perfScatterUs / perfFrames),
(unsigned long)(perfDriveUs / perfFrames),
(unsigned long)dmaFallbackCount,
(unsigned long)retina.busRecoveryCount(),
(unsigned long)stopSnapshotCount,
(unsigned long)_retinaLogLastFlushUs,
(unsigned long)_retinaLogMaxFlushUs,
(unsigned long)_retinaLogTotalSdBytes);
if(_retinaLogBuffer)
{
char line[384];
sprintf(line,
"PERF #%lu fps=%.2f avg=%lu us grab=%lu ctrl=%lu photo=%lu tile=%lu dma=%lu pack=%lu asm=%lu scat=%lu drive=%lu fb=%lu busrec=%lu snapshots=%lu sdlast=%lu sdmax=%lu sdbytes=%lu\n",
(unsigned long)retinaFrameCounter, fps, (unsigned long)avgUs,
(unsigned long)(perfGrabUs / perfFrames),
(unsigned long)(perfControlUs / perfFrames),
(unsigned long)(perfPhotoUs / perfFrames),
(unsigned long)(perfTileCopyUs / perfFrames),
(unsigned long)(perfTileDmaUs / perfFrames),
(unsigned long)(perfTilePackUs / perfFrames),
(unsigned long)(perfAsmUs / perfFrames),
(unsigned long)(perfScatterUs / perfFrames),
(unsigned long)(perfDriveUs / perfFrames),
(unsigned long)dmaFallbackCount,
(unsigned long)retina.busRecoveryCount(),
(unsigned long)stopSnapshotCount,
(unsigned long)_retinaLogLastFlushUs,
(unsigned long)_retinaLogMaxFlushUs,
(unsigned long)_retinaLogTotalSdBytes);
retinaRunLogAppend(line);
}
perfTotalUs = perfGrabUs = perfControlUs = perfPhotoUs = perfTileCopyUs = 0ULL;
perfTileDmaUs = perfTilePackUs = 0ULL;
perfAsmUs = perfScatterUs = perfDriveUs = 0ULL;
perfFrames = 0U;
}
}
if(UartCheckChar(UART0))
charLoop = UartGetChar(UART0);
}
// Capture the last partial interval before any shutdown I/O. This is the
// actual wall-clock activity time of the navigation loop.
{
U32 runClockNow = TimerGetCounts(TIMER0);
U32 runDeltaUs = ElapsedTimeUs(runClockPrev, runClockNow);
runClockPrev = runClockNow;
runClock.update(runClockNow, runDeltaUs);
}
// Whether the exit was automatic or requested from UART, stop the tracks
// first. If the timer expires in REVERSE/TURN, the manoeuvre is abandoned
// immediately at the next loop boundary; we never finish an escape action
// after the run deadline.
Retina3State exitState = retina.state();
U16 exitStateFrames = retina.stateFrames();
const char *runStopReason = runControlFault ? "CONTROL_FAULT" :
(runTimedOut ? "TIMEOUT" : "UART");
_retinaLastRunTimedOut = runTimedOut && !runControlFault;
if(runControlFault)
statusLed.setState(STATUS_LED_FAULT);
else if(runTimedOut)
statusLed.setState(STATUS_LED_TIMEOUT);
else
statusLed.setState(STATUS_LED_READY);
statusLed.showNow();
retina.abort();
DmaStop(DMA_CH_0);
DmaStop(DMA_CH_1);
retina.setLight(0U);
if(_retinaLogBuffer)
{
char line[256];
sprintf(line,
"RUN STOP reason=%s elapsed=%lu ms frame=%lu state=%s stateFrames=%u fault=%s\n",
runStopReason,
(unsigned long)(runClock.elapsedUs() / 1000ULL),
(unsigned long)retinaFrameCounter,
retina.stateName(), (unsigned int)exitStateFrames,
runControlFault ? retina.faultName() : "NONE");
retinaRunLogAppend(line);
}
if(runTimedOut)
RETINA_VPRINTF("RETINA RUN TIMEOUT: %lu ms, frame=%lu - motors STOP, clean shutdown\n",
(unsigned long)(runClock.elapsedUs() / 1000ULL), (unsigned long)retinaFrameCounter);
if(runControlFault)
{
RETINA_EPRINTF("RETINA CONTROL FAULT: %s at frame=%lu - motors STOP, clean shutdown\n",
retina.faultName(), (unsigned long)retinaFrameCounter);
_retinaEventLog.record(RETINA_EVENT_CONTROL_FAULT, 0U,
(U32)(runClock.elapsedUs() / 1000ULL), retinaFrameCounter,
(S32)exitState, (S32)exitStateFrames, 0);
}
_retinaEventLog.record(RETINA_EVENT_RUN_STOP, 0U,
(U32)(runClock.elapsedUs() / 1000ULL), retinaFrameCounter,
runControlFault ? 2 : (runTimedOut ? 1 : 0),
(S32)exitState, (S32)stopSnapshotCount);
// V2.0D deferred STOP AVI assembly. We are now outside the realtime
// locomotion path: motors are stopped, DMA is stopped and illumination is off.
if(stopFrameStorage && stopJpeg && stopSnapshotCount)
{
stopAvi = imgFile.openAVI(stopAviName, logicalW, logicalH);
if(stopAvi)
{
for(U32 sf = 0U; sf < stopSnapshotCount; ++sf)
{
U8 *dst = (U8 *)debugImage.data();
const U8 *src = stopFrameStorage + sf * stopFrameBytes;
for(U32 si = 0U; si < stopFrameBytes; ++si)
dst[si] = src[si];
U32 jpegSize = imgFile.jpegCompress(stopJpeg, &debugImage, 0, 4);
if(jpegSize)
{
imgFile.addFrameAVI(stopAvi, stopJpeg, jpegSize);
++stopAviWritten;
}
}
imgFile.setFrameRateAVI(stopAvi, 1.0f);
imgFile.closeAVI(stopAvi);
stopAvi = 0;
}
}
if(stopFrameStorage)
{
_free(stopFrameStorage);
stopFrameStorage = 0;
}
if(stopJpeg)
{
_free(stopJpeg);
stopJpeg = 0;
}
RETINA_VPRINTF("BUS DIAG camGainReadFail=%lu camShutterReadFail=%lu camOutOfRange=%lu gyroReadFail=%lu gyroNotReady=%lu busrec=%lu khz=%u duty=%u\n",
(unsigned long)cameraGainReadFail,
(unsigned long)cameraShutterReadFail,
(unsigned long)cameraReadbackOutOfRange,
(unsigned long)retina.gyroReadFailCount(),
(unsigned long)retina.gyroNotReadyCount(),
(unsigned long)retina.busRecoveryCount(),
(unsigned int)RETINA3_CONTROL_BUS_KHZ,
(unsigned int)RETINA3_CONTROL_BUS_DUTY);
if(_retinaLogBuffer)
{
char line[256];
sprintf(line,
"BUS DIAG camGainReadFail=%lu camShutterReadFail=%lu camOutOfRange=%lu gyroReadFail=%lu gyroNotReady=%lu busrec=%lu khz=%u duty=%u\n",
(unsigned long)cameraGainReadFail,
(unsigned long)cameraShutterReadFail,
(unsigned long)cameraReadbackOutOfRange,
(unsigned long)retina.gyroReadFailCount(),
(unsigned long)retina.gyroNotReadyCount(),
(unsigned long)retina.busRecoveryCount(),
(unsigned int)RETINA3_CONTROL_BUS_KHZ,
(unsigned int)RETINA3_CONTROL_BUS_DUTY);
retinaRunLogAppend(line);
}
if(perfFrames)
{
U32 avgUs = (U32)(perfTotalUs / perfFrames);
F32 fps = avgUs ? (1000000.0f / (F32)avgUs) : 0.0f;
RETINA_VPRINTF("PERF FINAL frames=%lu fps=%.2f avg=%lu us grab=%lu ctrl=%lu photo=%lu tile=%lu dma=%lu pack=%lu asm=%lu scat=%lu drive=%lu fb=%lu busrec=%lu\n",
(unsigned long)perfFrames, fps, (unsigned long)avgUs,
(unsigned long)(perfGrabUs / perfFrames),
(unsigned long)(perfControlUs / perfFrames),
(unsigned long)(perfPhotoUs / perfFrames),
(unsigned long)(perfTileCopyUs / perfFrames),
(unsigned long)(perfTileDmaUs / perfFrames),
(unsigned long)(perfTilePackUs / perfFrames),
(unsigned long)(perfAsmUs / perfFrames),
(unsigned long)(perfScatterUs / perfFrames),
(unsigned long)(perfDriveUs / perfFrames),
(unsigned long)dmaFallbackCount,
(unsigned long)retina.busRecoveryCount());
if(_retinaLogBuffer)
{
char line[320];
sprintf(line,
"PERF FINAL frames=%lu fps=%.2f avg=%lu us grab=%lu ctrl=%lu photo=%lu tile=%lu dma=%lu pack=%lu asm=%lu scat=%lu drive=%lu fb=%lu busrec=%lu\n",
(unsigned long)perfFrames, fps, (unsigned long)avgUs,
(unsigned long)(perfGrabUs / perfFrames),
(unsigned long)(perfControlUs / perfFrames),
(unsigned long)(perfPhotoUs / perfFrames),
(unsigned long)(perfTileCopyUs / perfFrames),
(unsigned long)(perfTileDmaUs / perfFrames),
(unsigned long)(perfTilePackUs / perfFrames),
(unsigned long)(perfAsmUs / perfFrames),
(unsigned long)(perfScatterUs / perfFrames),
(unsigned long)(perfDriveUs / perfFrames),
(unsigned long)dmaFallbackCount,
(unsigned long)retina.busRecoveryCount());
retinaRunLogAppend(line);
}
}
RETINA_VPRINTF("RETINA stopped at frame %lu, STOP frames=%lu, reason=%s, elapsed=%lu ms, LEDs=OFF\n",
(unsigned long)retinaFrameCounter, (unsigned long)stopSnapshotCount,
runStopReason,
(unsigned long)(runClock.elapsedUs() / 1000ULL));
if(_retinaLogBuffer)
{
char line[768];
sprintf(line,
"RUN END reason=%s elapsed=%lu ms frame=%lu STOP=%lu escapes=%lu clearRehit=%lu frontContact=%lu reverseVerified=%lu reverseTimeout=%lu turnVerified=%lu turnTimeout=%lu turnDegraded=%lu clearVerified=%lu clearTimeout=%lu boost=%lu boostInhibit=%lu stall=%lu busrec=%lu LED=OFF runtimeCheckpoints=%lu sdmax=%lu us logbytes=%lu sdwritten=%lu errors=%lu overflow=%u stopBuffered=%lu stopDropped=%lu stopAviWritten=%lu realtimeSdWrites=0\n",
runStopReason,
(unsigned long)(runClock.elapsedUs() / 1000ULL),
(unsigned long)retinaFrameCounter,
(unsigned long)stopSnapshotCount,
(unsigned long)retina.escapeCount(),
(unsigned long)retina.clearRehitCount(),
(unsigned long)_retinaFrontContactCountRun,
(unsigned long)retina.reverseVerifiedCount(),
(unsigned long)retina.reverseTimeoutCount(),
(unsigned long)retina.turnVerifiedCount(),
(unsigned long)retina.turnTimeoutCount(),
(unsigned long)retina.turnDegradedCount(),
(unsigned long)retina.clearVerifiedCount(),
(unsigned long)retina.clearTimeoutCount(),
(unsigned long)retina.boostCount(),
(unsigned long)retina.boostInhibitCount(),
(unsigned long)retina.stallCount(),
(unsigned long)retina.busRecoveryCount(),
(unsigned long)_retinaLogCheckpointCount,
(unsigned long)_retinaLogMaxFlushUs,
(unsigned long)_retinaLogUsed,
(unsigned long)_retinaLogTotalSdBytes,
(unsigned long)_retinaLogErrorCount,
_retinaLogOverflow ? 1U : 0U,
(unsigned long)stopSnapshotCount,
(unsigned long)stopSnapshotDropped,
(unsigned long)stopAviWritten);
retinaRunLogAppend(line);
sprintf(line,
"MINIMAL3 active=1 controller=FSM states=FORWARD/REVERSE/TURN/CLEAR/REFERENCE/FAULT escapes=%lu clearRehit=%lu contacts=%lu reverseVerified=%lu reverseTimeout=%lu turnVerified=%lu turnTimeout=%lu turnDegraded=%lu clearVerified=%lu clearTimeout=%lu boost=%lu boostInhibit=%lu stall=%lu taboo=0x%02x finalState=%s cameraTilt=%u postStopCal=0 topo=0 epi=0 soul=0 nav=0 planner=0 trap=0 backtrack=0 gaze=0\n",
(unsigned long)retina.escapeCount(),
(unsigned long)retina.clearRehitCount(),
(unsigned long)retina.contactCount(),
(unsigned long)retina.reverseVerifiedCount(),
(unsigned long)retina.reverseTimeoutCount(),
(unsigned long)retina.turnVerifiedCount(),
(unsigned long)retina.turnTimeoutCount(),
(unsigned long)retina.turnDegradedCount(),
(unsigned long)retina.clearVerifiedCount(),
(unsigned long)retina.clearTimeoutCount(),
(unsigned long)retina.boostCount(),
(unsigned long)retina.boostInhibitCount(),
(unsigned long)retina.stallCount(),
(unsigned int)retina.tabooMask(), retina.stateName(),
(unsigned int)retina.cameraTilt());
retinaRunLogAppend(line);
sprintf(line,
"TELEMETRY event=%lu/%lu overwrite=%lu file=%s path=%lu/%lu overwrite=%lu file=%s sampleEvery=%u flow=%lu/%lu overwrite=%lu file=%s rec=%lu bytes\n",
(unsigned long)_retinaEventLog.count(),
(unsigned long)_retinaEventLog.total(),
(unsigned long)_retinaEventLog.overwritten(), _retinaEventName,
(unsigned long)_retinaPathHistory.count(),
(unsigned long)_retinaPathHistory.total(),
(unsigned long)_retinaPathHistory.overwritten(), _retinaPathName,
(unsigned int)RETINA_PATH_SAMPLE_FRAMES,
(unsigned long)_retinaFlowLog.count(),
(unsigned long)_retinaFlowLog.total(),
(unsigned long)_retinaFlowLog.overwritten(), _retinaFlowName,
(unsigned long)sizeof(RetinaFlowRecord));
retinaRunLogAppend(line);
sprintf(line,
"ARCH V3 tag=%s acquisition=%lu maxGrab=%lu controllerBytes=%lu bus=%u/%u\n",
RETINA_ARCH_TAG,
(unsigned long)_retinaAcquisitionStage.frames(),
(unsigned long)_retinaAcquisitionStage.maxGrabUs(),
(unsigned long)sizeof(otRetinaMinimal),
(unsigned int)RETINA3_CONTROL_BUS_KHZ,
(unsigned int)RETINA3_CONTROL_BUS_DUTY);
retinaRunLogAppend(line);
// First final checkpoint now; retinaRunLogClose() writes the opposite
// slot once more, so a graceful stop leaves both files complete.
retinaRunLogFlush();
RETINA_VPRINTF("RETINA SDLOG: run=%04lu pair=%s/%s avi=%s log=%lu bytes checkpoints=%lu max=%lu us errors=%lu overflow=%u\n",
(unsigned long)_retinaRunIndex, _retinaLogNameA, _retinaLogNameB, _retinaStopAviName,
(unsigned long)_retinaLogUsed,
(unsigned long)_retinaLogCheckpointCount,
(unsigned long)_retinaLogMaxFlushUs,
(unsigned long)_retinaLogErrorCount,
_retinaLogOverflow ? 1U : 0U);
}
// Binary telemetry is written only after motors/DMA/AVI have been stopped;
// therefore V1.5 adds no SD traffic to the realtime navigation loop.
RETINA_EPRINTF("PROBE END IMU=%lu ODO=%lu PROB=%lu odom=(%ld,%ld) distQ=%lu mag=%s\n",(unsigned long)_retinaImuLog.total(),(unsigned long)_retinaOdometer.total(),(unsigned long)_retinaProbeLog.total(),(long)_retinaOdometer.xQ(),(long)_retinaOdometer.yQ(),(unsigned long)_retinaOdometer.distanceQ(),_retinaProbeMagPresent?"YES":"NO");
retinaTelemetryDump();
retinaTelemetryClose();
retinaRunLogClose();
}
// V3.1A public run entry. V3.0C navigation is frozen; science plane is passive.
void retinaRun()
{
retinaAutonomicCoreRun();
}
void setup()
{
U16 w, h;
UartEnableSignals(UART0);
UartSetBaud(UART0, 115200);
setStdout(4096, UART0);
RtcOn();
// This test has been written in this date
//RtcSetDate(2026, 8, 22);
//RtcSetTime(9,39, 0);
TimerSetAsCounter(TIMER0);
TimerEnable(TIMER0, true);
FpgaInit();
FpgaHiddenPort(HP_EXT_I2C, true);
I2cBusReset();
I2cBusFrequency(RETINA3_CONTROL_BUS_KHZ, RETINA3_CONTROL_BUS_DUTY);
PortConfig(PP_F, 12, P_OUT); // IR
PortConfig(PP_F, 13, P_OUT); // White
PortConfig(PP_F, 15, P_IN); // PF15 bumper / run trigger, active LOW
PortSet(PP_F, 12, true);
PortSet(PP_F, 13, true);
// 10-pixel WS2812B field status bar on the otherwise unused PF14.
// The waveform is copied from the proven otWS2812 16x16 implementation.
if(!statusLed.init(14U, 10U))
{
RETINA_EPRINTF("Error on WS2812 status init\n");
IDLE;
}
statusLed.setBrightness(24U);
statusLed.setState(STATUS_LED_READY);
statusLed.showNow();
if(pwm.init() == false)
{
RETINA_EPRINTF("Error on PWM init\n");
IDLE;
}
pwm.setPWMFreq(50);
pwm.setOutputMode(true);
// Keep motors in holding torque until otRetinaMinimal::init() takes ownership.
pwm.setPin(15, RETINA3_PWM_HOLD);
pwm.setPin(14, RETINA3_PWM_HOLD);
// Configure the port for the control of all encoders
FpgaPortPxSet( PD_INPUT, PD_INPUT, PD_INPUT, PD_INPUT,
PD_OUTPUT, PD_OUTPUT, PD_OUTPUT, PD_OUTPUT);
FpgaPxPut(0);
camera.init(otCamera::CM_STANDARD);
if(!sensor.init(otImageSensor::IS_MT9V032, otImageSensor::MT9V03x_00))
{
RETINA_EPRINTF("Can't find image sensor!\n");
IDLE;
}
RETINA_VPRINTF("Sensor native/current resolution: %dx%d\n", sensor.horPixel(), sensor.verPixel());
RETINA_VPRINTF("Init SD (persistent field archive)\n");
bool sdReadable = false;
U16 sdAttempt = 0U;
for(sdAttempt = 0U; sdAttempt < RETINA_SD_INIT_RETRIES; ++sdAttempt)
{
if(file.init())
{
sdReadable = true;
break;
}
RETINA_EPRINTF("SD init/read attempt %u/%u failed: %s\n",
(unsigned int)(sdAttempt + 1U),
(unsigned int)RETINA_SD_INIT_RETRIES,
file.status());
// Reset the wrapper before retrying a transient card/contact failure.
// On the final failure leave the object as-is so the already-tested
// format path can attempt to recover the unreadable filesystem.
if((sdAttempt + 1U) < RETINA_SD_INIT_RETRIES)
{
file.end();
DelayMs(250);
}
}
if(!sdReadable)
{
#if RETINA_SD_FORMAT_IF_UNREADABLE
statusLed.setState(STATUS_LED_CAUTION);
statusLed.showNow();
RETINA_EPRINTF("SD filesystem unreadable after %u attempts: FORMAT SD-DISK (destructive recovery)\n",
(unsigned int)RETINA_SD_INIT_RETRIES);
file.setStdout(printf);
file.showWriteActivity(true);
if(!file.format("SD-DISK"))
{
file.showWriteActivity(false);
RETINA_EPRINTF("SD FORMAT ERROR: %s\n", file.status());
statusLed.setState(STATUS_LED_FAULT);
statusLed.showNow();
file.end();
DelayMs(100);
IDLE;
}
file.showWriteActivity(false);
// Verify the newly formatted filesystem through a clean remount.
file.end();
DelayMs(100);
if(!file.init())
{
RETINA_EPRINTF("SD REMOUNT ERROR after format: %s\n", file.status());
statusLed.setState(STATUS_LED_FAULT);
statusLed.showNow();
file.end();
DelayMs(100);
IDLE;
}
sdReadable = true;
statusLed.setState(STATUS_LED_READY);
statusLed.showNow();
RETINA_VPRINTF("SD recovery format complete. Disk free: %lu Kbytes\n",
(unsigned long)file.free());
#else
RETINA_EPRINTF("SD filesystem unreadable and automatic format disabled\n");
statusLed.setState(STATUS_LED_FAULT);
statusLed.showNow();
file.end();
DelayMs(100);
IDLE;
#endif
}
else
{
RETINA_VPRINTF("SD filesystem readable: KEEP existing runs. Disk free: %lu Kbytes\n",
(unsigned long)file.free());
}
file.setStdout(printf);
// Set I/O access
imgFile.init(&file);
imu.init();
if(imu.check())
{
statusLed.setState(STATUS_LED_CALIBRATE);
statusLed.showNow();
imuCalibrate();
statusLed.setState(STATUS_LED_READY);
statusLed.showNow();
}
else
{
RETINA_EPRINTF("Can't find MPU9250\n");
IDLE;
}
// Keep MT9V032 2x2 hardware binning; V1.5 retains fully manual camera control.
sensor.setBinning(2, 2);
// V1.5 controller: one owner for motors, gyro drive assistance, pivot, light and tilt.
if(!retina.init(&pwm, &imu, &sensor))
{
RETINA_EPRINTF("Can't initialize otRetinaMinimal controller!\n");
IDLE;
}
retina.setLogger(retinaControlLogSink);
w = sensor.horPixel();
h = sensor.verPixel();
RETINA_VPRINTF("Retina sensor resolution, hardware binning 2x2: %dx%d\n", w, h);
RETINA_VPRINTF("Retina software decimation: %dx%d -> logical %dx%d samples\n",
RETINA_SW_DECIMATION, RETINA_SW_DECIMATION,
w / RETINA_SW_DECIMATION, h / RETINA_SW_DECIMATION);
RETINA_VPRINTF("Retina acquisition: native MT9V032 10-bit via U16 DMA; retina uses D9..D2 -> U8\n");
if(!image.create(w, h, 1, otImage::IMG_U16))
{
RETINA_EPRINTF("Can't create current retina image\n");
IDLE;
}
if(!previousImage.create(w, h, 1, otImage::IMG_U16))
{
RETINA_EPRINTF("Can't create previous retina image\n");
IDLE;
}
// STOP-event buffer: acquisition remains 10-bit in U16; only on a fresh
// STOP do we build a 376x240 D9..D2 U8 frame and append it to this run's STOPnnnn.AVI.
if(!debugImage.create(w / RETINA_SW_DECIMATION, h / RETINA_SW_DECIMATION, 1, otImage::IMG_U8))
{
RETINA_EPRINTF("Can't create U8 STOP snapshot image\n");
IDLE;
}
if((w / RETINA_CELL_W) > RETINA_MAX_COLS || (h / RETINA_CELL_H) > RETINA_MAX_ROWS)
{
RETINA_EPRINTF("Retina map too small for %dx%d image\n", w, h);
IDLE;
}
// PF15 stays in ordinary level mode during setup. This is intentional:
// PortGet is a true physical-level read only before EDGE/POLAR are armed.
// The falling-edge IRQ is enabled immediately before locomotion starts.
retinaPf15LevelMode();
_retinaPf15FallingSeq = 0U;
RETINA_EPRINTF("PF15 input ready: physical level=%s; IRQ will arm at run start\n",
PortGet(PP_F, RETINA_PF15_PIN) ? "HIGH" : "LOW");
}
void loop()
{
#if MOTOR_DIAGNOSTIC
U8 com = 0;
RETINA_VPRINTF("Motor/light diagnose started: 0..3 motion, .=stop, I/W/X light, +/- tilt, R=RETINA\n");
while(com != 'R')
{
if(UartCheckChar(UART0))
{
com = UartGetChar(UART0);
switch(com)
{
case '0':
case '1':
case '2':
case '3':
retina.diagnosticMove(com - '0', RETINA3_PWM_CRUISE, RETINA3_PWM_CRUISE);
break;
case '.':
retina.stop();
break;
case 'I':
retina.setLight(2U);
break;
case 'W':
retina.setLight(1U);
break;
case 'X':
retina.setLight(0U);
break;
// 200 is approx 120 degree
// 295 is parallel to ground
// 382 is 45 degree to ground
case '+':
if(retina.cameraTilt() < 4095U)
retina.setCameraTilt((U16)(retina.cameraTilt() + 1U));
RETINA_VPRINTF("%u\n", (unsigned int)retina.cameraTilt());
break;
case '-':
if(retina.cameraTilt() > 0U)
retina.setCameraTilt((U16)(retina.cameraTilt() - 1U));
RETINA_VPRINTF("%u\n", (unsigned int)retina.cameraTilt());
break;
case 'R':
retina.stop();
break;
}
}
}
#endif
// First run starts automatically as before. After a clean timed RUN
// TIMEOUT (600 s in this field configuration), PF15 changes context: release all parallel switches, then a new
// LOW level (operator press) requests the next run. Post-run input is polled
// in plain GPIO mode; the falling-edge IRQ is reserved for active-run safety.
// A control fault or UART stop does not arm another locomotion cycle.
retina.stop();
_retinaRunStartedByPf15 = false;
for(;;)
{
retinaRun();
retina.stop();
retinaPf15LevelMode();
if(!_retinaLastRunTimedOut)
break;
statusLed.setHazard(STATUS_HAZARD_NONE);
statusLed.setState(STATUS_LED_READY);
statusLed.showNow();
RETINA_VPRINTF("RUN COMPLETE: release PF15 bumper array; next press starts a new %lu s run ('.' -> idle)\n",
(unsigned long)RETINA_RUN_DURATION_SECONDS);
// Return PF15 to ordinary level mode before asking PortGet for release.
// In edge mode PortGet is the event latch, not the physical switch level.
if(!retinaPf15WaitReleased(true))
break;
// Post-run operator trigger deliberately uses ordinary GPIO level mode,
// not the interrupt. Human-command latency is irrelevant here and this
// prevents any stale/arming edge latch from being mistaken for a new run.
RETINA_VPRINTF("PF15 START READY: press any bumper switch for next run\n");
bool startRequested = false;
bool leaveIdle = false;
while(!startRequested && !leaveIdle)
{
if(UartCheckChar(UART0) && UartGetChar(UART0) == '.')
{
leaveIdle = true;
break;
}
if(retinaPf15PressedLevel())
startRequested = true;
else
DelayMs(10);
}
if(leaveIdle)
break;
// A manual press has been seen as a real LOW level. Require a clean
// release before the next run; retinaAutonomicCoreRun() will then arm
// the falling-edge IRQ again for high-priority contact safety.
++_retinaPf15RunTriggerCount;
RETINA_VPRINTF("PF15 START accepted: release switch...\n");
if(!retinaPf15WaitReleased(false))
break;
_retinaRunStartedByPf15 = true;
RETINA_VPRINTF("PF15 released: starting next run\n");
}
retina.stop();
IDLE;
}