Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1701 → Rev 1702

/C-OSD/arducam-osd/ArduCamOSD_Beta/ArduCamOSD_Beta.pde
0,0 → 1,269
/*
ArduCam OSD - The Arduino based Remote Camera Control and OSD.
 
File : ArduCamOSD_Beta.pde
Version : v0.95, 17 may 2011
 
Author(s): Sandro Benigno
FastSerial and BetterStream from Michael Smith
USB host and PTP library from Oleg Mazurov - circuitsathome.com
 
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
 
#include <FastSerial.h>
 
#undef PROGMEM
#define PROGMEM __attribute__(( section(".progmem.data") ))
 
#undef PSTR
#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];}))
 
#include <inttypes.h>
#include <avr/pgmspace.h>
#include <stdio.h>
 
#include "Spi.h"
#include "Wire.h" //Future i2c comunication. Uncommented to watch hex size
 
/****** USB INCLUDES ******/
#include <Max3421e.h>
#include <Max3421e_constants.h>
#include <Usb.h>
#include <ptp.h>
#include <ptpdebug.h>
#include <canonps.h>
#include <simpletimer.h>
#include "pseventparser.h"
#include "ptpobjinfoparser.h"
 
#define DEV_ADDR 1
//Example -> Canon PowerShot S3 IS
#define DATA_IN_EP 1
#define DATA_OUT_EP 2
#define INTERRUPT_EP 3
#define CONFIG_NUM 1
 
#define MAX_USB_STRING_LEN 64
 
/****** OSD INCLUDES ******/
#include "ArduCam_Max7456.h"
#include "OSD_Vars.h"
 
#define SerPri Serial.print
#define SerPriln Serial.println
#define SerAva Serial.available
#define SerRea Serial.read
 
void setup();
void loop();
 
FastSerialPort0(Serial);
 
/****** CAMSTATES CLASS ******/
class CamStateHandlers : public PSStateHandlers
{
bool stateConnected;
public:
CamStateHandlers() : stateConnected(false) {};
virtual void OnDeviceDisconnectedState(PTP *ptp);
virtual void OnDeviceInitializedState(PTP *ptp);
} CamStates;
/****** END CAMSTATES ******/
 
/****** OBJECTS ******/
OSD osd; //OSD object instance
CanonPS Ps(DEV_ADDR, DATA_IN_EP, DATA_OUT_EP, INTERRUPT_EP, CONFIG_NUM, &CamStates); //Canon PS object instance
SimpleTimer displayTimer, cameraTimer; //Timers
 
/********** HANDLERS**********/
 
void CamStateHandlers::OnDeviceDisconnectedState(PTP *ptp)
{
if (stateConnected)
{
cameraTimer.Disable();
stateConnected = false;
Notify(PSTR("Camera disconnected\r\n"));
}
}
 
void CamStateHandlers::OnDeviceInitializedState(PTP *ptp)
{
if (!stateConnected)
{
Notify(PSTR("Camera connected\r\n"));
stateConnected = true;
cameraTimer.Enable();
}
}
 
/**********END TIMERS HANDLERS******/
 
//Demo vars control (!!! Remember to delete it after APM integration)
float lastGPSUpdate = 0;
float lastBatUpdate = 0;
float lastHeadUpdate = 0;
float headIncrementSignal = 1;
 
void setup()
{
Serial.begin(57600);
pinMode(10, OUTPUT); //usb host CS
pinMode(6, OUTPUT); //OSD CS
unplugSlaves();
 
osd.init();
startPanels();
delay( 200 );
unplugSlaves();
Ps.Setup();
delay( 2000 );
osd.clear();
//Setting Timers
displayTimer.Set(&OnDisplayTimer, 150);
cameraTimer.Set(&OnCameraTimer, 500);
delay( 200 );
 
displayTimer.Enable(); //Starting OSD timer (camera timer depends on camera is attached or not)
}
 
void loop(){
unplugSlaves();
displayTimer.Run();
unplugSlaves();
Ps.Task();
cameraTimer.Run();
}
 
void OnCameraTimer()
{
readSerialCommand();
}
 
void OnDisplayTimer()
{
osd_job();
}
 
void writeOSD() //Interrupt function (ISR)
{
//Serial.println("Interrupt!");
detachInterrupt(0); //It will respect the new aquisition
unplugSlaves();
writePanels();//Check OSD_Panels Tab!
}
 
