Skip to content

Add BodyEmulator sketch; refactor shared code into lib/ #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
jenogc wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

Add BodyEmulator sketch; refactor shared code into lib/ #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
371 changes: 371 additions & 0 deletions BodyEmulator/BodyEmulator.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,371 @@
/**
* BodyEmulator.ino - Sony E-Mount camera body emulator (Teensy 3.5)
*
* Wiring:
* PIN0 (Serial1 RX) - LENS7 lens->body data (read lens responses)
* PIN10 (Serial2 TX) - BODY7 body->lens data (send body commands)
* PIN2 - LENS_CS_BODY INPUT monitor lens chip-select
* PIN3 - BODY_CS_LENS OUTPUT drive body chip-select
* PIN4 - BODY_VD_LENS OUTPUT generate VD clock
*
* Protocol flow:
* 1. Body generates VD pulses at VD_FREQUENCY_HZ via IntervalTimer.
* 2. Each VD window: pull BODY_CS_LENS LOW, send a message on Serial2 TX,
* wait for lens response on Serial1 RX, pull BODY_CS_LENS HIGH.
* 3. Init sequence sends 0x01, 0x07..0x10; each lens response advances
* the state machine.
* 4. Normal mode: periodic 0x04 queries; optional 0x03 AF/aperture commands
* queued via requestAF() / requestAperture().
*/

#include "Config.h"
#include "../lib/Constants.h"
#include "../lib/DebugTools.h"
#include "../lib/Message.h"
#include "../lib/Message05.h"

// ---------------------------------------------------------------------------
// State machine
// ---------------------------------------------------------------------------

enum BodyState {
STATE_INIT_CS_PULSE1, // first CS low/high edge (handshake beat 1)
STATE_INIT_CS_PULSE2, // second CS edge (handshake beat 2)
STATE_SEND_INIT_MSG, // send next init query
STATE_WAIT_INIT_RESPONSE, // wait for lens init response
STATE_NORMAL // normal: periodic 0x04 queries
};

static BodyState state = STATE_INIT_CS_PULSE1;

// Ordered init exchange table.
// The body sends msgType; expects lens to reply with expectResponse.
struct InitStep { byte msgType; byte expectResponse; };
static const InitStep INIT_STEPS[] = {
{0x01, 0x01}, {0x07, 0x07}, {0x08, 0x08}, {0x09, 0x09},
{0x0A, 0x0A}, {0x0B, 0x0B}, {0x0D, 0x0D}, {0x10, 0x10},
};
static const int NUM_INIT_STEPS = sizeof(INIT_STEPS) / sizeof(INIT_STEPS[0]);
static int initStep = 0;

// ---------------------------------------------------------------------------
// Runtime state
// ---------------------------------------------------------------------------

static boolean highspeedMode = false;
static byte seqNum = INITIAL_SEQUENCE_NUMBER;
static int unusedClockWindows = 0;

static byte lensToBodyBuffer[INPUT_BUFFER_SIZE] = {0};
static int lensToBodyPos = INVALID_POSITION;
static int packetLength = INVALID_POSITION;

static IntervalTimer vdTimer;
static volatile bool vdPending = false;

// Pending outgoing 0x03 command (AF / aperture). Applied next VD window.
static bool pendingCommand = false;
static byte pendingMsg03[23] = {0};

// Last parsed lens data (from 0x05 responses)
static int lastAperture = -1;
static byte lastAFByte21 = 0;
static byte lastAFByte22 = 0;

// ---------------------------------------------------------------------------
// VD clock ISR
// ---------------------------------------------------------------------------

void vdISR() {
digitalWrite(PIN_BODY_VD_LENS, !digitalRead(PIN_BODY_VD_LENS));
vdPending = true;
}

// ---------------------------------------------------------------------------
// CS handshake helpers
// ---------------------------------------------------------------------------

// Pull BODY_CS LOW and wait for lens to assert LENS_CS HIGH (~timeout 5 ms).
bool csStartExchange() {
digitalWrite(PIN_BODY_CS_LENS, LOW);
unsigned long t0 = micros();
while (!digitalRead(PIN_LENS_CS_BODY)) {
if (micros() - t0 > 5000) {
digitalWrite(PIN_BODY_CS_LENS, HIGH);
Serial.println("CS timeout: lens did not assert LENS_CS");
return false;
}
}
return true;
}

void csEndExchange() { digitalWrite(PIN_BODY_CS_LENS, HIGH); }

// ---------------------------------------------------------------------------
// Message send helpers
// ---------------------------------------------------------------------------

void sendBodyMessage(byte msgClass, byte seq, byte msgType,
const byte *body, int bodyLen) {
Message m(msgClass, seq, msgType, body, bodyLen);
m.prepForSending();
writeSerialDebugable(Serial2, m.outputBuffer, m.wireLength);
Serial2.flush();
if (DEBUG) flushDebugOutputBuffer("Body->Lens ");
}

