WSpacePlot::WSpacePlot(QSize size, bool dg,
QWidget* parent, QWidget *colorBarParent,
WGraphXAxis::Position xAxisPos,
WGraphYAxis::Position yAxisPos,
int border)
: WGraphWidget(parent,xAxisPos,yAxisPos,border),
paintColor(red), dataRange(0.0, 1.0),
recRects(0), cellLabelRect(QRect()),
colorBar(0), startCell(0,0), currCell(0,0),
selecting(true), dragEnabled(false), drawGrid(dg),
autoScale(true), multiSelect(true) {
if (colorBarParent) {
colorBar = new WColorBar(colorBarParent);
connect(this,SIGNAL(colorChanged(QColor)),
colorBar,SLOT(setColor(QColor)));
connect(colorBar,SIGNAL(rangeSelected(WRange)),
this,SLOT(selectRange(WRange)));
connect(colorBar,SIGNAL(yVisRangeChanged(WRange)),
this,SLOT(setDataRange(WRange)));
colorBar->show();
}
setCursor(crossCursor);
if (drawGrid) setBackgroundColor(white);
setFocusPolicy(TabFocus);
xAxis()->setBaseTick(1.0);
yAxis()->setBaseTick(1.0);
showGrid(false);
setSize(size);
autoPanTimer = new QTimer(this);
labelClearTimer = new QTimer(this);
connect(frame(),SIGNAL(aboutToDraw()),this,SLOT(checkDataRange()));
connect(autoPanTimer,SIGNAL(timeout()),this,SLOT(autoPan()));
connect(labelClearTimer,SIGNAL(timeout()),this,SLOT(clearCellLabel()));
connect(this,SIGNAL(frameResized()),SLOT(calcGeometry()));
connect(this,SIGNAL(visRangeChanged()),SLOT(calcGeometry()));
}
//--------------------Engine Process-----------------------------//
void engineProcess(float dt)
{
static int loopCounter;
tStopWatch sw;
loopCounter++;
LEDon();
DEBUG_LEDoff();
StopWatchInit(&sw);
MPU6050_ACC_get(AccData); // Getting Accelerometer data
unsigned long tAccGet = StopWatchLap(&sw);
MPU6050_Gyro_get(GyroData); // Getting Gyroscope data
unsigned long tGyroGet = StopWatchLap(&sw);
Get_Orientation(AccAngleSmooth, CameraOrient, AccData, GyroData, dt);
unsigned long tAccAngle = StopWatchLap(&sw);
// if we enable RC control
if (configData[9] == '1')
{
// Get the RX values and Averages
Get_RC_Step(Step, RCSmooth); // Get RC movement on all three AXIS
Step[PITCH] = Limit_Pitch(Step[PITCH], CameraOrient[PITCH]); // limit pitch to defined limits in header
}
// Pitch adjustments
//pitch_setpoint += Step[PITCH];
pitchRCOffset += Step[PITCH] / 1000.0;
pitch_angle_correction = constrain((CameraOrient[PITCH] + pitchRCOffset) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
pitch_setpoint += pitch_angle_correction; // Pitch return to zero after collision
// Roll Adjustments
//roll_setpoint += Step[ROLL];
rollRCOffset += Step[ROLL] / 1000.0;
// include the config roll offset which is scaled to 0 = -10.0 degrees, 100 = 0.0 degrees, and 200 = 10.0 degrees
roll_angle_correction = constrain((CameraOrient[ROLL] + rollRCOffset + Deg2Rad((configData[11] - 100) / 10.0)) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
roll_setpoint += roll_angle_correction; //Roll return to zero after collision
// if we enabled AutoPan on Yaw
if (configData[10] == '0')
{
//ADC1Ch13_yaw = ((ADC1Ch13_yaw * 99.0) + ((float)(readADC1(13) - 2000) / 4000.0)) / 100.0; // Average ADC value
//CameraOrient[YAW] = CameraOrient[YAW] + 0.01 * (ADC1Ch13_yaw - CameraOrient[YAW]);
yaw_setpoint = autoPan(Output[YAW], yaw_setpoint);
}
else
{
// Yaw Adjustments
yaw_setpoint += Step[YAW];
yawRCOffset += Step[YAW] / 1000.0;
}
#if 0
yaw_angle_correction = constrain((CameraOrient[YAW] + yawRCOffset) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
yaw_setpoint += yaw_angle_correction; // Yaw return to zero after collision
#endif
unsigned long tCalc = StopWatchLap(&sw);
pitch_PID();
roll_PID();
yaw_PID();
unsigned long tPID = StopWatchLap(&sw);
unsigned long tAll = StopWatchTotal(&sw);
printcounter++;
//if (printcounter >= 500 || dt > 0.0021)
if (printcounter >= 500)
{
if (debugPrint)
{
print("Loop: %7d, I2CErrors: %d, angles: roll %7.2f, pitch %7.2f, yaw %7.2f\r\n",
loopCounter, I2Cerrorcount, Rad2Deg(CameraOrient[ROLL]),
Rad2Deg(CameraOrient[PITCH]), Rad2Deg(CameraOrient[YAW]));
}
if (debugSense)
{
print(" dt %f, AccData: %8.3f | %8.3f | %8.3f, GyroData %7.3f | %7.3f | %7.3f \r\n",
dt, AccData[X_AXIS], AccData[Y_AXIS], AccData[Z_AXIS], GyroData[X_AXIS], GyroData[Y_AXIS], GyroData[Z_AXIS]);
}
if (debugPerf)
{
print("idle: %5.2f%%, time[µs]: attitude est. %4d, IMU acc %4d, gyro %4d, angle %4d, calc %4d, PID %4d\r\n",
GetIdlePerf(), tAll, tAccGet, tGyroGet, tAccAngle, tCalc, tPID);
}
if (debugRC)
{
print(" RC2avg: %7.2f | RC3avg: %7.2f | RC4avg: %7.2f | RStep:%7.3f PStep: %7.3f YStep: %7.3f\r\n",
RCSmooth[ROLL], RCSmooth[PITCH], RCSmooth[YAW], Step[ROLL], Step[PITCH], Step[YAW]);
}
if (debugOrient)
{
print("Roll_setpoint:%12.4f | Pitch_setpoint:%12.4f | Yaw_setpoint:%12.4f\r\n",
roll_setpoint, pitch_setpoint, yaw_setpoint);
}
if (debugCnt)
{
print("Counter min %3d, %3d, %3d, max %4d, %4d, %4d, count %3d, %3d, %3d, usbOverrun %4d\r\n",
MinCnt[ROLL], MinCnt[PITCH], MinCnt[YAW],
MaxCnt[ROLL], MaxCnt[PITCH], MaxCnt[YAW],
IrqCnt[ROLL], IrqCnt[PITCH], IrqCnt[YAW],
usbOverrun());
}
if (debugAutoPan)
{
print("Pitch_output:%3.2f | Roll_output:%3.2f | Yaw_output:%3.2f | centerpoint:%4.4f\n\r",
Output[PITCH],
Output[ROLL],
Output[YAW],
centerPoint);
}
printcounter = 0;
}
LEDoff();
}