void osd_job()
{
/*Its the place for data aquisition*/
/*JUST Simulated here as DEMO and test*/
if(millis() > lastGPSUpdate + 100){
lastGPSUpdate = millis();
lat+=0.0001;
lon+=0.002;
hom_dis+=1;
}
if(millis() > lastHeadUpdate + 100){
lastHeadUpdate = millis();
//looping forever
if(head >= 180){
head -= 360;
}else head+=5;
//or ping-pong
/*if(head >= 180) headIncrementSignal = -1;
if(head <= -180) headIncrementSignal = 1;
head += 1.8 * headIncrementSignal;*/
}
if(millis() > lastBatUpdate + 2000){
lastBatUpdate = millis();
alt++;
vel+=5;
bat_val -= 0.1;
}
//Next line will arm OSD function for the next VSync interruption
attachInterrupt(0, writeOSD, FALLING);
}
 
void readSerialCommand() {
char queryType;
if (SerAva()) {
queryType = SerRea();
switch (queryType) {
case 'A': //Activate Capture
Ps.Initialize(true);
//mapping capture target to SD card
Ps.SetDevicePropValue(PS_DPC_CaptureTransferMode, (uint16_t)0x0F);
break;
case 'C': //Capture!!!
Ps.Capture();
break;
case 'D': //Deactivate Capture
Ps.Operation(PS_OC_ViewfinderOff);
Ps.Initialize(false);
break;
case 'L': //Focus Lock
Ps.Operation(PS_OC_FocusLock);
delay(50);
break;
case 'O': //ViewFinder Output. 1 = LCD. 2 = AV.
Ps.SetDevicePropValue(PS_DPC_CameraOutput, (uint16_t)readFloatSerial());
break;
case 'U': //Focus Unlock
Ps.Operation(PS_OC_FocusUnlock);
break;
case 'V': //ViewFinder ON/OFF
if((uint16_t)readFloatSerial() == 0){
Ps.Operation(PS_OC_ViewfinderOff);
}
else Ps.Operation(PS_OC_ViewfinderOn);
break;
case 'X': //Close session forever
Ps.Initialize(false);
break;
case 'Z': //Set Zoom
Ps.SetDevicePropValue(PS_DPC_Zoom, (uint16_t)readFloatSerial());
break;
}
}
}
 
float readFloatSerial() {
byte index = 0;
byte timeout = 0;
char data[128] = "";
 
do {
if (SerAva() == 0) {
delay(10);
timeout++;
}
else {
data[index] = SerRea();
timeout = 0;
index++;
}
}
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
return atof(data);
}
 
void unplugSlaves(){
digitalWrite(10, HIGH); //unplug USB Host
digitalWrite(6, HIGH); //unplug OSD
}
/C-OSD/arducam-osd/ArduCamOSD_Beta/ArduCam_Max7456.cpp
0,0 → 1,160
#include <FastSerial.h>
 
#undef PROGMEM
#define PROGMEM __attribute__(( section(".progmem.data") ))
 
#undef PSTR
#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];}))
 
#include "ArduCam_Max7456.h"
#include "WProgram.h"
#include "Spi.h"
 
volatile int x;
 
OSD::OSD()
{
}
 
//------------------ init ---------------------------------------------------
 
void OSD::init()
{
pinMode(MAX7456_SELECT,OUTPUT);
digitalWrite(MAX7456_SELECT,HIGH); //disable device
pinMode(MAX7456_VSYNC, INPUT);
//digitalWrite(MAX7456_VSYNC,HIGH); //enabling pull-up resistor
// force soft reset on Max7456
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_VM0_reg);
Spi.transfer(MAX7456_RESET);
digitalWrite(MAX7456_SELECT,HIGH);
delay(50);
 
// set all rows to same charactor white level, 90%
digitalWrite(MAX7456_SELECT,LOW);
for (x = 0; x < MAX7456_screen_rows; x++)
{
Spi.transfer(x + 0x10);
Spi.transfer(MAX7456_WHITE_level_120);
}
// define sync (auto,int,ext) and
// making sure the Max7456 is enabled
control(1);
//digitalWrite(MAX7456_SELECT,HIGH);
 
//Serial.println("Initialized!");
}
 
//------------------ plug ---------------------------------------------------
 
void OSD::plug()
{
digitalWrite(MAX7456_SELECT,LOW);
}
 
//------------------ clear ---------------------------------------------------
 
void OSD::clear()
{
// clear the screen
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_DMM_reg);
Spi.transfer(MAX7456_CLEAR_display);
digitalWrite(MAX7456_SELECT,HIGH);
}
 
