From bc7cef94982dbe79125c4d0ca6374baad372efb9 Mon Sep 17 00:00:00 2001 From: "R. Belmont" Date: Tue, 1 Jan 2008 05:04:05 +0000 Subject: [PATCH] [NSS/SNES] Sync Andreas Naive's latest DSP-1 emulation. Improves accuracy and documentation value. Please co-credit Andreas Naive. --- src/mame/machine/snes.c | 57 +- src/mame/machine/snesdsp1.c | 2454 ++++++++++++++++++++--------------- 2 files changed, 1446 insertions(+), 1065 deletions(-) diff --git a/src/mame/machine/snes.c b/src/mame/machine/snes.c index e9d25715a71..aca1e0aaf8f 100644 --- a/src/mame/machine/snes.c +++ b/src/mame/machine/snes.c @@ -43,6 +43,7 @@ static emu_timer *snes_nmi_timer; static emu_timer *snes_hirq_timer; static UINT16 hblank_offset; static UINT16 snes_htmult; /* in 512 wide, we run HTOTAL double and halve it on latching */ +static UINT8 has_dsp1; // full graphic variables static UINT16 vram_fgr_high, vram_fgr_increment, vram_fgr_count, vram_fgr_mask, vram_fgr_shift, vram_read_buffer; @@ -279,8 +280,8 @@ static void snes_init_ram(void) { int i; - /* Init DSP1 interface */ - InitDSP1(); + /* Init DSP1 */ + DSP1_reset(); /* Init VRAM */ memset( snes_vram, 0, SNES_VRAM_SIZE ); @@ -436,27 +437,45 @@ READ8_HANDLER( snes_r_bank1 ) { if ((address >= 0x8000) && (offset >= 0x200000)) { - return dsp1_read(address); + if (address >= 0xc000) + return DSP1_getSr(); + else + return DSP1_getDr(); } } if( address <= 0x1fff ) /* Mirror of Low RAM */ + { return program_read_byte(0x7e0000 + address ); + } else if( address >= 0x2000 && address <= 0x5fff ) /* I/O */ + { return snes_r_io( address ); + } else if( address >= 0x6000 && address <= 0x7fff ) /* Reserved */ { if( snes_cart.mode == SNES_MODE_20 ) - return 0xff; - else - return dsp1_read(address); + { + return 0xff; + } + else + { + if (address >= 0x7000) + return DSP1_getSr(); + else + return DSP1_getDr(); + } } else { if( snes_cart.mode == SNES_MODE_20 ) + { return snes_ram[offset]; + } else /* MODE_21 */ + { return snes_ram[0xc00000 + offset]; + } } return 0xff; @@ -471,7 +490,10 @@ READ8_HANDLER( snes_r_bank2 ) { if (address >= 0x8000) { - return dsp1_read(address); + if (address >= 0xc000) + return DSP1_getSr(); + else + return DSP1_getDr(); } } @@ -482,7 +504,9 @@ READ8_HANDLER( snes_r_bank2 ) else if( address >= 0x6000 && address <= 0x7fff ) { if( snes_cart.mode == SNES_MODE_20 ) + { return 0xff; /* Reserved */ + } else /* MODE_21 */ { int mask; @@ -533,7 +557,10 @@ READ8_HANDLER( snes_r_bank6 ) if (address < 0x8000) { - return dsp1_read(address); + if (address >= 0x4000) + return DSP1_getSr(); + else + return DSP1_getDr(); } return 0xff; @@ -567,7 +594,7 @@ WRITE8_HANDLER( snes_w_bank1 ) if ((address >= 0x8000) && (offset >= 0x200000)) { - dsp1_write(address, data); + DSP1_setDr(data); return; } @@ -580,7 +607,7 @@ WRITE8_HANDLER( snes_w_bank1 ) logerror( "Attempt to write to reserved address: %X\n", offset ); else { - dsp1_write(address, data); + DSP1_setDr(data); return; } else @@ -594,7 +621,7 @@ WRITE8_HANDLER( snes_w_bank2 ) if (address >= 0x8000) { - dsp1_write(address, data); + DSP1_setDr(data); return; } @@ -603,10 +630,10 @@ WRITE8_HANDLER( snes_w_bank2 ) else if( address >= 0x2000 && address <= 0x5fff ) /* I/O */ snes_w_io( address, data ); else if( address >= 0x6000 && address <= 0x7fff ) - { + { if( snes_cart.mode == SNES_MODE_20 ) /* Reserved */ logerror( "Attempt to write to reserved address: %X\n", offset ); - else /* MODE_21 */ + else /* MODE_21 */ { int mask; @@ -629,9 +656,9 @@ WRITE8_HANDLER( snes_w_bank6 ) if (address < 0x8000) { - dsp1_write(address, data); + DSP1_setDr(data); return; - } + } } /* 0x800000 - 0xffffff */ diff --git a/src/mame/machine/snesdsp1.c b/src/mame/machine/snesdsp1.c index 81555e5a0fa..d497f5af382 100644 --- a/src/mame/machine/snesdsp1.c +++ b/src/mame/machine/snesdsp1.c @@ -4,1265 +4,1619 @@ File to handle emulation of the SNES "DSP-1" add-on chip. - R. Belmont - Code from http://users.tpg.com.au/trauma/dsp/dsp1.html - By The Dumper (Electrical Engineering), Neviksti (Physics), Overload (Computer Science), Andreas Naive (Mathematics) + Original C++ "dsp1emul.cpp" by Andreas Naive + Based on research by Overload, The Dumper, Neviksti and Andreas Naive + MAME/MESS C conversion by R. Belmont - This is up to date with the source version that was posted May 29, 2006. + This is up to date with the source version dated June 2006. ***************************************************************************/ -static INT16 dsp1_waitcmd, dsp1_in_cnt, dsp1_out_cnt, dsp1_in_idx, dsp1_out_idx, dsp1_first_parm, dsp1_cur_cmd; -static int has_dsp1; -static UINT8 dsp1_in[32], dsp1_out[16]; - #define DSP1_VERSION 0x0102 -// 1FH - Memory Dump (DSP1B Data ROM) -// Input byte(1FH) integer(Undefined) -// Output integer[1024] +// The DSP-1 status register has 16 bits, but only +// the upper 8 bits can be accessed from an external device, so all these +// positions are referred to the upper byte (bits D8 to D15) -static UINT16 DSP1ROM[1024]; +enum SrFlags {DRC=0x04, DRS=0x10, RQM=0x80}; -// Command Translation Table +// According to Overload's docs, these are the meanings of the flags: +// DRC: The Data Register Control (DRC) bit specifies the data transfer length to and from the host CPU. +// 0: Data transfer to and from the DSP-1 is 16 bits. +// 1: Data transfer to and from the DSP-1 is 8 bits. +// DRS: The Data Register Status (DRS) bit indicates the data transfer status in the case of transfering 16-bit data. +// 0: Data transfer has terminated. +// 1: Data transfer in progress. +// RQM: The Request for Master (RQM) indicates that the DSP1 is requesting host CPU for data read/write. +// 0: Internal Data Register Transfer. +// 1: External Data Register Transfer. -static const INT16 DSP1_CmdTable[64] = { - 0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0001, 0x0006, 0x000f, - 0x0008, 0x000d, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x000f, - 0x0010, 0x0011, 0x0002, 0x0013, 0x0014, 0x0011, 0x0006, 0x001f, - 0x0018, 0x001d, 0x001a, 0x001b, 0x001c, 0x001d, 0x000e, 0x001f, - 0x0020, 0x0021, 0x0002, 0x0023, 0x0004, 0x0021, 0x0006, 0x002f, - 0x0028, 0x002d, 0x001a, 0x002b, 0x000c, 0x002d, 0x000e, 0x002f, - 0x0010, 0x0001, 0x0002, 0x0003, 0x0014, 0x0001, 0x0006, 0x001f, - 0x0038, 0x000d, 0x001a, 0x000b, 0x001c, 0x000d, 0x000e, 0x001f}; +enum FsmMajorState {WAIT_COMMAND, READ_DATA, WRITE_DATA}; +enum MaxDataAccesses {MAX_READS=7, MAX_WRITES=1024}; -// Optimised for Performance - -static const INT16 DSP1_MulTable[256] = { - 0x0000, 0x0003, 0x0006, 0x0009, 0x000c, 0x000f, 0x0012, 0x0015, - 0x0019, 0x001c, 0x001f, 0x0022, 0x0025, 0x0028, 0x002b, 0x002f, - 0x0032, 0x0035, 0x0038, 0x003b, 0x003e, 0x0041, 0x0045, 0x0048, - 0x004b, 0x004e, 0x0051, 0x0054, 0x0057, 0x005b, 0x005e, 0x0061, - 0x0064, 0x0067, 0x006a, 0x006d, 0x0071, 0x0074, 0x0077, 0x007a, - 0x007d, 0x0080, 0x0083, 0x0087, 0x008a, 0x008d, 0x0090, 0x0093, - 0x0096, 0x0099, 0x009d, 0x00a0, 0x00a3, 0x00a6, 0x00a9, 0x00ac, - 0x00af, 0x00b3, 0x00b6, 0x00b9, 0x00bc, 0x00bf, 0x00c2, 0x00c5, - 0x00c9, 0x00cc, 0x00cf, 0x00d2, 0x00d5, 0x00d8, 0x00db, 0x00df, - 0x00e2, 0x00e5, 0x00e8, 0x00eb, 0x00ee, 0x00f1, 0x00f5, 0x00f8, - 0x00fb, 0x00fe, 0x0101, 0x0104, 0x0107, 0x010b, 0x010e, 0x0111, - 0x0114, 0x0117, 0x011a, 0x011d, 0x0121, 0x0124, 0x0127, 0x012a, - 0x012d, 0x0130, 0x0133, 0x0137, 0x013a, 0x013d, 0x0140, 0x0143, - 0x0146, 0x0149, 0x014d, 0x0150, 0x0153, 0x0156, 0x0159, 0x015c, - 0x015f, 0x0163, 0x0166, 0x0169, 0x016c, 0x016f, 0x0172, 0x0175, - 0x0178, 0x017c, 0x017f, 0x0182, 0x0185, 0x0188, 0x018b, 0x018e, - 0x0192, 0x0195, 0x0198, 0x019b, 0x019e, 0x01a1, 0x01a4, 0x01a8, - 0x01ab, 0x01ae, 0x01b1, 0x01b4, 0x01b7, 0x01ba, 0x01be, 0x01c1, - 0x01c4, 0x01c7, 0x01ca, 0x01cd, 0x01d0, 0x01d4, 0x01d7, 0x01da, - 0x01dd, 0x01e0, 0x01e3, 0x01e6, 0x01ea, 0x01ed, 0x01f0, 0x01f3, - 0x01f6, 0x01f9, 0x01fc, 0x0200, 0x0203, 0x0206, 0x0209, 0x020c, - 0x020f, 0x0212, 0x0216, 0x0219, 0x021c, 0x021f, 0x0222, 0x0225, - 0x0228, 0x022c, 0x022f, 0x0232, 0x0235, 0x0238, 0x023b, 0x023e, - 0x0242, 0x0245, 0x0248, 0x024b, 0x024e, 0x0251, 0x0254, 0x0258, - 0x025b, 0x025e, 0x0261, 0x0264, 0x0267, 0x026a, 0x026e, 0x0271, - 0x0274, 0x0277, 0x027a, 0x027d, 0x0280, 0x0284, 0x0287, 0x028a, - 0x028d, 0x0290, 0x0293, 0x0296, 0x029a, 0x029d, 0x02a0, 0x02a3, - 0x02a6, 0x02a9, 0x02ac, 0x02b0, 0x02b3, 0x02b6, 0x02b9, 0x02bc, - 0x02bf, 0x02c2, 0x02c6, 0x02c9, 0x02cc, 0x02cf, 0x02d2, 0x02d5, - 0x02d8, 0x02db, 0x02df, 0x02e2, 0x02e5, 0x02e8, 0x02eb, 0x02ee, - 0x02f1, 0x02f5, 0x02f8, 0x02fb, 0x02fe, 0x0301, 0x0304, 0x0307, - 0x030b, 0x030e, 0x0311, 0x0314, 0x0317, 0x031a, 0x031d, 0x0321}; - -static const INT16 DSP1_SinTable[256] = { - 0x0000, 0x0324, 0x0647, 0x096a, 0x0c8b, 0x0fab, 0x12c8, 0x15e2, - 0x18f8, 0x1c0b, 0x1f19, 0x2223, 0x2528, 0x2826, 0x2b1f, 0x2e11, - 0x30fb, 0x33de, 0x36ba, 0x398c, 0x3c56, 0x3f17, 0x41ce, 0x447a, - 0x471c, 0x49b4, 0x4c3f, 0x4ebf, 0x5133, 0x539b, 0x55f5, 0x5842, - 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e8, 0x66cf, 0x68a6, - 0x6a6d, 0x6c24, 0x6dca, 0x6f5f, 0x70e2, 0x7255, 0x73b5, 0x7504, - 0x7641, 0x776c, 0x7884, 0x798a, 0x7a7d, 0x7b5d, 0x7c29, 0x7ce3, - 0x7d8a, 0x7e1d, 0x7e9d, 0x7f09, 0x7f62, 0x7fa7, 0x7fd8, 0x7ff6, - 0x7fff, 0x7ff6, 0x7fd8, 0x7fa7, 0x7f62, 0x7f09, 0x7e9d, 0x7e1d, - 0x7d8a, 0x7ce3, 0x7c29, 0x7b5d, 0x7a7d, 0x798a, 0x7884, 0x776c, - 0x7641, 0x7504, 0x73b5, 0x7255, 0x70e2, 0x6f5f, 0x6dca, 0x6c24, - 0x6a6d, 0x68a6, 0x66cf, 0x64e8, 0x62f2, 0x60ec, 0x5ed7, 0x5cb4, - 0x5a82, 0x5842, 0x55f5, 0x539b, 0x5133, 0x4ebf, 0x4c3f, 0x49b4, - 0x471c, 0x447a, 0x41ce, 0x3f17, 0x3c56, 0x398c, 0x36ba, 0x33de, - 0x30fb, 0x2e11, 0x2b1f, 0x2826, 0x2528, 0x2223, 0x1f19, 0x1c0b, - 0x18f8, 0x15e2, 0x12c8, 0x0fab, 0x0c8b, 0x096a, 0x0647, 0x0324, - -0x0000, -0x0324, -0x0647, -0x096a, -0x0c8b, -0x0fab, -0x12c8, -0x15e2, - -0x18f8, -0x1c0b, -0x1f19, -0x2223, -0x2528, -0x2826, -0x2b1f, -0x2e11, - -0x30fb, -0x33de, -0x36ba, -0x398c, -0x3c56, -0x3f17, -0x41ce, -0x447a, - -0x471c, -0x49b4, -0x4c3f, -0x4ebf, -0x5133, -0x539b, -0x55f5, -0x5842, - -0x5a82, -0x5cb4, -0x5ed7, -0x60ec, -0x62f2, -0x64e8, -0x66cf, -0x68a6, - -0x6a6d, -0x6c24, -0x6dca, -0x6f5f, -0x70e2, -0x7255, -0x73b5, -0x7504, - -0x7641, -0x776c, -0x7884, -0x798a, -0x7a7d, -0x7b5d, -0x7c29, -0x7ce3, - -0x7d8a, -0x7e1d, -0x7e9d, -0x7f09, -0x7f62, -0x7fa7, -0x7fd8, -0x7ff6, - -0x7fff, -0x7ff6, -0x7fd8, -0x7fa7, -0x7f62, -0x7f09, -0x7e9d, -0x7e1d, - -0x7d8a, -0x7ce3, -0x7c29, -0x7b5d, -0x7a7d, -0x798a, -0x7884, -0x776c, - -0x7641, -0x7504, -0x73b5, -0x7255, -0x70e2, -0x6f5f, -0x6dca, -0x6c24, - -0x6a6d, -0x68a6, -0x66cf, -0x64e8, -0x62f2, -0x60ec, -0x5ed7, -0x5cb4, - -0x5a82, -0x5842, -0x55f5, -0x539b, -0x5133, -0x4ebf, -0x4c3f, -0x49b4, - -0x471c, -0x447a, -0x41ce, -0x3f17, -0x3c56, -0x398c, -0x36ba, -0x33de, - -0x30fb, -0x2e11, -0x2b1f, -0x2826, -0x2528, -0x2223, -0x1f19, -0x1c0b, - -0x18f8, -0x15e2, -0x12c8, -0x0fab, -0x0c8b, -0x096a, -0x0647, -0x0324 +struct DSP1_Command { + void (*callback)(INT16 *, INT16 *); + unsigned int reads; + unsigned int writes; }; -#define INCR 2048 -static double CosTable2[INCR]; -static double SinTable2[INCR]; +static const struct DSP1_Command mCommandTable[0x40]; +static const INT16 DSP1_MaxAZS_Exp[16]; +static const INT16 DSP1_SinTable[]; +static const INT16 DSP1_MulTable[]; +static UINT16 DSP1_DataRom[]; -#define Angle(x) (((x)/(65536/INCR)) & (INCR-1)) -#define Cos(x) ((double) CosTable2[x]) -#define Sin(x) ((double) SinTable2[x]) +static struct SharedData { // some RAM variables shared between commands + INT16 MatrixA[3][3]; // attitude matrix A + INT16 MatrixB[3][3]; + INT16 MatrixC[3][3]; + INT16 CentreX, CentreY, CentreZ; // center of projection + INT16 CentreZ_C, CentreZ_E; + INT16 VOffset; // vertical offset of the screen with regard to the centre of projection + INT16 Les, C_Les, E_Les; + INT16 SinAas, CosAas; + INT16 SinAzs, CosAzs; + INT16 SinAZS, CosAZS; + INT16 SecAZS_C1, SecAZS_E1; + INT16 SecAZS_C2, SecAZS_E2; + INT16 Nx, Ny, Nz; // normal vector to the screen (norm 1, points toward the center of projection) + INT16 Gx, Gy, Gz; // center of the screen (global coordinates) + INT16 Hx, Hy; // horizontal vector of the screen (Hz=0, norm 1, points toward the right of the screen) + INT16 Vx, Vy, Vz; // vertical vector of the screen (norm 1, points toward the top of the screen) -// init DSP1 math tables -static void InitDSP1(void) +} shared; + +static UINT8 mSr; // status register +static int mSrLowByteAccess; +static UINT16 mDr; // "internal" representation of the data register +static enum FsmMajorState mFsmMajorState; // current major state of the FSM +static UINT8 mCommand; // current command processed by the FSM +static UINT8 mDataCounter; // #UINT16 read/writes counter used by the FSM +static INT16 mReadBuffer[MAX_READS]; +static INT16 mWriteBuffer[MAX_WRITES]; +static UINT8 mFreeze; // need explanation? ;) + +static void DSP1_fsmStep(UINT8 read, UINT8 *data); // FSM logic + +// commands +static void DSP1_memoryTest(INT16 *input, INT16 *output); +static void DSP1_memoryDump(INT16 *input, INT16 *output); +static void DSP1_memorySize(INT16 *input, INT16 *output); +static void DSP1_multiply(INT16* input, INT16* output); +static void DSP1_multiply2(INT16* input, INT16* output); +static void DSP1_inverse(INT16 *input, INT16 *output); +static void DSP1_triangle(INT16 *input, INT16 *output); +static void DSP1_radius(INT16 *input, INT16 *output); +static void DSP1_range(INT16 *input, INT16 *output); +static void DSP1_range2(INT16 *input, INT16 *output); +static void DSP1_distance(INT16 *input, INT16 *output); +static void DSP1_rotate(INT16 *input, INT16 *output); +static void DSP1_polar(INT16 *input, INT16 *output); +static void DSP1_attitudeA(INT16 *input, INT16 *output); +static void DSP1_attitudeB(INT16 *input, INT16 *output); +static void DSP1_attitudeC(INT16 *input, INT16 *output); +static void DSP1_objectiveA(INT16 *input, INT16 *output); +static void DSP1_objectiveB(INT16 *input, INT16 *output); +static void DSP1_objectiveC(INT16 *input, INT16 *output); +static void DSP1_subjectiveA(INT16 *input, INT16 *output); +static void DSP1_subjectiveB(INT16 *input, INT16 *output); +static void DSP1_subjectiveC(INT16 *input, INT16 *output); +static void DSP1_scalarA(INT16 *input, INT16 *output); +static void DSP1_scalarB(INT16 *input, INT16 *output); +static void DSP1_scalarC(INT16 *input, INT16 *output); +static void DSP1_gyrate(INT16 *input, INT16 *output); +static void DSP1_parameter(INT16 *input, INT16 *output); +static void DSP1_raster(INT16 *input, INT16 *output); +static void DSP1_target(INT16 *input, INT16 *output); +static void DSP1_project(INT16 *input, INT16 *output); + +// auxiliar functions +static INT16 DSP1_sin(INT16 Angle); +static INT16 DSP1_cos(INT16 Angle); +static void inverse(INT16 Coefficient, INT16 Exponent, INT16 *iCoefficient, INT16 *iExponent); +static INT16 denormalizeAndClip(INT16 C, INT16 E); +static void normalize(INT16 m, INT16 *Coefficient, INT16 *Exponent); +static void normalizeDouble(INT32 Product, INT16 *Coefficient, INT16 *Exponent); +static INT16 shiftR(INT16 C, INT16 E); + +////////////////////////////////////////////////////////////////// + +UINT8 DSP1_getSr(void) +{ + mSrLowByteAccess = ~mSrLowByteAccess; + if (mSrLowByteAccess) + { + return 0; + } + else + { + return mSr; + } +} + +////////////////////////////////////////////////////////////////// + +UINT8 DSP1_getDr(void) +{ + UINT8 oDr; + + DSP1_fsmStep(1, &oDr); + return oDr; +} + +////////////////////////////////////////////////////////////////// + +void DSP1_setDr(UINT8 iDr) +{ + DSP1_fsmStep(0, &iDr); +} + +////////////////////////////////////////////////////////////////// + +void DSP1_reset(void) { UINT32 i; UINT8 *dspin = memory_region(REGION_USER6); - dsp1_waitcmd = dsp1_first_parm = 1; - - for (i=0; i>8); + else + *data = (UINT8)(mDr); + } + else + { + if (mSr&DRS) + { + mDr &= 0x00ff; + mDr |= *data<<8; + } + else + { + mDr &= 0xff00; + mDr |= *data; + } + } - S = DSP1_SinTable[Angle >> 8] + (DSP1_MulTable[Angle & 0xff] * DSP1_SinTable[0x40 + (Angle >> 8)] >> 15); - if (S > 32767) S = 32767; - return (INT16) S; + + switch (mFsmMajorState) + { + case WAIT_COMMAND: + mCommand = (UINT8)(mDr); + if (!(mCommand & 0xc0)) // valid command? + { + switch(mCommand) + { + // freeze cases + case 0x1a: + case 0x2a: + case 0x3a: + mFreeze = TRUE; + break; + // normal cases + default: + mDataCounter=0; + mFsmMajorState = READ_DATA; + mSr &= ~DRC; + break; + } + } + break; + case READ_DATA: + mSr ^= DRS; + if (!(mSr&DRS)) + { + mReadBuffer[mDataCounter++] = (INT16)(mDr); + if (mDataCounter >= mCommandTable[mCommand].reads) + { + (*mCommandTable[mCommand].callback)(mReadBuffer, mWriteBuffer); + if (0 != mCommandTable[mCommand].writes) // any output? + { + mDataCounter = 0; + mDr = (UINT16)(mWriteBuffer[mDataCounter]); + mFsmMajorState = WRITE_DATA; + } + else + { + mDr = 0x0080; // valid command completion + mFsmMajorState = WAIT_COMMAND; + mSr |= DRC; + } + } + } + break; + case WRITE_DATA: + mSr ^= DRS; + if (!(mSr&DRS)) + { + ++mDataCounter; + if (mDataCounter >= mCommandTable[mCommand].writes) + { + if ((mCommand == 0x0a)&&(mDr != 0x8000)) + { + // works in continuous mode + mReadBuffer[0]++; // next raster line + (*mCommandTable[mCommand].callback)(mReadBuffer, mWriteBuffer); + mDataCounter = 0; + mDr = (UINT16)(mWriteBuffer[mDataCounter]); + } + else + { + mDr = 0x0080; // valid command completion + mFsmMajorState = WAIT_COMMAND; + mSr |= DRC; + } + } + else + { + mDr = (UINT16)(mWriteBuffer[mDataCounter]); + } + } + break; + } + + + + // Now RQM would be set (except when executing Op1A -command equals 0x1a, 0x2a or 0x3a-). + if (mFreeze) + mSr &= ~RQM; } -static INT16 DSP1_Cos(INT16 Angle) -{ - int S; +////////////////////////////////////////////////////////////////// - if (Angle < 0) { - if (Angle == -32768) return -32768; - Angle = -Angle; - } - S = DSP1_SinTable[0x40 + (Angle >> 8)] - (DSP1_MulTable[Angle & 0xff] * DSP1_SinTable[Angle >> 8] >> 15); - if (S < -32768) S = -32767; - return (INT16) S; +// The info on this table follows Overload's docs. + +static const struct DSP1_Command mCommandTable[0x40] = +{ + {&DSP1_multiply, 2, 1}, //0x00 + {&DSP1_attitudeA, 4, 0}, //0x01 + {&DSP1_parameter, 7, 4}, //0x02 + {&DSP1_subjectiveA, 3, 3}, //0x03 + {&DSP1_triangle, 2, 2}, //0x04 + {&DSP1_attitudeA, 4, 0}, //0x01 + {&DSP1_project, 3, 3}, //0x06 + {&DSP1_memoryTest, 1, 1}, //0x0f + {&DSP1_radius, 3, 2}, //0x08 + {&DSP1_objectiveA, 3, 3}, //0x0d + {&DSP1_raster, 1, 4}, // 0x0a. This will normally work in continuous mode + {&DSP1_scalarA, 3, 1}, //0x0b + {&DSP1_rotate, 3, 2}, //0x0c + {&DSP1_objectiveA, 3, 3}, //0x0d + {&DSP1_target, 2, 2}, //0x0e + {&DSP1_memoryTest, 1, 1}, //0x0f + + {&DSP1_inverse, 2, 2}, //0x10 + {&DSP1_attitudeB, 4, 0}, //0x11 + {&DSP1_parameter, 7, 4}, //0x02 + {&DSP1_subjectiveB, 3, 3}, //0x13 + {&DSP1_gyrate, 6, 3}, //0x14 + {&DSP1_attitudeB, 4, 0}, //0x11 + {&DSP1_project, 3, 3}, //0x06 + {&DSP1_memoryDump, 1, 1024}, //0x1f + {&DSP1_range, 4, 1}, //0x18 + {&DSP1_objectiveB, 3, 3}, //0x1d + {0, 0, 0}, // 0x1a; the chip freezes + {&DSP1_scalarB, 3, 1}, //0x1b + {&DSP1_polar, 6, 3}, //0x1c + {&DSP1_objectiveB, 3, 3}, //0x1d + {&DSP1_target, 2, 2}, //0x0e + {&DSP1_memoryDump, 1, 1024}, //0x1f + + {&DSP1_multiply2, 2, 1}, //0x20 + {&DSP1_attitudeC, 4, 0}, //0x21 + {&DSP1_parameter, 7, 4}, //0x02 + {&DSP1_subjectiveC, 3, 3}, //0x23 + {&DSP1_triangle, 2, 2}, //0x04 + {&DSP1_attitudeC, 4, 0}, //0x21 + {&DSP1_project, 3, 3}, //0x06 + {&DSP1_memorySize, 1, 1}, //0x2f + {&DSP1_distance, 3, 1}, //0x28 + {&DSP1_objectiveC, 3, 3}, //0x2d + {0, 0, 0}, // 0x1a; the chip freezes + {&DSP1_scalarC, 3, 1}, //0x2b + {&DSP1_rotate, 3, 2}, //0x0c + {&DSP1_objectiveC, 3, 3}, //0x2d + {&DSP1_target, 2, 2}, //0x0e + {&DSP1_memorySize, 1, 1}, //0x2f + + {&DSP1_inverse, 2, 2}, //0x10 + {&DSP1_attitudeA, 4, 0}, //0x01 + {&DSP1_parameter, 7, 4}, //0x02 + {&DSP1_subjectiveA, 3, 3}, //0x03 + {&DSP1_gyrate, 6, 3}, //0x14 + {&DSP1_attitudeA, 4, 0}, //0x01 + {&DSP1_project, 3, 3}, //0x06 + {&DSP1_memoryDump, 1, 1024}, //0x1f + {&DSP1_range2, 4, 1}, //0x38 + {&DSP1_objectiveA, 3, 3}, //0x0d + {0, 0, 0}, // 0x1a; the chip freezes + {&DSP1_scalarA, 3, 1}, //0x0b + {&DSP1_polar, 6, 3}, //0x1c + {&DSP1_objectiveA, 3, 3}, //0x0d + {&DSP1_target, 2, 2}, //0x0e + {&DSP1_memoryDump, 1, 1024}, //0x1f +}; + +////////////////////////////////////////////////////////////////// + +static void DSP1_memoryTest(INT16 *input, INT16 *output) +{ +// INT16 *Size = &input[0]; + INT16 *Result = &output[0]; + + *Result = 0x0000; } -static void DSP1_Normalize(INT16 m, INT16 *Coefficient, INT16 *Exponent) +////////////////////////////////////////////////////////////////// + +static void DSP1_memoryDump(INT16 *input, INT16 *output) { - INT16 i = 0x4000; - INT16 e = 0; - - if (m < 0) - while ((m & i) && i) - { - i >>= 1; - e++; - } - else - while (!(m & i) && i) - { - i >>= 1; - e++; - } - - if (e > 0) - *Coefficient = m * DSP1ROM[0x0021 + e] << 1; - else - *Coefficient = m; - - *Exponent -= e; + memcpy(output, DSP1_DataRom, 1024); } -static void DSP1_NormalizeDouble(int Product, INT16 *Coefficient, INT16 *Exponent) +////////////////////////////////////////////////////////////////// + +static void DSP1_memorySize(INT16 *input, INT16 *output) { - INT16 n = Product & 0x7fff; - INT16 m = Product >> 15; - INT16 i = 0x4000; - INT16 e = 0; + INT16 *Size = &output[0]; - if (m < 0) - while ((m & i) && i) - { - i >>= 1; - e++; - } - else - while (!(m & i) && i) - { - i >>= 1; - e++; - } - - if (e > 0) - { - *Coefficient = m * DSP1ROM[0x0021 + e] << 1; - - if (e < 15) - *Coefficient += n * DSP1ROM[0x0040 - e] >> 15; - else - { - i = 0x4000; - - if (m < 0) - while ((n & i) && i) - { - i >>= 1; - e++; - } - else - while (!(n & i) && i) - { - i >>= 1; - e++; - } - - if (e > 15) - *Coefficient = n * DSP1ROM[0x0012 + e] << 1; - else - *Coefficient += n; - } - } - else - *Coefficient = m; - - *Exponent = e; + *Size = 0x0100; } -static INT16 DSP1_Truncate(INT16 C, INT16 E) +////////////////////////////////////////////////////////////////// + +// 16-bit multiplication + +static void DSP1_multiply(INT16 *input, INT16 *output) { - if (E > 0) { - if (C > 0) return 32767; else if (C < 0) return -32767; - } else { - if (E < 0) return C * DSP1ROM[0x0031 + E] >> 15; - } - return C; + INT16 Multiplicand = input[0]; + INT16 Multiplier = input[1]; + INT16 *Product = &output[0]; + + *Product = Multiplicand * Multiplier >> 15; } -// 00H - 16-bit Multiplication (Bit Perfect) -// Input byte(00H) integer(Multiplicand) integer(Multiplier) -// Output integer(Product) +////////////////////////////////////////////////////////////////// -static void DSP1_Multiply(INT16 Multiplicand, INT16 Multiplier, INT16 *Product) +// 16-bit multiplication. 'Alternative' method. Can anyone check this carefully? + +static void DSP1_multiply2(INT16 *input, INT16 *output) { - *Product = (Multiplicand * Multiplier >> 15); + INT16 Multiplicand = input[0]; + INT16 Multiplier = input[1]; + INT16* Product = &output[0]; + + *Product = (Multiplicand * Multiplier >> 15)+1; } -// 20H - 16-bit Multiplication (Bit Perfect) -// Input byte(20H) integer(Multiplicand) integer(Multiplier) -// Output integer(Product) +////////////////////////////////////////////////////////////////// -#ifdef UNUSED_FUNCTION -static void DSP1_Multiply1(INT16 Multiplicand, INT16 Multiplier, INT16 *Product) +// This command determines the inverse of a floating point decimal number. + +static void DSP1_inverse(INT16 *input, INT16 *output) { - *Product = (Multiplicand * Multiplier >> 15) + 1; -} -#endif + INT16 Coefficient = input[0]; + INT16 Exponent = input[1]; + INT16* iCoefficient = &output[0]; + INT16* iExponent = &output[1]; -// 10H - Inverse Calculation (Bit Perfect) -// Input byte(10H) integer(Coefficient) integer(Exponent) -// Output integer(Coefficient) integer(Exponent) - -static void DSP1_Inverse(INT16 Coefficient, INT16 Exponent, INT16 *iCoefficient, INT16 *iExponent) -{ - // Step One: Division by Zero - if (Coefficient == 0x0000) - { - *iCoefficient = 0x7fff; - *iExponent = 0x002f; - } - else - { - INT16 Sign = 1; - - // Step Two: Remove Sign - if (Coefficient < 0) - { - if (Coefficient < -32767) Coefficient = -32767; - Coefficient = -Coefficient; - Sign = -1; - } - - // Step Three: Normalize - while (Coefficient < 0x4000) - { - Coefficient <<= 1; - Exponent--; - } - - // Step Four: Special Case - if (Coefficient == 0x4000) - if (Sign == 1) *iCoefficient = 0x7fff; - else { - *iCoefficient = -0x4000; - Exponent--; - } - else { - // Step Five: Initial Guess - INT16 i = DSP1ROM[((Coefficient - 0x4000) >> 7) + 0x0065]; - - // Step Six: Iterate Newton's Method - i = (i + (-i * (Coefficient * i >> 15) >> 15)) << 1; - i = (i + (-i * (Coefficient * i >> 15) >> 15)) << 1; - - *iCoefficient = i * Sign; - } - - *iExponent = 1 - Exponent; - } + inverse(Coefficient, Exponent, iCoefficient, iExponent); } -// 04H - Trigonometric Calculation (Bit Perfect) -// Input byte(04H) integer(Angle) integer(Radius) -// Output integer(Sine) integer(Cosine) +////////////////////////////////////////////////////////////////// -static void DSP1_Triangle(INT16 Angle, INT16 Radius, INT16 *S, INT16 *C) +// Vector component calculation. Determines the X and Y components for a +// two-dimensional vector whose size and direction is known. +// Y = Radius * sin(Angle) +// X = Radius * cos(Angle) + +static void DSP1_triangle(INT16 *input, INT16 *output) { - *S = DSP1_Sin(Angle) * Radius >> 15; - *C = DSP1_Cos(Angle) * Radius >> 15; + INT16 Angle = input[0]; + INT16 Radius = input[1]; + INT16* Y = &output[0]; + INT16* X = &output[1]; + + *Y = DSP1_sin(Angle) * Radius >> 15; + *X = DSP1_cos(Angle) * Radius >> 15; } -// 08H - Vector Size Calculation (Bit Perfect) -// Input byte(08H) integer(X) integer(Y) integer(Z) -// Output double(Radius) +////////////////////////////////////////////////////////////////// -static void DSP1_Radius(INT16 X, INT16 Y, INT16 Z, int *Radius) +// Determines the squared norm of a vector (X,Y,Z) +// The output is Radius = X^2+Y^2+Z^2 (double integer) + +static void DSP1_radius(INT16 *input, INT16 *output) { - *Radius = (X * X + Y * Y + Z * Z) << 1; + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* RadiusLow = &output[0]; + INT16* RadiusHigh = &output[1]; + + INT32 Radius; + + Radius = (X * X + Y * Y + Z * Z) << 1; + *RadiusLow = (INT16)(Radius); + *RadiusHigh = (INT16)(Radius>>16); } -// 18H - Vector Size Comparison (Bit Perfect) -// Input byte(18H) integer(X) integer(Y) integer(Z) integer(Radius) -// Output integer(Range) +////////////////////////////////////////////////////////////////// -static void DSP1_Range(INT16 X, INT16 Y, INT16 Z, INT16 Radius, INT16 *Range) +// Vector size comparison. This command compares the size of the vector (X,Y,Z) and the distance (R) +// from a particular point, and so may be used to determine if a point is within the sphere or radius R. +// The output is D = X^2+Y^2+Z^2-R^2 + +static void DSP1_range(INT16 *input, INT16 *output) { - *Range = ((X * X + Y * Y + Z * Z - Radius * Radius) >> 15); + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16 Radius = input[3]; + INT16* Range = &output[0]; + + *Range = (X * X + Y * Y + Z * Z - Radius * Radius) >> 15; } -// 38H - Vector Size Comparison (Bit Perfect) -// Input byte(38H) integer(X) integer(Y) integer(Z) integer(Radius) -// Output integer(Range) +////////////////////////////////////////////////////////////////// -#ifdef UNUSED_FUNCTION -static void DSP1_Range1(INT16 X, INT16 Y, INT16 Z, INT16 Radius, INT16 *Range) +// Vector size comparison. 'Alternative' method. + +static void DSP1_range2(INT16 *input, INT16 *output) { - *Range = ((X * X + Y * Y + Z * Z - Radius * Radius) >> 15) + 1; + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16 Radius = input[3]; + INT16* Range = &output[0]; + + *Range = ((X * X + Y * Y + Z * Z - Radius * Radius) >> 15) + 1; } -#endif -// 28H - Vector Absolute Value Calculation (Bit Perfect) -// Input byte(28H) integer(X) integer(Y) integer(Z) -// Output integer(Distance) +////////////////////////////////////////////////////////////////// -static void DSP1_Distance(INT16 X, INT16 Y, INT16 Z, INT16 *Distance) +// This command calculates the norm of a (X,Y,Z) vector, or the distance from +// the point (X,Y,Z) to (0,0,0), as you prefer to see it. +// Distance = sqrt(X^2+Y^2+Z^2) +// The square root of a number 'a' is calculated by doing this: you +// write 'a' as b*2^2n, with 'b' between 1/4 and 1; then, you calculate +// c=sqrt(b) by using lineal interpolation between points of a +// look-up table and, finally, you output the result as c*2^n. + +static void DSP1_distance(INT16 *input, INT16 *output) { - int Radius = X * X + Y * Y + Z * Z; - INT16 C, E, Pos, Node1, Node2; + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* Distance = &output[0]; + INT16 Pos, Node1, Node2; - if (Radius == 0) *Distance = 0; - else - { - DSP1_NormalizeDouble(Radius, &C, &E); - if (E & 1) C = C * 0x4000 >> 15; + INT32 Radius = X * X + Y * Y + Z * Z; - Pos = C * 0x0040 >> 15; + + if (Radius == 0) Distance = 0; + else + { + INT16 C, E; + normalizeDouble(Radius, &C, &E); + if (E & 1) C = C * 0x4000 >> 15; - Node1 = DSP1ROM[0x00d5 + Pos]; - Node2 = DSP1ROM[0x00d6 + Pos]; + Pos = C * 0x0040 >> 15; - *Distance = ((Node2 - Node1) * (C & 0x1ff) >> 9) + Node1; + Node1 = DSP1_DataRom[0x00d5 + Pos]; + Node2 = DSP1_DataRom[0x00d6 + Pos]; + + *Distance = ((Node2 - Node1) * (C & 0x1ff) >> 9) + Node1; #if DSP1_VERSION < 0x0102 if (Pos & 1) *Distance -= (Node2 - Node1); -#endif +#endif *Distance >>= (E >> 1); - } + } } -// 0CH - 2D Coordinate Rotation (Bit Perfect) -// Input byte(0CH) integer(Angle) integer(X) integer(Y) -// Output integer(X) integer(Y) +////////////////////////////////////////////////////////////////// -static void DSP1_Rotate(INT16 Angle, INT16 X1, INT16 Y1, INT16 *X2, INT16 *Y2) +// Determines the (X2, Y2) coordinates obtained by rotating (X1, Y1) +// clockwise for an angle 'Angle'. The official documentation says +// 'counterclockwise', but it's obviously wrong (surprise! :P) +// +// In matrix notation: +// |X2| |cos(Angle) sin(Angle)| |X1| +// | | = | | | | +// |Y2| |-sin(Angle cos(Angle)| |Y1| + +static void DSP1_rotate(INT16 *input, INT16 *output) { - *X2 = (Y1 * DSP1_Sin(Angle) >> 15) + (X1 * DSP1_Cos(Angle) >> 15); - *Y2 = (Y1 * DSP1_Cos(Angle) >> 15) - (X1 * DSP1_Sin(Angle) >> 15); + INT16 Angle = input[0]; + INT16 X1 = input[1]; + INT16 Y1 = input[2]; + INT16* X2 = &output[0]; + INT16* Y2 = &output[1]; + + *X2 = (Y1 * DSP1_sin(Angle) >> 15) + (X1 * DSP1_cos(Angle) >> 15); + *Y2 = (Y1 * DSP1_cos(Angle) >> 15) - (X1 * DSP1_sin(Angle) >> 15); } -// 1CH - 3D Coordinate Rotation (Bit Perfect) -// Input byte(1CH) integer(Az) integer(Ay) integer(Ax) integer(X) integer(Y) integer(Z) -// Output integer(X) integer(Y) integer(Z) +////////////////////////////////////////////////////////////////// -#ifdef UNUSED_FUNCTION -static void DSP1_Polar(INT16 Az, INT16 Ay, INT16 Ax, INT16 X1, INT16 Y1, INT16 Z1, INT16 *X2, INT16 *Y2, INT16 *Z2) +// Calculate the coordinates (X2, Y2, Z2) obtained when rotating (X1, Y1, Z1) +// three-dimensionally. Rotation is done in the order of Az around the Z axis, +// Ay around the Y axis and Ax around the X axis. As occur with the "attitude" commands +// (see comments in the "gyrate" command), qthis doesn't match what explained in +// the official documentation, but it's coherent with what it is done in the "attitude" +// command (but not with the "gyrate" command). +// +// In matrix notation: +// |X2| |1 0 0 | |cosRy 0 -sinRy| | cosRz sinRz 0| |X1| +// |Y2| = |0 cosRx sinRx| | 0 1 0 | |-sinRz cosRz 0| |Y1| +// |Z2| |0 -sinRx cosRx| |sinRy 0 cosRy| | 0 0 1| |Z1| + +static void DSP1_polar(INT16 *input, INT16 *output) { - INT16 X, Y, Z; + INT16 Az = input[0]; + INT16 Ay = input[1]; + INT16 Ax = input[2]; + INT16 X1 = input[3]; + INT16 Y1 = input[4]; + INT16 Z1 = input[5]; + INT16* X2 = &output[0]; + INT16* Y2 = &output[1]; + INT16* Z2 = &output[2]; - // Rotate Around Z - X = (Y1 * DSP1_Sin(Az) >> 15) + (X1 * DSP1_Cos(Az) >> 15); - Y = (Y1 * DSP1_Cos(Az) >> 15) - (X1 * DSP1_Sin(Az) >> 15); - X1 = X; Y1 = Y; + INT16 X, Y, Z; - // Rotate Around Y - Z = (X1 * DSP1_Sin(Ay) >> 15) + (Z1 * DSP1_Cos(Ay) >> 15); - X = (X1 * DSP1_Cos(Ay) >> 15) - (Z1 * DSP1_Sin(Ay) >> 15); - *X2 = X; Z1 = Z; + // Rotate Around Z + X = (Y1 * DSP1_sin(Az) >> 15) + (X1 * DSP1_cos(Az) >> 15); + Y = (Y1 * DSP1_cos(Az) >> 15) - (X1 * DSP1_sin(Az) >> 15); + X1 = X; Y1 = Y; - // Rotate Around X - Y = (Z1 * DSP1_Sin(Ax) >> 15) + (Y1 * DSP1_Cos(Ax) >> 15); - Z = (Z1 * DSP1_Cos(Ax) >> 15) - (Y1 * DSP1_Sin(Ax) >> 15); - *Y2 = Y; *Z2 = Z; + // Rotate Around Y + Z = (X1 * DSP1_sin(Ay) >> 15) + (Z1 * DSP1_cos(Ay) >> 15); + X = (X1 * DSP1_cos(Ay) >> 15) - (Z1 * DSP1_sin(Ay) >> 15); + *X2 = X; Z1 = Z; + + // Rotate Around X + Y = (Z1 * DSP1_sin(Ax) >> 15) + (Y1 * DSP1_cos(Ax) >> 15); + Z = (Z1 * DSP1_cos(Ax) >> 15) - (Y1 * DSP1_sin(Ax) >> 15); + *Y2 = Y; *Z2 = Z; } -#endif -// 02H - Projection Parameter Setting (Bit Perfect) -// Input byte(02H) integer(Fx) integer(Fy) integer(Fz) integer(Lfe) integer(Les) integer(Aas) integer(Azs) -// Output integer(Vof) integer(Vva) integer(Cx) integer(Cy) +////////////////////////////////////////////////////////////////// -static const INT16 MaxAZS_Exp[16] = { - 0x38b4, 0x38b7, 0x38ba, 0x38be, 0x38c0, 0x38c4, 0x38c7, 0x38ca, - 0x38ce, 0x38d0, 0x38d4, 0x38d7, 0x38da, 0x38dd, 0x38e0, 0x38e4 -}; +// Set up the elements of an "attitude matrix" (there are other ones): +// S | cosRz sinRz 0| |cosRy 0 -sinRy| |1 0 0 | +// MatrixA = - |-sinRz cosRz 0| | 0 1 0 | |0 cosRx sinRx| +// 2 | 0 0 1| |sinRy 0 cosRy| |0 -sinRx cosRx| +// This matrix is thought to be used within the following framework: +// let's suppose we define positive rotations around a system of orthogonal axes in this manner: +// a rotation of +90 degrees around axis3 converts axis2 into axis1 +// a rotation of +90 degrees around axis2 converts axis1 into axis3 +// a rotation of +90 degrees around axis1 converts axis3 into axis2 +// and let's suppose that we have defined a new orthonormal axes system (FLU) +// by doing the following operations about the standard one (XYZ): +// first rotating the XYZ system around Z by an angle Rz (obtaining X'Y'Z'), +// then rotating the resulting system around Y by an angle Ry (obtaining X''Y''Z'') +// and, finally, rotating the resulting system around X by an angle Rx (obtaining FLU) +// This FLU (forward/left/up) system represents an "attitude" and, then, the matrix here defined +// is the change of coordinates matrix that transform coordinates in the FLU +// system (the "object coordinates") into the standard XYZ system (the "global coordinates"), +// multiplied by a scale factor S/2, that is: +// |x| S |f| +// |y| * - = MatrixA * |l| +// |z| 2 |u| +// In a similar way, if we use the transpose of the matrix, we can transform global coordinates +// into object coordinates: +// |f| S |x| +// |l| * - = MatrixA_transposed * |y| +// |u| 2 |z| +// +// input[0]: S +// input[1]: Rz +// input[2]: Ry +// input[3]: Rx -static INT16 FxParm, FyParm, FzParm, AasParm, AzsParm, LfeParm, LesParm; - -static void DSP1_Parameter(INT16 Fx, INT16 Fy, INT16 Fz, INT16 Lfe, INT16 Les, INT16 Aas, INT16 Azs, INT16 *Vof, INT16 *Vva, INT16 *Cx, INT16 *Cy) +static void DSP1_attitudeA(INT16 *input, INT16 *output) { - INT16 CSec, C, E; + INT16 S = input[0]; + INT16 Rz = input[1]; + INT16 Ry = input[2]; + INT16 Rx = input[3]; - // Copy Zenith angle for clipping - INT16 AzsB = Azs; + INT16 SinRz = DSP1_sin(Rz); + INT16 CosRz = DSP1_cos(Rz); + INT16 SinRy = DSP1_sin(Ry); + INT16 CosRy = DSP1_cos(Ry); + INT16 SinRx = DSP1_sin(Rx); + INT16 CosRx = DSP1_cos(Rx); - INT16 MaxAZS; + S >>= 1; - // store off parameters for use by the Project operation - FxParm = Fx; - FyParm = Fy; - FzParm = Fz; - AasParm = Aas; - AzsParm = Azs; - LfeParm = Lfe; - LesParm = Les; + shared.MatrixA[0][0] = (S * CosRz >> 15) * CosRy >> 15; + shared.MatrixA[0][1] = ((S * SinRz >> 15) * CosRx >> 15) + (((S * CosRz >> 15) * SinRx >> 15) * SinRy >> 15); + shared.MatrixA[0][2] = ((S * SinRz >> 15) * SinRx >> 15) - (((S * CosRz >> 15) * CosRx >> 15) * SinRy >> 15); - // Store Sine and Cosine of Azimuth and Zenith angle - SinAas = DSP1_Sin(Aas); - CosAas = DSP1_Cos(Aas); - SinAzs = DSP1_Sin(Azs); - CosAzs = DSP1_Cos(Azs); + shared.MatrixA[1][0] = -((S * SinRz >> 15) * CosRy >> 15); + shared.MatrixA[1][1] = ((S * CosRz >> 15) * CosRx >> 15) - (((S * SinRz >> 15) * SinRx >> 15) * SinRy >> 15); + shared.MatrixA[1][2] = ((S * CosRz >> 15) * SinRx >> 15) + (((S * SinRz >> 15) * CosRx >> 15) * SinRy >> 15); - Nx = SinAzs * -SinAas >> 15; - Ny = SinAzs * CosAas >> 15; - Nz = CosAzs * 0x7fff >> 15; - - // Center of Projection - CentreX = Fx + (Lfe * Nx >> 15); - CentreY = Fy + (Lfe * Ny >> 15); - CentreZ = Fz + (Lfe * Nz >> 15); - - Gx = CentreX - (Les * Nx >> 15); - Gy = CentreY - (Les * Ny >> 15); - Gz = CentreZ - (Les * Nz >> 15); - - Les_E = 0; - DSP1_Normalize(Les, &Les_C, &Les_E); - Les_G = Les; - - E = 0; - DSP1_Normalize(CentreZ, &C, &E); - - VPlane_C = C; - VPlane_E = E; - - // Determine clip boundary and clip Zenith angle if necessary - MaxAZS = MaxAZS_Exp[-E]; - - if (AzsB < 0) { - MaxAZS = -MaxAZS; - if (AzsB < MaxAZS + 1) AzsB = MaxAZS + 1; - } else { - if (AzsB > MaxAZS) AzsB = MaxAZS; - } - - // Store Sine and Cosine of clipped Zenith angle - SinAzsB = DSP1_Sin(AzsB); - CosAzsB = DSP1_Cos(AzsB); - - DSP1_Inverse(CosAzsB, 0, &SecAZS_C1, &SecAZS_E1); - DSP1_Normalize(C * SecAZS_C1 >> 15, &C, &E); - E += SecAZS_E1; - - C = DSP1_Truncate(C, E) * SinAzsB >> 15; - - CentreX += C * SinAas >> 15; - CentreY -= C * CosAas >> 15; - - *Cx = CentreX; - *Cy = CentreY; - - // Raster number of imaginary center and horizontal line - *Vof = 0; - - if ((Azs != AzsB) || (Azs == MaxAZS)) - { - INT16 Aux; - - if (Azs == -32768) Azs = -32767; - - C = Azs - MaxAZS; - if (C >= 0) C--; - Aux = ~(C << 2); - - C = Aux * DSP1ROM[0x0328] >> 15; - C = (C * Aux >> 15) + DSP1ROM[0x0327]; - *Vof -= (C * Aux >> 15) * Les >> 15; - - C = Aux * Aux >> 15; - Aux = (C * DSP1ROM[0x0324] >> 15) + DSP1ROM[0x0325]; - CosAzsB += (C * Aux >> 15) * CosAzsB >> 15; - } - - VOffset = Les * CosAzsB >> 15; - - DSP1_Inverse(SinAzsB, 0, &CSec, &E); - DSP1_Normalize(VOffset, &C, &E); - DSP1_Normalize(C * CSec >> 15, &C, &E); - - if (C == -32768) { C >>= 1; E++; } - - *Vva = DSP1_Truncate(-C, E); - - // Store Secant of clipped Zenith angle - DSP1_Inverse(CosAzsB, 0, &SecAZS_C2, &SecAZS_E2); + shared.MatrixA[2][0] = S * SinRy >> 15; + shared.MatrixA[2][1] = -((S * SinRx >> 15) * CosRy >> 15); + shared.MatrixA[2][2] = (S * CosRx >> 15) * CosRy >> 15; } -// 06H - Object Projection Calculation -// Input byte(06H) integer(X) integer(Y) integer(Z) -// Output integer(H) integer(V) integer(M) +////////////////////////////////////////////////////////////////// -static void DSP1_Project(INT16 X, INT16 Y, INT16 Z, INT16 *H, INT16 *V, UINT16 *M) +// Same than 'attitudeA', but with a difference attitude matrix (matrixB) + +static void DSP1_attitudeB(INT16 *input, INT16 *output) { - double Px, Py, Pz, Px1, Py1, Pz1, Px2, Py2, Pz2; - INT32 dTan; + INT16 S = input[0]; + INT16 Rz = input[1]; + INT16 Ry = input[2]; + INT16 Rx = input[3]; - Px = X - FxParm; - Py = Y - FyParm; - Pz = Z - FzParm; + INT16 SinRz = DSP1_sin(Rz); + INT16 CosRz = DSP1_cos(Rz); + INT16 SinRy = DSP1_sin(Ry); + INT16 CosRy = DSP1_cos(Ry); + INT16 SinRx = DSP1_sin(Rx); + INT16 CosRx = DSP1_cos(Rx); - // rotate around the Z axis - dTan = Angle(-AasParm+32768); - Px1 = (Px*Cos(dTan)+Py*-Sin(dTan)); - Py1 = (Px*Sin(dTan)+Py*Cos(dTan)); - Pz1 = Pz; + S >>= 1; - // rotate around the X axis - dTan = Angle(-AzsParm); - Px2 = Px1; - Py2 = (Py1*Cos(dTan)+Pz1*-Sin(dTan)); - Pz2 = (Py1*Sin(dTan)+Pz1*Cos(dTan)); + shared.MatrixB[0][0] = (S * CosRz >> 15) * CosRy >> 15; + shared.MatrixB[0][1] = ((S * SinRz >> 15) * CosRx >> 15) + (((S * CosRz >> 15) * SinRx >> 15) * SinRy >> 15); + shared.MatrixB[0][2] = ((S * SinRz >> 15) * SinRx >> 15) - (((S * CosRz >> 15) * CosRx >> 15) * SinRy >> 15); - Pz2 = Pz2-LfeParm; + shared.MatrixB[1][0] = -((S * SinRz >> 15) * CosRy >> 15); + shared.MatrixB[1][1] = ((S * CosRz >> 15) * CosRx >> 15) - (((S * SinRz >> 15) * SinRx >> 15) * SinRy >> 15); + shared.MatrixB[1][2] = ((S * CosRz >> 15) * SinRx >> 15) + (((S * SinRz >> 15) * CosRx >> 15) * SinRy >> 15); - if (Pz2 < 0) - { - double dM; - - *H = (INT16)(-Px2*LesParm/-Pz2); - *V = (INT16)(-Py2*LesParm/-Pz2); - - dM = (double)LesParm; - dM *= 256.0; - dM /= -Pz2; - - if (dM > 65535.0) - { - dM = 65535.0; - } - else if (dM < 0.0) - { - dM = 0.0; - } - - *M = (UINT16)dM; - } - else - { - *H = 0; - *V = 224; - *M = 65535; - } + shared.MatrixB[2][0] = S * SinRy >> 15; + shared.MatrixB[2][1] = -((S * SinRx >> 15) * CosRy >> 15); + shared.MatrixB[2][2] = (S * CosRx >> 15) * CosRy >> 15; } -// 0AH - Raster Data Calculation (Bit Perfect) -// Input byte(0AH) integer(Vs) -// Output integer(An) integer(Bn) integer(Cn) integer(Dn) +////////////////////////////////////////////////////////////////// -static void DSP1_Raster(INT16 Vs, INT16 *An, INT16 *Bn, INT16 *Cn, INT16 *Dn) +// Same than 'attitudeA', but with a difference attitude matrix (matrixC) + +static void DSP1_attitudeC(INT16 *input, INT16 *output) { - INT16 C, E, C1, E1; + INT16 S = input[0]; + INT16 Rz = input[1]; + INT16 Ry = input[2]; + INT16 Rx = input[3]; - DSP1_Inverse((Vs * SinAzs >> 15) + VOffset, 7, &C, &E); - E += VPlane_E; + INT16 SinRz = DSP1_sin(Rz); + INT16 CosRz = DSP1_cos(Rz); + INT16 SinRy = DSP1_sin(Ry); + INT16 CosRy = DSP1_cos(Ry); + INT16 SinRx = DSP1_sin(Rx); + INT16 CosRx = DSP1_cos(Rx); - C1 = C * VPlane_C >> 15; - E1 = E + SecAZS_E2; + S >>= 1; - DSP1_Normalize(C1, &C, &E); + shared.MatrixC[0][0] = (S * CosRz >> 15) * CosRy >> 15; + shared.MatrixC[0][1] = ((S * SinRz >> 15) * CosRx >> 15) + (((S * CosRz >> 15) * SinRx >> 15) * SinRy >> 15); + shared.MatrixC[0][2] = ((S * SinRz >> 15) * SinRx >> 15) - (((S * CosRz >> 15) * CosRx >> 15) * SinRy >> 15); - C = DSP1_Truncate(C, E); + shared.MatrixC[1][0] = -((S * SinRz >> 15) * CosRy >> 15); + shared.MatrixC[1][1] = ((S * CosRz >> 15) * CosRx >> 15) - (((S * SinRz >> 15) * SinRx >> 15) * SinRy >> 15); + shared.MatrixC[1][2] = ((S * CosRz >> 15) * SinRx >> 15) + (((S * SinRz >> 15) * CosRx >> 15) * SinRy >> 15); - *An = C * CosAas >> 15; - *Cn = C * SinAas >> 15; - - DSP1_Normalize(C1 * SecAZS_C2 >> 15, &C, &E1); - - C = DSP1_Truncate(C, E1); - - *Bn = C * -SinAas >> 15; - *Dn = C * CosAas >> 15; + shared.MatrixC[2][0] = S * SinRy >> 15; + shared.MatrixC[2][1] = -((S * SinRx >> 15) * CosRy >> 15); + shared.MatrixC[2][2] = (S * CosRx >> 15) * CosRy >> 15; } -// 0EH - Coordinate Calculation of a Selected Point on the Screen (Bit Perfect) -// Input byte(0EH) integer(H) integer(V) -// Output integer(X) integer(Y) +////////////////////////////////////////////////////////////////// -static void DSP1_Target(INT16 H, INT16 V, INT16 *X, INT16 *Y) +// Convert global coordinates (X,Y,Z) to object coordinates (F,L,U) +// See the comment in "attitudeA" for a explanation about the calculation. +// +// input[0]: X ; input[1]: Y ; input[2]: Z +// &output[0]: F ; &output[1]: L ; &output[2]: U + +static void DSP1_objectiveA(INT16 *input, INT16 *output) { - INT16 C, E, C1, E1; + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* F = &output[0]; + INT16* L = &output[1]; + INT16* U = &output[2]; - DSP1_Inverse((V * SinAzs >> 15) + VOffset, 8, &C, &E); - E += VPlane_E; - - C1 = C * VPlane_C >> 15; - E1 = E + SecAZS_E1; - - H <<= 8; - - DSP1_Normalize(C1, &C, &E); - - C = DSP1_Truncate(C, E) * H >> 15; - - *X = CentreX + (C * CosAas >> 15); - *Y = CentreY - (C * SinAas >> 15); - - V <<= 8; - - DSP1_Normalize(C1 * SecAZS_C1 >> 15, &C, &E1); - - C = DSP1_Truncate(C, E1) * V >> 15; - - *X += C * -SinAas >> 15; - *Y += C * CosAas >> 15; + *F = (shared.MatrixA[0][0] * X >> 15) + (shared.MatrixA[1][0] * Y >> 15) + (shared.MatrixA[2][0] * Z >> 15); + *L = (shared.MatrixA[0][1] * X >> 15) + (shared.MatrixA[1][1] * Y >> 15) + (shared.MatrixA[2][1] * Z >> 15); + *U = (shared.MatrixA[0][2] * X >> 15) + (shared.MatrixA[1][2] * Y >> 15) + (shared.MatrixA[2][2] * Z >> 15); } -// 01H - Set Attitude A (Bit Perfect) -// Input byte(01H) integer(M) integer(Az) integer(Ay) integer(Ax) -// Output None +////////////////////////////////////////////////////////////////// -static INT16 MatrixA[3][3]; +// Same than 'objectiveA', but for the 'B' attitude -static void DSP1_Attitude_A(INT16 M, INT16 Az, INT16 Ay, INT16 Ax) +static void DSP1_objectiveB(INT16 *input, INT16 *output) { - INT16 SinAz = DSP1_Sin(Az); - INT16 CosAz = DSP1_Cos(Az); - INT16 SinAy = DSP1_Sin(Ay); - INT16 CosAy = DSP1_Cos(Ay); - INT16 SinAx = DSP1_Sin(Ax); - INT16 CosAx = DSP1_Cos(Ax); + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* F = &output[0]; + INT16* L = &output[1]; + INT16* U = &output[2]; - M >>= 1; - - MatrixA[0][0] = (M * CosAz >> 15) * CosAy >> 15; - MatrixA[0][1] = -((M * SinAz >> 15) * CosAy >> 15); - MatrixA[0][2] = M * SinAy >> 15; - - MatrixA[1][0] = ((M * SinAz >> 15) * CosAx >> 15) + (((M * CosAz >> 15) * SinAx >> 15) * SinAy >> 15); - MatrixA[1][1] = ((M * CosAz >> 15) * CosAx >> 15) - (((M * SinAz >> 15) * SinAx >> 15) * SinAy >> 15); - MatrixA[1][2] = -((M * SinAx >> 15) * CosAy >> 15); - - MatrixA[2][0] = ((M * SinAz >> 15) * SinAx >> 15) - (((M * CosAz >> 15) * CosAx >> 15) * SinAy >> 15); - MatrixA[2][1] = ((M * CosAz >> 15) * SinAx >> 15) + (((M * SinAz >> 15) * CosAx >> 15) * SinAy >> 15); - MatrixA[2][2] = (M * CosAx >> 15) * CosAy >> 15; + *F = (shared.MatrixB[0][0] * X >> 15) + (shared.MatrixB[1][0] * Y >> 15) + (shared.MatrixB[2][0] * Z >> 15); + *L = (shared.MatrixB[0][1] * X >> 15) + (shared.MatrixB[1][1] * Y >> 15) + (shared.MatrixB[2][1] * Z >> 15); + *U = (shared.MatrixB[0][2] * X >> 15) + (shared.MatrixB[1][2] * Y >> 15) + (shared.MatrixB[2][2] * Z >> 15); } -// 11H - Set Attitude B (Bit Perfect) -// Input byte(11H) integer(M) integer(Az) integer(Ay) integer(Ax) -// Output None +////////////////////////////////////////////////////////////////// -static INT16 MatrixB[3][3]; +// Same than 'objectiveA', but for the 'C' attitude -static void DSP1_Attitude_B(INT16 M, INT16 Az, INT16 Ay, INT16 Ax) +static void DSP1_objectiveC(INT16 *input, INT16 *output) { - INT16 SinAz = DSP1_Sin(Az); - INT16 CosAz = DSP1_Cos(Az); - INT16 SinAy = DSP1_Sin(Ay); - INT16 CosAy = DSP1_Cos(Ay); - INT16 SinAx = DSP1_Sin(Ax); - INT16 CosAx = DSP1_Cos(Ax); + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* F = &output[0]; + INT16* L = &output[1]; + INT16* U = &output[2]; - M >>= 1; - - MatrixB[0][0] = (M * CosAz >> 15) * CosAy >> 15; - MatrixB[0][1] = -((M * SinAz >> 15) * CosAy >> 15); - MatrixB[0][2] = M * SinAy >> 15; - - MatrixB[1][0] = ((M * SinAz >> 15) * CosAx >> 15) + (((M * CosAz >> 15) * SinAx >> 15) * SinAy >> 15); - MatrixB[1][1] = ((M * CosAz >> 15) * CosAx >> 15) - (((M * SinAz >> 15) * SinAx >> 15) * SinAy >> 15); - MatrixB[1][2] = -((M * SinAx >> 15) * CosAy >> 15); - - MatrixB[2][0] = ((M * SinAz >> 15) * SinAx >> 15) - (((M * CosAz >> 15) * CosAx >> 15) * SinAy >> 15); - MatrixB[2][1] = ((M * CosAz >> 15) * SinAx >> 15) + (((M * SinAz >> 15) * CosAx >> 15) * SinAy >> 15); - MatrixB[2][2] = (M * CosAx >> 15) * CosAy >> 15; + *F = (shared.MatrixC[0][0] * X >> 15) + (shared.MatrixC[1][0] * Y >> 15) + (shared.MatrixC[2][0] * Z >> 15); + *L = (shared.MatrixC[0][1] * X >> 15) + (shared.MatrixC[1][1] * Y >> 15) + (shared.MatrixC[2][1] * Z >> 15); + *U = (shared.MatrixC[0][2] * X >> 15) + (shared.MatrixC[1][2] * Y >> 15) + (shared.MatrixC[2][2] * Z >> 15); } -// 21H - Set Attitude C (Bit Perfect) -// Input byte(21H) integer(M) integer(Az) integer(Ay) integer(Ax) -// Output None +////////////////////////////////////////////////////////////////// -static INT16 MatrixC[3][3]; +// Convert object coordinates (F,L,U) to object coordinates (X,Y,Z) +// See the comment in "attitudeA" for a explanation about the calculation. +// +// input[0]: F ; input[1]: L ; input[2]: U +// &output[0]: X ; &output[1]: Y ; &output[2]: Z -#ifdef UNUSED_FUNCTION -static void DSP1_Attitude_C(INT16 M, INT16 Az, INT16 Ay, INT16 Ax) +static void DSP1_subjectiveA(INT16 *input, INT16 *output) { - INT16 SinAz = DSP1_Sin(Az); - INT16 CosAz = DSP1_Cos(Az); - INT16 SinAy = DSP1_Sin(Ay); - INT16 CosAy = DSP1_Cos(Ay); - INT16 SinAx = DSP1_Sin(Ax); - INT16 CosAx = DSP1_Cos(Ax); + INT16 F = input[0]; + INT16 L = input[1]; + INT16 U = input[2]; + INT16* X = &output[0]; + INT16* Y = &output[1]; + INT16* Z = &output[2]; - M >>= 1; - - MatrixC[0][0] = (M * CosAz >> 15) * CosAy >> 15; - MatrixC[0][1] = -((M * SinAz >> 15) * CosAy >> 15); - MatrixC[0][2] = M * SinAy >> 15; - - MatrixC[1][0] = ((M * SinAz >> 15) * CosAx >> 15) + (((M * CosAz >> 15) * SinAx >> 15) * SinAy >> 15); - MatrixC[1][1] = ((M * CosAz >> 15) * CosAx >> 15) - (((M * SinAz >> 15) * SinAx >> 15) * SinAy >> 15); - MatrixC[1][2] = -((M * SinAx >> 15) * CosAy >> 15); - - MatrixC[2][0] = ((M * SinAz >> 15) * SinAx >> 15) - (((M * CosAz >> 15) * CosAx >> 15) * SinAy >> 15); - MatrixC[2][1] = ((M * CosAz >> 15) * SinAx >> 15) + (((M * SinAz >> 15) * CosAx >> 15) * SinAy >> 15); - MatrixC[2][2] = (M * CosAx >> 15) * CosAy >> 15; + *X = (shared.MatrixA[0][0] * F >> 15) + (shared.MatrixA[0][1] * L >> 15) + (shared.MatrixA[0][2] * U >> 15); + *Y = (shared.MatrixA[1][0] * F >> 15) + (shared.MatrixA[1][1] * L >> 15) + (shared.MatrixA[1][2] * U >> 15); + *Z = (shared.MatrixA[2][0] * F >> 15) + (shared.MatrixA[2][1] * L >> 15) + (shared.MatrixA[2][2] * U >> 15); } -#endif -// 0DH - Convert from Global to Object Coordinates A (Bit Perfect) -// Input byte(0DH) integer(X) integer(Y) integer(Z) -// Output integer(F) integer(L) integer(U) +////////////////////////////////////////////////////////////////// -static void DSP1_Objective_A(INT16 X, INT16 Y, INT16 Z, INT16 *F, INT16 *L, INT16 *U) +// Same than 'subjectiveA', but for the 'B' attitude + +static void DSP1_subjectiveB(INT16 *input, INT16 *output) { - *F = (X * MatrixA[0][0] >> 15) + (Y * MatrixA[0][1] >> 15) + (Z * MatrixA[0][2] >> 15); - *L = (X * MatrixA[1][0] >> 15) + (Y * MatrixA[1][1] >> 15) + (Z * MatrixA[1][2] >> 15); - *U = (X * MatrixA[2][0] >> 15) + (Y * MatrixA[2][1] >> 15) + (Z * MatrixA[2][2] >> 15); + INT16 F = input[0]; + INT16 L = input[1]; + INT16 U = input[2]; + INT16* X = &output[0]; + INT16* Y = &output[1]; + INT16* Z = &output[2]; + + *X = (shared.MatrixB[0][0] * F >> 15) + (shared.MatrixB[0][1] * L >> 15) + (shared.MatrixB[0][2] * U >> 15); + *Y = (shared.MatrixB[1][0] * F >> 15) + (shared.MatrixB[1][1] * L >> 15) + (shared.MatrixB[1][2] * U >> 15); + *Z = (shared.MatrixB[2][0] * F >> 15) + (shared.MatrixB[2][1] * L >> 15) + (shared.MatrixB[2][2] * U >> 15); } -// 1DH - Convert from Global to Object Coordinates B (Bit Perfect) -// Input byte(1DH) integer(X) integer(Y) integer(Z) -// Output integer(F) integer(L) integer(U) +////////////////////////////////////////////////////////////////// -void DSP1_Objective_B(INT16 X, INT16 Y, INT16 Z, INT16 *F, INT16 *L, INT16 *U) +// Same than 'subjectiveA', but for the 'C' attitude + +static void DSP1_subjectiveC(INT16 *input, INT16 *output) { - *F = (X * MatrixB[0][0] >> 15) + (Y * MatrixB[0][1] >> 15) + (Z * MatrixB[0][2] >> 15); - *L = (X * MatrixB[1][0] >> 15) + (Y * MatrixB[1][1] >> 15) + (Z * MatrixB[1][2] >> 15); - *U = (X * MatrixB[2][0] >> 15) + (Y * MatrixB[2][1] >> 15) + (Z * MatrixB[2][2] >> 15); + INT16 F = input[0]; + INT16 L = input[1]; + INT16 U = input[2]; + INT16* X = &output[0]; + INT16* Y = &output[1]; + INT16* Z = &output[2]; + + *X = (shared.MatrixC[0][0] * F >> 15) + (shared.MatrixC[0][1] * L >> 15) + (shared.MatrixC[0][2] * U >> 15); + *Y = (shared.MatrixC[1][0] * F >> 15) + (shared.MatrixC[1][1] * L >> 15) + (shared.MatrixC[1][2] * U >> 15); + *Z = (shared.MatrixC[2][0] * F >> 15) + (shared.MatrixC[2][1] * L >> 15) + (shared.MatrixC[2][2] * U >> 15); } -// 2DH - Convert from Global to Object Coordinates C (Bit Perfect) -// Input byte(2DH) integer(X) integer(Y) integer(Z) -// Output integer(F) integer(L) integer(U) +////////////////////////////////////////////////////////////////// -static void DSP1_Objective_C(INT16 X, INT16 Y, INT16 Z, INT16 *F, INT16 *L, INT16 *U) +// This command calculates the inner product (S) of a vector (X,Y,Z) and +// the first column of MatrixA. It should be noted that that first column +// represent the global coordinates of an unity vector in the forward +// direction in the object coordinate system (coordinates (1,0,0) in the FLU +// axes system). +// +// input[0]: X ; input[1]: Y ; input[2]: Z +// &output[0]: S + +static void DSP1_scalarA(INT16 *input, INT16 *output) { - *F = (X * MatrixC[0][0] >> 15) + (Y * MatrixC[0][1] >> 15) + (Z * MatrixC[0][2] >> 15); - *L = (X * MatrixC[1][0] >> 15) + (Y * MatrixC[1][1] >> 15) + (Z * MatrixC[1][2] >> 15); - *U = (X * MatrixC[2][0] >> 15) + (Y * MatrixC[2][1] >> 15) + (Z * MatrixC[2][2] >> 15); + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* S = &output[0]; + + *S = (X * shared.MatrixA[0][0] + Y * shared.MatrixA[1][0] + Z * shared.MatrixA[2][0]) >> 15; } -// 03H - Conversion from Object to Global Coordinates A (Bit Perfect) -// Input byte(03H) integer(F) integer(L) integer(U) -// Output integer(X) integer(Y) integer(Z) +////////////////////////////////////////////////////////////////// -static void DSP1_Subjective_A(INT16 F, INT16 L, INT16 U, INT16 *X, INT16 *Y, INT16 *Z) +// Same than 'scalarA', but for the 'B' attitude + +static void DSP1_scalarB(INT16 *input, INT16 *output) { - *X = (F * MatrixA[0][0] >> 15) + (L * MatrixA[1][0] >> 15) + (U * MatrixA[2][0] >> 15); - *Y = (F * MatrixA[0][1] >> 15) + (L * MatrixA[1][1] >> 15) + (U * MatrixA[2][1] >> 15); - *Z = (F * MatrixA[0][2] >> 15) + (L * MatrixA[1][2] >> 15) + (U * MatrixA[2][2] >> 15); + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* S = &output[0]; + + *S = (X * shared.MatrixB[0][0] + Y * shared.MatrixB[1][0] + Z * shared.MatrixB[2][0]) >> 15; } -// 13H - Conversion from Object to Global Coordinates B (Bit Perfect) -// Input byte(13H) integer(F) integer(L) integer(U) -// Output integer(X) integer(Y) integer(Z) +////////////////////////////////////////////////////////////////// -static void DSP1_Subjective_B(INT16 F, INT16 L, INT16 U, INT16 *X, INT16 *Y, INT16 *Z) +// Same than 'scalarA', but for the 'C' attitude + +static void DSP1_scalarC(INT16 *input, INT16 *output) { - *X = (F * MatrixB[0][0] >> 15) + (L * MatrixB[1][0] >> 15) + (U * MatrixB[2][0] >> 15); - *Y = (F * MatrixB[0][1] >> 15) + (L * MatrixB[1][1] >> 15) + (U * MatrixB[2][1] >> 15); - *Z = (F * MatrixB[0][2] >> 15) + (L * MatrixB[1][2] >> 15) + (U * MatrixB[2][2] >> 15); + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* S = &output[0]; + + *S = (X * shared.MatrixC[0][0] + Y * shared.MatrixC[1][0] + Z * shared.MatrixC[2][0]) >> 15; } -// 23H - Conversion from Object to Global Coordinates C (Bit Perfect) -// Input byte(23H) integer(F) integer(L) integer(U) -// Output integer(X) integer(Y) integer(Z) +////////////////////////////////////////////////////////////////// -static void DSP1_Subjective_C(INT16 F, INT16 L, INT16 U, INT16 *X, INT16 *Y, INT16 *Z) +// This command determines the final attitude angles after the body with attitude angles (Ax, Ay, Az) with +// respect to the global coordinates is rotated by the minor angular displacements (DeltaF, DeltaL, DeltaU). +// It means that the XYZ axes are rotated by (Ax, Ay, Az) to obtain the FLU axes and, then, these +// are rotated by (DeltaF, DeltaL, DeltaU). The command calculates and return the new FLU angles respect to the +// XYZ system (Rx, Ry, Rz) +// The formulae are: +// Rx = Ax + (DeltaU*sin(Ay)+DeltaF*cos(Ay)) +// Ry = Ay + DeltaL - tan(Ax)*(DeltaU*cos(Ay)+DeltaF*sin(Ay)) +// Rz = Az + sec(Ax)*(DeltaU*cos(Ay)-DeltaF*sin(Ay)) +// +// Now the discussion: according to the official documentation, as described in various commands, you pass from +// XYZ to FLU by doing the rotations in the order Y, X, Z. In this command, the formulae are coherent with the +// fact that Y is the first axis to do a rotation around it. However, in the "attitude" command, while the official +// document describe it that way, we have discovered, when reverse engineering the command, that the calculated +// matrix do the rotation around Y in the second place. This incoherent behaviour of various commands is, in my +// opinion, a pretty severe implementation error. However, if you only use small "minor displacements", the error term +// introduced by that incoherence should be almost negligible. + +static void DSP1_gyrate(INT16 *input, INT16 *output) { - *X = (F * MatrixC[0][0] >> 15) + (L * MatrixC[1][0] >> 15) + (U * MatrixC[2][0] >> 15); - *Y = (F * MatrixC[0][1] >> 15) + (L * MatrixC[1][1] >> 15) + (U * MatrixC[2][1] >> 15); - *Z = (F * MatrixC[0][2] >> 15) + (L * MatrixC[1][2] >> 15) + (U * MatrixC[2][2] >> 15); + INT16 Az = input[0]; + INT16 Ax = input[1]; + INT16 Ay = input[2]; + INT16 U = input[3]; + INT16 F = input[4]; + INT16 L = input[5]; + INT16* Rz = &output[0]; + INT16* Rx = &output[1]; + INT16* Ry = &output[2]; + + INT16 CSec, ESec, CSin, C, E; + INT16 SinAy = DSP1_sin(Ay); + INT16 CosAy = DSP1_cos(Ay); + + inverse(DSP1_cos(Ax), 0, &CSec, &ESec); + + // Rotation Around Z + normalizeDouble(U * CosAy - F * SinAy, &C, &E); + + E = ESec - E; + + normalize(C * CSec >> 15, &C, &E); + + *Rz = Az + denormalizeAndClip(C, E); + + // Rotation Around X + *Rx = Ax + (U * SinAy >> 15) + (F * CosAy >> 15); + + // Rotation Around Y + normalizeDouble(U * CosAy + F * SinAy, &C, &E); + + E = ESec - E; + + normalize(DSP1_sin(Ax), &CSin, &E); + + normalize(-(C * (CSec * CSin >> 15) >> 15), &C, &E); + + *Ry = Ay + denormalizeAndClip(C, E) + L; } -// 0BH - Calculation of Inner Product with Forward Attitude A and a Vector (Bit Perfect) -// Input byte(0BH) integer(X) integer(Y) integer(Z) -// Output integer(S) +////////////////////////////////////////////////////////////////// -static void DSP1_Scalar_A(INT16 X, INT16 Y, INT16 Z, INT16 *S) +static const INT16 DSP1_MaxAZS_Exp[16] = { + 0x38b4, 0x38b7, 0x38ba, 0x38be, 0x38c0, 0x38c4, 0x38c7, 0x38ca, + 0x38ce, 0x38d0, 0x38d4, 0x38d7, 0x38da, 0x38dd, 0x38e0, 0x38e4 +}; + +////////////////////////////////////////////////////////////////// + + +// Set-up the projection framework. Besides returning some values, it store in RAM some values that +// will be used by the other three projection commands (raster, target an project) +// Input: +// (Fx, Fy, Fz)-> coordinates of base point (global coordinates) +// Lfe-> distance between the base point and the viewpoint (center of projection) +// Les-> distance between the base point and the screen +// Aas-> azimuth angle (0 degrees is east; 90 degrees is north) +// Azs-> zenith angle (0 degrees is zenith) +// Output: +// Vof-> raster line of imaginary center (whatever it means ;) ) +// Vva-> raster line representing the horizon line +// (Cx, Cy)-> coordinates of the projection of the center of the screen over the ground (ground coordinates) + +static void DSP1_parameter(INT16 *input, INT16 *output) { - *S = ((X * MatrixA[0][0]) + (Y * MatrixA[0][1]) + (Z * MatrixA[0][2])) >> 15; + INT16 Fx = input[0]; + INT16 Fy = input[1]; + INT16 Fz = input[2]; + INT16 Lfe = input[3]; + INT16 Les = input[4]; + INT16 Aas = input[5]; + INT16 Azs = input[6]; + INT16* Vof = &output[0]; + INT16* Vva = &output[1]; + INT16* Cx = &output[2]; + INT16* Cy = &output[3]; + + INT16 CSec, C, E; + INT16 LfeNx, LfeNy, LfeNz; + INT16 LesNx, LesNy, LesNz; + INT16 AZS, MaxAZS; + + // Copy Zenith angle for clipping + AZS = Azs; + + // Store Les and his coefficient and exponent when normalized + shared.Les = Les; + shared.E_Les=0; + normalize(Les, &shared.C_Les, &shared.E_Les); + + // Store Sine and Cosine of Azimuth and Zenith angle + shared.SinAas = DSP1_sin(Aas); + shared.CosAas = DSP1_cos(Aas); + shared.SinAzs = DSP1_sin(Azs); + shared.CosAzs = DSP1_cos(Azs); + + // normal vector to the screen (norm 1, points toward the center of projection) + shared.Nx = shared.SinAzs * -shared.SinAas >> 15; + shared.Ny = shared.SinAzs * shared.CosAas >> 15; + shared.Nz = shared.CosAzs * 0x7fff >> 15; + + // horizontal vector of the screen (Hz=0, norm 1, points toward the right of the screen) + shared.Hx = shared.CosAas*0x7fff>>15; + shared.Hy = shared.SinAas*0x7fff>>15; + + // vertical vector of the screen (norm 1, points toward the top of the screen) + shared.Vx = shared.CosAzs*-shared.SinAas>>15; + shared.Vy = shared.CosAzs*shared.CosAas>>15; + shared.Vz = -shared.SinAzs*0x7fff>>15; + + LfeNx = Lfe*shared.Nx>>15; + LfeNy = Lfe*shared.Ny>>15; + LfeNz = Lfe*shared.Nz>>15; + + // Center of Projection + shared.CentreX = Fx+LfeNx; + shared.CentreY = Fy+LfeNy; + shared.CentreZ = Fz+LfeNz; + + LesNx = Les*shared.Nx>>15; + LesNy = Les*shared.Ny>>15; + LesNz = Les*shared.Nz>>15; + + // center of the screen (global coordinates) + shared.Gx=shared.CentreX-LesNx; + shared.Gy=shared.CentreY-LesNy; + shared.Gz=shared.CentreZ-LesNz; + + + E = 0; + normalize(shared.CentreZ, &C, &E); + + shared.CentreZ_C = C; + shared.CentreZ_E = E; + + // Determine clip boundary and clip Zenith angle if necessary + // (Why to clip? Maybe to avoid the screen can only show sky with no ground? Only a guess...) + MaxAZS = DSP1_MaxAZS_Exp[-E]; + + if (AZS < 0) { + MaxAZS = -MaxAZS; + if (AZS < MaxAZS + 1) AZS = MaxAZS + 1; + } else { + if (AZS > MaxAZS) AZS = MaxAZS; + } + + // Store Sine and Cosine of clipped Zenith angle + shared.SinAZS = DSP1_sin(AZS); + shared.CosAZS = DSP1_cos(AZS); + + // calculate the separation of (cx, cy) from the projection of + // the 'centre of projection' over the ground... (CentreZ*tg(AZS)) + inverse(shared.CosAZS, 0, &shared.SecAZS_C1, &shared.SecAZS_E1); + normalize(C * shared.SecAZS_C1 >> 15, &C, &E); + E += shared.SecAZS_E1; + C = denormalizeAndClip(C, E) * shared.SinAZS >> 15; + + // ... and then take into account the position of the centre of + // projection and the azimuth angle + shared.CentreX += C * shared.SinAas >> 15; + shared.CentreY -= C * shared.CosAas >> 15; + + *Cx = shared.CentreX; + *Cy = shared.CentreY; + + // Raster number of imaginary center and horizontal line + *Vof = 0; + + if ((Azs != AZS) || (Azs == MaxAZS)) + { + INT16 Aux; + + // correct vof and vva when Azs is outside the 'non-clipping interval' + // we have only some few Taylor coefficients, so we cannot guess which ones + // are the approximated functions and, what is worse, we don't know why + // the own clipping stuff (and, particularly, this correction) is done + if (Azs == -32768) Azs = -32767; + + C = Azs - MaxAZS; + if (C >= 0) C--; + Aux = ~(C << 2); + + // Vof += x+(1/3)*x^3, where x ranges from 0 to PI/4 when Azs-MaxAZS goes from 0 to 0x2000 + C = Aux * DSP1_DataRom[0x0328] >> 15; + C = (C * Aux >> 15) + DSP1_DataRom[0x0327]; + *Vof -= (C * Aux >> 15) * Les >> 15; + + // CosAZS *= 1+(1/2)*x^2+(5/24)*x^24, where x ranges from 0 to PI/4 when Azs-MaxAZS goes from 0 to 0x2000 + C = Aux * Aux >> 15; + Aux = (C * DSP1_DataRom[0x0324] >> 15) + DSP1_DataRom[0x0325]; + shared.CosAZS += (C * Aux >> 15) * shared.CosAZS >> 15; + } + + // vertical offset of the screen with regard to the horizontal plane + // containing the centre of projection + shared.VOffset = Les * shared.CosAZS >> 15; + + // The horizon line (the line in the screen that is crossed by the horizon plane + // -the horizontal plane containing the 'centre of projection'-), + // will be at distance Les*cotg(AZS) from the centre of the screen. This is difficult + // to explain but easily seen in a graph. To better see it, consider it in this way: + // Les*tg(AZS-90), draw some lines and apply basic trigonometry. ;) + inverse(shared.SinAZS, 0, &CSec, &E); + normalize(shared.VOffset, &C, &E); + normalize(C * CSec >> 15, &C, &E); + + if (C == -32768) { C >>= 1; E++; } + + *Vva = denormalizeAndClip(-C, E); + + // Store Secant of clipped Zenith angle + inverse(shared.CosAZS, 0, &shared.SecAZS_C2, &shared.SecAZS_E2); } -// 1BH - Calculation of Inner Product with Forward Attitude B and a Vector (Bit Perfect) -// Input byte(1BH) integer(X) integer(Y) integer(Z) -// Output integer(S) +////////////////////////////////////////////////////////////////// -static void DSP1_Scalar_B(INT16 X, INT16 Y, INT16 Z, INT16 *S) +// Calculates the matrix which transform an object situated on a raster line (Vs) into +// his projection over the ground. The modified SecAZS is used here, so +// i don't understand the fine details, but, basically, it's done +// this way: The vertical offset between the point of projection and the +// raster line is calculated (Vs*SinAzs>>15)+VOffset, then the height of +// the center of projection is measured in that units (*CentreZ_C). If, now +// you consider the "reference case" (center of projection at an unit of height), +// the projection of a thin strip containing the raster line will have the same +// width (as the raster line would be on the ground in this case, but will suffer a +// change of scale in height (as the ground and the vertical axis would form an angle of 180-Azs degrees). +// This scale factor, when the angle 'center of screen-center of projection-raster line' is small, +// can be aproximated by the one of the center of the screen, 1/cos(Azs).(**) (Here is when it's used +// SecAZS). By last, you have to consider the effect of the azimuth angle Aas, and you are done. +// +// Using matrix notation: +// |A B| Centre_ZS | cos(Aas) -sin(Aas)| |1 0| +// ProjectionMatrix = | | = ----------- * | | * | | +// |C D| Vs*sin(Azs) |sin(Aas) cos(Aas)| |0 sec(Azs)| +// +// (**) +// If Les=1, the vertical offset between the center +// of projection and the center of the screen is Cos(Azs); then, if the vertical +// offset is 1, the ratio of the projection over the ground respect to the +// line on the screen is 1/cos(Azs). + +static void DSP1_raster(INT16 *input, INT16 *output) { - *S = ((X * MatrixB[0][0]) + (Y * MatrixB[0][1]) + (Z * MatrixB[0][2])) >> 15; + INT16 Vs = input[0]; + INT16* An = &output[0]; + INT16* Bn = &output[1]; + INT16* Cn = &output[2]; + INT16* Dn = &output[3]; + + INT16 C, E, C1, E1; + + inverse((Vs * shared.SinAzs >> 15) + shared.VOffset, 7, &C, &E); + + E += shared.CentreZ_E; + C1 = C * shared.CentreZ_C >> 15; + + E1 = E + shared.SecAZS_E2; + + normalize(C1, &C, &E); + C = denormalizeAndClip(C, E); + + *An = C * shared.CosAas >> 15; + *Cn = C * shared.SinAas >> 15; + + normalize(C1 * shared.SecAZS_C2 >> 15, &C, &E1); + C = denormalizeAndClip(C, E1); + + *Bn = C * -shared.SinAas >> 15; + *Dn = C * shared.CosAas >> 15; } -// 2BH - Calculation of Inner Product with Forward Attitude C and a Vector (Bit Perfect) -// Input byte(2BH) integer(X) integer(Y) integer(Z) -// Output integer(S) +////////////////////////////////////////////////////////////////// -static void DSP1_Scalar_C(INT16 X, INT16 Y, INT16 Z, INT16 *S) +// Calculate the projection over the ground of a selected point of screen +// It simply apply the projection matrix described in the "Raster" command +// to the vector (H,V) transposed, and add the result to the position of +// the centre of projection. +// The only special point to take into account is the directions on the screen: +// H is positive rightward, but V is positive downward; this is why +// the signs take that configuration + +static void DSP1_target(INT16 *input, INT16 *output) { - *S = ((X * MatrixC[0][0]) + (Y * MatrixC[0][1]) + (Z * MatrixC[0][2])) >> 15; + INT16 H = input[0]; + INT16 V = input[1]; + INT16* X = &output[0]; + INT16* Y = &output[1]; + + INT16 C, E, C1, E1; + + inverse((V * shared.SinAzs >> 15) + shared.VOffset, 8, &C, &E); + + E += shared.CentreZ_E; + C1 = C * shared.CentreZ_C >> 15; + + E1 = E + shared.SecAZS_E1; + + H <<= 8; + normalize(C1, &C, &E); + C = denormalizeAndClip(C, E) * H >> 15; + + *X = shared.CentreX + (C * shared.CosAas >> 15); + *Y = shared.CentreY - (C * shared.SinAas >> 15); + + V <<= 8; + normalize(C1 * shared.SecAZS_C1 >> 15, &C, &E1); + C = denormalizeAndClip(C, E1) * V >> 15; + + *X += C * -shared.SinAas >> 15; + *Y += C * shared.CosAas >> 15; } -// 14H - 3D Angle Rotation (Bit Perfect) -// Input byte(14H) integer(Az) integer(Ax) integer(Ay) integer(U) integer(F) integer(L) -// Output integer(Rz) integer(Rx) integer(Ry) +////////////////////////////////////////////////////////////////// -static void DSP1_Gyrate(INT16 Az, INT16 Ax, INT16 Ay, INT16 U, INT16 F, INT16 L, INT16 *Rz, INT16 *Rx, INT16 *Ry) +// Calculation of the projection over the screen (H,V) of an object (X,Y,Z) and his +// 'enlargement ratio' (M). The positive directions on the screen are as described +// in the targe command. M is scaled down by 2^-7, that is, M==0x0100 means ratio 1:1 + +static void DSP1_project(INT16 *input, INT16 *output) { - INT16 CSec, ESec, CSin, C, E; + INT16 X = input[0]; + INT16 Y = input[1]; + INT16 Z = input[2]; + INT16* H = &output[0]; + INT16* V = &output[1]; + INT16* M = &output[2]; - DSP1_Inverse(DSP1_Cos(Ax), 0, &CSec, &ESec); + INT32 aux, aux4; + INT16 E, E2, E3, E4, E5, refE, E6, E7; + INT16 C2, C4, C6, C8, C9, C10, C11, C12, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26; + INT16 Px, Py, Pz; - // Rotation Around Z - DSP1_NormalizeDouble(U * DSP1_Cos(Ay) - F * DSP1_Sin(Ay), &C, &E); + E4=E3=E2=E=E5=0; - E = ESec - E; + normalizeDouble((INT32)(X)-shared.Gx, &Px, &E4); + normalizeDouble((INT32)(Y)-shared.Gy, &Py, &E); + normalizeDouble((INT32)(Z)-shared.Gz, &Pz, &E3); + Px>>=1; E4--; // to avoid overflows when calculating the scalar products + Py>>=1; E--; + Pz>>=1; E3--; - DSP1_Normalize(C * CSec >> 15, &C, &E); + refE = (E> 15) + (F * DSP1_Cos(Ay) >> 15); + C11=- (Px*shared.Nx>>15); + C8=- (Py*shared.Ny>>15); + C9=- (Pz*shared.Nz>>15); + C12=C11+C8+C9; // this cannot overflow! - // Rotation Around Y - DSP1_NormalizeDouble(U * DSP1_Cos(Ay) + F * DSP1_Sin(Ay), &C, &E); + aux4=C12; // de-normalization with 32-bits arithmetic + refE = 16-refE; // refE can be up to 3 + if (refE>=0) + aux4 <<=(refE); + else + aux4 >>=-(refE); + if (aux4==-1) aux4 = 0; // why? + aux4>>=1; - E = ESec - E; + aux = (UINT16)(shared.Les) + aux4; // Les - the scalar product of P with the normal vector of the screen + normalizeDouble(aux, &C10, &E2); + E2 = 15-E2; - DSP1_Normalize(DSP1_Sin(Ax), &CSin, &E); + inverse(C10, 0, &C4, &E4); + C2=C4*shared.C_Les>>15; // scale factor - DSP1_Normalize(-(C * (CSec * CSin >> 15) >> 15), &C, &E); - *Ry = Ay + DSP1_Truncate(C, E) + L; + // H + E7=0; + C16= (Px*shared.Hx>>15); + C20= (Py*shared.Hy>>15); + C17=C16+C20; // scalar product of P with the normalized horizontal vector of the screen... + + C18=C17*C2>>15; // ... multiplied by the scale factor + normalize(C18, &C19, &E7); + *H=denormalizeAndClip(C19, shared.E_Les-E2+refE+E7); + + // V + E6=0; + C21 = Px*shared.Vx>>15; + C22 = Py*shared.Vy>>15; + C23 = Pz*shared.Vz>>15; + C24=C21+C22+C23; // scalar product of P with the normalized vertical vector of the screen... + + C26=C24*C2>>15; // ... multiplied by the scale factor + normalize(C26, &C25, &E6); + *V=denormalizeAndClip(C25, shared.E_Les-E2+refE+E6); + + // M + normalize(C2, &C6, &E4); + *M=denormalizeAndClip(C6, E4+shared.E_Les-E2-7); // M is the scale factor divided by 2^7 } -// 0FH - Memory Test (Bit Perfect) -// Input byte(0FH) integer(Size) -// Output integer(Result) +////////////////////////////////////////////////////////////////// -static void DSP1_MemoryTest(INT16 Size, INT16 *Result) +// Calculate the sine of the input parameter +// this is done by linear interpolation between +// the points of a look-up table + +static INT16 DSP1_sin(INT16 Angle) { - *Result = 0x0000; + INT32 S; + + if (Angle < 0) { + if (Angle == -32768) return 0; + return -sin(-Angle); + } + S = DSP1_SinTable[Angle >> 8] + (DSP1_MulTable[Angle & 0xff] * DSP1_SinTable[0x40 + (Angle >> 8)] >> 15); + if (S > 32767) S = 32767; + return (INT16) S; } -// 2FH - Memory Size Calculation (Bit Perfect) -// Input byte(2FH) integer(Undefined) -// Output integer(Size) +////////////////////////////////////////////////////////////////// -static void DSP1_MemorySize(INT16 *Size) +// Calculate the cosine of the input parameter. +// It's used the same method than in sin(INT16) + +static INT16 DSP1_cos(INT16 Angle) { - *Size = 0x0100; + INT32 S; + + if (Angle < 0) { + if (Angle == -32768) return -32768; + Angle = -Angle; + } + S = DSP1_SinTable[0x40 + (Angle >> 8)] - (DSP1_MulTable[Angle & 0xff] * DSP1_SinTable[Angle >> 8] >> 15); + if (S < -32768) S = -32767; + return (INT16) S; } -// ******************************************************************************************** -// MAME/MESS interface for above DSP1 code -// ******************************************************************************************** +////////////////////////////////////////////////////////////////// -// DSP1 interface -static UINT8 dsp1_read(UINT16 address) +// Determines the inverse of a floating point decimal number +// iCoefficient*2^iExponent = 1/(Coefficient*2^Exponent), with the output +// normalized (iCoefficient represents a number whose absolute value is between 1/2 and 1) +// To invert 'Coefficient' a first initial guess is taken from a look-up table +// and, then, two iterations of the Newton method (applied to the function +// f(x)=1/(2*x)-Coefficient) are done. This results in a close approximation (iCoefficient) to a number 'y' +// that verify Coefficient*y=1/2. This is why you have to correct the exponent by one +// unit at the end. + +static void inverse(INT16 Coefficient, INT16 Exponent, INT16 *iCoefficient, INT16 *iExponent) { - // check data vs. status - if (((address & 0xf000) == 0x6000) || ((address & 0x7fff) < 0x4000)) - { - // data reg - if (dsp1_out_cnt) - { - UINT8 temp = (UINT8)dsp1_out[dsp1_out_idx++]; - if (--dsp1_out_cnt == 0) - { - if ((dsp1_cur_cmd == 0x1a) || (dsp1_cur_cmd == 0x0a)) - { - INT16 tr, tr1, tr2, tr3; + // Step One: Division by Zero + if (Coefficient == 0x0000) + { + *iCoefficient = 0x7fff; + *iExponent = 0x002f; + } + else + { + INT16 Sign = 1; - DSP1_Raster(dsp1_in[0]|dsp1_in[1]<<8, &tr, &tr1, &tr2, &tr3); - dsp1_in[0]++; - if (dsp1_in[0] == 0) dsp1_in[1]++; - dsp1_out_cnt = 8; - dsp1_out[0] = tr&0xff; - dsp1_out[1] = (tr>>8)&0xff; - dsp1_out[2] = tr1&0xff; - dsp1_out[3] = (tr1>>8)&0xff; - dsp1_out[4] = tr2&0xff; - dsp1_out[5] = (tr2>>8)&0xff; - dsp1_out[6] = tr3&0xff; - dsp1_out[7] = (tr3>>8)&0xff; - dsp1_out_idx = 0; - } - else if (dsp1_cur_cmd == 0x1f) - { - if ((dsp1_out_idx % 2) != 0) - { - return DSP1ROM[dsp1_out_idx>>1] & 0xff; - } - else - { - return (DSP1ROM[dsp1_out_idx>>1] >> 8) & 0xff; - } - } - } - dsp1_waitcmd = 1; -// printf("dsp_r: %02x\n", temp); - return temp; - } - else - { -// printf("dsp_r: %02x\n", 0xff); - return 0xff; // indicate "no data" - } - } + // Step Two: Remove Sign + if (Coefficient < 0) + { + if (Coefficient < -32767) Coefficient = -32767; + Coefficient = -Coefficient; + Sign = -1; + } - // status register -// printf("dsp_r: %02x\n", 0x80); - return 0x80; + // Step Three: Normalize + while (Coefficient < 0x4000) + { + Coefficient <<= 1; + Exponent--; + } + + // Step Four: Special Case + if (Coefficient == 0x4000) + if (Sign == 1) *iCoefficient = 0x7fff; + else { + *iCoefficient = -0x4000; + Exponent--; + } + else { + // Step Five: Initial Guess + INT16 i = DSP1_DataRom[((Coefficient - 0x4000) >> 7) + 0x0065]; + + // Step Six: Iterate Newton's Method + i = (i + (-i * (Coefficient * i >> 15) >> 15)) << 1; + i = (i + (-i * (Coefficient * i >> 15) >> 15)) << 1; + + *iCoefficient = i * Sign; + } + + *iExponent = 1 - Exponent; + } } -static void dsp1_write(UINT16 address, UINT8 data) +////////////////////////////////////////////////////////////////// + +static INT16 denormalizeAndClip(INT16 C, INT16 E) { - // check data vs. status - if (((address & 0xf000) == 0x6000) || ((address & 0x7fff) < 0x4000)) - { -// printf("DSP_w: %02x cmd %02x wait %d dsp1_in_cnt %d\n", data, dsp1_cur_cmd, dsp1_waitcmd, dsp1_in_cnt); - - if (((dsp1_cur_cmd == 0x0a) || (dsp1_cur_cmd == 0x1a)) && (dsp1_out_cnt != 0)) - { - dsp1_out_cnt--; - dsp1_out_idx++; - return; - } - else if (dsp1_waitcmd) - { - dsp1_cur_cmd = data; - dsp1_in_idx = 0; - dsp1_waitcmd = 0; - dsp1_first_parm = 1; - switch (data) - { - case 0x00: dsp1_in_cnt = 2; break; - case 0x10: case 0x30: dsp1_in_cnt = 2; break; - case 0x20: dsp1_in_cnt = 2; break; - case 0x04: case 0x24: dsp1_in_cnt = 2; break; - case 0x08: dsp1_in_cnt = 3; break; - case 0x18: dsp1_in_cnt = 4; break; - case 0x28: dsp1_in_cnt = 3; break; - case 0x38: dsp1_in_cnt = 4; break; - case 0x0c: case 0x2c: dsp1_in_cnt = 3; break; - case 0x1c: case 0x3c: dsp1_in_cnt = 6; break; - case 0x02: case 0x12: case 0x22: case 0x32: dsp1_in_cnt = 7; break; - case 0x0a: dsp1_in_cnt = 1; break; - case 0x1a: case 0x2a: case 0x3a: dsp1_cur_cmd = 0x1a; dsp1_in_cnt = 1; break; - case 0x06: case 0x16: case 0x26: case 0x36: dsp1_in_cnt = 3; break; - case 0x0e: case 0x1e: case 0x2e: case 0x3e: dsp1_in_cnt = 2; break; - case 0x01: case 0x05: case 0x31: case 0x35: dsp1_in_cnt = 4; break; - case 0x11: case 0x15: dsp1_in_cnt = 4; break; - case 0x21: case 0x25: dsp1_in_cnt = 4; break; - case 0x0d: case 0x3d: case 0x09: case 0x39: dsp1_in_cnt = 3; break; - case 0x19: case 0x1d: dsp1_in_cnt = 3; break; - case 0x29: case 0x2d: dsp1_in_cnt = 3; break; - case 0x03: case 0x33: dsp1_in_cnt = 3; break; - case 0x13: dsp1_in_cnt = 3; break; - case 0x23: dsp1_in_cnt = 3; break; - case 0x0b: case 0x3b: dsp1_in_cnt = 3; break; - case 0x1b: dsp1_in_cnt = 3; break; - case 0x2b: dsp1_in_cnt = 3; break; - case 0x14: case 0x34: dsp1_in_cnt = 6; break; - case 0x07: case 0x0f: dsp1_in_cnt = 1; break; - case 0x27: case 0x2F: dsp1_in_cnt=1; break; - case 0x17: case 0x37: case 0x3F: dsp1_cur_cmd = 0x1f; dsp1_in_cnt = 1; break; - case 0x1f: dsp1_in_cnt = 1; break; - - case 0x80: - dsp1_in_cnt = 0; - dsp1_waitcmd = 1; - dsp1_first_parm = 1; - break; - } - - // that gives us parameter lengths in words, convert to bytes - dsp1_in_cnt *= 2; - } - else - { - dsp1_in[dsp1_in_idx++] = data; - dsp1_first_parm = 0; - } - - if ((dsp1_waitcmd) || ((dsp1_first_parm) && (data == 0x80))) - { - dsp1_waitcmd = 1; - dsp1_first_parm = 0; - } - else if (((dsp1_first_parm) && (dsp1_in_cnt != 0)) || ((dsp1_in_cnt == 0) && (dsp1_in_idx == 0))) - { - } - else - { - if (dsp1_in_cnt) - { - if (--dsp1_in_cnt == 0) - { - INT16 tr = 0, tr1 = 0, tr2 = 0, tr3 = 0; - INT32 ltr; - - // time to run the command - dsp1_waitcmd = 1; - dsp1_out_idx = 0; - switch (dsp1_cur_cmd) - { - case 0x00: - DSP1_Multiply(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, &tr); - dsp1_out_cnt = 2; - break; - - case 0x01: - case 0x05: - case 0x31: - case 0x35: - DSP1_Attitude_A(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, dsp1_in[6]|dsp1_in[7]<<8); - dsp1_out_cnt = 0; - break; - - case 0x02: - case 0x12: - case 0x22: - case 0x32: - DSP1_Parameter(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, dsp1_in[6]|dsp1_in[7]<<8, - dsp1_in[8]|dsp1_in[9]<<8, dsp1_in[10]|dsp1_in[11]<<8, dsp1_in[12]|dsp1_in[13]<<8, &tr, &tr1, &tr2, &tr3); - dsp1_out_cnt = 8; - break; - - case 0x03: - case 0x33: - DSP1_Subjective_A(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr, &tr1, &tr2); - dsp1_out_cnt = 6; - break; - - case 0x04: - case 0x24: - DSP1_Triangle(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, &tr, &tr1); - dsp1_out_cnt = 4; - break; - - case 0x06: - case 0x16: - case 0x26: - case 0x36: - DSP1_Project(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr, &tr1, (UINT16 *) &tr2); - dsp1_out_cnt = 6; - break; - - case 0x0a: - case 0x1a: - case 0x2a: - case 0x3a: - DSP1_Raster(dsp1_in[0]|dsp1_in[1]<<8, &tr, &tr1, &tr2, &tr3); - dsp1_in[0]++; - if (dsp1_in[0] == 0) dsp1_in[1]++; - dsp1_out_cnt = 8; - dsp1_in_idx = 0; - break; - - case 0x08: - DSP1_Radius(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, <r); - tr = (ltr & 0xffff); - tr1 = (ltr >> 16) & 0xffff; - dsp1_out_cnt = 4; - break; - - case 0x09: - case 0x0d: - case 0x39: - case 0x3d: - DSP1_Objective_A(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr, &tr1, &tr2); - dsp1_out_cnt = 6; - break; - - case 0x0b: - case 0x3b: - DSP1_Scalar_A(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr); - dsp1_out_cnt = 2; - break; - - case 0x0c: - case 0x2c: - DSP1_Rotate(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr, &tr1); - dsp1_out_cnt = 4; - break; - - case 0x0e: - case 0x1e: - case 0x2e: - case 0x3e: - DSP1_Target(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, &tr, &tr1); - dsp1_out_cnt = 4; - break; - - case 0x07: - case 0x0f: - DSP1_MemoryTest(dsp1_in[0]|dsp1_in[1]<<8, &tr); - dsp1_out_cnt = 2; - break; - - case 0x10: - case 0x30: - DSP1_Inverse(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, &tr, &tr1); - dsp1_out_cnt = 4; - break; - - case 0x11: - case 0x15: - DSP1_Attitude_B(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, dsp1_in[6]|dsp1_in[7]<<8); - dsp1_out_cnt = 0; - break; - - case 0x13: - DSP1_Subjective_B(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr, &tr1, &tr2); - dsp1_out_cnt = 6; - break; - - case 0x14: - case 0x34: - DSP1_Gyrate(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, dsp1_in[6]|dsp1_in[7]<<8, - dsp1_in[8]|dsp1_in[9]<<8, dsp1_in[10]|dsp1_in[11]<<8, &tr, &tr1, &tr2); - dsp1_out_cnt = 6; - break; - - case 0x18: - DSP1_Range(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, dsp1_in[6]|dsp1_in[7]<<8, &tr); - dsp1_out_cnt = 2; - break; - - case 0x19: - case 0x1d: - DSP1_Objective_B(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr, &tr1, &tr2); - dsp1_out_cnt = 6; - break; - - case 0x1b: - DSP1_Scalar_B(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr); - dsp1_out_cnt = 2; - break; - - case 0x1f: - dsp1_out_cnt = 2048; - break; - - case 0x23: - DSP1_Subjective_C(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr, &tr1, &tr2); - dsp1_out_cnt = 6; - break; - - case 0x28: - DSP1_Distance(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr); - dsp1_out_cnt = 2; - break; - - case 0x29: - case 0x2d: - DSP1_Objective_C(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr, &tr1, &tr2); - dsp1_out_cnt = 6; - break; - - case 0x2b: - DSP1_Scalar_C(dsp1_in[0]|dsp1_in[1]<<8, dsp1_in[2]|dsp1_in[3]<<8, dsp1_in[4]|dsp1_in[5]<<8, &tr); - dsp1_out_cnt = 2; - break; - - case 0x27: - case 0x2f: - DSP1_MemorySize(&tr); - dsp1_out_cnt = 2; - break; - - default: - printf("Unhandled DSP1 command: %02x\n", dsp1_cur_cmd); - break; - - } - - // copy up to 8 bytes of result into the buffer - dsp1_out[0] = tr&0xff; - dsp1_out[1] = (tr>>8)&0xff; - dsp1_out[2] = tr1&0xff; - dsp1_out[3] = (tr1>>8)&0xff; - dsp1_out[4] = tr2&0xff; - dsp1_out[5] = (tr2>>8)&0xff; - dsp1_out[6] = tr3&0xff; - dsp1_out[7] = (tr3>>8)&0xff; - } - } - } - } + if (E > 0) { + if (C > 0) return 32767; else if (C < 0) return -32767; + } else { + if (E < 0) return C * DSP1_DataRom[0x0031 + E] >> 15; + } + return C; } +////////////////////////////////////////////////////////////////// + +// Normalize the input number (m), understood as ranging from -1 to 1, +// to the form: Coefficient*2^Exponent, +// where the absolute value of Coefficient is >= 1/2 +// (Coefficient>=0x4000 or Coefficient <= (INT16)0xc001) + +static void normalize(INT16 m, INT16 *Coefficient, INT16 *Exponent) +{ + INT16 i = 0x4000; + INT16 e = 0; + + if (m < 0) + while ((m & i) && i) + { + i >>= 1; + e++; + } + else + while (!(m & i) && i) + { + i >>= 1; + e++; + } + + if (e > 0) + *Coefficient = m * DSP1_DataRom[0x21 + e] << 1; + else + *Coefficient = m; + + *Exponent -= e; +} + +////////////////////////////////////////////////////////////////// + +// Same than 'normalize' but with an INT32 input + +static void normalizeDouble(INT32 Product, INT16 *Coefficient, INT16 *Exponent) +{ + INT16 n = Product & 0x7fff; + INT16 m = Product >> 15; + INT16 i = 0x4000; + INT16 e = 0; + + if (m < 0) + while ((m & i) && i) + { + i >>= 1; + e++; + } + else + while (!(m & i) && i) + { + i >>= 1; + e++; + } + + if (e > 0) + { + *Coefficient = m * DSP1_DataRom[0x0021 + e] << 1; + + if (e < 15) + *Coefficient += n * DSP1_DataRom[0x0040 - e] >> 15; + else + { + i = 0x4000; + + if (m < 0) + while ((n & i) && i) + { + i >>= 1; + e++; + } + else + while (!(n & i) && i) + { + i >>= 1; + e++; + } + + if (e > 15) + *Coefficient = n * DSP1_DataRom[0x0012 + e] << 1; + else + *Coefficient += n; + } + } + else + *Coefficient = m; + + *Exponent = e; +} + +////////////////////////////////////////////////////////////////// + +// Shift to the right + +static INT16 shiftR(INT16 C, INT16 E) +{ + return (C * DSP1_DataRom[0x0031 + E] >> 15); +} + +////////////////////////////////////////////////////////////////// + +static const INT16 DSP1_SinTable[256] = { + 0x0000, 0x0324, 0x0647, 0x096a, 0x0c8b, 0x0fab, 0x12c8, 0x15e2, + 0x18f8, 0x1c0b, 0x1f19, 0x2223, 0x2528, 0x2826, 0x2b1f, 0x2e11, + 0x30fb, 0x33de, 0x36ba, 0x398c, 0x3c56, 0x3f17, 0x41ce, 0x447a, + 0x471c, 0x49b4, 0x4c3f, 0x4ebf, 0x5133, 0x539b, 0x55f5, 0x5842, + 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e8, 0x66cf, 0x68a6, + 0x6a6d, 0x6c24, 0x6dca, 0x6f5f, 0x70e2, 0x7255, 0x73b5, 0x7504, + 0x7641, 0x776c, 0x7884, 0x798a, 0x7a7d, 0x7b5d, 0x7c29, 0x7ce3, + 0x7d8a, 0x7e1d, 0x7e9d, 0x7f09, 0x7f62, 0x7fa7, 0x7fd8, 0x7ff6, + 0x7fff, 0x7ff6, 0x7fd8, 0x7fa7, 0x7f62, 0x7f09, 0x7e9d, 0x7e1d, + 0x7d8a, 0x7ce3, 0x7c29, 0x7b5d, 0x7a7d, 0x798a, 0x7884, 0x776c, + 0x7641, 0x7504, 0x73b5, 0x7255, 0x70e2, 0x6f5f, 0x6dca, 0x6c24, + 0x6a6d, 0x68a6, 0x66cf, 0x64e8, 0x62f2, 0x60ec, 0x5ed7, 0x5cb4, + 0x5a82, 0x5842, 0x55f5, 0x539b, 0x5133, 0x4ebf, 0x4c3f, 0x49b4, + 0x471c, 0x447a, 0x41ce, 0x3f17, 0x3c56, 0x398c, 0x36ba, 0x33de, + 0x30fb, 0x2e11, 0x2b1f, 0x2826, 0x2528, 0x2223, 0x1f19, 0x1c0b, + 0x18f8, 0x15e2, 0x12c8, 0x0fab, 0x0c8b, 0x096a, 0x0647, 0x0324, + -0x0000, -0x0324, -0x0647, -0x096a, -0x0c8b, -0x0fab, -0x12c8, -0x15e2, + -0x18f8, -0x1c0b, -0x1f19, -0x2223, -0x2528, -0x2826, -0x2b1f, -0x2e11, + -0x30fb, -0x33de, -0x36ba, -0x398c, -0x3c56, -0x3f17, -0x41ce, -0x447a, + -0x471c, -0x49b4, -0x4c3f, -0x4ebf, -0x5133, -0x539b, -0x55f5, -0x5842, + -0x5a82, -0x5cb4, -0x5ed7, -0x60ec, -0x62f2, -0x64e8, -0x66cf, -0x68a6, + -0x6a6d, -0x6c24, -0x6dca, -0x6f5f, -0x70e2, -0x7255, -0x73b5, -0x7504, + -0x7641, -0x776c, -0x7884, -0x798a, -0x7a7d, -0x7b5d, -0x7c29, -0x7ce3, + -0x7d8a, -0x7e1d, -0x7e9d, -0x7f09, -0x7f62, -0x7fa7, -0x7fd8, -0x7ff6, + -0x7fff, -0x7ff6, -0x7fd8, -0x7fa7, -0x7f62, -0x7f09, -0x7e9d, -0x7e1d, + -0x7d8a, -0x7ce3, -0x7c29, -0x7b5d, -0x7a7d, -0x798a, -0x7884, -0x776c, + -0x7641, -0x7504, -0x73b5, -0x7255, -0x70e2, -0x6f5f, -0x6dca, -0x6c24, + -0x6a6d, -0x68a6, -0x66cf, -0x64e8, -0x62f2, -0x60ec, -0x5ed7, -0x5cb4, + -0x5a82, -0x5842, -0x55f5, -0x539b, -0x5133, -0x4ebf, -0x4c3f, -0x49b4, + -0x471c, -0x447a, -0x41ce, -0x3f17, -0x3c56, -0x398c, -0x36ba, -0x33de, + -0x30fb, -0x2e11, -0x2b1f, -0x2826, -0x2528, -0x2223, -0x1f19, -0x1c0b, + -0x18f8, -0x15e2, -0x12c8, -0x0fab, -0x0c8b, -0x096a, -0x0647, -0x0324}; + + ////////////////////////////////////////////////////////////////// + +// Optimised for Performance +static const INT16 DSP1_MulTable[256] = { + 0x0000, 0x0003, 0x0006, 0x0009, 0x000c, 0x000f, 0x0012, 0x0015, + 0x0019, 0x001c, 0x001f, 0x0022, 0x0025, 0x0028, 0x002b, 0x002f, + 0x0032, 0x0035, 0x0038, 0x003b, 0x003e, 0x0041, 0x0045, 0x0048, + 0x004b, 0x004e, 0x0051, 0x0054, 0x0057, 0x005b, 0x005e, 0x0061, + 0x0064, 0x0067, 0x006a, 0x006d, 0x0071, 0x0074, 0x0077, 0x007a, + 0x007d, 0x0080, 0x0083, 0x0087, 0x008a, 0x008d, 0x0090, 0x0093, + 0x0096, 0x0099, 0x009d, 0x00a0, 0x00a3, 0x00a6, 0x00a9, 0x00ac, + 0x00af, 0x00b3, 0x00b6, 0x00b9, 0x00bc, 0x00bf, 0x00c2, 0x00c5, + 0x00c9, 0x00cc, 0x00cf, 0x00d2, 0x00d5, 0x00d8, 0x00db, 0x00df, + 0x00e2, 0x00e5, 0x00e8, 0x00eb, 0x00ee, 0x00f1, 0x00f5, 0x00f8, + 0x00fb, 0x00fe, 0x0101, 0x0104, 0x0107, 0x010b, 0x010e, 0x0111, + 0x0114, 0x0117, 0x011a, 0x011d, 0x0121, 0x0124, 0x0127, 0x012a, + 0x012d, 0x0130, 0x0133, 0x0137, 0x013a, 0x013d, 0x0140, 0x0143, + 0x0146, 0x0149, 0x014d, 0x0150, 0x0153, 0x0156, 0x0159, 0x015c, + 0x015f, 0x0163, 0x0166, 0x0169, 0x016c, 0x016f, 0x0172, 0x0175, + 0x0178, 0x017c, 0x017f, 0x0182, 0x0185, 0x0188, 0x018b, 0x018e, + 0x0192, 0x0195, 0x0198, 0x019b, 0x019e, 0x01a1, 0x01a4, 0x01a8, + 0x01ab, 0x01ae, 0x01b1, 0x01b4, 0x01b7, 0x01ba, 0x01be, 0x01c1, + 0x01c4, 0x01c7, 0x01ca, 0x01cd, 0x01d0, 0x01d4, 0x01d7, 0x01da, + 0x01dd, 0x01e0, 0x01e3, 0x01e6, 0x01ea, 0x01ed, 0x01f0, 0x01f3, + 0x01f6, 0x01f9, 0x01fc, 0x0200, 0x0203, 0x0206, 0x0209, 0x020c, + 0x020f, 0x0212, 0x0216, 0x0219, 0x021c, 0x021f, 0x0222, 0x0225, + 0x0228, 0x022c, 0x022f, 0x0232, 0x0235, 0x0238, 0x023b, 0x023e, + 0x0242, 0x0245, 0x0248, 0x024b, 0x024e, 0x0251, 0x0254, 0x0258, + 0x025b, 0x025e, 0x0261, 0x0264, 0x0267, 0x026a, 0x026e, 0x0271, + 0x0274, 0x0277, 0x027a, 0x027d, 0x0280, 0x0284, 0x0287, 0x028a, + 0x028d, 0x0290, 0x0293, 0x0296, 0x029a, 0x029d, 0x02a0, 0x02a3, + 0x02a6, 0x02a9, 0x02ac, 0x02b0, 0x02b3, 0x02b6, 0x02b9, 0x02bc, + 0x02bf, 0x02c2, 0x02c6, 0x02c9, 0x02cc, 0x02cf, 0x02d2, 0x02d5, + 0x02d8, 0x02db, 0x02df, 0x02e2, 0x02e5, 0x02e8, 0x02eb, 0x02ee, + 0x02f1, 0x02f5, 0x02f8, 0x02fb, 0x02fe, 0x0301, 0x0304, 0x0307, + 0x030b, 0x030e, 0x0311, 0x0314, 0x0317, 0x031a, 0x031d, 0x0321}; + +////////////////////////////////////////////////////////////////// + +// Data ROM, as logged from a DSP-1B with the 0x1f command; +// it contains the tables and constants used by the commands. +// The tables used are: two shift tables (0x022-0x031 and 0x031-0x040 -this last one +// with an error in 0x03c which has survived to all the DSP-1 revisions-); a inverse +// table (used as initial guess) at 0x065-0x0e4; a square root table (used also +// as initial guess) at 0x0e5-0x115; two sin and cos tables (used as nodes to construct +// a interpolation curve) at, respectively, 0x116-0x197 and 0x196-0x215. +// As a curiosity, in the positions 0x21c-0x31c it's contained a +// 257-points arccos table that, apparently, have been not used anywhere +// (maybe for the MaxAZS_Exp table?). +static UINT16 DSP1_DataRom[1024];