void sendInitMessage(byte msgType) {
const byte emptyBody[1] = {0};
sendBodyMessage(MESSAGE_CLASS_INIT, 0, msgType, emptyBody, 1);
}

void sendQuery04() {
const byte emptyBody[4] = {0};
sendBodyMessage(MESSAGE_CLASS_NORMAL, seqNum++, 0x04, emptyBody, sizeof(emptyBody));
}

void sendCommand03(const byte *body, int bodyLen) {
sendBodyMessage(MESSAGE_CLASS_NORMAL, seqNum++, 0x03, body, bodyLen);
}

// ---------------------------------------------------------------------------
// Public lens-control API
// ---------------------------------------------------------------------------

/**
* Queue an AF focus position request.
* position is a 16-bit value; low byte -> body[21], high byte -> body[22]
* in the outgoing 0x03 message. Applied during the next VD window.
*/
void requestAF(uint16_t position) {
memset(pendingMsg03, 0, sizeof(pendingMsg03));
pendingMsg03[21] = position & 0xFF;
pendingMsg03[22] = (position >> 8) & 0xFF;
pendingCommand = true;
Serial.print("AF queued: ");
Serial.println(position);
}

/**
* Queue an aperture change request. value is a 16-bit aperture code.
* TODO: confirm exact byte offsets from packet captures; currently uses
* bytes [0] and [1] as a placeholder.
*/
void requestAperture(uint16_t value) {
memset(pendingMsg03, 0, sizeof(pendingMsg03));
pendingMsg03[0] = value & 0xFF;
pendingMsg03[1] = (value >> 8) & 0xFF;
pendingCommand = true;
Serial.print("Aperture queued: ");
Serial.println(value);
}

// ---------------------------------------------------------------------------
// Receive and parse lens->body responses
// ---------------------------------------------------------------------------

void processLensMessage(Message *msg) {
unusedClockWindows = 0;

if (DEBUG) {
Serial.print(LENS_TO_BODY);
printHexBuffer(msg->outputBuffer, msg->wireLength);
Serial.print(' ');
Serial.println(micros());
}

switch (msg->messageType) {

// Init responses: check expected type and advance state machine.
case 0x01: case 0x07: case 0x08: case 0x09:
case 0x0A: case 0x0B: case 0x0D: case 0x10:
if (state == STATE_WAIT_INIT_RESPONSE &&
msg->messageType == INIT_STEPS[initStep].expectResponse) {
initStep++;
if (initStep >= NUM_INIT_STEPS) {
state = STATE_NORMAL;
Serial.println(INIT_COMPLETE_MSG);
} else {
state = STATE_SEND_INIT_MSG;
}
}
break;

// 0x05: primary lens data - aperture value, AF position bytes.
case 0x05:
if (msg->bodyLength > 31) {
lastAperture = (msg->body[31] << 8) | msg->body[30];
Serial.print("Aperture: ");
Serial.println(lastAperture);
}
if (msg->bodyLength > 78) {
lastAFByte21 = msg->body[77];
lastAFByte22 = msg->body[78];
Serial.print("AF bytes: 0x");
Serial.print(lastAFByte21, HEX);
Serial.print(" 0x");
Serial.println(lastAFByte22, HEX);
}
break;

// 0x06: secondary lens data - log raw bytes.
case 0x06:
if (DEBUG) {
Serial.print("0x06 body: ");
printHexBuffer(msg->body, msg->bodyLength);
Serial.println();
}
break;

default:
Serial.print("Unknown lens msg 0x");
Serial.println(msg->messageType, HEX);
break;
}
}

void receiveFromLens() {
while (Serial1.available() > 0) {
int b = Serial1.read();
if (lensToBodyPos == INVALID_POSITION) {
if (b == START_BYTE) lensToBodyPos = 0;
else continue;
}
lensToBodyBuffer[lensToBodyPos] = b;
lensToBodyPos++;
if (lensToBodyPos >= INPUT_BUFFER_SIZE) { lensToBodyPos = INVALID_POSITION; continue; }
if (lensToBodyPos == 3) packetLength = (lensToBodyBuffer[2] << 8) | lensToBodyBuffer[1];
if (packetLength == lensToBodyPos) {
if (b == END_BYTE) {
Message *msg = new Message(lensToBodyBuffer, lensToBodyPos);
processLensMessage(msg);
delete msg;
}
lensToBodyPos = INVALID_POSITION;
}
}
}

// ---------------------------------------------------------------------------
// Speed helpers (re-apply pin assignments on every begin() call)
// ---------------------------------------------------------------------------

void setLowspeedMode() {
if (!highspeedMode) return;
Serial1.setRX(PIN_SERIAL1_RX);
Serial1.setTX(PIN_SERIAL1_TX_LISTEN);
Serial1.begin(750000, SERIAL_8N1);
Serial2.setRX(PIN_SERIAL2_RX);
Serial2.setTX(PIN_SERIAL2_TX);
Serial2.begin(750000, SERIAL_8N1);
highspeedMode = false;
}

