driver_main.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (Apache 2.0)
 * 
 *  Copyright (c) 2019, The MITRE Corporation.
 *  All rights reserved.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     https://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Sections of this project contains content developed by The MITRE Corporation.
 * If this code is used in a deployment or embedded within another project,
 * it is requested that you send an email to opensource@mitre.org in order to
 * let us know where this software is being used.
 *********************************************************************/
 
//STD includes
#include <cstdio>
#include <string>
#include <cmath>
#include <functional>
#include <termios.h>
#include <typeinfo>
#include <set>
#include <iostream>
#include <fstream>
#include <unistd.h>
 
// RS232
#include "rs232.h"
 
// KVH Includes
#include "kvh_geo_fog_3d_driver.hpp"
 
namespace kvh
{
 
  Driver::Driver(bool _debug) : connected_(false),
                                port_("/dev/ttyUSB0"),
                                debug_(_debug)
  {
  } // END Driver()
 
  Driver::~Driver()
  {
    Cleanup();
  } // END ~Driver()
 
  // PRIVATE FUNCTIONS
 
  int Driver::SendPacket(an_packet_t *_anPacket)
  {
    // Encode packet. Adds header including LRC and CRC
    an_packet_encode(_anPacket);
    // Send AN packet via serial port
    if (SendBuf(an_packet_pointer(_anPacket), an_packet_size(_anPacket)))
    {
      packetRequests_.push_back(static_cast<packet_id_e>(_anPacket->id));
      return 0;
    }
    else
    {
      return -1;
    }
  } // END SendPacket()
 
  // PUBLIC FUNCTIONS
 
  int Driver::Init(const std::string &_port, KvhPacketRequest &_packetsRequested)
  {
    return Driver::Init(_port, _packetsRequested, defaultOptions_);
  }
 
  int Driver::Init(const std::string &_port, KvhPacketRequest &_packetsRequested, KvhInitOptions _initOptions)
  {
    int returnValue = 0;
    int errorCode;
    debug_ = _initOptions.debugOn;
    if (debug_)
      printf("Debug statements enabled.\n");
 
    if ((errorCode = packetStorage_.Init(_packetsRequested)) != 0)
    {
      if (debug_)
        printf("Unable to intialize packet storage. Error code: %d", errorCode);
      return -1;
    }
    // CREATE PACKET PERIODS PACKET
 
    packet_periods_packet_t packetPeriods;
    if (deviceConfig_.CreatePacketPeriodsPacket(_packetsRequested, packetPeriods) < 0)
    {
      if (debug_)
        printf("Unable to create packet periods packet properly.");
      return -2;
    }
 
    // CALCULATING BUAD RATE
 
    // dataThroughput from above, 11 from their equation
    int minBaudRequired = deviceConfig_.CalculateRequiredBaud(_packetsRequested);
    if (debug_)
      printf("Calculated required minimum baud rate: %d\n", minBaudRequired);
 
    // \todo Handle returned value appropriately
    if (minBaudRequired < _initOptions.baudRate)
    {
      returnValue = 1;
    }
 
    // SETTING UP KALMAN FILTER OPTIONS
 
    filter_options_packet_t filterOptions;
    if (deviceConfig_.CreateFilterOptionsPacket(filterOptions, true, _initOptions.filterVehicleType,
                                                _initOptions.gnssEnabled, _initOptions.atmosphericAltitudeEnabled, _initOptions.velocityHeadingEnabled,
                                                _initOptions.reversingDetectionEnabled, _initOptions.motionAnalysisEnabled) != 0)
    {
      return -3;
    }
 
    // SETTING UP ODOMETER OPTIONS
    odometer_configuration_packet_t odometerOptions;
    if (deviceConfig_.CreateOdometerOptionsPacket(odometerOptions, true, static_cast<float>(_initOptions.odomPulseToMeters), false) != 0 )
    {
      return -2;
    }
    
    // CONNECTING TO KVH
 
    port_ = _port;
    char portArr[4096];
    strncpy(portArr, port_.c_str(), 4096);
    if (debug_) printf("Baud: %d\n", _initOptions.baudRate);
    if (OpenComport(portArr, _initOptions.baudRate) != 0)
    {
      if (debug_)
        printf("Unable to establish connection.\n");
      return -4;
    }
    // We are connected to the KVH!
    connected_ = true;
 
    // SENDING CONFIGURATION PACKETS
 
    an_packet_t *requestPacket;
    int packetError;
 
    if (debug_)
      printf("Sending packet_periods.\n");
 
    requestPacket = encode_packet_periods_packet(&packetPeriods);
    packetError = SendPacket(requestPacket);
    an_packet_free(&requestPacket);
    requestPacket = nullptr;
    if (packetError)
    {
      return -5;
    }
 
    if (debug_)
      printf("Sending filter options packet.");
 
    requestPacket = encode_filter_options_packet(&filterOptions);
    packetError = SendPacket(requestPacket);
    requestPacket = nullptr;
    if (packetError != 0)
    {
      return -6;
    }
 
    // INITIALISE AN DECODER
 
    if (debug_)
      printf("Initializing decoder.\n");
    an_decoder_initialise(&anDecoder_);
 
    return returnValue;
 
  } // END Init()
 
