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(); }