void setHighspeedMode() {
if (highspeedMode) return;
Serial1.setRX(PIN_SERIAL1_RX);
Serial1.setTX(PIN_SERIAL1_TX_LISTEN);
Serial1.begin(1500000, SERIAL_8N1);
Serial2.setRX(PIN_SERIAL2_RX);
Serial2.setTX(PIN_SERIAL2_TX);
Serial2.begin(1500000, SERIAL_8N1);
highspeedMode = true;
}

// ---------------------------------------------------------------------------
// Arduino entry points
// ---------------------------------------------------------------------------

void setup() {
Serial.begin(115200);

// Serial1: RX only - reads lens->body responses.
Serial1.setRX(PIN_SERIAL1_RX);
Serial1.setTX(PIN_SERIAL1_TX_LISTEN); // TX unused here; routed to safe pin
Serial1.begin(750000, SERIAL_8N1);

// Serial2: TX for body->lens commands; RX kept for future sniffer mode.
Serial2.setRX(PIN_SERIAL2_RX);
Serial2.setTX(PIN_SERIAL2_TX);
Serial2.begin(750000, SERIAL_8N1);

pinMode(PIN_LENS_CS_BODY, INPUT);
pinMode(PIN_BODY_CS_LENS, OUTPUT);
digitalWrite(PIN_BODY_CS_LENS, HIGH); // idle HIGH

pinMode(PIN_BODY_VD_LENS, OUTPUT);
digitalWrite(PIN_BODY_VD_LENS, LOW);

// Toggle pin at 2x target freq; each toggle = half-period of VD square wave.
vdTimer.begin(vdISR, 1000000UL / (VD_FREQUENCY_HZ * 2));

Serial.println("BodyEmulator starting...");
}

void loop() {
// Drain RX buffer continuously, not just on VD windows.
receiveFromLens();

if (!vdPending) return;
vdPending = false;

switch (state) {

// --- Init handshake beat 1 ------------------------------------------
// Pull CS LOW then HIGH so the lens sees the first CHANGE interrupt.
// The lens emulator expects edge 1 -> waits ~990 us -> drives LENS_CS HIGH.
case STATE_INIT_CS_PULSE1:
digitalWrite(PIN_BODY_CS_LENS, LOW);
delayMicroseconds(1200);
digitalWrite(PIN_BODY_CS_LENS, HIGH);
state = STATE_INIT_CS_PULSE2;
break;

// --- Init handshake beat 2 ------------------------------------------
// Second edge: lens waits ~305 us -> drives LENS_CS LOW -> init done.
case STATE_INIT_CS_PULSE2:
digitalWrite(PIN_BODY_CS_LENS, LOW);
delayMicroseconds(500);
digitalWrite(PIN_BODY_CS_LENS, HIGH);
state = STATE_SEND_INIT_MSG;
break;

// --- Send next init query -------------------------------------------
case STATE_SEND_INIT_MSG:
if (initStep < NUM_INIT_STEPS && csStartExchange()) {
sendInitMessage(INIT_STEPS[initStep].msgType);
csEndExchange();
state = STATE_WAIT_INIT_RESPONSE;
unusedClockWindows = 0;
}
break;

// --- Waiting for lens init response ---------------------------------
// receiveFromLens() -> processLensMessage() advances state.
case STATE_WAIT_INIT_RESPONSE:
unusedClockWindows++;
if (unusedClockWindows > 10) {
Serial.print("Init timeout, retrying step ");
Serial.println(initStep);
unusedClockWindows = 0;
state = STATE_SEND_INIT_MSG;
}
break;

// --- Normal operation -----------------------------------------------
// Each VD window: send pending 0x03 command (if any) then a 0x04 query.
case STATE_NORMAL:
if (!csStartExchange()) break;
if (pendingCommand) {
sendCommand03(pendingMsg03, sizeof(pendingMsg03));
pendingCommand = false;
csEndExchange();
delayMicroseconds(10);
if (csStartExchange()) { sendQuery04(); csEndExchange(); }
} else {
sendQuery04();
csEndExchange();
}
break;
}
}
22 changes: 22 additions & 0 deletions BodyEmulator/Config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
#ifndef Config_h
#define Config_h

/** Print debug information. May introduce latency that affects timing. */
#define DEBUG true

/** Print message timing information. */
#define DEBUG_TIMING true

/**
* VD clock frequency in Hz. A real NEX-7 runs at ~60 Hz.
* Each VD cycle opens one communication window with the lens.
*/
#define VD_FREQUENCY_HZ 60

/**
* Sequence number of the first normal-mode message sent to the lens.
* Incremented with each outgoing normal message.
*/
#define INITIAL_SEQUENCE_NUMBER 1

#endif
Loading