void FloatArrayDataRef::setValue(QString &newValue) {
// Check that value starts with [ and ends with ]
if(!newValue.startsWith('[') || !newValue.endsWith(']')) {
INFO << "Invalid array value";
return;
}
// Remove [] and split values
QString arrayString = newValue.mid(1, newValue.length() - 2);
QStringList values = arrayString.split(',');
// Limit number of values to write to ref length or number of given values
int numberOfValuesToWrite = qMin(_length, values.size());
// Convert values to float and copy to _valueArray
for(int i=0;i<numberOfValuesToWrite;i++) {
bool ok = true;
float value = values[i].toFloat(&ok);
if(!ok) {
INFO << "Invalid value " << values[i] << "in array";
return;
}
_valueArray[i]=value;
}
XPLMSetDatavf(_ref, _valueArray, 0, numberOfValuesToWrite);
}
float GetBodyRates(float elapsedMe, float elapsedSim, int counter, void* refcon)
{
(void)elapsedSim;
(void)counter;
(void)refcon;
pendingElapsedTime += elapsedMe;
ReceiveFromComPort();
if (pendingElapsedTime < 0.025) // Don't run faster than 40Hz
{
return -1;
}
union intbb Temp2;
float phi, theta, psi;
float alpha, beta;
float P_flight, Q_flight, R_flight;
float ax, ay, az;
float gravity_acceleration_x, gravity_acceleration_y, gravity_acceleration_z;
// Angular rates in X-Plane are specified relative to the flight path, not to the aircraft,
// for reasons unknown. So that means we need to rotate by alpha and beta to get angular rates
// in the aircraft body frame, which is what the UDB measures.
// Retrieve rates and slip angles, and convert to radians
P_flight = XPLMGetDataf(drP) / 180 * PI;
Q_flight = XPLMGetDataf(drQ) / 180 * PI;
R_flight = XPLMGetDataf(drR) / 180 * PI;
alpha = XPLMGetDataf(drAlpha) / 180 * PI;
beta = XPLMGetDataf(drBeta) / 180 * PI;
// On 25th Jan 2015, Bill Premerlani confirmed with Austin Meyer, author of X-Plane
// that P, Q and R are rotations in the body frame. So they do not need to be rotated into
// any other frame of reference, other than a small sign correction for the UDB frame conventions.
// Austin Meyer said: "now, i CAN say that P is roll, Q is pitch, and R is yaw, all in degrees per second
//about the aircraft axis,..... (i just looked at the code to confirm this)"
P_plane = P_flight;
Q_plane = -Q_flight; // convert from NED to UDB
R_plane = R_flight;
// Angular rate
// multiply by 5632 (constant from UDB code)
// Divide by SCALEGYRO(3.0 for red board)
// 1 * 5632 / 3.0 = 1877.33
Temp2.BB = (int)(P_plane * 1877.33);
Store2LE(&NAV_BODYRATES[6], Temp2);
Temp2.BB = (int)(Q_plane * 1877.33);
Store2LE(&NAV_BODYRATES[8], Temp2);
Temp2.BB = (int)(R_plane * 1877.33);
Store2LE(&NAV_BODYRATES[10], Temp2);
// Our euler angles:
// X-Plane angles are in degrees.
// Phi is roll, roll to right is positive
// Theta is pitch, pitch up is positive
// Psi is yaw, relative to north. North is 0/360.
// Convert these angles to radians first.
phi = (float)((XPLMGetDataf(drPhi) / 180) * PI);
theta = (float)((XPLMGetDataf(drTheta) / 180) * PI);
psi = (float)((XPLMGetDataf(drPsi) / 180) * PI);
// set up a vertical reference for the plotting computations
// vertical in earth frame:
ax = 0;
ay = -(float)9.8;
az = 0;
// get the acceleration loading (gravity-acceleration) in the body frame in "g"s,
// and convert to meter/sec/sec
// x, y, and z are "UDB" coordinates, x is left wing, y is forward, and z is down.
gravity_acceleration_x = (float)((XPLMGetDataf(drg_side)) * 9.8);
gravity_acceleration_y = (float)((XPLMGetDataf(drg_axil)) * 9.8);
gravity_acceleration_z = (float)((XPLMGetDataf(drg_nrml)) * 9.8);
// Convert from OGL frame to Aircraft body fixed frame
// This produces a vertical reference in body frame
OGLtoBCBF(ax, ay, az, phi, theta, psi);
ax_plane = ax;
ay_plane = -ay; // convert from NED to UDB
az_plane = az;
// Lastly we need to convert from X-Plane units (m/s^2) to the arbitrary units used by the UDB
// Accelerations are in m/s^2
// Divide by 9.8 to get g's
// Multiply by 5280 (constant from UDB code)
// Divide by SCALEACCEL (2.64 for red board)
// 1 / 9.8 * 5280 / 2.64 = 204.8
Temp2.BB = (int)(gravity_acceleration_x * 204.8);
Store2LE(&NAV_BODYRATES[12], Temp2);
Temp2.BB = (int)(gravity_acceleration_y * 204.8);
Store2LE(&NAV_BODYRATES[14], Temp2);
Temp2.BB = (int)(gravity_acceleration_z * 204.8);
Store2LE(&NAV_BODYRATES[16], Temp2);
CalculateChecksum(NAV_BODYRATES);
SendToComPort(sizeof(NAV_BODYRATES), NAV_BODYRATES);
while (pendingElapsedTime >= 0.025) // Don't run slower than 40Hz
{
GPSCount++;
if (!IsConnected())
{
ConnectionCount++;
if (ConnectionCount % 160 == 0) // attempt reconnection every 4 seconds when disconnected
{
AttemptConnection();
ConnectionCount = 0;
}
}
if (GPSCount % 10 == 0)
{
GetGPSData();
GPSCount = 0;
}
pendingElapsedTime -= (float)0.025;
}
ServosToControls();
// float ThrottleSetting = 0; //SurfaceDeflections[CHANNEL_THROTTLE];
// float throttle[8] = {ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting,
// ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting};
XPLMSetDatavf(drThro, ThrottleSettings, 0, 8);
static float prevBrakeSetting = PARKBRAKE_ON;
if (BrakeSetting != prevBrakeSetting)
{
prevBrakeSetting = BrakeSetting;
XPLMSetDataf(drBrake, BrakeSetting);
// LoggingFile.mLogFile << "Set parkbrake to " << BrakeSetting << endl;
}
return -1; // get called back on every frame
}
float GetBodyRates(float elapsedMe, float elapsedSim, int counter, void * refcon)
{
union intbb Temp2;
float phi, theta, psi;
float alpha, beta;
float P_flight, Q_flight, R_flight;
float ax, ay, az;
// Angular rates in X-Plane are specified relative to the flight path, not to the aircraft,
// for reasons unknown. So that means we need to rotate by alpha and beta to get angular rates
// in the aircraft body frame, which is what the UDB measures.
// Retrieve rates and slip angles, and convert to radians
P_flight = XPLMGetDataf(drP) / 180 * PI ;
Q_flight = XPLMGetDataf(drQ) / 180 * PI ;
R_flight = XPLMGetDataf(drR) / 180 * PI ;
alpha = XPLMGetDataf(drAlpha) / 180 * PI;
beta = XPLMGetDataf(drBeta) / 180 * PI;
FLIGHTtoBCBF(P_flight, Q_flight, R_flight, alpha, beta);
P_plane = P_flight;
Q_plane = Q_flight;
R_plane = R_flight;
// Angular rate
// multiply by 5632 (constant from UDB code)
// Divide by SCALEGYRO(3.0 for red board)
// 1 * 5632 / 3.0 = 1877.33
Temp2.BB = (int)(P_plane * 1877.33);
Store2LE(&NAV_BODYRATES[6], Temp2);
Temp2.BB = (int)(Q_plane * 1877.33);
Store2LE(&NAV_BODYRATES[8], Temp2);
Temp2.BB = (int)(R_plane * 1877.33);
Store2LE(&NAV_BODYRATES[10], Temp2);
// Our euler angles:
// X-Plane angles are in degrees.
// Phi is roll, roll to right is positive
// Theta is pitch, pitch up is positive
// Psi is yaw, relative to north. North is 0/360.
// Convert these angles to radians first.
phi =(XPLMGetDataf(drPhi)) / 180 * PI * -1.0;
theta = (XPLMGetDataf(drTheta)) / 180 * PI;
psi = (XPLMGetDataf(drPsi)) / 180 * PI * -1.0;
// Get accelerations in OpenGL coordinate frame
//ax = XPLMGetDataf(drLocal_ax);
//ay = XPLMGetDataf(drLocal_ay);
//az = XPLMGetDataf(drLocal_ay);
ax = 0;
ay = 0;
az = 0;
// Gravity is not included in ay, we need to add it. OGL y axis is +ve up,
// so g is -9.8.
ay -= (float)9.8;
// Convert from OGL frame to Aircraft body fixed frame
OGLtoBCBF(ax, ay, az, phi, theta, psi);
ax_plane = ax;
ay_plane = ay;
az_plane = az;
// Lastly we need to convert from X-Plane units (m/s^2) to the arbitrary units used by the UDB
// Accelerations are in m/s^2
// Divide by 9.8 to get g's
// Multiply by 5280 (constant from UDB code)
// Divide by SCALEACCEL (2.64 for red board)
// 1 / 9.8 * 5280 / 2.64 = 204.8
Temp2.BB = (int)(ax * 204.8);
Store2LE(&NAV_BODYRATES[12], Temp2);
Temp2.BB = (int)(ay * 204.8);
Store2LE(&NAV_BODYRATES[14], Temp2);
Temp2.BB = (int)(az * 204.8);
Store2LE(&NAV_BODYRATES[16], Temp2);
CalculateChecksum(NAV_BODYRATES);
SendToComPort(sizeof(NAV_BODYRATES),NAV_BODYRATES);
ReceiveFromComPort();
ServosToControls();
// float ThrottleSetting = 0; //SurfaceDeflections[CHANNEL_THROTTLE];
// float throttle[8] = {ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting,
// ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting};
XPLMSetDatavf(drThro, ThrottleSettings,0,8);
return -1;
}
int sp_process(uint32_t msg) {
//sprintf(tmp, "-> CP: sp_controller.sp_process: msg: %d\n", msg);
//XPLMDebugString(tmp);
int res = 0;
gEngineKnob = msg & SP_READ_ENGINES_KNOB_MASK;
uint32_t landingGearUp = msg & SP_READ_GEARLEVER_UP_MASK;
uint32_t landingGearDown = msg & SP_READ_GEARLEVER_DOWN_MASK;
uint32_t lightsLanding = msg & SP_READ_LIGHTS_LANDING_MASK;
uint32_t lightsTaxi = msg & SP_READ_LIGHTS_TAXI_MASK;
uint32_t lightsStrobe = msg & SP_READ_LIGHTS_STROBE_MASK;
uint32_t lightsNav = msg & SP_READ_LIGHTS_NAV_MASK;
uint32_t lightsBeacon = msg & SP_READ_LIGHTS_BEACON_MASK;
uint32_t lightsPanel = msg & SP_READ_LIGHTS_PANEL_MASK;
uint32_t cowl = msg & SP_READ_COWL_MASK;
uint32_t pitot = msg & SP_READ_PITOT_HEAT_MASK;
uint32_t deice = msg & SP_READ_DE_ICE_MASK;
uint32_t fuelPump = msg & SP_READ_FUEL_PUMP_MASK;
uint32_t avionicsMaster = msg & SP_READ_AVIONICS_MASTER_MASK;
uint32_t masterBattery = msg & SP_READ_MASTER_BAT_MASK;
uint32_t masterAltBattery = msg & SP_READ_MASTER_ALT_MASK;
if (gEngineKnob) {
gEngineKnobState = gEngineKnob;
sp_process_knob(gEngineKnob);
}
if (landingGearUp) {
gSpGearSwitchUp = 1;
XPLMCommandOnce(gSpLandingGearUpCmdRef);
} else if (landingGearDown) {
gSpGearSwitchUp = 0;
XPLMCommandOnce(gSpLandingGearDownCmdRef);
}
if (lightsLanding) {
XPLMCommandOnce(gSpLightsLandingOnCmdRef);
} else {
XPLMCommandOnce(gSpLightsLandingOffCmdRef);
}
if (lightsTaxi) {
XPLMCommandOnce(gSpLightsTaxiOnCmdRef);
} else {
XPLMCommandOnce(gSpLightsTaxiOffCmdRef);
}
if (lightsPanel) {
XPLMSetDataf(gSpCockpitLightsDataRef, 1);
} else {
XPLMSetDataf(gSpCockpitLightsDataRef, 0);
}
if (lightsBeacon) {
XPLMCommandOnce(gSpLightsBeaconOnCmdRef);
} else {
XPLMCommandOnce(gSpLightsBeaconOffCmdRef);
}
if (lightsNav) {
XPLMCommandOnce(gSpLightsNavOnCmdRef);
} else {
XPLMCommandOnce(gSpLightsNavOffCmdRef);
}
if (lightsStrobe) {
XPLMCommandOnce(gSpLightsStrobeOnCmdRef);
} else {
XPLMCommandOnce(gSpLightsStrobeOffCmdRef);
}
if (masterBattery) {
if (gSpNumberOfBatteries == 1) {
gSpBatArrayOn[0] = 1;
} else if (gSpNumberOfBatteries == 2) {
gSpBatArrayOn[0] = 1;
gSpBatArrayOn[1] = 1;
} else if (gSpNumberOfBatteries == 3) {
gSpBatArrayOn[0] = 1;
gSpBatArrayOn[1] = 1;
gSpBatArrayOn[2] = 1;
} else if (gSpNumberOfBatteries == 4) {
gSpBatArrayOn[0] = 1;
gSpBatArrayOn[1] = 1;
gSpBatArrayOn[2] = 1;
gSpBatArrayOn[3] = 1;
} else if (gSpNumberOfBatteries == 5) {
gSpBatArrayOn[0] = 1;
gSpBatArrayOn[1] = 1;
gSpBatArrayOn[2] = 1;
gSpBatArrayOn[3] = 1;
gSpBatArrayOn[4] = 1;
} else if (gSpNumberOfBatteries == 6) {
gSpBatArrayOn[0] = 1;
gSpBatArrayOn[1] = 1;
gSpBatArrayOn[2] = 1;
gSpBatArrayOn[3] = 1;
gSpBatArrayOn[4] = 1;
gSpBatArrayOn[5] = 1;
} else if (gSpNumberOfBatteries == 7) {
gSpBatArrayOn[0] = 1;
gSpBatArrayOn[1] = 1;
gSpBatArrayOn[2] = 1;
gSpBatArrayOn[3] = 1;
gSpBatArrayOn[4] = 1;
gSpBatArrayOn[5] = 1;
gSpBatArrayOn[6] = 1;
} else if (gSpNumberOfBatteries == 8) {
gSpBatArrayOn[0] = 1;
gSpBatArrayOn[1] = 1;
gSpBatArrayOn[2] = 1;
gSpBatArrayOn[3] = 1;
gSpBatArrayOn[4] = 1;
gSpBatArrayOn[5] = 1;
gSpBatArrayOn[6] = 1;
gSpBatArrayOn[7] = 1;
}
XPLMSetDatavi(gSpBatteryArrayOnDataRef, gSpBatArrayOn, 0, 8);
} else {
if (gSpNumberOfBatteries == 1) {
gSpBatArrayOn[0] = 0;
} else if (gSpNumberOfBatteries == 2) {
gSpBatArrayOn[0] = 0;
gSpBatArrayOn[1] = 0;
} else if (gSpNumberOfBatteries == 3) {
gSpBatArrayOn[0] = 0;
gSpBatArrayOn[1] = 0;
gSpBatArrayOn[2] = 0;
} else if (gSpNumberOfBatteries == 4) {
gSpBatArrayOn[0] = 0;
gSpBatArrayOn[1] = 0;
gSpBatArrayOn[2] = 0;
gSpBatArrayOn[3] = 0;
} else if (gSpNumberOfBatteries == 5) {
gSpBatArrayOn[0] = 0;
gSpBatArrayOn[1] = 0;
gSpBatArrayOn[2] = 0;
gSpBatArrayOn[3] = 0;
gSpBatArrayOn[4] = 0;
} else if (gSpNumberOfBatteries == 6) {
gSpBatArrayOn[0] = 0;
gSpBatArrayOn[1] = 0;
gSpBatArrayOn[2] = 0;
gSpBatArrayOn[3] = 0;
gSpBatArrayOn[4] = 0;
gSpBatArrayOn[5] = 0;
} else if (gSpNumberOfBatteries == 7) {
gSpBatArrayOn[0] = 0;
gSpBatArrayOn[1] = 0;
gSpBatArrayOn[2] = 0;
gSpBatArrayOn[3] = 0;
gSpBatArrayOn[4] = 0;
gSpBatArrayOn[5] = 0;
gSpBatArrayOn[6] = 0;
} else if (gSpNumberOfBatteries == 8) {
gSpBatArrayOn[0] = 0;
gSpBatArrayOn[1] = 0;
gSpBatArrayOn[2] = 0;
gSpBatArrayOn[3] = 0;
gSpBatArrayOn[4] = 0;
gSpBatArrayOn[5] = 0;
gSpBatArrayOn[6] = 0;
gSpBatArrayOn[7] = 0;
}
XPLMSetDatavi(gSpBatteryArrayOnDataRef, gSpBatArrayOn, 0, 8);
}
if (masterAltBattery) {
XPLMCommandOnce(gSpMasterAltBatteryOnCmdRef);
if (gSpNumberOfGenerators == 1) {
XPLMCommandOnce(gSpGeneratorOn1CmdRef);
} else if (gSpNumberOfGenerators == 2) {
XPLMCommandOnce(gSpGeneratorOn1CmdRef);
XPLMCommandOnce(gSpGeneratorOn2CmdRef);
} else if (gSpNumberOfGenerators == 3) {
XPLMCommandOnce(gSpGeneratorOn1CmdRef);
XPLMCommandOnce(gSpGeneratorOn2CmdRef);
XPLMCommandOnce(gSpGeneratorOn3CmdRef);
} else if (gSpNumberOfGenerators == 4) {
XPLMCommandOnce(gSpGeneratorOn1CmdRef);
XPLMCommandOnce(gSpGeneratorOn2CmdRef);
XPLMCommandOnce(gSpGeneratorOn3CmdRef);
XPLMCommandOnce(gSpGeneratorOn4CmdRef);
}
} else {
XPLMCommandOnce(gSpMasterAltBatteryOffCmdRef);
if (gSpNumberOfGenerators == 1) {
XPLMCommandOnce(gSpGeneratorOff1CmdRef);
} else if (gSpNumberOfGenerators == 2) {
XPLMCommandOnce(gSpGeneratorOff1CmdRef);
XPLMCommandOnce(gSpGeneratorOff2CmdRef);
} else if (gSpNumberOfGenerators == 3) {
XPLMCommandOnce(gSpGeneratorOff1CmdRef);
XPLMCommandOnce(gSpGeneratorOff2CmdRef);
XPLMCommandOnce(gSpGeneratorOff3CmdRef);
} else if (gSpNumberOfGenerators == 4) {
XPLMCommandOnce(gSpGeneratorOff1CmdRef);
XPLMCommandOnce(gSpGeneratorOff2CmdRef);
XPLMCommandOnce(gSpGeneratorOff3CmdRef);
XPLMCommandOnce(gSpGeneratorOff4CmdRef);
}
}
if (avionicsMaster) {
XPLMCommandOnce(gSpMasterAvionicsOnCmdRef);
} else {
XPLMCommandOnce(gSpMasterAvionicsOffCmdRef);
}
if (fuelPump) {
if (gSpNumberOfEngines == 1) {
XPLMCommandOnce(gSpFuelPumpOn1CmdRef);
} else if (gSpNumberOfEngines == 2) {
XPLMCommandOnce(gSpFuelPumpOn1CmdRef);
XPLMCommandOnce(gSpFuelPumpOn2CmdRef);
} else if (gSpNumberOfEngines == 3) {
XPLMCommandOnce(gSpFuelPumpOn1CmdRef);
XPLMCommandOnce(gSpFuelPumpOn2CmdRef);
XPLMCommandOnce(gSpFuelPumpOn3CmdRef);
} else if (gSpNumberOfEngines == 4) {
XPLMCommandOnce(gSpFuelPumpOn1CmdRef);
XPLMCommandOnce(gSpFuelPumpOn2CmdRef);
XPLMCommandOnce(gSpFuelPumpOn3CmdRef);
XPLMCommandOnce(gSpFuelPumpOn4CmdRef);
}
} else {
if (gSpNumberOfEngines == 1) {
XPLMCommandOnce(gSpFuelPumpOff1CmdRef);
} else if (gSpNumberOfEngines == 2) {
XPLMCommandOnce(gSpFuelPumpOff1CmdRef);
XPLMCommandOnce(gSpFuelPumpOff2CmdRef);
} else if (gSpNumberOfEngines == 3) {
XPLMCommandOnce(gSpFuelPumpOff1CmdRef);
XPLMCommandOnce(gSpFuelPumpOff2CmdRef);
XPLMCommandOnce(gSpFuelPumpOff3CmdRef);
} else if (gSpNumberOfEngines == 4) {
XPLMCommandOnce(gSpFuelPumpOff1CmdRef);
XPLMCommandOnce(gSpFuelPumpOff2CmdRef);
XPLMCommandOnce(gSpFuelPumpOff3CmdRef);
XPLMCommandOnce(gSpFuelPumpOff4CmdRef);
}
}
if (deice) {
XPLMSetDatai(gSpAntiIceDataRef, 1);
} else {
XPLMSetDatai(gSpAntiIceDataRef, 0);
}
if (pitot) {
XPLMCommandOnce(gSpPitotHeatOnCmdRef);
} else {
XPLMCommandOnce(gSpPitotHeatOffCmdRef);
}
if (cowl) {
if (gSpNumberOfEngines == 1) {
gSpOpencowl[0] = 1;
} else if (gSpNumberOfEngines == 2) {
gSpOpencowl[0] = 1;
gSpOpencowl[1] = 1;
} else if (gSpNumberOfEngines == 3) {
gSpOpencowl[0] = 1;
gSpOpencowl[1] = 1;
gSpOpencowl[2] = 1;
} else if (gSpNumberOfEngines == 4) {
gSpOpencowl[0] = 1;
gSpOpencowl[1] = 1;
gSpOpencowl[2] = 1;
gSpOpencowl[3] = 1;
}
XPLMSetDatavf(gSpCowlFlapsDataRef, gSpOpencowl, 0, 8);
} else {
if (gSpNumberOfEngines == 1) {
gSpClosecowl[0] = 1;
} else if (gSpNumberOfEngines == 2) {
gSpClosecowl[0] = 1;
gSpClosecowl[1] = 1;
} else if (gSpNumberOfEngines == 3) {
gSpClosecowl[0] = 1;
gSpClosecowl[1] = 1;
gSpClosecowl[2] = 1;
} else if (gSpNumberOfEngines == 4) {
gSpClosecowl[0] = 1;
gSpClosecowl[1] = 1;
gSpClosecowl[2] = 1;
gSpClosecowl[3] = 1;
}
XPLMSetDatavf(gSpCowlFlapsDataRef, gSpClosecowl, 0, 8);
}
return res;
}
