759 lines
30 KiB
C
759 lines
30 KiB
C
// See chunk5Flight.h. State cells keep their disassembly addresses; the
|
|
// 16-bit helpers wrap like the 6502 ADC/SBC chains.
|
|
|
|
#include "chunk4.h"
|
|
#include "chunk5Flight.h"
|
|
#include "chunk5Main.h"
|
|
#include "chunk5Setup.h"
|
|
#include "chunk5Zp.h"
|
|
#include "fs2Symbols.h"
|
|
#include "machine.h"
|
|
|
|
#define ZP_TABLE_PTR 0xB8
|
|
#define ZP_TMP_B7 0xB7
|
|
#define ZP_TMP_BC 0xBC
|
|
#define ZP_POS_X 0x5A
|
|
#define ZP_POS_Y 0x5E
|
|
#define ZP_POS_Z 0x62
|
|
#define ZP_ATT_PITCH 0x6C // $6C/$6D
|
|
|
|
#define RAM_PITCH_ANGLE 0x09AD
|
|
#define RAM_BANK_ANGLE 0x09AF
|
|
#define RAM_TOUCHDOWN 0x089E
|
|
#define RAM_SPIN_DECAY 0x08B6
|
|
#define RAM_GEAR_WARNING 0x08C3
|
|
#define RAM_WORK_PREV 0x08B7
|
|
#define RAM_SLEW_RESET 0x08C2
|
|
#define SLEW_PITCH_FLIP_POS 0x41
|
|
#define SLEW_PITCH_FLIP_NEG 0xC0
|
|
#define RAM_BOMB_RESPAWN 0x0838
|
|
#define RAM_BOMB_COUNT 0x0A54
|
|
#define RAM_CLIMB_RATE 0x0843
|
|
#define RAM_HEADING_ERROR 0x0830
|
|
#define RAM_AUTO_TRIM 0x09A9
|
|
#define RAM_INSTR_FLAGS 0x0915
|
|
#define RAM_TRIM_TABLE 0x0DD6
|
|
#define RAM_RUDDER_TABLE 0x0DB8
|
|
#define RAM_LIFT_TABLE 0x93BF
|
|
#define RAM_CLIMB_TABLE 0x0DCC
|
|
#define RAM_TURN_TABLE 0x0DC2
|
|
#define RAM_STALL_SEED_A 0x3A36
|
|
#define RAM_STALL_SEED_B 0x3E36
|
|
#define RAM_STALL_SEED_C 0x22B6
|
|
#define RAM_STALL_SEED_D 0x5A36
|
|
#define RAM_STALL_SEED_E 0x5E36
|
|
#define RAM_STALL_SEED_F 0x42B6
|
|
|
|
#define ALTITUDE_GROUND 0x03
|
|
#define CLIMB_AIRSPEED_MIN 0x0A
|
|
#define ENVELOPE_TIMER_INIT 0x06
|
|
#define FULL_TANK 0x19
|
|
|
|
|
|
static uint16_t add16(uint16_t a, uint16_t b);
|
|
static uint16_t ax(uint16_t addr);
|
|
static void integrateClimbRateStep(void);
|
|
static uint8_t mapYokePosToSlewDelta(uint8_t pos, uint8_t *outHi, uint8_t *outTop);
|
|
static uint16_t sub16(uint16_t a, uint16_t b);
|
|
|
|
|
|
static uint16_t add16(uint16_t a, uint16_t b) {
|
|
return (uint16_t)(a + b);
|
|
}
|
|
|
|
|
|
// LDAX addr.
|
|
static uint16_t ax(uint16_t addr) {
|
|
return ramRead16(addr);
|
|
}
|
|
|
|
|
|
void chunk5ApplySlewDeltas(void) {
|
|
bool reset = (fs2Ram[RAM_SLEW_RESET] & 1) != 0;
|
|
fs2Ram[RAM_SLEW_RESET] >>= 1;
|
|
if (reset) {
|
|
chunk5ResetYokeAndSlew();
|
|
}
|
|
uint8_t hi;
|
|
uint8_t top;
|
|
uint8_t lo = mapYokePosToSlewDelta(fs2Ram[SYM_YokeHorizPos], &hi, &top);
|
|
uint32_t sum = (uint32_t)lo + fs2Ram[0x5B];
|
|
fs2Ram[0x5B] = (uint8_t)sum;
|
|
sum = (uint32_t)hi + fs2Ram[0x5C] + (sum >> 8);
|
|
fs2Ram[0x5C] = (uint8_t)sum;
|
|
sum = (uint32_t)top + fs2Ram[0x5D] + (sum >> 8);
|
|
fs2Ram[0x5D] = (uint8_t)sum;
|
|
lo = mapYokePosToSlewDelta((uint8_t)(0u - fs2Ram[SYM_YokeVertPos]), &hi, &top);
|
|
sum = (uint32_t)lo + fs2Ram[0x63];
|
|
fs2Ram[0x63] = (uint8_t)sum;
|
|
sum = (uint32_t)hi + fs2Ram[0x64] + (sum >> 8);
|
|
fs2Ram[0x64] = (uint8_t)sum;
|
|
sum = (uint32_t)top + fs2Ram[0x65] + (sum >> 8);
|
|
fs2Ram[0x65] = (uint8_t)sum;
|
|
// Altitude rate, sign-extended into $5F/$60/$61.
|
|
uint8_t rate = fs2Ram[SYM_SlewAltRate];
|
|
uint8_t ext = (rate & 0x80) ? 0xFF : 0x00;
|
|
sum = (uint32_t)rate + fs2Ram[ZP_ALT];
|
|
fs2Ram[ZP_ALT] = (uint8_t)sum;
|
|
sum = (uint32_t)ext + fs2Ram[ZP_ALT + 1] + (sum >> 8);
|
|
fs2Ram[ZP_ALT + 1] = (uint8_t)sum;
|
|
sum = (uint32_t)ext + fs2Ram[ZP_ALT + 2] + (sum >> 8);
|
|
fs2Ram[ZP_ALT + 2] = (uint8_t)sum;
|
|
if (fs2Ram[ZP_ALT + 2] & 0x80) {
|
|
fs2Ram[ZP_ALT] = 0;
|
|
fs2Ram[ZP_ALT + 1] = 0;
|
|
fs2Ram[ZP_ALT + 2] = 0;
|
|
}
|
|
// Pitch: doubled slew rate, sign flipped when the bank is inverted.
|
|
uint8_t y = (uint8_t)(fs2Ram[SYM_SlewPitchRate] << 1);
|
|
if ((uint8_t)(fs2Ram[ZP_ATT_BANK + 1] + 0x40) & 0x80) {
|
|
y = (uint8_t)(0u - y);
|
|
}
|
|
uint8_t pitchHi = (uint8_t)(y + fs2Ram[ZP_ATT_PITCH + 1]);
|
|
fs2Ram[ZP_ATT_PITCH + 1] = pitchHi;
|
|
bool flip;
|
|
if (pitchHi & 0x80) {
|
|
flip = pitchHi < SLEW_PITCH_FLIP_NEG;
|
|
} else {
|
|
flip = pitchHi >= SLEW_PITCH_FLIP_POS;
|
|
}
|
|
if (flip) {
|
|
// Slew_FlipOver: past the vertical, negate the pitch and turn
|
|
// pitch, bank and heading through 180 degrees.
|
|
ramWrite16(ZP_ATT_PITCH, (uint16_t)(0u - ramRead16(ZP_ATT_PITCH)));
|
|
fs2Ram[ZP_ATT_PITCH + 1] ^= 0x80;
|
|
fs2Ram[ZP_ATT_BANK + 1] ^= 0x80;
|
|
fs2Ram[ZP_ATT_HEADING + 1] ^= 0x80;
|
|
}
|
|
fs2Ram[ZP_ATT_BANK + 1] = (uint8_t)(fs2Ram[SYM_SlewRollRate] + fs2Ram[ZP_ATT_BANK + 1]);
|
|
fs2Ram[ZP_ATT_HEADING + 1] = (uint8_t)(fs2Ram[ZP_ATT_HEADING + 1] - fs2Ram[SYM_SlewYawRate]);
|
|
chunk5SyncPositionFromLive();
|
|
}
|
|
|
|
|
|
void chunk5CheckFlightEnvelope(void) {
|
|
uint8_t t = fs2Ram[RAM_ENVELOPE_TIMER];
|
|
t--;
|
|
if ((t & 0x80) == 0) {
|
|
fs2Ram[RAM_ENVELOPE_TIMER] = t;
|
|
return;
|
|
}
|
|
bool warn = false;
|
|
if ((uint8_t)(fs2Ram[RAM_PITCH_ANGLE + 1] + 0x0F) >= 0x1E) {
|
|
warn = true;
|
|
} else if ((uint8_t)(fs2Ram[RAM_BANK_ANGLE + 1] + 0x16) >= 0x2C) {
|
|
warn = true;
|
|
} else if ((uint8_t)(fs2Ram[RAM_AIRSPEED + 1] + 0x53) >= 0xA6) {
|
|
warn = true;
|
|
}
|
|
if (warn) {
|
|
fs2Ram[RAM_ENVELOPE_TIMER] = ENVELOPE_TIMER_INIT;
|
|
}
|
|
}
|
|
|
|
|
|
// ClampAltitudeOnTouchdown (LA524).
|
|
void chunk5ClampAltitudeOnTouchdown(void) {
|
|
ramWrite16(ZP_C2, (uint16_t)(ramRead16(ZP_ALT) + 9));
|
|
uint16_t p = chunk4MultiplyAXByC2(0x4E0C);
|
|
uint8_t x = (uint8_t)(p >> 8);
|
|
uint16_t d = (uint16_t)x - fs2Ram[RAM_CLIMB_NEEDLE];
|
|
uint8_t y = (uint8_t)d;
|
|
uint16_t d2 = (uint16_t)fs2Ram[0xC9] - fs2Ram[RAM_CLIMB_NEEDLE + 1] - ((d & 0x100) ? 1u : 0u);
|
|
uint8_t a = (uint8_t)d2;
|
|
if ((a & 0x80) || (a == 0 && y < ALTITUDE_GROUND)) {
|
|
a = 0;
|
|
y = ALTITUDE_GROUND;
|
|
}
|
|
fs2Ram[ZP_ALT] = y;
|
|
fs2Ram[ZP_ALT + 1] = a;
|
|
fs2Ram[ZP_ALT + 2] = 0;
|
|
}
|
|
|
|
|
|
void chunk5CombineSideslipTerms(void) {
|
|
uint16_t sum = add16(ax(0x09F0), ax(0x09B1));
|
|
ramWrite16(0x09EE, sub16(sum, ax(0x09E8)));
|
|
sum = add16(sum, ax(0x09E8));
|
|
ramWrite16(0x09EC, add16(sum, ax(0x09C5)));
|
|
}
|
|
|
|
|
|
void chunk5ComputeDayPhase48K(void) {
|
|
uint8_t hours = fs2Ram[SYM_Hours];
|
|
uint8_t phase;
|
|
if (hours < 5) {
|
|
phase = 4;
|
|
} else if (hours < 6) {
|
|
phase = 2;
|
|
} else if (hours < 0x13) {
|
|
phase = 1;
|
|
} else if (hours < 0x14) {
|
|
phase = 2;
|
|
} else {
|
|
phase = 4;
|
|
}
|
|
fs2Ram[RAM_DAY_PHASE] = phase;
|
|
uint8_t scale = 1;
|
|
uint8_t x = fs2Ram[SYM_Season];
|
|
do {
|
|
fs2Ram[RAM_SEASON_SCALE] = scale;
|
|
scale = (uint8_t)(scale << 1);
|
|
x--;
|
|
} while (x != 0);
|
|
}
|
|
|
|
|
|
void chunk5ComputeFlightDerivedValues(void) {
|
|
if (fs2Ram[SYM_SlewMode] != 0) {
|
|
return;
|
|
}
|
|
ramWrite16(0x0A03, (uint16_t)chunk5L1778(fs2Ram[RAM_PITCH_ANGLE + 1], fs2Ram[RAM_PITCH_ANGLE]));
|
|
ramWrite16(0x09B7, (uint16_t)chunk4SinByteAngle(fs2Ram[RAM_PITCH_ANGLE + 1]));
|
|
ramWrite16(0x0A05, (uint16_t)chunk5L1778(fs2Ram[RAM_BANK_ANGLE + 1], fs2Ram[RAM_BANK_ANGLE]));
|
|
uint16_t cosBank = (uint16_t)chunk4SinByteAngle(fs2Ram[RAM_BANK_ANGLE + 1]);
|
|
ramWrite16(0x09BB, cosBank);
|
|
ramWrite16(0x09C7, cosBank);
|
|
// Rudder authority: table at $0DD6 by airspeed, times rudder.
|
|
ramWrite16(ZP_TABLE_PTR, RAM_TRIM_TABLE);
|
|
uint8_t a = chunk4UpdateAltimeterIndicator();
|
|
uint16_t p = chunk4MultiplyXYAndHalve(fs2Ram[SYM_RudderPos], a);
|
|
fs2Ram[0x09D4] = (uint8_t)(p >> 8);
|
|
fs2Ram[0x09D3] = (uint8_t)p;
|
|
ramWrite16(ZP_TABLE_PTR, RAM_RUDDER_TABLE);
|
|
chunk4UpdateAltimeterIndicator();
|
|
uint16_t v = (uint16_t)chunk4ScaleC2ByAX(ax(0x0A5A));
|
|
fs2Ram[0x09AB] = (uint8_t)v;
|
|
fs2Ram[ZP_C2] = (uint8_t)v;
|
|
uint8_t hi = (uint8_t)((v >> 8) + 7);
|
|
fs2Ram[0x09AC] = hi;
|
|
fs2Ram[ZP_C2 + 1] = hi;
|
|
ramWrite16(0x09EA, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A2D)));
|
|
ramWrite16(ZP_TABLE_PTR, RAM_LIFT_TABLE);
|
|
chunk4UpdateAltimeterIndicator();
|
|
ramWrite16(ZP_A5, ax(ZP_C2));
|
|
ramWrite16(ZP_C2, ax(ZP_A5));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09EC));
|
|
ramWrite16(0x0A19, add16(v, ax(0x09EA)));
|
|
if (fs2Ram[RAM_GEAR_WARNING] != 0) {
|
|
ramWrite16(0x0A19, 0);
|
|
}
|
|
ramWrite16(ZP_C2, ax(ZP_A5));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09EE));
|
|
ramWrite16(0x0A1B, add16(v, ax(0x09EA)));
|
|
// Stall / spin test on the airspeed-derived term.
|
|
uint8_t term = fs2Ram[0x09AC];
|
|
uint8_t fault = 0;
|
|
if (term & 0x80) {
|
|
if (term >= 0xD1) {
|
|
fault = 1;
|
|
}
|
|
} else {
|
|
uint8_t seed = (term < 0x29) ? 0x80 : 0xAA;
|
|
fs2Ram[RAM_STALL_SEED_A] = seed;
|
|
fs2Ram[RAM_STALL_SEED_B] = seed;
|
|
fs2Ram[RAM_STALL_SEED_C] = seed;
|
|
fs2Ram[RAM_STALL_SEED_D] = seed;
|
|
fs2Ram[RAM_STALL_SEED_E] = seed;
|
|
fs2Ram[RAM_STALL_SEED_F] = seed;
|
|
if (term >= 0x2C) {
|
|
fault = 1;
|
|
}
|
|
}
|
|
uint8_t x = 0;
|
|
if (fault) {
|
|
fs2Ram[RAM_SPIN_DECAY] = 0x0F;
|
|
fs2Ram[RAM_ENVELOPE_TIMER] = 0x06;
|
|
x = 0x06;
|
|
}
|
|
// CFDV_StoreFault: LDAX reloads X, so this is airspeed squared.
|
|
(void)x;
|
|
ramWrite16(ZP_C2, ax(RAM_AIRSPEED));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(RAM_AIRSPEED));
|
|
ramWrite16(0x0A13, v);
|
|
ramWrite16(ZP_C2, v);
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x0A19));
|
|
v = (uint16_t)(v << 2);
|
|
ramWrite16(0x09F6, v);
|
|
ramWrite16(ZP_C2, ax(0x0A13));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x0A1B));
|
|
v = (uint16_t)(v << 2);
|
|
ramWrite16(0x09F8, v);
|
|
uint8_t pitchFactor = 0;
|
|
if ((uint16_t)(ramRead16(ZP_ALT) - 0x14) & 0x8000) {
|
|
pitchFactor = 0x0B;
|
|
}
|
|
ramWrite16(ZP_C2, add16(ax(0x09F8), ax(0x09F6)));
|
|
chunk4ScaleC2ByAXIntoC2(ax(0x09A9));
|
|
uint8_t xHi = (uint8_t)(pitchFactor + fs2Ram[0x09F3]);
|
|
ramWrite16(0x09F4, (uint16_t)chunk4ScaleC2ByAX((uint16_t)(fs2Ram[0x09F2] | ((uint16_t)xHi << 8))));
|
|
if (fs2Ram[RAM_SPIN_DECAY] != 0) {
|
|
fs2Ram[RAM_SPIN_DECAY]--;
|
|
ramWrite16(0x09F4, 0);
|
|
}
|
|
ramWrite16(ZP_C2, ax(0x0A03));
|
|
ramWrite16(0x0A09, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A17)));
|
|
ramWrite16(ZP_C2, ax(0x09B7));
|
|
ramWrite16(0x09BF, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A17)));
|
|
ramWrite16(ZP_C2, ax(0x09F4));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09C7));
|
|
ramWrite16(0x0A0F, sub16(v, ax(0x09BF)));
|
|
ramWrite16(ZP_C2, ax(0x0A19));
|
|
ramWrite16(0x0A1D, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A19)));
|
|
ramWrite16(ZP_C2, ax(0x0A1B));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x0A1B));
|
|
ramWrite16(0x0A1F, v);
|
|
// clc; adc $0A1D; bcc; inx; clc; adc $09E6 ...
|
|
uint16_t lo = (uint16_t)((uint8_t)v) + fs2Ram[0x0A1D];
|
|
uint8_t xx = (uint8_t)((v >> 8) + (lo >> 8));
|
|
uint16_t lo2 = (uint16_t)((uint8_t)lo) + fs2Ram[0x09E6];
|
|
fs2Ram[ZP_C2] = (uint8_t)lo2;
|
|
uint8_t c3 = (uint8_t)(xx + fs2Ram[0x0A1E] + (lo2 >> 8));
|
|
c3 = (uint8_t)(c3 + fs2Ram[0x09E7]);
|
|
c3 = (uint8_t)(c3 + fs2Ram[0x09D9]);
|
|
fs2Ram[ZP_C2 + 1] = c3;
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09B5));
|
|
ramWrite16(ZP_C2, add16(v, ax(0x09B3)));
|
|
chunk4ScaleC2ByAXIntoC2(ax(0x0A13));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09A9));
|
|
ramWrite16(0x09CF, add16(v, ax(0x0A09)));
|
|
if ((fs2Ram[RAM_AIRSPEED] | fs2Ram[RAM_AIRSPEED + 1]) != 0) {
|
|
uint16_t s = (uint16_t)fs2Ram[0x09CF] + fs2Ram[0x0A2B];
|
|
fs2Ram[0x09CF] = (uint8_t)s;
|
|
if (s & 0x100) {
|
|
fs2Ram[0x09D0]++;
|
|
}
|
|
}
|
|
// CFDV_ClimbRate.
|
|
v = sub16(ax(RAM_PROP_DRAG), ax(0x09CF));
|
|
ramWrite16(0x09D7, v);
|
|
ramWrite16(ZP_C2, v);
|
|
ramWrite16(0x09D1, (uint16_t)chunk4ScaleC2ByAX(ax(0x09FB)));
|
|
ramWrite16(ZP_TABLE_PTR, RAM_CLIMB_TABLE);
|
|
chunk4UpdateAltimeterIndicator();
|
|
chunk4ScaleC2ByAXIntoC2(ax(0x0A05));
|
|
// CFDV_ScaleTimes4 with $09F4.
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09F4));
|
|
v = (uint16_t)(v << 2);
|
|
if (fs2Ram[SYM_OnGroundFlag] == 0) {
|
|
v = add16(v, ax(0x09C3));
|
|
}
|
|
ramWrite16(0x09CD, v);
|
|
if (fs2Ram[SYM_OnGroundFlag] != 0 && (fs2Ram[RAM_AIRSPEED] | fs2Ram[RAM_AIRSPEED + 1]) != 0) {
|
|
uint16_t t = ax(0x0A64);
|
|
t = chunk4AxDiv2(t);
|
|
t = chunk4AxDiv2(t);
|
|
t = chunk4AxDiv2(t);
|
|
ramWrite16(0x09CD, t);
|
|
}
|
|
ramWrite16(ZP_TABLE_PTR, RAM_TURN_TABLE);
|
|
chunk4UpdateAltimeterIndicator();
|
|
uint16_t climb;
|
|
if (fs2Ram[RAM_ENVELOPE_TIMER] != 0) {
|
|
climb = ax(0x0A0F);
|
|
} else {
|
|
climb = chunk4AxDiv2(ax(0x0A0F));
|
|
}
|
|
v = (uint16_t)chunk4ScaleC2ByAX(climb);
|
|
ramWrite16(0x09C9, v);
|
|
if (fs2Ram[SYM_OnGroundFlag] != 0 && (v & 0x8000)) {
|
|
ramWrite16(0x09C9, 0);
|
|
}
|
|
ramWrite16(ZP_C2, 0xFD76);
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09C9));
|
|
v = add16(v, ax(0x09D1));
|
|
if (fs2Ram[RAM_ENVELOPE_TIMER] != 0) {
|
|
v = ax(0x09D1);
|
|
}
|
|
// Airspeed += v with overflow capping at $64.
|
|
uint16_t lo3 = (uint16_t)((uint8_t)v) + fs2Ram[RAM_AIRSPEED];
|
|
fs2Ram[RAM_AIRSPEED] = (uint8_t)lo3;
|
|
int sum = (int)(int8_t)(v >> 8) + (int)(int8_t)fs2Ram[RAM_AIRSPEED + 1] + (int)(lo3 >> 8);
|
|
uint8_t hiA = (uint8_t)sum;
|
|
if (sum > 127 || sum < -128) {
|
|
hiA = 0x64;
|
|
}
|
|
if (hiA & 0x80) {
|
|
hiA = 0;
|
|
fs2Ram[RAM_AIRSPEED] = 0;
|
|
}
|
|
fs2Ram[RAM_AIRSPEED + 1] = hiA;
|
|
// Sideslip accumulation and the +/-90 degree flip.
|
|
v = add16(ax(RAM_PITCH_ANGLE), ax(0x09C9));
|
|
ramWrite16(RAM_PITCH_ANGLE, v);
|
|
bool negative = (v & 0x8000) != 0;
|
|
if (negative) {
|
|
ramWrite16(RAM_PITCH_ANGLE, (uint16_t)(0u - v));
|
|
}
|
|
// CFDV_SideslipFlip on the (possibly negated) value.
|
|
uint16_t s = ax(RAM_PITCH_ANGLE);
|
|
if ((uint8_t)(s >> 8) >= 0x40) {
|
|
uint16_t n = chunk4NegateAX(s);
|
|
fs2Ram[RAM_PITCH_ANGLE] = (uint8_t)n;
|
|
fs2Ram[RAM_PITCH_ANGLE + 1] = (uint8_t)((n >> 8) ^ 0x80);
|
|
fs2Ram[RAM_BANK_ANGLE + 1] ^= 0x80;
|
|
fs2Ram[RAM_HEADING + 1] ^= 0x80;
|
|
}
|
|
if (negative) {
|
|
ramWrite16(RAM_PITCH_ANGLE, (uint16_t)(0u - ax(RAM_PITCH_ANGLE)));
|
|
}
|
|
}
|
|
|
|
|
|
void chunk5ComputeSideslipDerived(void) {
|
|
uint16_t rudder = chunk4AxDiv2((uint16_t)fs2Ram[SYM_RudderPos]);
|
|
fs2Ram[ZP_TMP_B7] = (uint8_t)rudder;
|
|
uint16_t yoke = chunk4AxDiv2((uint16_t)fs2Ram[SYM_YokeHorizPos]);
|
|
uint8_t diff = (uint8_t)((uint8_t)yoke - fs2Ram[ZP_TMP_B7]);
|
|
fs2Ram[RAM_SIDESLIP] = diff;
|
|
fs2Ram[ZP_C2 + 1] = diff;
|
|
fs2Ram[ZP_C2] = 0;
|
|
ramWrite16(0x09E6, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A29)));
|
|
ramWrite16(ZP_C2, ax(0x09C1));
|
|
ramWrite16(0x09C3, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A25)));
|
|
ramWrite16(ZP_C2, ax(0x09C1));
|
|
ramWrite16(0x0A0B, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A27)));
|
|
ramWrite16(ZP_C2, ax(0x09C1));
|
|
ramWrite16(0x09C5, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A23)));
|
|
chunk5CombineSideslipTerms();
|
|
}
|
|
|
|
|
|
void chunk5IntegrateClimbRate(void) {
|
|
if (fs2Ram[RAM_AIRSPEED + 1] < CLIMB_AIRSPEED_MIN) {
|
|
ramWrite16(RAM_CLIMB_NEEDLE, ax(RAM_CLIMB_RATE));
|
|
}
|
|
integrateClimbRateStep();
|
|
integrateClimbRateStep();
|
|
}
|
|
|
|
|
|
void chunk5IntegratePhysicsStep(void) {
|
|
fs2Ram[RAM_SLEW_RESET] = 1;
|
|
// Delta-time bucket from the frame work counter.
|
|
uint8_t lo = fs2Ram[ZP_WORK_LO];
|
|
uint16_t d = (uint16_t)lo - fs2Ram[RAM_WORK_PREV];
|
|
fs2Ram[RAM_WORK_PREV] = lo;
|
|
fs2Ram[0x09DE] = (uint8_t)d;
|
|
uint8_t hi = fs2Ram[ZP_WORK_HI];
|
|
uint8_t a = (uint8_t)(hi - fs2Ram[RAM_WORK_PREV + 1] - ((d & 0x100) ? 1 : 0));
|
|
fs2Ram[RAM_WORK_PREV + 1] = hi;
|
|
for (;;) {
|
|
fs2Ram[ZP_TICK_COUNT]--;
|
|
if (fs2Ram[ZP_TICK_COUNT] & 0x80) {
|
|
break;
|
|
}
|
|
int sum = (int)(int8_t)a + 0x10;
|
|
a = (uint8_t)sum;
|
|
if (sum > 127) {
|
|
a = 0x7F;
|
|
break;
|
|
}
|
|
}
|
|
fs2Ram[ZP_TICK_COUNT]++;
|
|
fs2Ram[0x09DF] = a;
|
|
fs2Ram[ZP_C2 + 1] = a;
|
|
fs2Ram[ZP_C2] = 0;
|
|
uint16_t v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09CD));
|
|
ramWrite16(RAM_HEADING, add16(v, ax(RAM_HEADING)));
|
|
ramWrite16(ZP_C2, ax(0x09DE));
|
|
ramWrite16(0x09E0, (uint16_t)chunk4ScaleC2ByAX(ax(RAM_AIRSPEED)));
|
|
uint16_t accel = (fs2Ram[SYM_OnGroundFlag] != 0) ? 0 : ax(0x0A0B);
|
|
uint8_t angle = (uint8_t)(((uint16_t)add16(accel, ax(RAM_HEADING))) >> 8);
|
|
fs2Ram[ZP_TMP_B7] = angle;
|
|
ramWrite16(0x0A07, (uint16_t)chunk4SinShiftedByteAngle(angle));
|
|
ramWrite16(0x09BD, (uint16_t)chunk4SinByteAngle(angle));
|
|
ramWrite16(ZP_C2, ax(0x09B7));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09E0));
|
|
ramWrite16(0x09B9, v);
|
|
ramWrite16(ZP_C2, v);
|
|
ramWrite16(0x09D5, (uint16_t)chunk4ScaleC2ByAX(ax(0x0A07)));
|
|
ramWrite16(ZP_C2, ax(0x0A03));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09E0));
|
|
ramWrite16(0x09A7, add16(v, ax(RAM_TURB_X)));
|
|
ramWrite16(ZP_C2, ax(0x09BD));
|
|
ramWrite16(0x0A01, (uint16_t)chunk4ScaleC2ByAX(ax(0x09B9)));
|
|
// ($09FF/$0A00) = ($09F6 - $09F8) << 2.
|
|
v = sub16(ax(0x09F6), ax(0x09F8));
|
|
v = (uint16_t)(v << 2);
|
|
ramWrite16(0x09FF, v);
|
|
ramWrite16(ZP_C2, v);
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09FD));
|
|
ramWrite16(0x09CB, sub16(v, ax(RAM_TURN_LATCH)));
|
|
if ((fs2Ram[RAM_TURN_LATCH] | fs2Ram[RAM_TURN_LATCH + 1]) != 0) {
|
|
ramWrite16(RAM_TURN_LATCH, 0);
|
|
fs2Ram[RAM_LANDING_FLAG] = 1;
|
|
}
|
|
ramWrite16(ZP_C2, ax(0x09E0));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x09CB));
|
|
v = (uint16_t)(v << 1);
|
|
ramWrite16(RAM_BANK_ANGLE, add16(v, ax(RAM_BANK_ANGLE)));
|
|
// Position integration with sign extension into the top bytes.
|
|
uint16_t dz = ax(0x0A01);
|
|
uint8_t ext = (dz & 0x8000) ? 0xFF : 0x00;
|
|
uint32_t s = (uint32_t)(uint8_t)dz + fs2Ram[0x62];
|
|
fs2Ram[0x62] = (uint8_t)s;
|
|
s = (uint32_t)(dz >> 8) + fs2Ram[0x63] + (s >> 8);
|
|
fs2Ram[0x63] = (uint8_t)s;
|
|
s = (uint32_t)ext + fs2Ram[0x64] + (s >> 8);
|
|
fs2Ram[0x64] = (uint8_t)s;
|
|
s = (uint32_t)ext + fs2Ram[0x65] + (s >> 8);
|
|
fs2Ram[0x65] = (uint8_t)s;
|
|
uint16_t dx = ax(0x09D5);
|
|
ext = (dx & 0x8000) ? 0xFF : 0x00;
|
|
s = (uint32_t)(uint8_t)dx + fs2Ram[0x5A];
|
|
fs2Ram[0x5A] = (uint8_t)s;
|
|
s = (uint32_t)(dx >> 8) + fs2Ram[0x5B] + (s >> 8);
|
|
fs2Ram[0x5B] = (uint8_t)s;
|
|
s = (uint32_t)ext + fs2Ram[0x5C] + (s >> 8);
|
|
fs2Ram[0x5C] = (uint8_t)s;
|
|
s = (uint32_t)ext + fs2Ram[0x5D] + (s >> 8);
|
|
fs2Ram[0x5D] = (uint8_t)s;
|
|
uint16_t dy = ax(0x09A7);
|
|
ext = (dy & 0x8000) ? 0xFF : 0x00;
|
|
s = (uint32_t)(uint8_t)dy + fs2Ram[0x5E];
|
|
fs2Ram[0x5E] = (uint8_t)s;
|
|
s = (uint32_t)(dy >> 8) + fs2Ram[0x5F] + (s >> 8);
|
|
fs2Ram[0x5F] = (uint8_t)s;
|
|
s = (uint32_t)ext + fs2Ram[0x60] + (s >> 8);
|
|
fs2Ram[0x60] = (uint8_t)s;
|
|
if (fs2Ram[0x60] & 0x80) {
|
|
fs2Ram[0x5E] = 0;
|
|
fs2Ram[0x60] = 0;
|
|
fs2Ram[0x5F] = ALTITUDE_GROUND;
|
|
}
|
|
if (fs2Ram[RAM_TOUCHDOWN] != 0) {
|
|
chunk5ClampAltitudeOnTouchdown();
|
|
fs2Ram[RAM_TOUCHDOWN]--;
|
|
}
|
|
// Attitude for the view.
|
|
ramWrite16(ZP_C2, ax(0x09AB));
|
|
v = (uint16_t)chunk4ScaleC2ByAX(ax(0x0A21));
|
|
v = sub16(v, ax(RAM_PITCH_ANGLE));
|
|
if (fs2Ram[SYM_OnGroundFlag] == 0) {
|
|
v = add16(v, ax(0x09DC));
|
|
}
|
|
fs2Ram[ZP_ATT_PITCH] = (uint8_t)v;
|
|
fs2Ram[ZP_ATT_PITCH + 1] = (uint8_t)((v >> 8) + fs2Ram[0x09FA]);
|
|
fs2Ram[0x09FA] = 0;
|
|
ramWrite16(ZP_ATT_BANK, (uint16_t)(0u - ax(RAM_BANK_ANGLE)));
|
|
ramWrite16(ZP_ATT_HEADING, ax(RAM_HEADING));
|
|
// OnGroundFlag from the 24-bit altitude vs $000301.
|
|
uint32_t alt = (uint32_t)fs2Ram[0x5E] | ((uint32_t)fs2Ram[0x5F] << 8) | ((uint32_t)fs2Ram[0x60] << 16);
|
|
uint32_t cmp = (alt - 0x000301u) & 0xFFFFFF;
|
|
fs2Ram[SYM_OnGroundFlag] = (cmp & 0x800000) ? 0xFF : 0x00;
|
|
if (fs2Ram[SYM_OnGroundFlag] != 0) {
|
|
ramWrite16(RAM_BANK_ANGLE, 0);
|
|
ramWrite16(0x09CB, 0);
|
|
if (fs2Ram[RAM_PITCH_ANGLE + 1] & 0x80) {
|
|
ramWrite16(RAM_PITCH_ANGLE, 0);
|
|
}
|
|
if (fs2Ram[RAM_TOUCHDOWN] != 0) {
|
|
chunk5BrakesOrGuns();
|
|
} else {
|
|
uint8_t pitch = fs2Ram[0x0A16];
|
|
if (pitch & 0x80) {
|
|
if ((uint8_t)(pitch + 8) & 0x80) {
|
|
fs2Ram[RAM_CRASH_CODE] = 4;
|
|
fs2Ram[RAM_AIRSPEED + 1] = 0;
|
|
}
|
|
if ((uint8_t)(fs2Ram[0x6F] + 0x0A) >= 0x14) {
|
|
fs2Ram[RAM_CRASH_CODE] = 4;
|
|
}
|
|
}
|
|
}
|
|
uint8_t x = 0;
|
|
if (fs2Ram[0x09E2] == 0) {
|
|
fs2Ram[0x09E2]--;
|
|
x = fs2Ram[0x0A16];
|
|
}
|
|
fs2Ram[0x09FA] = x;
|
|
} else {
|
|
if (fs2Ram[0x09E2] != 0) {
|
|
fs2Ram[0x09E2] = 0;
|
|
if ((fs2Ram[0x63] & 0x03) == 0) {
|
|
uint8_t dmg = (uint8_t)(fs2Ram[SYM_UpdateCounter] + (fs2Ram[0x63] & 0x03));
|
|
fs2Ram[0x09A0] = dmg;
|
|
fs2Ram[0x09A1] = (dmg & 0x80) ? 0xFF : 0x00;
|
|
}
|
|
}
|
|
}
|
|
// IPS_Airspeed: $0A15 = $09A7 / $09DE.
|
|
ramWrite16(ZP_C4, ax(0x09DE));
|
|
chunk4DivideSigned16(fs2Ram[0x09A8], fs2Ram[0x09A7]);
|
|
ramWrite16(0x0A15, ax(ZP_C2));
|
|
}
|
|
|
|
|
|
void chunk5ResetAircraftSystems(void) {
|
|
fs2Ram[0x0994] = FULL_TANK;
|
|
fs2Ram[0x0997] = FULL_TANK;
|
|
fs2Ram[SYM_InstrumentOperationalFlags] = 0xFF;
|
|
fs2Ram[RAM_INSTR_FLAGS] = 0xFF;
|
|
fs2Ram[0x089B] = 0xFF;
|
|
fs2Ram[0x089C] = 0xFF;
|
|
fs2Ram[0x08A4] = 0;
|
|
fs2Ram[RAM_ENGINE_FAULTS] = 0;
|
|
fs2Ram[0x099A] = 1;
|
|
fs2Ram[0x0999] = 1;
|
|
fs2Ram[0x08A5]++;
|
|
}
|
|
|
|
|
|
void chunk5SyncPositionFromLive(void) {
|
|
fs2Ram[SYM_NorthPosition + 1] = (uint8_t)(fs2Ram[0x65] + 0x40);
|
|
fs2Ram[SYM_NorthPosition] = fs2Ram[0x64];
|
|
fs2Ram[SYM_EastPosition + 1] = (uint8_t)(fs2Ram[0x5D] + 0x40);
|
|
fs2Ram[SYM_EastPosition] = fs2Ram[0x5C];
|
|
}
|
|
|
|
|
|
void chunk5UpdateAutoTrimAndYaw(void) {
|
|
if (fs2Ram[SYM_SlewMode] != 0) {
|
|
return;
|
|
}
|
|
// $C2 = error * 2 + error / 2, then $09A9 = $7FFE - $C2 with the
|
|
// borrow chained from the last add (the 6502 code has no sec).
|
|
uint16_t err = ax(RAM_HEADING_ERROR);
|
|
uint8_t lo = (uint8_t)err;
|
|
uint8_t hi = (uint8_t)(err >> 8);
|
|
uint8_t twiceLo = (uint8_t)(lo << 1);
|
|
uint8_t twiceHi = (uint8_t)((hi << 1) | (lo >> 7));
|
|
uint8_t r1 = (uint8_t)((twiceHi >> 1) | (hi & 0x80));
|
|
uint8_t hiP = (uint8_t)(r1 >> 1);
|
|
uint8_t loP = (uint8_t)((lo >> 1) | ((r1 & 1) << 7));
|
|
uint16_t s1 = (uint16_t)loP + twiceLo + (lo & 1);
|
|
uint16_t s2 = (uint16_t)hiP + twiceHi + (s1 >> 8);
|
|
fs2Ram[ZP_C2] = (uint8_t)s1;
|
|
fs2Ram[ZP_C2 + 1] = (uint8_t)s2;
|
|
int d1 = 0xFE - (int)(uint8_t)s1 - ((s2 & 0x100) ? 0 : 1);
|
|
int d2 = 0x7F - (int)(uint8_t)s2 - ((d1 < 0) ? 1 : 0);
|
|
fs2Ram[RAM_AUTO_TRIM] = (uint8_t)d1;
|
|
fs2Ram[RAM_AUTO_TRIM + 1] = (uint8_t)d2;
|
|
// Yaw integrator decay toward zero by 5 when the rudder is centred.
|
|
if (fs2Ram[SYM_RudderPos] == 0) {
|
|
int16_t yaw = (int16_t)ax(RAM_BANK_ANGLE);
|
|
if (yaw < 0) {
|
|
yaw = (int16_t)(yaw + 5);
|
|
if (yaw > 0) {
|
|
yaw = 0;
|
|
}
|
|
} else {
|
|
yaw = (int16_t)(yaw - 5);
|
|
if (yaw < 0) {
|
|
yaw = 0;
|
|
}
|
|
}
|
|
ramWrite16(RAM_BANK_ANGLE, (uint16_t)yaw);
|
|
}
|
|
if (fs2Ram[RAM_BOMB_RESPAWN] != 0) {
|
|
if (fs2Ram[RAM_AIRSPEED + 1] == 0) {
|
|
chunk5ResetAircraftSystems();
|
|
if (fs2Ram[RAM_BOMB_RESPAWN] == 2) {
|
|
fs2Ram[RAM_BOMB_COUNT] = 0x64;
|
|
fs2Ram[SYM_WW1AceBombsStr] = '5';
|
|
}
|
|
}
|
|
fs2Ram[RAM_BOMB_RESPAWN] = 0;
|
|
}
|
|
}
|
|
|
|
|
|
void chunk5UpdateSimpleEngine(void) {
|
|
uint8_t y = (uint8_t)(fs2Ram[RAM_THROTTLE] >> 1);
|
|
uint8_t x = (fs2Ram[RAM_TANK_SELECT] != 0) ? fs2Ram[0x0997] : fs2Ram[0x0994];
|
|
x--;
|
|
if (x & 0x80) {
|
|
y = 0;
|
|
}
|
|
fs2Ram[RAM_TARGET_RPM] = y;
|
|
fs2Ram[ZP_C2 + 1] = (uint8_t)(y << 1);
|
|
fs2Ram[ZP_C2] = 0;
|
|
chunk4ScaleC2ByAXIntoC2(0x0320);
|
|
ramWrite16(RAM_PROP_DRAG, (uint16_t)chunk4ScaleC2ByAX(ax(0x09A9)));
|
|
// `lda RealityMode; bne store` stores the flag itself as the RPM.
|
|
uint8_t rpm = fs2Ram[SYM_RealityMode];
|
|
if (rpm == 0) {
|
|
rpm = fs2Ram[RAM_TARGET_RPM];
|
|
if (rpm < RPM_IDLE_FLOOR) {
|
|
rpm = RPM_IDLE_FLOOR;
|
|
}
|
|
}
|
|
fs2Ram[RAM_TARGET_RPM] = rpm;
|
|
if ((fs2Ram[SYM_InputTickCounter] & 0x1F) == 0) {
|
|
uint16_t burn = chunk4MultiplyXYAndHalve(fs2Ram[RAM_THROTTLE], FUEL_BURN_INDEX);
|
|
fs2Ram[ZP_A5] = (uint8_t)burn;
|
|
fs2Ram[ZP_A5 + 1] = (uint8_t)(burn >> 8);
|
|
uint8_t slot = (fs2Ram[RAM_TANK_SELECT] != 0) ? 3 : 0;
|
|
uint16_t base = (uint16_t)(RAM_FUEL_LEFT + slot);
|
|
uint16_t s = (uint16_t)fs2Ram[ZP_A5] + fs2Ram[base];
|
|
fs2Ram[base] = (uint8_t)s;
|
|
if ((burn >> 8) & 0x80) {
|
|
uint16_t s2 = (uint16_t)(burn >> 8) + fs2Ram[base + 1] + (s >> 8);
|
|
fs2Ram[base + 1] = (uint8_t)s2;
|
|
uint16_t s3 = (uint16_t)0xFF + fs2Ram[base + 2] + (s2 >> 8);
|
|
if (((uint8_t)s3 & 0x80) == 0) {
|
|
fs2Ram[base + 2] = (uint8_t)s3;
|
|
}
|
|
}
|
|
}
|
|
fs2Ram[RAM_CHT] = 5;
|
|
fs2Ram[RAM_OIL_TEMP] = 5;
|
|
}
|
|
|
|
|
|
// ICR_Step: nudge the needle toward the climb rate and the altitude the
|
|
// opposite way, clamping the altitude at ground level.
|
|
static void integrateClimbRateStep(void) {
|
|
uint16_t climb = ax(RAM_CLIMB_RATE);
|
|
uint16_t needle = ax(RAM_CLIMB_NEEDLE);
|
|
if (climb == needle) {
|
|
return;
|
|
}
|
|
uint8_t y = (climb < needle) ? 0xFF : 0x00;
|
|
uint8_t a = (y & 0x80) ? y : 1;
|
|
uint16_t s = (uint16_t)a + fs2Ram[RAM_CLIMB_NEEDLE];
|
|
fs2Ram[RAM_CLIMB_NEEDLE] = (uint8_t)s;
|
|
fs2Ram[RAM_CLIMB_NEEDLE + 1] = (uint8_t)(y + fs2Ram[RAM_CLIMB_NEEDLE + 1] + (s >> 8));
|
|
y = (uint8_t)(y ^ 0xFF);
|
|
a = (y & 0x80) ? y : 1;
|
|
s = (uint16_t)a + fs2Ram[ZP_ALT];
|
|
fs2Ram[ZP_ALT] = (uint8_t)s;
|
|
uint8_t hi = (uint8_t)(y + fs2Ram[ZP_ALT + 1] + (s >> 8));
|
|
fs2Ram[ZP_ALT + 1] = hi;
|
|
if ((hi & 0x80) || (hi == 0 && fs2Ram[ZP_ALT] < ALTITUDE_GROUND)) {
|
|
fs2Ram[ZP_ALT] = ALTITUDE_GROUND;
|
|
fs2Ram[ZP_ALT + 1] = 0;
|
|
}
|
|
}
|
|
|
|
|
|
// MapYokePosToSlewDelta: a 24-bit delta from the yoke deflection.
|
|
static uint8_t mapYokePosToSlewDelta(uint8_t pos, uint8_t *outHi, uint8_t *outTop) {
|
|
int x = (int8_t)pos;
|
|
uint32_t v;
|
|
if (x < 0) {
|
|
v = 0xFFFFFF;
|
|
do {
|
|
v = (v << 1) & 0xFFFFFF;
|
|
x += 4;
|
|
} while (x < 0);
|
|
} else {
|
|
v = 0;
|
|
for (;;) {
|
|
x -= 4;
|
|
if (x < 0) {
|
|
break;
|
|
}
|
|
v = ((v << 1) | 1) & 0xFFFFFF;
|
|
}
|
|
}
|
|
*outHi = (uint8_t)(v >> 8);
|
|
*outTop = (uint8_t)(v >> 16);
|
|
return (uint8_t)v;
|
|
}
|
|
|
|
|
|
static uint16_t sub16(uint16_t a, uint16_t b) {
|
|
return (uint16_t)(a - b);
|
|
}
|