331 lines
14 KiB
C
331 lines
14 KiB
C
/*
|
|
* Copyright (c) 2024 Scott Duensing, scott@kangaroopunch.com
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in
|
|
* all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
|
|
|
|
#include "a23d2.h"
|
|
#include "mem.h"
|
|
|
|
#include "flight.h"
|
|
|
|
|
|
// Extern.
|
|
airplaneT _plane;
|
|
|
|
|
|
#define SEGMENT_FLIGHT_MODEL_1
|
|
|
|
|
|
// This flight model is basically the one from "Build Your Own Flight Sim
|
|
// in C++" by Michael Radtke and Chris Lampton.
|
|
void lightPlane(void) {
|
|
// Flight model. Power Dynamics.
|
|
if (_plane.ignition) {
|
|
// Start engine.
|
|
if (!_plane.engine) _plane.engine = true;
|
|
// Adjust RPM.
|
|
if (_plane.rpm < (375 + (_plane.throttle * 117))) _plane.rpm += _plane.loopTime * 0.5;
|
|
if (_plane.rpm > (375 + (_plane.throttle * 117))) _plane.rpm -= _plane.loopTime * 0.5;
|
|
} else {
|
|
// Stop engine.
|
|
if (_plane.engine) _plane.engine = false;
|
|
// Run down engine.
|
|
if (_plane.rpm > 0) _plane.rpm -= (int16_t)(_plane.loopTime / 2);
|
|
if (_plane.rpm < 0) _plane.rpm = 0;
|
|
}
|
|
|
|
// Flight model. Flight Dynamics.
|
|
// Calculate speed from RPM.
|
|
//_plane.iSpeed = _plane.rpm / 17.5;
|
|
_plane.FiSpeed = fixDiv(iToFix(_plane.rpm), fToFix(17.5));
|
|
// Modify speed by pitch.
|
|
//_plane.iSpeed += (_plane.pitch * 1.5);
|
|
_plane.FiSpeed = fixAdd(_plane.FiSpeed, fixMul(_plane.Fpitch, fToFix(1.5)));
|
|
// Horizontal acceleration - thrust.
|
|
//_plane.hAccel = ((_plane.rpm * (_plane.iSpeed - _plane.hSpeed)) / 10000);
|
|
//_plane.hAccel /= 1000;
|
|
//_plane.hAccel *= _plane.loopTime;
|
|
_plane.FtmpX = fixSub(_plane.FiSpeed, _plane.FhSpeed);
|
|
_plane.FtmpX = fixMul(iToFix(_plane.rpm), _plane.FtmpX);
|
|
_plane.FhAccel = fixDiv(_plane.FtmpX, iToFix(10000));
|
|
_plane.FhAccel = fixDiv(_plane.FhAccel, iToFix(1000));
|
|
_plane.FhAccel = fixMul(_plane.FhAccel, iToFix(_plane.loopTime));
|
|
if (_plane.brake && !_plane.airborne) {
|
|
// Handle brakes.
|
|
if (_plane.FhSpeed > 0) {
|
|
//_plane.hSpeed -= 1;
|
|
_plane.FhSpeed = fixSub(_plane.FhSpeed, iToFix(1));
|
|
} else {
|
|
//_plane.hSpeed = 0;
|
|
_plane.FhSpeed = iToFix(0);
|
|
}
|
|
} else {
|
|
// Accelerate normally.
|
|
//_plane.hSpeed += _plane.hAccel;
|
|
_plane.FhSpeed = fixAdd(_plane.FhSpeed, _plane.FhAccel);
|
|
}
|
|
// Force speed to range -1..1.
|
|
//_plane.lSpeed = (_plane.hSpeed / 65) - 1;
|
|
_plane.FlSpeed = fixSub(fixDiv(_plane.FhSpeed, iToFix(65)), iToFix(1));
|
|
//if (_plane.lSpeed > 1) _plane.lSpeed = 1;
|
|
if (_plane.FlSpeed > iToFix(1)) _plane.FlSpeed = iToFix(1);
|
|
// Lift curve.
|
|
//_plane.lVeloc = Degs(ourAtan(_plane.lSpeed));
|
|
_plane.FlVeloc = fixDegs(fixAtan(_plane.FlSpeed));
|
|
// Force lift to range 0..90.
|
|
//_plane.lVeloc += 45;
|
|
_plane.FlVeloc = fixAdd(_plane.FlVeloc, iToFix(45));
|
|
// Shift range to 0..-17.
|
|
//_plane.lVeloc /= 5.29;
|
|
_plane.FlVeloc = fixDiv(_plane.FlVeloc, fToFix(5.29));
|
|
// Multiply by pitch modifier.
|
|
//_plane.lVeloc *= (-(_plane.pitch * 0.157) + 1);
|
|
_plane.FlVeloc = fixAdd(_plane.FlVeloc, fixAdd(-(fixMul(_plane.Fpitch, fToFix(0.157))), iToFix(1)));
|
|
// Time slice.
|
|
//_plane.lVeloc /= 1000;
|
|
//_plane.lVeloc *= _plane.loopTime;
|
|
_plane.FlVeloc = fixDiv(_plane.FlVeloc, iToFix(1000));
|
|
_plane.FlVeloc = fixMul(_plane.FlVeloc, iToFix(_plane.loopTime));
|
|
// Gravity.
|
|
//_plane.gVeloc = _plane.loopTime * (-16.0 / 1000); // -16.0 is ft/sec for gravity.
|
|
_plane.FgVeloc = fixMul(iToFix(_plane.loopTime), fToFix(-0.016)); // -16.0 is ft/sec for gravity.
|
|
// Sum vertical velocity.
|
|
//_plane.vSpeed = _plane.gVeloc + _plane.lVeloc;
|
|
_plane.FvSpeed = fixAdd(_plane.FgVeloc, _plane.FlVeloc);
|
|
// No vertical speed if we're on the ground.
|
|
if ((!_plane.airborne) && (_plane.FvSpeed < 0)) _plane.FvSpeed = 0;
|
|
// Save climb rate in ft/min.
|
|
//_plane.climbRate = _plane.vSpeed / _plane.loopTime;
|
|
//_plane.climbRate *= 60000L;
|
|
_plane.FclimbRate = fixDiv(_plane.FvSpeed, iToFix(_plane.loopTime));
|
|
_plane.FclimbRate = fixMul(_plane.FclimbRate, iToFix(60000L)); //***TODO*** This long could be an issue.
|
|
// Expand to ft/hr.
|
|
//_plane.deltaZ = _plane.hSpeed * 5280;
|
|
_plane.FdeltaZ = fixMul(_plane.FhSpeed, iToFix(5280));
|
|
// Get ft/ms.
|
|
//_plane.deltaZ /= 3600000L;
|
|
//_plane.deltaZ *= _plane.loopTime;
|
|
_plane.FdeltaZ = fixDiv(_plane.FdeltaZ, iToFix(3600000L)); //***TODO*** This long could be an issue.
|
|
_plane.FdeltaZ = fixMul(_plane.FdeltaZ, iToFix(_plane.loopTime));
|
|
// Find effective angle of flight.
|
|
if (_plane.FdeltaZ) {
|
|
//_plane.efAOF = -(ourAtan(_plane.vSpeed / _plane.deltaZ));
|
|
_plane.FefAOF = -(fixAtan(fixDiv(_plane.FvSpeed, _plane.FdeltaZ)));
|
|
} else {
|
|
//_plane.efAOF = -(ourAtan(_plane.vSpeed));
|
|
_plane.FefAOF = -(fixAtan(_plane.FvSpeed));
|
|
}
|
|
// Convert to degrees.
|
|
//_plane.AOA = Degs(_plane.efAOF);
|
|
_plane.FAOA = fixDegs(_plane.FefAOF);
|
|
// Handle stalling.
|
|
//if (((_plane.pitch < _plane.AOA) && (_plane.AOA < 0)) && (_plane.hSpeed < 40)) {
|
|
if (((_plane.Fpitch < _plane.FAOA) && (_plane.FAOA < iToFix(0))) && (_plane.FhSpeed < iToFix(40))) {
|
|
//if ((_plane.pitch - _plane.AOA) < -20) _plane.stall = true;
|
|
if (fixSub(_plane.Fpitch, _plane.FAOA) < iToFix(-20)) _plane.stall = true;
|
|
}
|
|
if (_plane.stall) {
|
|
//if (_plane.pitch > 30) {
|
|
if (_plane.Fpitch > iToFix(30)) {
|
|
_plane.stall = false;
|
|
} else {
|
|
//_plane.pitch++;
|
|
_plane.Fpitch = fixAdd(_plane.Fpitch, iToFix(1));
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
#define SEGMENT_FLIGHT_MODEL_2
|
|
|
|
|
|
void lightPlane2(void) {
|
|
// Flight model. Inertial Damping.
|
|
if (_plane.FdPitch != iToFix(0)) {
|
|
//_plane.dPitch -= _plane.dPitch / 10;
|
|
_plane.FdPitch = fixSub(_plane.FdPitch, fixDiv(_plane.FdPitch, iToFix(10)));
|
|
//if (((_plane.dPitch > 0) && (_plane.dPitch < 0.01)) || ((_plane.dPitch < 0) && (_plane.dPitch > -0.01))) _plane.dPitch = 0;
|
|
if (((_plane.FdPitch > iToFix(0)) && (_plane.FdPitch < fToFix(0.01))) || ((_plane.FdPitch < iToFix(0)) && (_plane.FdPitch > fToFix(-0.01)))) _plane.FdPitch = iToFix(0);
|
|
}
|
|
if (_plane.FdYaw != iToFix(0)) {
|
|
//_plane.dYaw -= _plane.dYaw / 10;
|
|
_plane.FdYaw = fixSub(_plane.FdYaw, fixDiv(_plane.FdYaw, iToFix(10)));
|
|
//if (((_plane.dYaw > 0) && (_plane.dYaw < 0.01)) || ((_plane.dYaw < 0) && (_plane.dYaw > -0.01))) _plane.dYaw = 0;
|
|
if (((_plane.FdYaw > iToFix(0)) && (_plane.FdYaw < fToFix(0.01))) || ((_plane.FdYaw < iToFix(0)) && (_plane.FdYaw > fToFix(-0.01)))) _plane.FdYaw = iToFix(0);
|
|
}
|
|
if (_plane.FdRoll != iToFix(0)) {
|
|
//_plane.dRoll -= _plane.dRoll / 10;
|
|
_plane.FdRoll = fixSub(_plane.FdRoll, fixDiv(_plane.FdRoll, iToFix(10)));
|
|
//if (((_plane.dRoll > 0) && (_plane.dRoll < 0.01)) || ((_plane.dRoll < 0) && (_plane.dRoll > -0.01))) _plane.dRoll = 0;
|
|
if (((_plane.FdRoll > iToFix(0)) && (_plane.FdRoll < fToFix(0.01))) || ((_plane.FdRoll < iToFix(0)) && (_plane.FdRoll > fToFix(-0.01)))) _plane.FdRoll = iToFix(0);
|
|
}
|
|
|
|
// Flight model. Rate of Change.
|
|
if (_plane.airborne) {
|
|
if (_plane.aileron != 0) {
|
|
//_plane.torque = ((_plane.hSpeed * _plane.aileron) / 10000);
|
|
_plane.Ftorque = fixDiv(fixAdd(_plane.FhSpeed, iToFix(_plane.aileron)), iToFix(10000));
|
|
//if (_plane.dRoll != (_plane.torque * _plane.loopTime)) _plane.dRoll += _plane.torque * 6;
|
|
if (_plane.FdRoll != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdRoll = fixAdd(_plane.FdRoll, fixMul(_plane.Ftorque, iToFix(6)));
|
|
}
|
|
}
|
|
if (_plane.elevator != 0) {
|
|
//_plane.torque = ((_plane.hSpeed * _plane.elevator) / 10000);
|
|
_plane.Ftorque = fixDiv(fixAdd(_plane.FhSpeed, iToFix(_plane.elevator)), iToFix(10000));
|
|
//if ((!_plane.airborne) && (_plane.torque > 0)) _plane.torque = 0; //***FIX*** This is dumb.
|
|
if ((!_plane.airborne) && (_plane.Ftorque > iToFix(0))) _plane.Ftorque = iToFix(0); //***FIX*** This is dumb.
|
|
//if (_plane.dPitch != (_plane.torque * _plane.loopTime)) _plane.dPitch += _plane.torque * 1.5;
|
|
if (_plane.FdPitch != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdPitch = fixAdd(_plane.FdRoll, fixMul(_plane.Ftorque, fToFix(1.5)));
|
|
}
|
|
if (_plane.FhSpeed != iToFix(0)) {
|
|
_plane.Ftorque = iToFix(0);
|
|
//if (_plane.rudder != 0) _plane.torque = -((_plane.hSpeed * _plane.rudder) / 10000);
|
|
if (_plane.rudder != 0) _plane.Ftorque = -(fixDiv(fixMul(_plane.FhSpeed, iToFix(_plane.rudder)), iToFix(10000)));
|
|
_plane.Ftorque2 = iToFix(0);
|
|
if ((_plane.Froll > iToFix(0)) && (_plane.Froll <= iToFix(90))) { //***FIX*** This is dumb.
|
|
//_plane.torque2 = _plane.roll * 0.00050;
|
|
_plane.Ftorque2 = fixMul(_plane.Froll, fToFix(0.00050));
|
|
} else {
|
|
if ((_plane.Froll < iToFix(0)) && (_plane.Froll >= iToFix(-90))) {
|
|
//_plane.torque2 = _plane.roll * 0.00050;
|
|
_plane.Ftorque2 = fixMul(_plane.Froll, fToFix(0.00050));
|
|
}
|
|
}
|
|
//_plane.torque += _plane.torque2;
|
|
_plane.Ftorque = fixAdd(_plane.Ftorque, _plane.Ftorque2);
|
|
//if (_plane.dYaw != (_plane.torque * _plane.loopTime)) _plane.dYaw += _plane.torque * 1.5;
|
|
if (_plane.FdYaw != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdYaw = fixAdd(_plane.FdYaw, fixMul(_plane.Ftorque, fToFix(1.5)));
|
|
}
|
|
|
|
// Flight model. Apply Rotations.
|
|
// Transform pitch into components of yaw and pitch based on roll.
|
|
_plane.Froll = fixAdd(_plane.Froll, _plane.FdRoll);
|
|
_plane.Fyaw = fixAdd(_plane.Fyaw, _plane.FdYaw);
|
|
//_plane.pitch += (_plane.dPitch * cosD(_plane.roll));
|
|
_plane.Fpitch = fixAdd(_plane.Fpitch, fixMul(_plane.FdPitch, fixCosD(_plane.Froll)));
|
|
//_plane.yaw += -(_plane.dPitch * sinD(_plane.roll));
|
|
_plane.Fyaw = fixAdd(_plane.Fyaw, -fixMul(_plane.FdPitch, fixSinD(_plane.Froll)));
|
|
if (_plane.Froll > iToFix(180)) {
|
|
//_plane.roll = -180 + (_plane.roll - 180);
|
|
_plane.Froll = fixAdd(iToFix(-180), fixSub(_plane.Froll, iToFix(180)));
|
|
} else {
|
|
//if (_plane.roll < -180) _plane.roll = 180 + (_plane.roll + 180);
|
|
if (_plane.Froll < iToFix(-180)) _plane.Froll = fixAdd(iToFix(180), fixAdd(_plane.Froll, iToFix(180)));
|
|
}
|
|
if (_plane.Fyaw > iToFix(180)) {
|
|
//_plane.yaw = -180 + (_plane.yaw - 180);
|
|
_plane.Fyaw = fixAdd(iToFix(-180), fixSub(_plane.Fyaw, iToFix(180)));
|
|
} else {
|
|
//if (_plane.yaw < -180) _plane.yaw = 180 + (_plane.yaw + 180);
|
|
if (_plane.Fyaw < iToFix(-180)) _plane.Fyaw = fixAdd(iToFix(180), fixAdd(_plane.Fyaw, iToFix(180)));
|
|
}
|
|
// Handle special case when aircraft pitch passes vertical.
|
|
if ((_plane.Fpitch > iToFix(90)) || (_plane.Fpitch < iToFix(-90))) {
|
|
if (_plane.Froll >= iToFix(0)) {
|
|
_plane.Froll = fixSub(_plane.Froll, iToFix(180));
|
|
} else {
|
|
if (_plane.Froll < iToFix(0)) _plane.Froll = fixAdd(_plane.Froll, iToFix(180));
|
|
}
|
|
if (_plane.Fyaw >= iToFix(0)) {
|
|
_plane.Fyaw = fixSub(_plane.Fyaw, iToFix(180));
|
|
} else {
|
|
if (_plane.Fyaw < iToFix(0)) _plane.Fyaw = fixAdd(_plane.Fyaw, iToFix(180));
|
|
}
|
|
if (_plane.Fpitch > iToFix(0)) {
|
|
_plane.Fpitch = fixSub(iToFix(180), _plane.Fpitch);
|
|
} else {
|
|
if (_plane.Fpitch < iToFix(0)) _plane.Fpitch = fixSub(iToFix(-180), _plane.Fpitch);
|
|
}
|
|
}
|
|
// Dampen everything out to 0 if they get close enough.
|
|
if ((_plane.Fpitch > fToFix(-0.5)) && (_plane.Fpitch < fToFix(0.5))) _plane.Fpitch = iToFix(0);
|
|
if ((_plane.Froll > fToFix(-0.5)) && (_plane.Froll < fToFix(0.5))) _plane.Froll = iToFix(0);
|
|
if ((_plane.Fyaw > fToFix(-0.5)) && (_plane.Fyaw < fToFix(0.5))) _plane.Fyaw = iToFix(0);
|
|
}
|
|
|
|
|
|
#define SEGMENT_FLIGHT_MODEL_3
|
|
|
|
|
|
void moveAircraft(void) {
|
|
// Calculate new aircraft position. Each coordinate is 1 foot in 3D space.
|
|
_plane.FtmpX = iToFix(0);
|
|
_plane.FtmpY = iToFix(0);
|
|
_plane.FtmpZ = _plane.FdeltaZ;
|
|
|
|
// Order of these points is significant.
|
|
// Rotate in X.
|
|
//_plane.newY = -(_plane.tmpZ * sinD(_plane.AOA));
|
|
_plane.FnewY = -fixMul(_plane.FtmpZ, fixSinD(_plane.FAOA));
|
|
//_plane.newZ = (_plane.tmpZ * cosD(_plane.AOA));
|
|
_plane.FnewZ = fixMul(_plane.FtmpZ, fixCosD(_plane.FAOA));
|
|
_plane.FtmpY = _plane.FnewY;
|
|
_plane.FtmpZ = _plane.FnewZ;
|
|
// Rotate in Y.
|
|
//_plane.newX = (_plane.tmpZ * sinD(_plane.yaw));
|
|
_plane.FnewX = fixMul(_plane.FtmpZ, fixSinD(_plane.Fyaw));
|
|
//_plane.newZ = (_plane.tmpZ * cosD(_plane.yaw));
|
|
_plane.FnewZ = fixMul(_plane.FtmpZ, fixCosD(_plane.Fyaw));
|
|
|
|
// Translate rotated point back to where it should be
|
|
// relative to it's last position.
|
|
_plane.FcollectX = fixAdd(_plane.FcollectX, _plane.FnewX);
|
|
if ((_plane.FcollectX > iToFix(1)) || (_plane.FcollectX < iToFix(-1))) {
|
|
_plane.x -= fixToI(_plane.FcollectX);
|
|
_plane.FcollectX = iToFix(0);
|
|
}
|
|
_plane.FcollectY = fixAdd(_plane.FcollectY, _plane.FnewY);
|
|
if ((_plane.FcollectY > iToFix(1)) || (_plane.FcollectY < iToFix(-1))) {
|
|
_plane.y -= fixToI(_plane.FcollectY);
|
|
_plane.FcollectY = iToFix(0);
|
|
}
|
|
_plane.FcollectZ = fixAdd(_plane.FcollectZ, _plane.FnewZ);
|
|
if ((_plane.FcollectZ > iToFix(1)) || (_plane.FcollectZ < iToFix(-1))) {
|
|
_plane.z += fixToI(_plane.FcollectZ);
|
|
_plane.FcollectZ = iToFix(0);
|
|
}
|
|
|
|
// Are we flying?
|
|
if ((!_plane.airborne) && (_plane.y != 0)) _plane.airborne = true;
|
|
}
|
|
|
|
|
|
#define SEGMENT_SETUP
|
|
|
|
|
|
void resetAircraft(void) {
|
|
// Initialize airplane.
|
|
scdMemSet((byte *)&_plane, 0, sizeof(airplaneT));
|
|
|
|
_plane.brake = true;
|
|
_plane.loopTime = 40;
|
|
}
|
|
|
|
|
|
#define SEGMENT_MAIN
|
|
|
|
|
|
void updateAircraft(void) {
|
|
// Do the actual flying!
|
|
lightPlane();
|
|
lightPlane2();
|
|
moveAircraft();
|
|
}
|