  int Driver::Once()
  {
    an_packet_t *anPacket;
    int bytesRec = 0;
    int unexpectedPackets = 0;
 
    // Check if new packets have been sent
    if ((bytesRec = PollComport(an_decoder_pointer(&anDecoder_), an_decoder_size(&anDecoder_))) > 0)
    {
      /* increment the decode buffer length by the number of bytes received */
      an_decoder_increment(&anDecoder_, bytesRec);
 
      /* decode all the packets in the buffer */
      while ((anPacket = an_packet_decode(&anDecoder_)) != NULL)
      {
        // If we get an acknowledgment packet from sending packets
        // Acknowledgement packet is different than the others so we keep it seperate
        if (anPacket->id == packet_id_acknowledge)
        {
          acknowledge_packet_t ackP;
          if (decode_acknowledge_packet(&ackP, anPacket) == 0)
          {
            if (debug_)
            {
              printf("*********************************\n"
                     "Acknowledging packet from packet id: %d\n Result of packet %d\n"
                     "********************************\n",
                     ackP.packet_id, ackP.acknowledge_result);
            }
          }
          else
          {
            if (debug_)
              printf("Unable to decode acknowledge packet properly.\n");
          }
        }
        else
        {
          if (DecodePacket(anPacket) < 0)
            unexpectedPackets++;
        }
 
        /* Ensure that you free the an_packet when your done with it or you will leak memory */
        an_packet_free(&anPacket);
      }
    }
 
    if (debug_) printf("Recieved %d unexpected packets during transmission.\n", unexpectedPackets);
  } // END Once()
 
  bool Driver::PacketIsUpdated(packet_id_e _packetId)
  {
    return packetStorage_.PacketIsUpdated(_packetId);
  }
 
  int Driver::SetPacketUpdated(packet_id_e _packetId, bool _updateStatus)
  {
    return packetStorage_.SetPacketUpdated(_packetId, _updateStatus);
  }
 
  int Driver::AddPacket(packet_id_e _packetId)
  {
    return packetStorage_.AddPacket(_packetId);
  }
 
  int Driver::RequestPacket(packet_id_e _requestedPacket)
  {
    an_packet_t* anPacket = encode_request_packet(_requestedPacket);
    if (SendPacket(anPacket) != 0)
    {
      if (debug_)
        printf("Unable to send packet request.\n");
      return -1;
    }
 
    return 0;
  }
 
  int Driver::Cleanup()
  {
    if (connected_)
      CloseComport();
    return 0;
  } // END Cleanup()
 
} // namespace kvh