lds_polar_graph.cpp

Go to the documentation of this file.

/****************************************************************************
**
** Copyright (C) 2016 The Qt Company Ltd.
** Contact: https://www.qt.io/licensing/
**
** This file is part of the Qt Charts module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:GPL$
** Commercial License Usage
** Licensees holding valid commercial Qt licenses may use this file in
** accordance with the commercial license agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and The Qt Company. For licensing terms
** and conditions see https://www.qt.io/terms-conditions. For further
** information use the contact form at https://www.qt.io/contact-us.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3 or (at your option) any later version
** approved by the KDE Free Qt Foundation. The licenses are as published by
** the Free Software Foundation and appearing in the file LICENSE.GPL3
** included in the packaging of this file. Please review the following
** information to ensure the GNU General Public License requirements will
** be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "lds_polar_graph.h"

QT_CHARTS_USE_NAMESPACE

LdsPolarGraph::LdsPolarGraph(QWidget *parent) :
  QChartView(parent),
  angular_min_(0),
  angular_max_(360),
  radial_min_(0),
  radial_max(4000),
  chart_(new QPolarChart),
  series_(new QScatterSeries),
  baud_rate_(230400),
  port_("/dev/ttyUSB0"),
  shutting_down_(false),
  serial_(io_, port_)
{
  port_ = "/dev/ttyUSB0"; // input your portname, ex) Linux: "/dev/ttyUSB0", Windows: "COM1",  Mac: "/dev/tty.SLAB_USBtoUART" or "/dev/tty.usbserial-*"

  serial_.set_option(boost::asio::serial_port_base::baud_rate(baud_rate_));
  boost::asio::write(serial_, boost::asio::buffer("b", 1));

  connect(&timer_, SIGNAL(timeout()), this, SLOT(loadData()));
  timer_.start(0);  // msec

  series_->setPen(QColor(Qt::transparent));
  series_->setPen(QColor(255,0,0));
  series_->setMarkerSize(2);

  chart_->legend()->setVisible(false);
  chart_->addSeries(series_);

  QValueAxis *angularAxis = new QValueAxis();
  angularAxis->setTickCount(9);
  angularAxis->setLabelFormat("%.1f");
  angularAxis->setShadesVisible(true);
  angularAxis->setShadesBrush(QBrush(QColor(249, 249, 255)));
  chart_->addAxis(angularAxis, QPolarChart::PolarOrientationAngular);

  QValueAxis *radialAxis = new QValueAxis();
  radialAxis->setTickCount(9);
  radialAxis->setLabelFormat("%d");
  chart_->addAxis(radialAxis, QPolarChart::PolarOrientationRadial);

  series_->attachAxis(radialAxis);
  series_->attachAxis(angularAxis);

  radialAxis->setRange(radial_min_, radial_max);
  angularAxis->setRange(angular_min_, angular_max_);

  this->setChart(chart_);
  this->setRenderHint(QPainter::Antialiasing);
}

LdsPolarGraph::~LdsPolarGraph()
{
  // Stop motor of LDS
  boost::asio::write(serial_, boost::asio::buffer("e", 1));
}

void LdsPolarGraph::loadData()
{
  uint16_t *laser_sensor_data;
  laser_sensor_data = poll();

  series_->clear();

  for (int i = angular_min_; i < angular_max_; i++)
  {
    series_->append(i, laser_sensor_data[i]);
  }

  this->setChart(chart_);
}

uint16_t* LdsPolarGraph::poll()
{
  bool      got_scan    = false;
  int       index       = 0;
  uint8_t   start_count = 0;
  uint32_t  motor_speed = 0;
  uint16_t  rpms        = 0;
  static uint16_t range_data[360]     = {0, };
  static uint16_t intensity_data[360] = {0, };
  boost::array<uint8_t, 2520> raw_bytes;

  while (!shutting_down_ && !got_scan)
  {
    // Wait until first data sync of frame: 0xFA, 0xA0
    boost::asio::read(serial_, boost::asio::buffer(&raw_bytes[start_count],1));

    if(start_count == 0)
    {
      if(raw_bytes[start_count] == 0xFA)
      {
        start_count = 1;
      }
    }
    else if(start_count == 1)
    {
      if(raw_bytes[start_count] == 0xA0)
      {
        start_count = 0;

        // Now that entire start sequence has been found, read in the rest of the message
        got_scan = true;

        boost::asio::read(serial_,boost::asio::buffer(&raw_bytes[2], 2518));

        // Read data in sets of 6
        for(uint16_t i = 0; i < raw_bytes.size(); i=i+42)
        {
          if(raw_bytes[i] == 0xFA && raw_bytes[i+1] == (0xA0 + i / 42)) //&& CRC check
          {
            motor_speed += (raw_bytes[i+3] << 8) + raw_bytes[i+2]; //accumulate count for avg. time increment
            rpms=(raw_bytes[i+3]<<8|raw_bytes[i+2])/10;

            for(uint16_t j = i+4; j < i+40; j=j+6)
            {
              index = 6*(i/42) + (j-4-i)/6;

              // Four bytes per reading
              uint8_t byte0 = raw_bytes[j];
              uint8_t byte1 = raw_bytes[j+1];
              uint8_t byte2 = raw_bytes[j+2];
              uint8_t byte3 = raw_bytes[j+3];

              // range and intensity data
              uint16_t intensity = (byte1 << 8) + byte0;
              intensity_data[359-index] = intensity;
              uint16_t range = (byte3 << 8) + byte2;
              range_data[359-index] = range;

              //printf ("i[%d]=%d,",359-index, intensity);
              //printf ("r[%d]=%d,",359-index, range);
            }
          }
        }
      }
      else
      {
        start_count = 0;
      }
    }
  }
  return range_data;
}