//------------------ set panel -----------------------------------------------
 
void
OSD::setPanel(uint8_t st_col, uint8_t st_row){
start_col = st_col;
start_row = st_row;
col = st_col;
row = st_row;
//Serial.printf("setPanel -> %d %d %d %d\n", start_col, start_row, max_col, max_row);
}
 
//------------------ open panel ----------------------------------------------
 
void
OSD::openPanel(void){
unsigned int linepos;
byte settings, char_address_hi, char_address_lo;
//find [start address] position
linepos = row*30+col;
// divide 16 bits into hi & lo byte
char_address_hi = linepos >> 8;
char_address_lo = linepos;
 
//Auto increment turn writing fast (less SPI commands).
//No need to set next char address. Just send them
settings = MAX7456_INCREMENT_auto; //To Enable DMM Auto Increment
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_DMM_reg); //dmm
Spi.transfer(settings);
 
Spi.transfer(MAX7456_DMAH_reg); // set start address high
Spi.transfer(char_address_hi);
 
Spi.transfer(MAX7456_DMAL_reg); // set start address low
Spi.transfer(char_address_lo);
//Serial.printf("setPos -> %d %d\n", col, row);
}
 
//------------------ close panel ---------------------------------------------
 
void
OSD::closePanel(void){
Spi.transfer(MAX7456_DMDI_reg);
Spi.transfer(MAX7456_END_string); //This is needed "trick" to finish auto increment
digitalWrite(MAX7456_SELECT,HIGH);
//Serial.println("close");
row++; //only after finish the auto increment the new row will really act as desired
}
 
//------------------ write ---------------------------------------------------
 
void
OSD::write(uint8_t c){
if(c == '|'){
closePanel(); //It does all needed to finish auto increment and change current row
openPanel(); //It does all needed to re-enable auto increment
}
else{
Spi.transfer(MAX7456_DMDI_reg);
Spi.transfer(c);
}
}
 
//------------------
 
void
OSD::control(uint8_t ctrl){
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_VM0_reg);
switch(ctrl){
case 0:
Spi.transfer(MAX7456_DISABLE_display);
break;
case 1:
//Spi.transfer(MAX7456_ENABLE_display_vert | MAX7456_SYNC_internal);
//Spi.transfer(MAX7456_ENABLE_display_vert | MAX7456_SYNC_external);
Spi.transfer(MAX7456_ENABLE_display_vert | MAX7456_SYNC_autosync);
break;
}
digitalWrite(MAX7456_SELECT,HIGH);
}
 
//------------------ pure virtual ones (just overriding) ---------------------
 
int OSD::available(void){
}
int OSD::read(void){
}
int OSD::peek(void){
}
void OSD::flush(void){
}
/C-OSD/arducam-osd/ArduCamOSD_Beta/ArduCam_Max7456.h
0,0 → 1,88
#ifndef ArduCam_Max7456_h
#define ArduCam_Max7456_h
 
/******* FROM DATASHEET *******/
 
//#define isPAL
#define MAX7456_SELECT 6//SS
#define MAX7456_VSYNC 2//INT0
 
#ifdef isPAL
#define MAX7456_MODE 0x40 //PAL mask 10000000
#else
#define MAX7456_MODE 0x00 //NTSC mask 00000000 ("|" will do nothing)
#endif
 
//MAX7456 reg write addresses
#define MAX7456_VM0_reg 0x00
#define MAX7456_VM1_reg 0x01
#define MAX7456_DMM_reg 0x04
#define MAX7456_DMAH_reg 0x05
#define MAX7456_DMAL_reg 0x06
#define MAX7456_DMDI_reg 0x07
#define MAX7456_OSDM_reg 0x0c //not used. Is to set mix
 
//MAX7456 reg write addresses to recording NVM process
#define MAX7456_CMM_reg 0x08
#define MAX7456_CMAH_reg 0x09
#define MAX7456_CMAL_reg 0x0a
#define MAX7456_CMDI_reg 0x0b
#define MAX7456_STAT_reg 0xa0 //0xa[X]
 
//DMM commands
#define MAX7456_CLEAR_display 0x04
#define MAX7456_CLEAR_display_vert 0x06
 
#define MAX7456_INCREMENT_auto 0x03
#define MAX7456_SETBG_local 0x20 //00100000 force local BG to defined brightness level VM1[6:4]
 
#define MAX7456_END_string 0xff
 
