From 6ba4def632e22ade4b49bee022d2c10ae1fe4aec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Apr 2012 12:21:00 +0200 Subject: [PATCH 1/3] Adding waypoints also works without position --- src/ui/WaypointList.cc | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 1c9bdb4..f3eeb62 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -246,12 +246,22 @@ void WaypointList::addEditable() if (uas) { // Create first waypoint at current MAV position - addCurrentPositionWaypoint(); + if (uas->localPositionKnown() || uas->globalPositionKnown()) + { + addCurrentPositionWaypoint(); + } + else + { + // MAV connected, but position unknown, add default waypoint + updateStatusLabel(tr("WARNING: No position known. Adding default LOCAL (NED) waypoint")); + wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); + WPM->addWaypointEditable(wp); + } } else { //Since no UAV available, create first default waypoint. - updateStatusLabel(tr("No UAV. Added default LOCAL (NED) waypoint")); + updateStatusLabel(tr("No UAV connected. Adding default LOCAL (NED) waypoint")); wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); WPM->addWaypointEditable(wp); //create a popup notifying the user about the limitations of offline editing From 04725cb1eff561d0d8547020b1d2ab3ff51fc990 Mon Sep 17 00:00:00 2001 From: Bryant Mairs Date: Thu, 12 Apr 2012 23:24:44 -0700 Subject: [PATCH 2/3] Fixed a stack overflow bug when using the regression fitting command in logfile plot. Solution involved just allocating on the heap instead of the stack. --- src/ui/QGCDataPlot2D.cc | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/ui/QGCDataPlot2D.cc b/src/ui/QGCDataPlot2D.cc index 94d6426..d23a56d 100644 --- a/src/ui/QGCDataPlot2D.cc +++ b/src/ui/QGCDataPlot2D.cc @@ -561,7 +561,10 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil bool QGCDataPlot2D::calculateRegression() { // TODO Add support for quadratic / cubic curve fitting - return calculateRegression(ui->xRegressionComboBox->currentText(), ui->yRegressionComboBox->currentText(), "linear"); + qDebug() << ui->xRegressionComboBox->currentText(); + qDebug() << ui->yRegressionComboBox->currentText(); + //return false; + return calculateRegression(ui->xRegressionComboBox->currentText(), ui->yRegressionComboBox->currentText(), "quadratic"); } /** @@ -579,9 +582,15 @@ bool QGCDataPlot2D::calculateRegression(QString xName, QString yName, QString me ui->xRegressionComboBox->setCurrentIndex(curveNames.indexOf(xName)); ui->yRegressionComboBox->setCurrentIndex(curveNames.indexOf(yName)); } + + // Create a couple of arrays for us to use to temporarily store some of the data from the plot. + // These arrays are allocated on the heap as they are far too big to go in the stack and will + // cause an overflow. + // TODO: Look into if this would be better done by having a getter return const double pointers instead + // of using memcpy(). const int size = 100000; - double x[size]; - double y[size]; + double *x = new double[size]; + double *y = new double[size]; int copied = plot->data(yName, x, y, size); if (method == "linear") { @@ -604,14 +613,12 @@ bool QGCDataPlot2D::calculateRegression(QString xName, QString yName, QString me function = tr("Linear regression failed. (Limit: %1 data points. Try with less)").arg(size); result = false; } - } else if (method == "quadratic") { - - } else if (method == "cubic") { - } else { function = tr("Regression method %1 not found").arg(method); result = false; } + + delete x, y; } else { // xName == yName function = tr("Please select different X and Y dimensions, not %1 = %2").arg(xName, yName); @@ -635,7 +642,7 @@ bool QGCDataPlot2D::calculateRegression(QString xName, QString yName, QString me * the match of the regression. * @return 1 on success, 0 on failure (e.g. because of infinite slope) */ -int QGCDataPlot2D::linearRegression(double* x,double* y,int n,double* a,double* b,double* r) +int QGCDataPlot2D::linearRegression(double *x, double *y, int n, double *a, double *b, double *r) { int i; double sumx=0,sumy=0,sumx2=0,sumy2=0,sumxy=0; From 9c1c1d8674f714eb6cc1843ed9b642d5a8022ada Mon Sep 17 00:00:00 2001 From: Bryant Mairs Date: Thu, 12 Apr 2012 23:38:19 -0700 Subject: [PATCH 3/3] Needed to remove debugging/test code before committing. Whoops. Also changed calculateRegression(...) to return a bool, as that's all it needs. --- src/ui/QGCDataPlot2D.cc | 23 +++++++++-------------- src/ui/QGCDataPlot2D.h | 2 +- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/src/ui/QGCDataPlot2D.cc b/src/ui/QGCDataPlot2D.cc index d23a56d..9013ea3 100644 --- a/src/ui/QGCDataPlot2D.cc +++ b/src/ui/QGCDataPlot2D.cc @@ -560,11 +560,8 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil bool QGCDataPlot2D::calculateRegression() { - // TODO Add support for quadratic / cubic curve fitting - qDebug() << ui->xRegressionComboBox->currentText(); - qDebug() << ui->yRegressionComboBox->currentText(); - //return false; - return calculateRegression(ui->xRegressionComboBox->currentText(), ui->yRegressionComboBox->currentText(), "quadratic"); + // TODO: Add support for quadratic / cubic curve fitting + return calculateRegression(ui->xRegressionComboBox->currentText(), ui->yRegressionComboBox->currentText(), "linear"); } /** @@ -597,8 +594,7 @@ bool QGCDataPlot2D::calculateRegression(QString xName, QString yName, QString me double a; // Y-axis crossing double b; // Slope double r; // Regression coefficient - int lin = linearRegression(x, y, copied, &a, &b, &r); - if(lin == 1) { + if (linearRegression(x, y, copied, &a, &b, &r)) { function = tr("%1 = %2 * %3 + %4 | R-coefficient: %5").arg(yName, QString::number(b), xName, QString::number(a), QString::number(r)); // Plot curve @@ -608,14 +604,13 @@ bool QGCDataPlot2D::calculateRegression(QString xName, QString yName, QString me plot->setStyleText("lines"); // x-value of the current rightmost x position in the plot plot->appendData(tr("regression %1-%2").arg(xName, yName), plot->invTransform(QwtPlot::xBottom, plot->width() - plot->width()*0.08f), (a + b*plot->invTransform(QwtPlot::xBottom, plot->width() - plot->width() * 0.08f))); - result = true; + + result = true; } else { function = tr("Linear regression failed. (Limit: %1 data points. Try with less)").arg(size); - result = false; } } else { function = tr("Regression method %1 not found").arg(method); - result = false; } delete x, y; @@ -642,7 +637,7 @@ bool QGCDataPlot2D::calculateRegression(QString xName, QString yName, QString me * the match of the regression. * @return 1 on success, 0 on failure (e.g. because of infinite slope) */ -int QGCDataPlot2D::linearRegression(double *x, double *y, int n, double *a, double *b, double *r) +bool QGCDataPlot2D::linearRegression(double *x, double *y, int n, double *a, double *b, double *r) { int i; double sumx=0,sumy=0,sumx2=0,sumy2=0,sumxy=0; @@ -652,7 +647,7 @@ int QGCDataPlot2D::linearRegression(double *x, double *y, int n, double *a, doub *b = 0; *r = 0; if (n < 2) - return(FALSE); + return true; /* Conpute some things we need */ for (i=0; i