device_configuration.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.
 *********************************************************************/

#include "kvh_geo_fog_3d_device_configuration.hpp"

// RS 232
#include "rs232.h"

namespace kvh
{

  int KvhDeviceConfig::CreatePacketPeriodsPacket(KvhPacketRequest &_packetsRequested, packet_periods_packet_t &_packetPeriods)
  {
    if (_packetsRequested.size() > MAXIMUM_PACKET_PERIODS)
    {
      return -1;
    }
    // Make permanent in case it has a hot reset, otherwise an error is likely
    _packetPeriods.permanent = 1;
    // Clear all exisiting packet periods and replace with new ones
    _packetPeriods.clear_existing_packets = 1;

    std::set<packet_id_e> packetIdList; // To hold list of already added packet id's
    // \todo Figure out what to do with duplicated and unsupported variables
    int duplicated = 0;
    int unsupported = 0;
    int currentPacketPeriod = 0;

    for (int i = 0; i < _packetsRequested.size(); i++)
    {
      std::pair<packet_id_e, int> packet = _packetsRequested[i];

      if (supportedPackets_.count(packet.first) == 0)
      {
        unsupported += 1;
        continue;
      }

      if (packetIdList.count(packet.first) > 0)
      {
        duplicated += 1; // Found duplicate, increase counter
        continue;
      }

      // packet_period_t period = {packet id, period}
      packet_period_t period;
      period.packet_id = _packetsRequested[i].first;
      period.period = 1000 / _packetsRequested[i].second; // Using formula for rate to period derived above
      _packetPeriods.packet_periods[currentPacketPeriod] = period;

      // Record we have seen this packet id to look for duplicates
      packetIdList.insert(packet.first);
      currentPacketPeriod++;
    }

    return unsupported + duplicated;
  }

  int KvhDeviceConfig::CreateFilterOptionsPacket(
    filter_options_packet_t &_filterOptions,
    bool _permanent,
    uint8_t _vehicle_type,
    bool _internal_gnss_enabled,
    bool _atmospheric_altitude_enabled,
    bool _velocity_heading_enabled,
    bool _reversing_detection_enabled,
    bool _motion_analysis_enabled)
  {
    if (_vehicle_type > 13)
    {
      return -1;
    }

    _filterOptions.permanent = _permanent;
    _filterOptions.vehicle_type = _vehicle_type;
    _filterOptions.internal_gnss_enabled = _internal_gnss_enabled; // Set if we want to test using gps or not
    _filterOptions.reserved = 0;                                   // Need to set to 0, have found problems with other packets not doing this
    _filterOptions.atmospheric_altitude_enabled = _atmospheric_altitude_enabled;
    _filterOptions.velocity_heading_enabled = _velocity_heading_enabled;
    _filterOptions.reversing_detection_enabled = _reversing_detection_enabled;
    _filterOptions.motion_analysis_enabled = _motion_analysis_enabled;

    return 0;
  }

  int KvhDeviceConfig::CreateOdometerOptionsPacket(
    odometer_configuration_packet_t &_odometerOptions,
    bool _permanent,
    float _odom_pulse_to_meters,
    bool _odom_auto_cal)
  {
    _odometerOptions.permanent = _permanent;
    _odometerOptions.automatic_calibration = _odom_auto_cal;
    _odometerOptions.pulse_length = _odom_pulse_to_meters;

    return 0;
  }
  
  int KvhDeviceConfig::SetBaudRate(std::string _port, int _curBaudRate, int _primaryBaudRate, int _gpioBaudRate, int _auxBaudRate)
  {
    // Create the baud rate packet that we want to send
    baud_rates_packet_t baudRatePacket;
    baudRatePacket.permanent = 1;
    baudRatePacket.primary_baud_rate = _primaryBaudRate;
    baudRatePacket.gpio_1_2_baud_rate = _gpioBaudRate;
    baudRatePacket.auxiliary_baud_rate = _auxBaudRate;
    baudRatePacket.reserved = 0;

    an_packet_t *requestPacket = encode_baud_rates_packet(&baudRatePacket);

    char portArr[4096];
    strncpy(portArr, _port.c_str(), 4096);
    if (OpenComport(portArr, _curBaudRate) != 0)
    {
      return -1;
    }

    an_packet_encode(requestPacket);
    // Send AN packet via serial port
    if (!SendBuf(an_packet_pointer(requestPacket), an_packet_size(requestPacket)))
    {
      return -2;
    }
    an_packet_free(&requestPacket);
    requestPacket = nullptr;

    CloseComport();

    return 0;
  }

  int KvhDeviceConfig::FindCurrentBaudRate(std::string _port, int _startingBaud)
  {
    std::vector<int> baudRates = {
      1200, 1800, 2400, 4800, 9600,
      19200, 57600, 115200, 230400,
      460800, 500000, 576000, 921600, 1000000};

    char portArr[4096];
    strncpy(portArr, _port.c_str(), 4096);

    for (int baudRate : baudRates)
    {
      // Skip over baud rates lower than what we want to start at
      if (baudRate < _startingBaud)
      {
        continue;
      }

      printf("Opening with port: %s, baudrate: %d...\n", portArr, baudRate);
      if (OpenComport(portArr, baudRate) != 0)
      {
        continue; // Try next baud rate
      }

      an_decoder_t anDecoder;
      an_packet_t *anPacket;
      an_decoder_initialise(&anDecoder);
 
      // Check for data over the next second
      for (int i = 0; i < 20; i++)
      {
        int bytesRec = 0;
      
        // Check if new packets have been sent
        if ((bytesRec = PollComport(an_decoder_pointer(&anDecoder), an_decoder_size(&anDecoder))) > 0)
        {
          an_decoder_increment(&anDecoder, bytesRec);
          // See if we can decode a packet
          while ((anPacket = an_packet_decode(&anDecoder)) != NULL)
          {
            // If so, we have found the correct baud rate
            printf("Found a packet\n");
            CloseComport();
            an_packet_free(&anPacket);
            printf("Found connection at %d baud\n", baudRate);
            return baudRate;
          }
        }
        usleep(10000);
      }
      CloseComport();
    }
    return -1;
  }

  int KvhDeviceConfig::CalculateRequiredBaud(KvhPacketRequest &_packetsRequested)
  {
    std::set<packet_id_e> packetIdList;
    int dataThroughput = 0;

    for (int i = 0; i < _packetsRequested.size(); i++)
    {
      std::pair<packet_id_e, uint16_t> packet = _packetsRequested[i];

      // Check for duplicate packets or unsupported packet id's
      if (packetSize_.count(packet.first) == 0)
      {
        return -1;
      }
      else if (packetIdList.count(packet.first) > 0)
      {
        return -2;
      }

      // Max allowed hz is 1000
      if (packet.second > 1000)
      {
        return -3;
      }

      // Increase required baudrate by (struct_size + 5) * rate
      dataThroughput += (packetSize_[packet.first] + 5) * packet.second;
      packetIdList.insert(packet.first);
    }

    return dataThroughput * 11; // Minimum baud = Data throughput * 11
  }

} // namespace kvh