Matrix HaloRenderingThread::getOrientationMatrix(CrystalDescriptor::OrientationType orientation)
{
Matrix m;
switch (orientation)
{
case CrystalDescriptor::OT_RANDOM:
{
/* First rotate the crystal around the Z axis:*/
Matrix zRot = createRotationMatrix(Vector3(0, 0, 1), randFloat(-3.14159265, 3.14159265));
/* Then rotate the Z axis into an uniformly distributed position*/
const Vector3 newAxis = getRandomVector();
Vector3 rotAxis = Vector3(0, 0, 1) % newAxis;
Matrix axisRot = getIdentityMatrix();
if (rotAxis * rotAxis != 0)
{
rotAxis /= ~rotAxis;
double rotAngle = acos(Vector3(0, 0, 1) * newAxis);
axisRot = createRotationMatrix(rotAxis, rotAngle);
}
return zRot % axisRot;
}
break;
case CrystalDescriptor::OT_PARRY:
{
/* For hexagonal prisms, one side face is horizontal. */
m = createRotationMatrix(Vector3(0, 1, 0), randFloat(0, 3.1415926536));
}
break;
case CrystalDescriptor::OT_COLUMN:
{
/* Main axis horizontal. */
Matrix n = createRotationMatrix(Vector3(0, 0, 1), randFloat(0, 3.1415926536));
Matrix o = createRotationMatrix(Vector3(0, 1, 0), randFloat(0, 3.1415926536));
m = n % o;
}
break;
case CrystalDescriptor::OT_PLATE:
{
/* Main axis vertical. */
m = createRotationMatrix(Vector3(1, 0, 0), 3.14159265636 * 0.5) % createRotationMatrix(Vector3(0, 1, 0), randFloat(0, 3.1415926536));
}
break;
case CrystalDescriptor::OT_LOWITZ:
{
/* Rotation around a diagonal axis. The axis is horizontal.*/
m =
createRotationMatrix(Vector3(1, 0, 0), 3.14159265636 * randFloat(-3.1415 * 0.5, 3.1415 * 0.5)) %
createRotationMatrix(Vector3(0, 1, 0), randFloat(0, 3.1415926536));
}
break;
default:
{
sendNotifyString(wxT("Invalid orientation type."), 1);
}
}
return m;
}
//first create a translation matrix that moves cube to (0,0,0)
//create rotation matrix from cube's axis, then multiply with translation matrix
//create a translation matrix that moves back to orginal position, multiply that
//matrix with the one above. Then multiply all vertices with final matrix
void rotateCube(Cube *cube){
float translationMatrix[16];
float rotationMatrix[16];
float tempMatrix[16];
float finalMatrix[16];
createTranslationMatrix(-cube->center[0], -cube->center[1], -cube->center[2], translationMatrix);
createRotationMatrix(rotationMatrix, cube->angle, cube->axis);
mat4MultMat4Float(rotationMatrix, translationMatrix, tempMatrix);
createTranslationMatrix(cube->center[0], cube->center[1], cube->center[2], translationMatrix);
mat4MultMat4Float(translationMatrix, tempMatrix, finalMatrix);
for(int i = 0; i < 8; i++){
mat4MultVec4Float(finalMatrix, cube->vertices[i], cube->vertices[i]);
cube->vertices[i][3] = 1.0;
}
}
Matrix3D<float> getViewMatrix() const
{
// The viewMatrix will be used to modify the vertex coordinates in order
// to transform object coordinates as viewed-by-camera (or eye) coordinates.
// Standard x,y,z values are used. Obviously the z value used here will have
// to be in the range near side < z < far side as defined by the frustum used
// in the projection matrix (see getProjectionMatrix() above).
Matrix3D<float> viewMatrix (Vector3D<float> (0.0f, 0.0f, -50.0f));
// The rotation matrix will be applied on each frame.
// The vector passed here contains the Euler angle values for each axis
// The empiric values used as params will create a slight but constant tilting
// on the x-axis, a periodic rotation on the y-axis and no rotation on the
// z-axis.
Matrix3D<float> rotationMatrix
= createRotationMatrix (Vector3D<float> (-0.3f, 5.0f * std::sin (getFrameCounter() * 0.01f), 0.0f));
return rotationMatrix * viewMatrix;
}
int main(void)
{
///////////////////////////////////////////////////////////////////////////
uint32_t currentTime;
systemReady = false;
systemInit();
systemReady = true;
evrPush(EVR_StartingMain, 0);
while (1)
{
evrCheck();
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
if (eepromConfig.useMs5611 == true)
{
if (newTemperatureReading && newPressureReading)
{
d1Value = d1.value;
d2Value = d2.value;
calculateMs5611Temperature();
calculateMs5611PressureAltitude();
newTemperatureReading = false;
newPressureReading = false;
}
}
else
{
if (newTemperatureReading && newPressureReading)
{
uncompensatedTemperatureValue = uncompensatedTemperature.value;
uncompensatedPressureValue = uncompensatedPressure.value;
calculateBmp085Temperature();
calculateBmp085PressureAltitude();
newTemperatureReading = false;
newPressureReading = false;
}
}
sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]);
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]);
newMagData = false;
magDataUpdate = true;
batMonTick();
cliCom();
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendAttitude();
mavlinkSendVfrHud();
}
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop
if (eepromConfig.useMpu6050 == true)
{
computeMpu6050TCBias();
sensors.accel500Hz[XAXIS] = ((float)accelData500Hz[XAXIS] - accelTCBias[XAXIS]) * MPU6050_ACCEL_SCALE_FACTOR;
sensors.accel500Hz[YAXIS] = -((float)accelData500Hz[YAXIS] - accelTCBias[YAXIS]) * MPU6050_ACCEL_SCALE_FACTOR;
sensors.accel500Hz[ZAXIS] = -((float)accelData500Hz[ZAXIS] - accelTCBias[ZAXIS]) * MPU6050_ACCEL_SCALE_FACTOR;
sensors.gyro500Hz[ROLL ] = ((float)gyroData500Hz[ROLL ] - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * MPU6050_GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroData500Hz[PITCH] - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * MPU6050_GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroData500Hz[YAW ] - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * MPU6050_GYRO_SCALE_FACTOR;
}
else
{
sensors.accel500Hz[XAXIS] = -((float)accelData500Hz[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS];
sensors.accel500Hz[YAXIS] = -((float)accelData500Hz[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS];
sensors.accel500Hz[ZAXIS] = -((float)accelData500Hz[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS];
// HJI sensors.accel500Hz[XAXIS] = firstOrderFilter(sensors.accel500Hz[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]);
// HJI sensors.accel500Hz[YAXIS] = firstOrderFilter(sensors.accel500Hz[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]);
// HJI sensors.accel500Hz[ZAXIS] = firstOrderFilter(sensors.accel500Hz[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]);
computeMpu3050TCBias();
sensors.gyro500Hz[ROLL ] = ((float)gyroData500Hz[ROLL ] - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * MPU3050_GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroData500Hz[PITCH] - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * MPU3050_GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroData500Hz[YAW ] - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * MPU3050_GYRO_SCALE_FACTOR;
}
MargAHRSupdate( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
magDataUpdate,
dt500Hz );
magDataUpdate = false;
computeAxisCommands(dt500Hz);
mixTable();
writeMotors();
if (eepromConfig.receiverType == SPEKTRUM)
writeServos();
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = sensors.accel500Hz[XAXIS]; // No sensor averaging so use the 500 Hz value
sensors.accel100Hz[YAXIS] = sensors.accel500Hz[YAXIS]; // No sensor averaging so use the 500 Hz value
sensors.accel100Hz[ZAXIS] = sensors.accel500Hz[ZAXIS]; // No sensor averaging so use the 500 Hz value
// HJI sensors.accel100Hz[XAXIS] = firstOrderFilter(sensors.accel100Hz[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]);
// HJI sensors.accel100Hz[YAXIS] = firstOrderFilter(sensors.accel100Hz[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]);
// HJI sensors.accel100Hz[ZAXIS] = firstOrderFilter(sensors.accel100Hz[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]);
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
if (armed == true)
{
if ( eepromConfig.activeTelemetry == 1 )
{
// 500 Hz Accels
telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS]);
}
if ( eepromConfig.activeTelemetry == 2 )
{
// 500 Hz Gyros
telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ],
sensors.gyro500Hz[PITCH],
sensors.gyro500Hz[YAW ]);
}
if ( eepromConfig.activeTelemetry == 4 )
{
// 500 Hz Attitudes
telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ],
sensors.attitude500Hz[PITCH],
sensors.attitude500Hz[YAW ]);
}
if ( eepromConfig.activeTelemetry == 8 )
{
// Vertical Variables
telemPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate);
}
if ( eepromConfig.activeTelemetry == 16 )
{
// Vertical Variables
telemPortPrintF("%9.4f, %9.4f, %9.4f,%1d, %9.4f, %9.4f\n", verticalVelocityCmd,
hDotEstimate,
hEstimate,
verticalModeState,
throttleCmd,
eepromConfig.PID[HDOT_PID].iTerm);
}
}
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
if (batMonVeryLowWarning > 0)
{
BEEP_TOGGLE;
batMonVeryLowWarning--;
}
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == false)
execUpCount++;
if ((execUpCount == 5) && (execUp == false))
{
execUp = true;
LED0_OFF;
LED1_OFF;
pwmEscInit();
homeData.magHeading = sensors.attitude500Hz[YAW];
}
if (batMonLowWarning > 0)
{
BEEP_TOGGLE;
batMonLowWarning--;
}
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendHeartbeat();
mavlinkSendSysStatus();
}
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
int main(void)
{
///////////////////////////////////////////////////////////////////////////
uint32_t currentTime;
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
if (eepromConfig.osdEnabled)
{
if (eepromConfig.osdDisplayAlt)
displayAltitude(sensors.pressureAlt10Hz, 0.0f, DISENGAGED);
if (eepromConfig.osdDisplayAH)
displayArtificialHorizon(sensors.attitude500Hz[ROLL], sensors.attitude500Hz[PITCH], flightMode);
if (eepromConfig.osdDisplayAtt)
displayAttitude(sensors.attitude500Hz[ROLL], sensors.attitude500Hz[PITCH], flightMode);
if (eepromConfig.osdDisplayHdg)
displayHeading(heading.mag);
}
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
if (newMagData == true)
{
sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS];
sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]);
newMagData = false;
magDataUpdate = true;
}
d1Average = d1Sum / 10;
d1Sum = 0;
calculateTemperature();
calculatePressureAltitude();
pressureAltValid = true;
switch (eepromConfig.gpsType)
{
///////////////////////
case NO_GPS: // No GPS installed
break;
///////////////////////
case MEDIATEK_3329_BINARY: // MediaTek 3329 in binary mode
decodeMediaTek3329BinaryMsg();
break;
///////////////////////
case MEDIATEK_3329_NMEA: // MediaTek 3329 in NMEA mode
decodeNMEAsentence();
break;
///////////////////////
case UBLOX: // UBLOX in binary mode
decodeUbloxMsg();
break;
///////////////////////
}
cliCom();
rfCom();
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
TIM_Cmd(TIM10, DISABLE);
timerValue = TIM_GetCounter(TIM10);
TIM_SetCounter(TIM10, 0);
TIM_Cmd(TIM10, ENABLE);
dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop
computeMPU6000TCBias();
/*
sensorTemp1 = computeMPU6000SensorTemp();
sensorTemp2 = sensorTemp1 * sensorTemp1;
sensorTemp3 = sensorTemp2 * sensorTemp1;
*/
sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
/*
sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f +
eepromConfig.accelBiasP0[XAXIS] +
eepromConfig.accelBiasP1[XAXIS] * sensorTemp1 +
eepromConfig.accelBiasP2[XAXIS] * sensorTemp2 +
eepromConfig.accelBiasP3[XAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f +
eepromConfig.accelBiasP0[YAXIS] +
eepromConfig.accelBiasP1[YAXIS] * sensorTemp1 +
eepromConfig.accelBiasP2[YAXIS] * sensorTemp2 +
eepromConfig.accelBiasP3[YAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f +
eepromConfig.accelBiasP0[ZAXIS] +
eepromConfig.accelBiasP1[ZAXIS] * sensorTemp1 +
eepromConfig.accelBiasP2[ZAXIS] * sensorTemp2 +
eepromConfig.accelBiasP3[ZAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR;
*/
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
/*
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL ] / 2.0f +
gyroBiasP0[ROLL ] +
eepromConfig.gyroBiasP1[ROLL ] * sensorTemp1 +
eepromConfig.gyroBiasP2[ROLL ] * sensorTemp2 +
eepromConfig.gyroBiasP3[ROLL ] * sensorTemp3 ) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f +
gyroBiasP0[PITCH] +
eepromConfig.gyroBiasP1[PITCH] * sensorTemp1 +
eepromConfig.gyroBiasP2[PITCH] * sensorTemp2 +
eepromConfig.gyroBiasP3[PITCH] * sensorTemp3 ) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f +
gyroBiasP0[YAW ] +
eepromConfig.gyroBiasP1[YAW ] * sensorTemp1 +
eepromConfig.gyroBiasP2[YAW ] * sensorTemp2 +
eepromConfig.gyroBiasP3[YAW ] * sensorTemp3 ) * GYRO_SCALE_FACTOR;
*/
MargAHRSupdate( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt500Hz );
magDataUpdate = false;
computeAxisCommands(dt500Hz);
mixTable();
writeServos();
writeMotors();
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
TIM_Cmd(TIM11, DISABLE);
timerValue = TIM_GetCounter(TIM11);
TIM_SetCounter(TIM11, 0);
TIM_Cmd(TIM11, ENABLE);
dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
if ( highSpeedTelem1Enabled == true )
{
// 500 Hz Accels
telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS]);
}
if ( highSpeedTelem2Enabled == true )
{
// 500 Hz Gyros
telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ],
sensors.gyro500Hz[PITCH],
sensors.gyro500Hz[YAW ]);
}
if ( highSpeedTelem3Enabled == true )
{
// Roll Rate, Roll Rate Command
telemetryPrintF("%9.4f, %9.4f\n", sensors.gyro500Hz[ROLL],
rxCommand[ROLL]);
}
if ( highSpeedTelem4Enabled == true )
{
// Pitch Rate, Pitch Rate Command
telemetryPrintF("%9.4f, %9.4f\n", sensors.gyro500Hz[PITCH],
rxCommand[PITCH]);
}
if ( highSpeedTelem5Enabled == true )
{
// Yaw Rate, Yaw Rate Command
telemetryPrintF("%9.4f, %9.4f\n", sensors.gyro500Hz[YAW],
rxCommand[YAW]);
}
if ( highSpeedTelem6Enabled == true )
{
// 500 Hz Attitudes
telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ],
sensors.attitude500Hz[PITCH],
sensors.attitude500Hz[YAW ]);
}
if ( highSpeedTelem7Enabled == true )
{
// Vertical Variables
telemetryPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt10Hz,
hDotEstimate,
hEstimate);
}
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
if (execUp == true)
BLUE_LED_TOGGLE;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == true)
GREEN_LED_TOGGLE;
if (execUp == false)
execUpCount++;
if ((execUpCount == 5) && (execUp == false))
execUp = true;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
wxThread::ExitCode HaloRenderingThread::Entry()
{
wxCommandEvent evt(wxEVT_HALO_RENDERING_THREAD_NOTIFY, wxID_ANY);
setupStruct.resultValid = false;
setupStruct.imageBuffer = 0;
setupStruct.rayPaths = new map<RayPathId, RayPathDescriptor>;
if (setupStruct.crystals.size() == 0)
{
evt.SetString(wxT("No crystal population set up."));
evt.SetInt(1);
setupStruct.notifee->AddPendingEvent(evt);
return 0;
}
// Cast rays on crystals and refracted rays will form a pixel.
int crystalTypeIndex = 0;
CrystalDescriptor *currentDescriptor = &setupStruct.crystals[0];
const int N_CRYSTAL_TYPES = setupStruct.crystals.size();
int crystalsRemaining = currentDescriptor->populationWeight;
Mesh currentMesh;
size_t percentStep = setupStruct.crystalCount / 100;
if (percentStep < 1) percentStep = 1;
Vector3 sunPos(0, 0, -100000);
rotate2d(sunPos.y, sunPos.z, setupStruct.solarAltitude * 3.14159265636 / 180.0);
// Two prependicular vector
Vector3 prepX = sunPos % Vector3(0, 1, 0); // FIXME: Sun on the zenith
Vector3 prepY = sunPos % prepX;
prepX /= ~prepX;
prepY /= ~prepY;
setupStruct.deleteBuffer = freeBuffer;
setupStruct.deleteMap = freeMap;
size_t size = setupStruct.imageSize;
setupStruct.imageBuffer = new uint32_t[size * size * 6];
memset(setupStruct.imageBuffer, 0, size * size * 6 * sizeof(uint32_t));
uint32_t *leftPlane = setupStruct.imageBuffer; // 0th plane
uint32_t *rightPlane = setupStruct.imageBuffer + size * size; // 1st plane
uint32_t *topPlane = setupStruct.imageBuffer + 2 * size * size; // 2nd plane
uint32_t *bottomPlane = setupStruct.imageBuffer + 3 * size * size; // 3rd plane
uint32_t *backPlane = setupStruct.imageBuffer + 4 * size * size; // 4th plane
uint32_t *frontPlane = setupStruct.imageBuffer + 5 * size * size; // 5th plane
double halfImageSize = size * 0.5;
int maxRays = (setupStruct.maxRayCastInfoSize * 1000000) / (sizeof(RayPathDescriptor) + sizeof(RayPathId));
if (maxRays == 0) maxRays = 1;
int recordRayModulus = setupStruct.crystalCount / maxRays;
if (!recordRayModulus) recordRayModulus = 1;
struct RefractionColor
{
int rgb;
double refractionIndex;
};
RefractionColor colors[] =
{
{ 0x0000FF, 1.3072 },
{ 0x0080FF, 1.3094 },
{ 0x00FFFF, 1.31 },
{ 0x00FF80, 1.3107 },
{ 0x00FF00, 1.3114 },
{ 0x80FF00, 1.3125 },
{ 0xFFFF00, 1.3136 },
{ 0xFF8000, 1.3147 },
{ 0xFF0000, 1.3158 },
{ 0xFF0040, 1.3172 },
{ 0xFF0080, 1.32 },
};
const int N_COLORS = sizeof(colors) / sizeof(colors[0]);
// Cast many rays.
for (size_t i = 0; i < setupStruct.crystalCount; i++)
{
if (setupStruct.cancelled)
{
// If the user shut the rendering down...
delete[] setupStruct.imageBuffer;
setupStruct.imageBuffer = 0;
return 0;
}
if (!crystalsRemaining)
{
// We iterate through the crystals based on their population weights.
crystalTypeIndex++;
if (crystalTypeIndex >= N_CRYSTAL_TYPES) crystalTypeIndex = 0;
currentDescriptor = &setupStruct.crystals[crystalTypeIndex];
crystalsRemaining = currentDescriptor->populationWeight;
}
// Get the raw mesh
currentMesh = currentDescriptor->mesh;
// Rotate it according to the orientation.
Matrix orientationMatrix = getOrientationMatrix(currentDescriptor->orientation);
// Apply wobbliness
Vector3 wobbleRotationAxis(1, 0, 0);
rotate2d(wobbleRotationAxis.x, wobbleRotationAxis.z, randFloat(0, 3.1415));
double wobblinessLimit = currentDescriptor->wobbliness * 3.1415 / 180.0;
Matrix wobbleMatrix = createRotationMatrix(
wobbleRotationAxis,
randFloatNormal(
0,
wobblinessLimit
)
);
Matrix transformation = orientationMatrix % wobbleMatrix;
transformMesh(currentMesh, transformation);
// Crystal created, now cast a ray on it.
Vector3 offset = prepX * randFloat(-1, 1) + prepY * randFloat(-1, 1);
vector<RayPath> rayPaths;
// Compute color here
double colorCode = randFloat(0, N_COLORS - 1);
RefractionColor prev = colors[(int)(floor(colorCode))];
RefractionColor next = colors[(int)(ceil(colorCode))];
double kColor = colorCode - floor(colorCode);
RefractionColor currentColor;
currentColor.rgb = prev.rgb;
currentColor.refractionIndex = (1 - kColor) * prev.refractionIndex + kColor * next.refractionIndex;
// Compute real poisition of the ray (Sun is a disk)
Vector3 realSunPos = sunPos;
Vector3 rayRotVector;
double rayRotVectorLength;
do
{
rayRotVector = realSunPos % getRandomVector();
rayRotVectorLength = ~rayRotVector;
}
while (rayRotVectorLength == 0);
rayRotVector /= rayRotVectorLength;
realSunPos = transformVector(
createRotationMatrix(
rayRotVector,
randFloat(
0,
setupStruct.solarDiskRadius * 3.1415926536 / 180.0
)
),
realSunPos
);
computeRayPathInGlassMesh(currentMesh, currentColor.refractionIndex, realSunPos + offset, -realSunPos, 0.01, rayPaths);
/* Project rays on the six planes. */
for (size_t j = 0; j < rayPaths.size(); j++)
{
RayPath ¤t = rayPaths[j];
size_t pathLength = current.size();
Vector3 exitDir = current[pathLength - 1].first - current[pathLength - 2].first;
Vector3 projectionDir = -exitDir;
double xPos, yPos;
// select the plane to project on.
uint32_t *plane;
double xm = 1, ym = 1;
int planeId;
if ((fabs(projectionDir.x) > fabs(projectionDir.y)) && (fabs(projectionDir.x) > fabs(projectionDir.z)))
{
if (projectionDir.x < 0)
{
plane = leftPlane;
planeId = 0;
xm = -1;
}
else
{
plane = rightPlane;
planeId = 1;
}
xPos = xm * projectionDir.z / fabs(projectionDir.x) * halfImageSize + halfImageSize;
yPos = -ym * projectionDir.y / fabs(projectionDir.x) * halfImageSize + halfImageSize;
}
if ((fabs(projectionDir.y) > fabs(projectionDir.x)) && (fabs(projectionDir.y) > fabs(projectionDir.z)))
{
if (projectionDir.y < 0)
{
plane = bottomPlane;
planeId = 3;
ym = -1;
}
else
{
plane = topPlane;
planeId = 2;
}
xPos = xm * projectionDir.x / fabs(projectionDir.y) * halfImageSize + halfImageSize;
yPos = -ym * projectionDir.z / fabs(projectionDir.y) * halfImageSize + halfImageSize;
}
if ((fabs(projectionDir.z) > fabs(projectionDir.x)) && (fabs(projectionDir.z) > fabs(projectionDir.y)))
{
if (projectionDir.z < 0)
{
plane = frontPlane;
planeId = 5;
}
else
{
plane = backPlane;
planeId = 4;
xm = -1;
}
xPos = xm * projectionDir.x / fabs(projectionDir.z) * halfImageSize + halfImageSize;
yPos = -ym * projectionDir.y / fabs(projectionDir.z) * halfImageSize + halfImageSize;
}
// Calculate the new intensity of the pixel.
xPos = clampInInt(xPos, 0, size - 1);
yPos = clampInInt(yPos, 0, size - 1);
int prevPixel = plane[(int)(yPos) * size + (int)(xPos)];
int nextPixel = 0;
double intensity = current[pathLength - 2].second * 20;
for (int j = 0; j < 3; j++)
{
int currentSaturation = (prevPixel >> (8 * j)) & 0xFF;
int toAdd = (int)(((currentColor.rgb >> (8 * j)) & 0xFF) * intensity) >> 8;
int nextSaturation;
if (currentSaturation + toAdd > 255) nextSaturation = 255;
else nextSaturation = currentSaturation + toAdd;
nextPixel += (1 << (8 * j)) * nextSaturation * setupStruct.pixelIntensity;
}
plane[(int)(yPos) * size + (int)(xPos)] = nextPixel;
// Record the ray if needed
if (!(i % recordRayModulus))
{
RayPathId pixelId(planeId, (int)xPos, (int)yPos);
RayPathDescriptor theDescriptor(
¤tDescriptor->mesh,
transformation,
realSunPos + offset,
-realSunPos,
intensity
);
map<RayPathId, RayPathDescriptor>::iterator it = setupStruct.rayPaths->find(pixelId);
if (it == setupStruct.rayPaths->end())
{
// If not found, insert it as new
setupStruct.rayPaths->insert(
make_pair(
pixelId,
theDescriptor
)
);
}
else if (it->second.intensity < intensity)
{
// If found, update it if the current ray is more intense
it->second = theDescriptor;
}
}
}
if (i % percentStep == 0)
{
sendNotifyString(wxString::Format(wxT("Casting rays: %d%%"), i / percentStep));
}
crystalsRemaining--;
}
evt.SetString(wxT("Completed."));
evt.SetInt(1); //< 1 means the operation is finished.
setupStruct.resultValid = true;
setupStruct.notifee->AddPendingEvent(evt);
return 0;
}
int main(void)
{
///////////////////////////////////////////////////////////////////////////
uint32_t currentTime;
#ifdef _DTIMING
#define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4)
#define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4)
#define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5)
#define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5)
#define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2)
#define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2)
#define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3)
#define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3)
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
//GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5;
//GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// PB0_DISABLE;
LA4_DISABLE;
LA2_DISABLE;
LA3_DISABLE;
LA1_DISABLE;
#endif
systemInit();
systemReady = true;
evrPush(EVR_StartingMain, 0);
while (1)
{
evrCheck();
///////////////////////////////
if (frame_50Hz)
{
#ifdef _DTIMING
LA2_ENABLE;
#endif
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
if (newTemperatureReading && newPressureReading)
{
d1Value = d1.value;
d2Value = d2.value;
calculateTemperature();
calculatePressureAltitude();
newTemperatureReading = false;
newPressureReading = false;
}
sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]);
rssiMeasure();
updateMax7456(currentTime, 0);
executionTime50Hz = micros() - currentTime;
#ifdef _DTIMING
LA2_DISABLE;
#endif
}
///////////////////////////////
if (frame_10Hz)
{
#ifdef _DTIMING
LA4_ENABLE;
#endif
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
if (newMagData == true)
{
sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS + eepromConfig.externalHMC5883] - eepromConfig.magBias[XAXIS + eepromConfig.externalHMC5883];
sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS + eepromConfig.externalHMC5883] - eepromConfig.magBias[YAXIS + eepromConfig.externalHMC5883];
sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS + eepromConfig.externalHMC5883] - eepromConfig.magBias[ZAXIS + eepromConfig.externalHMC5883]);
newMagData = false;
magDataUpdate = true;
}
decodeUbloxMsg();
batMonTick();
cliCom();
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendAttitude();
mavlinkSendVfrHud();
}
executionTime10Hz = micros() - currentTime;
#ifdef _DTIMING
LA4_DISABLE;
#endif
}
///////////////////////////////
if (frame_500Hz)
{
#ifdef _DTIMING
LA1_ENABLE;
#endif
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
TIM_Cmd(TIM10, DISABLE);
timerValue = TIM_GetCounter(TIM10);
TIM_SetCounter(TIM10, 0);
TIM_Cmd(TIM10, ENABLE);
dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop
computeMPU6000TCBias();
sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] * 0.5f - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS];
sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] * 0.5f - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS];
sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS];
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt500Hz);
magDataUpdate = false;
computeAxisCommands(dt500Hz);
mixTable();
writeServos();
writeMotors();
executionTime500Hz = micros() - currentTime;
#ifdef _DTIMING
LA1_DISABLE;
#endif
}
///////////////////////////////
if (frame_100Hz)
{
#ifdef _DTIMING
LA3_ENABLE;
#endif
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
TIM_Cmd(TIM11, DISABLE);
timerValue = TIM_GetCounter(TIM11);
TIM_SetCounter(TIM11, 0);
TIM_Cmd(TIM11, ENABLE);
dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] * 0.1f - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] * 0.1f - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS];
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
if (armed == true)
{
if ( eepromConfig.activeTelemetry == 1 )
{
// 500 Hz Accels
telemPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS]);
}
if ( eepromConfig.activeTelemetry == 2 )
{
// 500 Hz Gyros
telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ],
sensors.gyro500Hz[PITCH],
sensors.gyro500Hz[YAW ]);
}
if ( eepromConfig.activeTelemetry == 4 )
{
// 500 Hz Attitudes
telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ],
sensors.attitude500Hz[PITCH],
sensors.attitude500Hz[YAW ]);
}
if ( eepromConfig.activeTelemetry == 8 )
{
// Vertical Variables
telemPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate,
ms5611Temperature);
}
if ( eepromConfig.activeTelemetry == 16)
{
// Vertical Variables
telemPortPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f, %9.4f\n", verticalVelocityCmd,
hDotEstimate,
hEstimate,
ms5611Temperature,
verticalModeState,
throttleCmd,
eepromConfig.PID[HDOT_PID].iTerm);
}
}
executionTime100Hz = micros() - currentTime;
#ifdef _DTIMING
LA3_DISABLE;
#endif
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
gpsUpdated();
//if (eepromConfig.mavlinkEnabled == true)
//{
// mavlinkSendGpsRaw();
//}
if (batMonVeryLowWarning > 0)
{
LED1_TOGGLE;
batMonVeryLowWarning--;
}
if (execUp == true)
BLUE_LED_TOGGLE;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == true)
GREEN_LED_TOGGLE;
if (execUp == false)
execUpCount++;
if ((execUpCount == 5) && (execUp == false))
{
execUp = true;
pwmEscInit();
homeData.magHeading = sensors.attitude500Hz[YAW];
}
if (batMonLowWarning > 0)
{
LED1_TOGGLE;
batMonLowWarning--;
}
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendHeartbeat();
mavlinkSendSysStatus();
}
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
int main(void)
{
///////////////////////////////////////////////////////////////////////////
uint32_t currentTime;
systemReady = false;
systemInit();
systemReady = true;
evrPush(EVR_StartingMain, 0);
while (1)
{
evrCheck();
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
if (newTemperatureReading && newPressureReading)
{
d1Value = d1.value;
d2Value = d2.value;
calculateTemperature();
calculatePressureAltitude();
newTemperatureReading = false;
newPressureReading = false;
}
sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]);
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
if (newMagData == true)
{
sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS];
sensors.mag10Hz[YAXIS] = -((float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS]);
sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]);
newMagData = false;
magDataUpdate = true;
}
decodeUbloxMsg();
batMonTick();
cliCom();
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendAttitude();
mavlinkSendVfrHud();
}
else
{
rfCom();
}
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
TIM_Cmd(TIM6, DISABLE);
timerValue = TIM_GetCounter(TIM6);
TIM_SetCounter(TIM6, 0);
TIM_Cmd(TIM6, ENABLE);
dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop
computeMPU6000TCBias();
sensors.accel500Hz[XAXIS] = -((float)accelSummedSamples500Hz[XAXIS] * 0.5f - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS];
sensors.accel500Hz[YAXIS] = ((float)accelSummedSamples500Hz[YAXIS] * 0.5f - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS];
sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS];
//sensors.accel500Hz[XAXIS] = firstOrderFilter(sensors.accel500Hz[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]);
//sensors.accel500Hz[YAXIS] = firstOrderFilter(sensors.accel500Hz[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]);
//sensors.accel500Hz[ZAXIS] = firstOrderFilter(sensors.accel500Hz[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]);
sensors.gyro500Hz[ROLL ] = -((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = ((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
MargAHRSupdate( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt500Hz );
magDataUpdate = false;
computeAxisCommands(dt500Hz);
mixTable();
writeServos();
writeMotors();
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
TIM_Cmd(TIM7, DISABLE);
timerValue = TIM_GetCounter(TIM7);
TIM_SetCounter(TIM7, 0);
TIM_Cmd(TIM7, ENABLE);
dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] * 0.1f - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS];
sensors.accel100Hz[YAXIS] = ((float)accelSummedSamples100Hz[YAXIS] * 0.1f - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS];
//sensors.accel100Hz[XAXIS] = firstOrderFilter(sensors.accel100Hz[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]);
//sensors.accel100Hz[YAXIS] = firstOrderFilter(sensors.accel100Hz[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]);
//sensors.accel100Hz[ZAXIS] = firstOrderFilter(sensors.accel100Hz[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]);
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
if (armed == true)
{
if ( eepromConfig.activeTelemetry == 1 )
{
// 500 Hz Accels
telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS]);
}
if ( eepromConfig.activeTelemetry == 2 )
{
// 500 Hz Gyros
telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ],
sensors.gyro500Hz[PITCH],
sensors.gyro500Hz[YAW ]);
}
if ( eepromConfig.activeTelemetry == 4 )
{
// 500 Hz Attitudes
telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ],
sensors.attitude500Hz[PITCH],
sensors.attitude500Hz[YAW ]);
}
if ( eepromConfig.activeTelemetry == 8 )
{
// Vertical Variables
telemetryPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate,
ms5611Temperature);
}
if ( eepromConfig.activeTelemetry == 16 )
{
// Vertical Variables
telemetryPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f, %9.4f\n", verticalVelocityCmd,
hDotEstimate,
hEstimate,
ms5611Temperature,
verticalModeState,
throttleCmd,
eepromConfig.PID[HDOT_PID].iTerm);
}
}
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
gpsUpdated();
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendGpsRaw();
}
if (batMonVeryLowWarning > 0)
{
BEEP_TOGGLE;
batMonVeryLowWarning--;
}
if (execUp == true)
LED0_TOGGLE;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == false)
execUpCount++;
if ((execUpCount == 5) && (execUp == false))
{
execUp = true;
pwmEscInit();
homeData.magHeading = sensors.attitude500Hz[YAW];
}
if (batMonLowWarning > 0)
{
BEEP_TOGGLE;
batMonLowWarning--;
}
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendHeartbeat();
mavlinkSendSysStatus();
}
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
void CrystalEditorFrame::OnMouseMove(wxMouseEvent& event)
{
if (!dragging) return;
// Turn mouse movement to a movement of a virtual trackball (which is as big as the window).
Vector3 currentMousePos(event.GetX(), event.GetY(), 0);
int width, height;
GetSize(&width, &height);
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, width);
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, height);
double windowSize = std::min(width, height);
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, windowSize);
Vector3 center(0.5 * width, 0.5 * height, 0);
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, center);
Vector3 normPrevPos = (prevMousePos - center) / windowSize;
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, normPrevPos);
Vector3 normPos = (currentMousePos - center) / windowSize;
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, normPos);
// Invert vertical axis, because in camera space Y grow upward.
normPrevPos.y = -normPrevPos.y;
normPos.y = -normPos.y;
// Calculate the Z position on the trackball
normPrevPos.z = normPrevPos * normPrevPos <= 1 ? sqrt(1.0 - normPrevPos * normPrevPos) : 0;
normPos.z = normPos * normPos <= 1 ? sqrt(1.0 - normPos * normPos) : 0;
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, normPrevPos);
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, normPos);
// Calculate the rotation axis
Vector3 rotAxis = normPrevPos % normPos;
double rotAxisLength = ~rotAxis;
if (rotAxisLength == 0)
{
// No rotation.
return;
}
rotAxis /= rotAxisLength;
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, rotAxis);
// Calculate the real rotation axis and the angle.
double rotAngle = acos((normPrevPos * normPos)/(~normPrevPos * ~normPos));
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, rotAngle);
Vector3 realRotAxis = rotAxis.x * right + rotAxis.y * upward + rotAxis.z * backward; // Create the true rotation axis (in camera space).
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, realRotAxis);
Matrix rotation = createRotationMatrix(realRotAxis, -rotAngle); // Negative as we rotate the view, not the object
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, rotation);
backward = transformVector(rotation, backward);
right = transformVector(rotation, right);
upward = transformVector(rotation, upward);
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, backward);
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, right);
log_var(CRYSTAL_EDITOR_MOUSE_MOVE, upward);
fix3VectorSystem(right, upward, backward);
prevMousePos.x = event.GetX();
prevMousePos.y = event.GetY();
prevMousePos.z = 0;
// Calculate ray path
updateRayPaths();
mainPanel->Refresh();
log_printf(CRYSTAL_EDITOR_MOUSE_MOVE, "========================\n");
}
int main(void)
{
uint32_t currentTime;
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]);
newMagData = false;
magDataUpdate = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAlt10Hz = pressureAlt;
cliCom();
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt200Hz );
magDataUpdate = false;
computeAxisCommands(dt200Hz);
mixTable();
writeServos();
writeMotors();
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
//computeGyroTCBias();
//sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
//sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
//sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
//computeAxisCommands(dt100Hz);
//mixTable();
//writeServos();
//writeMotors();
if ( rfTelem1Enabled == true )
{
// 200 Hz Accels
cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel200Hz[XAXIS],
sensors.accel200Hz[YAXIS],
sensors.accel200Hz[ZAXIS]);
}
if ( rfTelem2Enabled == true )
{
// 200 Hz Gyros
cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro200Hz[ROLL ],
sensors.gyro200Hz[PITCH],
sensors.gyro200Hz[YAW ]);
}
if ( rfTelem3Enabled == true )
{
// Roll Rate, Roll Rate Command
cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[ROLL],
rxCommand[ROLL]);
}
if ( rfTelem4Enabled == true )
{
// Pitch Rate, Pitch Rate Command
cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[PITCH],
rxCommand[PITCH]);
}
if ( rfTelem5Enabled == true )
{
// Yaw Rate, Yaw Rate Command
cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[YAW],
rxCommand[YAW]);
}
if ( rfTelem6Enabled == true )
{
// 200 Hz Attitudes
cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude200Hz[ROLL ],
sensors.attitude200Hz[PITCH],
sensors.attitude200Hz[YAW ]);
}
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == false)
execUpCount++;
if ((execUpCount == 5) && (execUp == false))
{
execUp = true;
LED0_OFF;
LED1_OFF;
}
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
int main(void)
{
///////////////////////////////////////////////////////////////////////////
#ifdef _DTIMING
#define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4)
#define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4)
#define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5)
#define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5)
#define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2)
#define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2)
#define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3)
#define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3)
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
//GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5;
//GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// PB0_DISABLE;
LA4_DISABLE;
LA2_DISABLE;
LA3_DISABLE;
LA1_DISABLE;
#endif
uint32_t currentTime;
systemInit();
systemReady = true;
evrPush(EVR_StartingMain, 0);
while (1)
{
evrCheck();
///////////////////////////////
if (frame_50Hz)
{
#ifdef _DTIMING
LA2_ENABLE;
#endif
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
if (newTemperatureReading && newPressureReading)
{
d1Value = d1.value;
d2Value = d2.value;
calculateTemperature();
calculatePressureAltitude();
newTemperatureReading = false;
newPressureReading = false;
}
sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]);
executionTime50Hz = micros() - currentTime;
#ifdef _DTIMING
LA2_DISABLE;
#endif
}
///////////////////////////////
if (frame_10Hz)
{
#ifdef _DTIMING
LA4_ENABLE;
#endif
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
if (newMagData == true)
{
sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS];
sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]);
newMagData = false;
magDataUpdate = true;
}
cliCom();
rfCom();
batMonTick();
///////////////////////////
executionTime10Hz = micros() - currentTime;
#ifdef _DTIMING
LA4_DISABLE;
#endif
}
///////////////////////////////
if (frame_500Hz)
{
#ifdef _DTIMING
LA1_ENABLE;
#endif
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
TIM_Cmd(TIM10, DISABLE);
timerValue = TIM_GetCounter(TIM10);
TIM_SetCounter(TIM10, 0);
TIM_Cmd(TIM10, ENABLE);
dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop
computeMPU6000TCBias();
/*
sensorTemp1 = computeMPU6000SensorTemp();
sensorTemp2 = sensorTemp1 * sensorTemp1;
sensorTemp3 = sensorTemp2 * sensorTemp1;
*/
sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500HzMXR[XAXIS] = -(accelSummedSamples500HzMXR[XAXIS] / 2.0f - eepromConfig.accelBiasMXR[XAXIS]) * eepromConfig.accelScaleFactorMXR[XAXIS];
sensors.accel500HzMXR[YAXIS] = -(accelSummedSamples500HzMXR[YAXIS] / 2.0f - eepromConfig.accelBiasMXR[YAXIS]) * eepromConfig.accelScaleFactorMXR[YAXIS];
sensors.accel500HzMXR[ZAXIS] = (accelSummedSamples500HzMXR[ZAXIS] / 2.0f - eepromConfig.accelBiasMXR[ZAXIS]) * eepromConfig.accelScaleFactorMXR[ZAXIS];
/*
sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f +
eepromConfig.accelBiasP0[XAXIS] +
eepromConfig.accelBiasP1[XAXIS] * sensorTemp1 +
eepromConfig.accelBiasP2[XAXIS] * sensorTemp2 +
eepromConfig.accelBiasP3[XAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f +
eepromConfig.accelBiasP0[YAXIS] +
eepromConfig.accelBiasP1[YAXIS] * sensorTemp1 +
eepromConfig.accelBiasP2[YAXIS] * sensorTemp2 +
eepromConfig.accelBiasP3[YAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f +
eepromConfig.accelBiasP0[ZAXIS] +
eepromConfig.accelBiasP1[ZAXIS] * sensorTemp1 +
eepromConfig.accelBiasP2[ZAXIS] * sensorTemp2 +
eepromConfig.accelBiasP3[ZAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR;
*/
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
/*
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL ] / 2.0f +
gyroBiasP0[ROLL ] +
eepromConfig.gyroBiasP1[ROLL ] * sensorTemp1 +
eepromConfig.gyroBiasP2[ROLL ] * sensorTemp2 +
eepromConfig.gyroBiasP3[ROLL ] * sensorTemp3 ) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f +
gyroBiasP0[PITCH] +
eepromConfig.gyroBiasP1[PITCH] * sensorTemp1 +
eepromConfig.gyroBiasP2[PITCH] * sensorTemp2 +
eepromConfig.gyroBiasP3[PITCH] * sensorTemp3 ) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f +
gyroBiasP0[YAW ] +
eepromConfig.gyroBiasP1[YAW ] * sensorTemp1 +
eepromConfig.gyroBiasP2[YAW ] * sensorTemp2 +
eepromConfig.gyroBiasP3[YAW ] * sensorTemp3 ) * GYRO_SCALE_FACTOR;
*/
#if defined(MPU_ACCEL)
MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt500Hz);
#endif
#if defined(MXR_ACCEL)
sensors.accel500HzMXR[XAXIS] = firstOrderFilter(sensors.accel500HzMXR[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]);
sensors.accel500HzMXR[YAXIS] = firstOrderFilter(sensors.accel500HzMXR[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]);
sensors.accel500HzMXR[ZAXIS] = firstOrderFilter(sensors.accel500HzMXR[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]);
MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500HzMXR[XAXIS], sensors.accel500HzMXR[YAXIS], sensors.accel500HzMXR[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt500Hz);
#endif
magDataUpdate = false;
computeAxisCommands(dt500Hz);
mixTable();
writeServos();
writeMotors();
executionTime500Hz = micros() - currentTime;
#ifdef _DTIMING
LA1_DISABLE;
#endif
}
///////////////////////////////
if (frame_100Hz)
{
#ifdef _DTIMING
LA3_ENABLE;
#endif
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
TIM_Cmd(TIM11, DISABLE);
timerValue = TIM_GetCounter(TIM11);
TIM_SetCounter(TIM11, 0);
TIM_Cmd(TIM11, ENABLE);
dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel100HzMXR[XAXIS] = -(accelSummedSamples100HzMXR[XAXIS] / 10.0f - eepromConfig.accelBiasMXR[XAXIS]) * eepromConfig.accelScaleFactorMXR[XAXIS];
sensors.accel100HzMXR[YAXIS] = -(accelSummedSamples100HzMXR[YAXIS] / 10.0f - eepromConfig.accelBiasMXR[YAXIS]) * eepromConfig.accelScaleFactorMXR[YAXIS];
sensors.accel100HzMXR[ZAXIS] = (accelSummedSamples100HzMXR[ZAXIS] / 10.0f - eepromConfig.accelBiasMXR[ZAXIS]) * eepromConfig.accelScaleFactorMXR[ZAXIS];
sensors.accel100HzMXR[XAXIS] = firstOrderFilter(sensors.accel100HzMXR[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]);
sensors.accel100HzMXR[YAXIS] = firstOrderFilter(sensors.accel100HzMXR[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]);
sensors.accel100HzMXR[ZAXIS] = firstOrderFilter(sensors.accel100HzMXR[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]);
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
if (armed == true)
{
if ( eepromConfig.activeTelemetry == 1 )
{
// 500 Hz Accels
openLogPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS],
sensors.accel500HzMXR[XAXIS],
sensors.accel500HzMXR[YAXIS],
sensors.accel500HzMXR[ZAXIS]);
}
if ( eepromConfig.activeTelemetry == 2 )
{
// 500 Hz Gyros
openLogPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ],
sensors.gyro500Hz[PITCH],
sensors.gyro500Hz[YAW ]);
}
if ( eepromConfig.activeTelemetry == 4 )
{
// 500 Hz Attitudes
openLogPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ],
sensors.attitude500Hz[PITCH],
sensors.attitude500Hz[YAW ]);
}
if ( eepromConfig.activeTelemetry == 8 )
{
// Vertical Variables
openLogPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate,
ms5611Temperature);
}
if ( eepromConfig.activeTelemetry == 16)
{
// Vertical Variables
openLogPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f, %9.4f\n", verticalVelocityCmd,
hDotEstimate,
hEstimate,
ms5611Temperature,
verticalModeState,
throttleCmd,
eepromConfig.PID[HDOT_PID].iTerm);
}
}
executionTime100Hz = micros() - currentTime;
#ifdef _DTIMING
LA3_DISABLE;
#endif
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
if (execUp == true)
BLUE_LED_TOGGLE;
while (batMonVeryLowWarning > 0)
{
//BEEP_TOGGLE;
batMonVeryLowWarning--;
}
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == true)
GREEN_LED_TOGGLE;
if (execUp == false)
execUpCount++;
if ((execUpCount == 5) && (execUp == false))
{
execUp = true;
pwmEscInit(eepromConfig.escPwmRate);
}
while (batMonLowWarning > 0)
{
//BEEP_TOGGLE;
batMonLowWarning--;
}
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
int main(void)
{
///////////////////////////////////////////////////////////////////////////
uint32_t currentTime;
arm_matrix_instance_f32 a;
arm_matrix_instance_f32 b;
arm_matrix_instance_f32 x;
systemReady = false;
systemInit();
systemReady = true;
evrPush(EVR_StartingMain, 0);
#ifdef _DTIMING
#define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4)
#define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4)
#define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5)
#define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5)
#define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2)
#define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2)
#define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3)
#define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3)
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
//GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5;
//GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// PB0_DISABLE;
LA4_DISABLE;
LA2_DISABLE;
LA3_DISABLE;
LA1_DISABLE;
#endif
while (1)
{
checkUsbActive(false);
evrCheck();
///////////////////////////////
if (frame_50Hz)
{
#ifdef _DTIMING
LA2_ENABLE;
#endif
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
if (newTemperatureReading && newPressureReading)
{
d1Value = d1.value;
d2Value = d2.value;
calculateTemperature();
calculatePressureAltitude();
newTemperatureReading = false;
newPressureReading = false;
}
sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]);
rssiMeasure();
updateMax7456(currentTime, 0);
executionTime50Hz = micros() - currentTime;
#ifdef _DTIMING
LA2_DISABLE;
#endif
}
///////////////////////////////
if (frame_10Hz)
{
#ifdef _DTIMING
LA4_ENABLE;
#endif
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
if (newMagData == true)
{
nonRotatedMagData[XAXIS] = (rawMag[XAXIS].value * magScaleFactor[XAXIS]) - eepromConfig.magBias[XAXIS + eepromConfig.externalHMC5883];
nonRotatedMagData[YAXIS] = (rawMag[YAXIS].value * magScaleFactor[YAXIS]) - eepromConfig.magBias[YAXIS + eepromConfig.externalHMC5883];
nonRotatedMagData[ZAXIS] = (rawMag[ZAXIS].value * magScaleFactor[ZAXIS]) - eepromConfig.magBias[ZAXIS + eepromConfig.externalHMC5883];
arm_mat_init_f32(&a, 3, 3, (float *)hmcOrientationMatrix);
arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedMagData);
arm_mat_init_f32(&x, 3, 1, sensors.mag10Hz);
arm_mat_mult_f32(&a, &b, &x);
newMagData = false;
magDataUpdate = true;
}
decodeUbloxMsg();
batMonTick();
cliCom();
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendAttitude();
mavlinkSendVfrHud();
}
executionTime10Hz = micros() - currentTime;
#ifdef _DTIMING
LA4_DISABLE;
#endif
}
///////////////////////////////
if (frame_500Hz)
{
#ifdef _DTIMING
LA1_ENABLE;
#endif
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
TIM_Cmd(TIM10, DISABLE);
timerValue = TIM_GetCounter(TIM10);
TIM_SetCounter(TIM10, 0);
TIM_Cmd(TIM10, ENABLE);
dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop
computeMPU6000TCBias();
nonRotatedAccelData[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] * 0.5f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
nonRotatedAccelData[YAXIS] = ((float)accelSummedSamples500Hz[YAXIS] * 0.5f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
nonRotatedAccelData[ZAXIS] = ((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix);
arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedAccelData);
arm_mat_init_f32(&x, 3, 1, sensors.accel500Hz);
arm_mat_mult_f32(&a, &b, &x);
nonRotatedGyroData[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] * 0.5f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
nonRotatedGyroData[PITCH] = ((float)gyroSummedSamples500Hz[PITCH] * 0.5f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
nonRotatedGyroData[YAW ] = ((float)gyroSummedSamples500Hz[YAW] * 0.5f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix);
arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedGyroData);
arm_mat_init_f32(&x, 3, 1, sensors.gyro500Hz);
arm_mat_mult_f32(&a, &b, &x);
MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt500Hz);
magDataUpdate = false;
computeAxisCommands(dt500Hz);
mixTable();
writeServos();
writeMotors();
executionTime500Hz = micros() - currentTime;
#ifdef _DTIMING
LA1_DISABLE;
#endif
}
///////////////////////////////
if (frame_100Hz)
{
#ifdef _DTIMING
LA3_ENABLE;
#endif
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
TIM_Cmd(TIM11, DISABLE);
timerValue = TIM_GetCounter(TIM11);
TIM_SetCounter(TIM11, 0);
TIM_Cmd(TIM11, ENABLE);
dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop
nonRotatedAccelData[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] * 0.1f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
nonRotatedAccelData[YAXIS] = ((float)accelSummedSamples100Hz[YAXIS] * 0.1f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
nonRotatedAccelData[ZAXIS] = ((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix);
arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedAccelData);
arm_mat_init_f32(&x, 3, 1, sensors.accel100Hz);
arm_mat_mult_f32(&a, &b, &x);
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
if (armed == true)
{
if ( eepromConfig.activeTelemetry == 1 )
{
// Roll Loop
openLogPortPrintF("1,%1d,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f\n", flightMode,
rateCmd[ROLL],
sensors.gyro500Hz[ROLL],
ratePID[ROLL],
attCmd[ROLL],
sensors.attitude500Hz[ROLL],
attPID[ROLL]);
}
if ( eepromConfig.activeTelemetry == 2 )
{
// Pitch Loop
openLogPortPrintF("2,%1d,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f\n", flightMode,
rateCmd[PITCH],
sensors.gyro500Hz[PITCH],
ratePID[PITCH],
attCmd[PITCH],
sensors.attitude500Hz[PITCH],
attPID[PITCH]);
}
if ( eepromConfig.activeTelemetry == 4 )
{
// Sensors
openLogPortPrintF("3,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS],
sensors.gyro500Hz[ROLL],
sensors.gyro500Hz[PITCH],
sensors.gyro500Hz[YAW],
sensors.mag10Hz[XAXIS],
sensors.mag10Hz[YAXIS],
sensors.mag10Hz[ZAXIS],
sensors.attitude500Hz[ROLL],
sensors.attitude500Hz[PITCH],
sensors.attitude500Hz[YAW]);
}
if ( eepromConfig.activeTelemetry == 8 )
{
}
if ( eepromConfig.activeTelemetry == 16)
{
// Vertical Variables
openLogPortPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f\n", verticalVelocityCmd,
hDotEstimate,
hEstimate,
ms5611Temperature,
verticalModeState,
throttleCmd);
}
}
executionTime100Hz = micros() - currentTime;
#ifdef _DTIMING
LA3_DISABLE;
#endif
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
if (gpsValid() == true)
{
}
//if (eepromConfig.mavlinkEnabled == true)
//{
// mavlinkSendGpsRaw();
//}
if (batMonVeryLowWarning > 0)
{
LED1_TOGGLE;
batMonVeryLowWarning--;
}
if (execUp == true)
BLUE_LED_TOGGLE;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == true)
GREEN_LED_TOGGLE;
if (execUp == false)
execUpCount++;
// Initialize sensors after being warmed up
if ((execUpCount == 20) && (execUp == false))
{
computeMPU6000RTData();
initMag();
initPressure();
}
// Initialize PWM and set mag after sensor warmup
if ((execUpCount == 25) && (execUp == false))
{
execUp = true;
pwmEscInit();
homeData.magHeading = sensors.attitude500Hz[YAW];
}
if (batMonLowWarning > 0)
{
LED1_TOGGLE;
batMonLowWarning--;
}
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendHeartbeat();
mavlinkSendSysStatus();
}
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