//VM0 commands mixed with mode NTSC or PAL mode
#define MAX7456_ENABLE_display (0x08 | MAX7456_MODE) //Don't used. Bad sync. 00001000
#define MAX7456_ENABLE_display_vert (0x0c | MAX7456_MODE) //Much better. Good sync 00001100
#define MAX7456_RESET (0x02 | MAX7456_MODE)
#define MAX7456_DISABLE_display (0x00 | MAX7456_MODE)
//VM0 command modifiers
#define MAX7456_SYNC_autosync 0x10
#define MAX7456_SYNC_internal 0x30
#define MAX7456_SYNC_external 0x20
//VM1 command modifiers
#define MAX7456_WHITE_level_80 0x03
#define MAX7456_WHITE_level_90 0x02
#define MAX7456_WHITE_level_100 0x01
#define MAX7456_WHITE_level_120 0x00
 
//If PAL
#ifdef isPAL
#define MAX7456_screen_size 480
#define MAX7456_screen_rows 15
#else
#define MAX7456_screen_size 390
#define MAX7456_screen_rows 13
#endif
 
//------------------ the OSD class -----------------------------------------------
 
class OSD: public BetterStream
{
public:
OSD(void);
void init(void);
void clear(void);
void plug(void);
void setPanel(uint8_t start_col, uint8_t start_row);
void openPanel(void);
void closePanel(void);
void control(uint8_t ctrl);
virtual int available(void);
virtual int read(void);
virtual int peek(void);
virtual void flush(void);
virtual void write(uint8_t c);
using BetterStream::write;
private:
uint8_t start_col, start_row, col, row;
};
 
#endif
/C-OSD/arducam-osd/ArduCamOSD_Beta/OSD_Panels.pde
0,0 → 1,114
/******* STARTUP PANEL *******/
 
void startPanels(){
osd.clear();
panLogo(10,5);
}
 
/******* PANELS - POSITION *******/
 
void writePanels(){
//osd.clear();
int offset = 2;
#ifdef isPAL
offset = 0;
#endif
//Top = no offset in NTSC
panHome(2,1); //1x2
panBattery(22,1); //7x1
//Bottom = need offset in NTSC
panCenter(13,8-offset); //4x2
panGPS(2,12-offset); //12x3
panCompass(16,12-offset); //13x3
//osd.control(1);
}
 
/******* PANELS - DEFINITION *******/
 
//------------------ Panel: Home ---------------------------------------------------
 
void panHome(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%c%5.0f%c|%c%5.0f%c|%c%5.0f%c",0x1f,hom_dis,0x8d,0x85,alt,0x8d,0x86,vel,0x88);
osd.closePanel();
}
 
//------------------ Panel: Center -------------------------------------------------
 
void panCenter(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%c%c%c%c|%c%c%c%c",0x05,0x03,0x04,0x05,0x15,0x13,0x14,0x15);
osd.closePanel();
}
 
//------------------ Panel: Battery ------------------------------------------------
 
void panBattery(int first_col, int first_line){
char buf_bat[6] = {0xb4,0xb5,0xb6,0xb7,0xb8,0xb9}; //dead, critic, 25%, 50%, 75%, 100%
float critic = bat_cel * 3.0;
float full = bat_cel * 3.7;
float level = round((bat_val - critic) * (5 - 0) / (full - critic) + 0);
level = constrain(level, 0, 5);
if(bat_val < 0) bat_val = 0;
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%4.1f%c%c", bat_val, 0x76, buf_bat[(int)level]);
osd.closePanel();
}
 
//------------------ Panel: Startup ArduCam OSD LOGO -------------------------------
 
void panLogo(int first_col, int first_line){
char buf_logo[][12] = { //11 Null terminated
{0x20,0x20,0x20,0x20,0xba,0xbb,0xbc,0xbd,0xbe},
{0x20,0x20,0x20,0x20,0xca,0xcb,0xcc,0xcd,0xce},
{"ArduCam OSD"}
};
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%s|%s|%s", buf_logo[0], buf_logo[1], buf_logo[2]);
osd.closePanel();
}
 
//------------------ Panel: GPS ---------------------------------------------------
 
void panGPS(int first_col, int first_line){
char buf_lock[3] = {0x10,0x11};
osd.setPanel(first_col, first_line);
osd.openPanel();
switch (gps_lock){
case 0:
osd.printf("%c%c%2.0f %c|%c%s|%c%s", 0x0f, 0xe3,(float)num_sat, buf_lock[0], 0x83, " ---- ", 0x84, " ---- ");
break;
case 1:
osd.printf("%c%c%2.0f %c|%c%11.6f|%c%11.6f", 0x0f, 0xe3,(float)num_sat, buf_lock[1], 0x83, lat, 0x84, lon);
break;
}
osd.closePanel();
}
 
