void ScanSector(SectorScanData* sectorScanData, int rowNumber, float leftMaxSlope, float rightMinSlope) { LOGD("new sector scan from row #%d, slope %f -> %f", rowNumber, leftMaxSlope, rightMinSlope); if (leftMaxSlope < rightMinSlope) { LOGD("left slope < right slope; abort current scan"); return; } FieldOfViewData* fieldOfViewData = sectorScanData->fieldOfViewData; float nextRowLeftMaxSlope = leftMaxSlope; bool previousCellIsSolid = false; for (int currentRowNumber = rowNumber; currentRowNumber <= fieldOfViewData->viewDistance && !previousCellIsSolid; ++currentRowNumber) { ZPositionDiff rowFirstCellDiff = kLeftEdgeDiff * currentRowNumber; ZPositionDiff rowCellAfterLastDiff = kRightEdgeDiff * currentRowNumber + kRowDiff; float currentRowLeftMaxSlope = nextRowLeftMaxSlope; LOGD("scan row #%d, slope %f -> %f", currentRowNumber, currentRowLeftMaxSlope, rightMinSlope); for (ZPositionDiff cellDiff = rowFirstCellDiff; cellDiff != rowCellAfterLastDiff; cellDiff += kRowDiff) { float cellRightSlope = CalcSlope(cellDiff, CellSlope::Right); if (cellRightSlope > currentRowLeftMaxSlope) { continue; } float cellLeftSlope = CalcSlope(cellDiff, CellSlope::Left); if (cellLeftSlope < rightMinSlope) { break; } if (fieldOfViewData->CellIsIsTooFar(cellDiff)) { continue; } LOGD("cell %s with slopes [%f; %f] is visible", cellDiff.ToString().c_str(), cellLeftSlope, cellRightSlope); ZPositionDiff realSectorCellDiff = sectorScanData->conversionMatrix * cellDiff; fieldOfViewData->MarkCellAsVisible(realSectorCellDiff); bool cellIsSolid = fieldOfViewData->CellIsSolid(realSectorCellDiff); if (cellIsSolid) { nextRowLeftMaxSlope = cellRightSlope; if (previousCellIsSolid) { continue; } else { LOGD("current cell is first solid; initiating subscan"); ScanSector(sectorScanData, currentRowNumber + 1, currentRowLeftMaxSlope, cellLeftSlope); } } else if (previousCellIsSolid) { currentRowLeftMaxSlope = nextRowLeftMaxSlope; } previousCellIsSolid = cellIsSolid; } } }
double GuideAlgorithmLowpass::result(double input) { m_history.Add(input); ArrayOfDbl sortedHistory(m_history); sortedHistory.Sort(dbl_sort_func); m_history.RemoveAt(0); unsigned int numpts = m_history.GetCount(); double median = sortedHistory[sortedHistory.GetCount()/2]; double slope = CalcSlope(m_history); double dReturn = median + m_slopeWeight*slope; if (fabs(dReturn) > fabs(input)) { Debug.Write(wxString::Format("GuideAlgorithmLowpass::Result() input %.2f is < calculated value %.2f, using input\n", input, dReturn)); dReturn = input; } //TODO: Undertand this. I think this is wrong, since it divides the orignial input // by 11 if it was less than the computed value. And since the computed // value is median + slope, I'm not sure that it should be divided by 11 // either. But the goal of this exercise is to be bug for bug compatible // with PHD 1.x if (fabs(input) < m_minMove) { dReturn = 0.0; } Debug.Write(wxString::Format("GuideAlgorithmLowpass::Result() returns %.2f from input %.2f\n", dReturn, input)); return dReturn; }