//------------------ Panel: Compass -----------------------------------------------
 
void panCompass(int first_col, int first_line){
char buf_Rule[36] = {0xc2,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0,
0xc4,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0,
0xc3,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0,
0xc5,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0};
char buf_mark[14] = {0x20,0xc0,0xc0,0xc0,0xc0,0xc0,0xc7,0xc0,0xc0,0xc0,0xc0,0xc0,0x20,'\0'};
char buf_show[12];
buf_show[11] = '\0';
int start;
start = round((head * 18)/180);
start -= 5;
if(start < 0) start += 36;
for(int x=0; x <= 10; x++){
buf_show[x] = buf_Rule[start];
if(++start > 35) start = 0;
}
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf(" %c%c%4.0f%c|%s|%c%s%c", 0xc8, 0xc9, head, 0xb0, buf_mark, 0xd0, buf_show, 0xd1);
osd.closePanel();
}
/C-OSD/arducam-osd/ArduCamOSD_Beta/OSD_Vars.h
0,0 → 1,29
#define on 1
#define off 0
 
/*Enabling disabling Panels Switches*/
//Not working yet!!! Just here to remember:
 
int swBat = on;
int swCam = off;
int swDir = off;
int swGPS = on;
int swAlt = off;
int swTim = off;
int swDis = off;
 
/*Panels variables*/
//Will come from APM telem port
 
float bat_val = 12.05; //current voltage
float bat_cel = 3; //number of cells
float lat = 34.178851; //latidude
float lon = -118.501655; //longitude
float hom_dis = 0; //distance from home <-------- !!! (probably must be calculated here)
float head = 170.0; //heading angle = compass
float alt = 0; //altitude <-------- !!! ( Need to know the default unit meters foots?)
float vel = 0; //velocity <-------- !!! ( Need to know the default unit meters/sec kmeters/h milles/h?)
int num_sat = 3; //number of satelites
int gps_lock = 1; //GPS lock... need verify if is boolean or not <--------- !!! ( Remember!!! Very important on startup time!!!)
 
 
/C-OSD/arducam-osd/ArduCamOSD_Beta/Spi.cpp
0,0 → 1,51
#include "WProgram.h"
#include "Spi.h"
 
//---------- constructor ----------------------------------------------------
 
SPI::SPI()
{
// initialize the SPI pins
pinMode(SCK_PIN, OUTPUT);
pinMode(MOSI_PIN, OUTPUT);
pinMode(MISO_PIN, INPUT);
pinMode(SS_PIN, OUTPUT); // <------- !!! (Remember! This pin will select USB host chip Max3421)
 
// enable SPI Master, MSB, SPI mode 0, FOSC/4
mode(0);
}
 
//------------------ mode ---------------------------------------------------
 
void SPI::mode(byte config)
{
byte tmp;
 
// enable SPI master with configuration byte specified
SPCR = 0;
SPCR = (config & 0x7F) | (1<<SPE) | (1<<MSTR);
tmp = SPSR;
tmp = SPDR;
}
 
//------------------ transfer -----------------------------------------------
 
byte SPI::transfer(byte value)
{
SPDR = value;
while (!(SPSR & (1<<SPIF))) ;
return SPDR;
}
 
byte SPI::transfer(byte value, byte period)
{
SPDR = value;
if (period > 0) delayMicroseconds(period);
while (!(SPSR & (1<<SPIF))) ;
return SPDR;
}
 
 
//---------- preinstantiate SPI object --------------------------------------
 
SPI Spi = SPI();
/C-OSD/arducam-osd/ArduCamOSD_Beta/Spi.h
0,0 → 1,22
#ifndef Spi_h
#define Spi_h
 
#include "WProgram.h"
 
#define SCK_PIN 13
#define MISO_PIN 12
#define MOSI_PIN 11
#define SS_PIN 10 // <------- !!! (Remember! This pin will select USB host chip Max3421)
 
class SPI
{
public:
SPI(void);
void mode(byte);
byte transfer(byte);
byte transfer(byte, byte);
};
 
extern SPI Spi;
 
#endif
/C-OSD/arducam-osd/ArduCamOSD_Beta/pseventparser.cpp
0,0 → 1,86
#include "pseventparser.h"
 
void PSEventParser::Parse(const uint16_t len, const uint8_t *pbuf, const uint32_t &offset)
{
uint16_t cntdn = (uint16_t)len;
uint8_t *p = (uint8_t*)pbuf;
 
switch (nStage)
{
case 0:
p += 12;
cntdn -= 12;
 
if (!cntdn)
return;
nStage ++;
 
case 1:
//Notify(PSTR("\r\nEvent Block Size:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 2:
if (!valueParser.Parse(&p, &cntdn))
return;
 
//PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 3:
//Notify(PSTR("\r\nNumber of Fields:\t"));
theBuffer.valueSize = 2;
valueParser.Initialize(&theBuffer);
nStage ++;
case 4:
if (!valueParser.Parse(&p, &cntdn))
return;
 
//PrintHex<uint16_t>(*((uint16_t*)theBuffer.pValue));
nStage ++;
case 5:
//Notify(PSTR("\r\nEvent Code:\t"));
theBuffer.valueSize = 2;
valueParser.Initialize(&theBuffer);
nStage ++;
case 6:
if (!valueParser.Parse(&p, &cntdn))
return;
 
eventCode = *((uint16_t*)theBuffer.pValue);
//PrintHex<uint16_t>(*((uint16_t*)theBuffer.pValue));
nStage ++;
case 7:
//Notify(PSTR("\r\nTransaction ID:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 8:
if (!valueParser.Parse(&p, &cntdn))
return;
 
//PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 9:
if (eventCode == PTP_EC_ObjectAdded)
Notify(PSTR("\r\nObject Added:\t\t"));
 
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 10:
if (eventCode == PTP_EC_ObjectAdded)
{
if (!valueParser.Parse(&p, &cntdn))
return;
 
objHandle = *((uint32_t*)theBuffer.pValue);
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
Notify(PSTR("\r\n"));
}
if (eventCode == PTP_EC_CaptureComplete)
Notify(PSTR("\r\nCapture complete.\r\n"));
nStage ++;
case 11:
nStage = 0;
}
}
/C-OSD/arducam-osd/ArduCamOSD_Beta/pseventparser.h
0,0 → 1,29
#ifndef __PSEVENTPARSER_H__
#define __PSEVENTPARSER_H__
 
#include <inttypes.h>
#include <avr/pgmspace.h>
#include "ptpcallback.h"
#include "ptpdebug.h"
#include "canonps.h"
 
class PSEventParser : public PTPReadParser
{
MultiValueBuffer theBuffer;
uint32_t varBuffer;
uint8_t nStage;
uint16_t eventCode;
uint32_t objHandle;
 
MultiByteValueParser valueParser;
 
public:
PSEventParser() : nStage(0), varBuffer(0), objHandle(0)
{
theBuffer.pValue = &varBuffer;
};
uint32_t GetObjHandle() { return objHandle; };
virtual void Parse(const uint16_t len, const uint8_t *pbuf, const uint32_t &offset);
};
 
#endif // __PSEVENTPARSER_H__
/C-OSD/arducam-osd/ArduCamOSD_Beta/ptpobjinfoparser.cpp
0,0 → 1,298
#include "ptpobjinfoparser.h"
 
const char* PTPObjInfoParser::acNames[] PROGMEM =
{
msgUndefined,
msgAssociation,
msgScript,
msgExecutable,
msgText,
msgHTML,
msgDPOF,
msgAIFF,
msgWAV,
msgMP3,
msgAVI,
msgMPEG,
msgASF,
msgQT
};
 
const char* PTPObjInfoParser::imNames[] PROGMEM =
{
msgUndefined,
msgEXIF_JPEG,
msgTIFF_EP,
msgFlashPix,
msgBMP,
msgCIFF,
msgUndefined_0x3806,
msgGIF,
msgJFIF,
msgPCD,
msgPICT,
msgPNG,
msgUndefined_0x380C,
msgTIFF,
msgTIFF_IT,
msgJP2,
msgJPX,
};
 
void PTPObjInfoParser::PrintFormat(uint16_t op)
{
Serial.print(op, HEX);
Serial.print("\t");
//Notify(msgTab);
 
if ((((op >> 8) & 0xFF) == 0x30) && ((op & 0xFF) <= (PTP_OFC_QT & 0xFF)))
Notify((char*)pgm_read_word(&acNames[(op & 0xFF)]));
else
if ((((op >> 8) & 0xFF) == 0x38) && ((op & 0xFF) <= (PTP_OFC_JPX & 0xFF)))
Notify((char*)pgm_read_word(&imNames[(op & 0xFF)]));
else
{
switch (op)
{
case MTP_OFC_Undefined_Firmware:
Notify(msgUndefined_Firmware);
break;
case MTP_OFC_Windows_Image_Format:
Notify(msgWindows_Image_Format);
break;
case MTP_OFC_Undefined_Audio:
Notify(msgUndefined_Audio);
break;
case MTP_OFC_WMA:
Notify(msgWMA);
break;
case MTP_OFC_OGG:
Notify(msgOGG);
break;
case MTP_OFC_AAC:
Notify(msgAAC);
break;
case MTP_OFC_Audible:
Notify(msgAudible);
break;
case MTP_OFC_FLAC:
Notify(msgFLAC);
break;
case MTP_OFC_Undefined_Video:
Notify(msgUndefined_Video);
break;
case MTP_OFC_WMV:
Notify(msgWMV);
break;
case MTP_OFC_MP4_Container:
Notify(msgMP4_Container);
break;
case MTP_OFC_MP2:
Notify(msgMP2);
break;
case MTP_OFC_3GP_Container:
Notify(msg3GP_Container);
break;
default:
Notify(PSTR("Vendor defined"));
}
}
Notify(PSTR("\r\n"));
}
 
void PTPObjInfoParser::Parse(const uint16_t len, const uint8_t *pbuf, const uint32_t &offset)
{
uint16_t cntdn = (uint16_t)len;
uint8_t *p = (uint8_t*)pbuf;
 
switch (nStage)
{
case 0:
p += 12;
cntdn -= 12;
nStage ++;
case 1:
Notify(PSTR("Storage ID:\t\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 2:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 3:
Notify(PSTR("\r\nObject Format:\t\t"));
theBuffer.valueSize = 2;
valueParser.Initialize(&theBuffer);
nStage ++;
case 4:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintFormat(*((uint16_t*)theBuffer.pValue));
nStage ++;
case 5:
Notify(PSTR("Protection Status:\t"));
theBuffer.valueSize = 2;
valueParser.Initialize(&theBuffer);
nStage ++;
case 6:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint16_t>(*((uint16_t*)theBuffer.pValue));
nStage ++;
case 7:
Notify(PSTR("\r\nObject Compressed Size:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 8:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 9:
Notify(PSTR("\r\nThumb Format:\t\t"));
theBuffer.valueSize = 2;
valueParser.Initialize(&theBuffer);
nStage ++;
case 10:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintFormat(*((uint16_t*)theBuffer.pValue));
nStage ++;
case 11:
Notify(PSTR("Thumb Compressed Size:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 12:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 13:
Notify(PSTR("\r\nThumb Pix Width:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 14:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 15:
Notify(PSTR("\r\nThumb Pix Height:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 16:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 17:
Notify(PSTR("\r\nImage Pix Width:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 18:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 19:
Notify(PSTR("\r\nImage Pix Height:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 20:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 21:
Notify(PSTR("\r\nImage Bit Depth:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 22:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 23:
Notify(PSTR("\r\nParent Object:\t\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 24:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 25:
Notify(PSTR("\r\nAssociation Type:\t"));
theBuffer.valueSize = 2;
valueParser.Initialize(&theBuffer);
nStage ++;
case 26:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint16_t>(*((uint16_t*)theBuffer.pValue));
nStage ++;
case 27:
Notify(PSTR("\r\nAssociation Desc:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 28:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 29:
Notify(PSTR("\r\nSequence Number:\t"));
theBuffer.valueSize = 4;
valueParser.Initialize(&theBuffer);
nStage ++;
case 30:
if (!valueParser.Parse(&p, &cntdn))
return;
PrintHex<uint32_t>(*((uint32_t*)theBuffer.pValue));
nStage ++;
case 31:
Notify(PSTR("\r\nFile Name:\t\t"));
arrayParser.Initialize(1, 2, &theBuffer);
nStage ++;
case 32:
if (!arrayParser.Parse(&p, &cntdn, (PTP_ARRAY_EL_FUNC)&PrintChar))
return;
nStage ++;
case 33:
Notify(PSTR("\r\nCapture Date:\t\t"));
arrayParser.Initialize(1, 2, &theBuffer);
nStage ++;
case 34:
if (!arrayParser.Parse(&p, &cntdn, (PTP_ARRAY_EL_FUNC)&PrintChar))
return;
nStage ++;
case 35:
Notify(PSTR("\r\nModification Date:\t"));
arrayParser.Initialize(1, 2, &theBuffer);
nStage ++;
case 36:
if (!arrayParser.Parse(&p, &cntdn, (PTP_ARRAY_EL_FUNC)&PrintChar))
return;
nStage ++;
case 37:
Notify(PSTR("\r\nKeywords:\t"));
arrayParser.Initialize(1, 2, &theBuffer);
nStage ++;
case 38:
if (!arrayParser.Parse(&p, &cntdn, (PTP_ARRAY_EL_FUNC)&PrintChar))
return;
Notify(PSTR("\r\n"));
nStage = 0;
}
}
/C-OSD/arducam-osd/ArduCamOSD_Beta/ptpobjinfoparser.h
0,0 → 1,83
#ifndef __PTPOBJINFOPARSER_H__
#define __PTPOBJINFOPARSER_H__
 
#include <ptp.h>
#include <mtpconst.h>
#include <ptpcallback.h>
 
const char msgUndefined [] PROGMEM = "Undefined";
 
// Ancillary formats
const char msgAssociation [] PROGMEM = "Association";
const char msgScript [] PROGMEM = "Script";
const char msgExecutable [] PROGMEM = "Executable";
const char msgText [] PROGMEM = "Text";
const char msgHTML [] PROGMEM = "HTML";
const char msgDPOF [] PROGMEM = "DPOF";
const char msgAIFF [] PROGMEM = "AIFF";
const char msgWAV [] PROGMEM = "WAV";
const char msgMP3 [] PROGMEM = "MP3";
const char msgAVI [] PROGMEM = "AVI";
const char msgMPEG [] PROGMEM = "MPEG";
const char msgASF [] PROGMEM = "ASF";
const char msgQT [] PROGMEM = "QT";
 
// Image formats
const char msgEXIF_JPEG [] PROGMEM = "EXIF_JPEG";
const char msgTIFF_EP [] PROGMEM = "TIFF_EP";
const char msgFlashPix [] PROGMEM = "FlashPix";
const char msgBMP [] PROGMEM = "BMP";
const char msgCIFF [] PROGMEM = "CIFF";
const char msgUndefined_0x3806 [] PROGMEM = "Undefined_0x3806";
const char msgGIF [] PROGMEM = "GIF";
const char msgJFIF [] PROGMEM = "JFIF";
const char msgPCD [] PROGMEM = "PCD";
const char msgPICT [] PROGMEM = "PICT";
const char msgPNG [] PROGMEM = "PNG";
const char msgUndefined_0x380C [] PROGMEM = "Undefined_0x380C";
const char msgTIFF [] PROGMEM = "TIFF";
const char msgTIFF_IT [] PROGMEM = "TIFF_IT";
const char msgJP2 [] PROGMEM = "JP2";
const char msgJPX [] PROGMEM = "JPX";
 
// MTP Object Formats
const char msgUndefined_Firmware [] PROGMEM = "Undefined_Firmware";
const char msgWindows_Image_Format [] PROGMEM = "Windows_Image_Format";
const char msgUndefined_Audio [] PROGMEM = "Undefined_Audio";
const char msgWMA [] PROGMEM = "WMA";
const char msgOGG [] PROGMEM = "OGG";
const char msgAAC [] PROGMEM = "AAC";
const char msgAudible [] PROGMEM = "Audible";
const char msgFLAC [] PROGMEM = "FLAC";
const char msgUndefined_Video [] PROGMEM = "Undefined_Video";
const char msgWMV [] PROGMEM = "WMV";
const char msgMP4_Container [] PROGMEM = "MP4_Container";
const char msgMP2 [] PROGMEM = "MP2";
const char msg3GP_Container [] PROGMEM = "3GP_Container";
 
 
class PTPObjInfoParser : public PTPReadParser
{
static const char* acNames[];
static const char* imNames[];
 
MultiValueBuffer theBuffer;
uint32_t varBuffer;
uint8_t nStage;
 
MultiByteValueParser valueParser;
PTPListParser arrayParser;
 
static void PrintChar(MultiValueBuffer *p)
{
if (((unsigned char*)p->pValue)[0])
Serial.print(((unsigned char*)p->pValue)[0]);
};
void PrintFormat(uint16_t op);
 
public:
PTPObjInfoParser() : nStage(0) { theBuffer.pValue = (uint8_t*)&varBuffer; };
virtual void Parse(const uint16_t len, const uint8_t *pbuf, const uint32_t &offset);
};
 
#endif // __PTPOBJINFOPARSER_H__