Adapting to real 32bit data.

Author: janixgomez

CMakeLists.txt

@@ -72,6 +72,7 @@ else()
               lib/StatusRx.cpp
               lib/SonarPlayer.cpp
               lib/OculusMessageHandler.cpp
+              lib/SimpleFireMessage.cpp
               lib/IoServiceThread.cpp )
 
 

include/liboculus/Constants.h

@@ -52,6 +52,8 @@ namespace liboculus {
 
         const float AzimuthBeamwidthDeg = 0.6;
         const float AzimuthBeamwidthRad = 0.6*M_PI/180.0;
+
+        const float MaxRange = 40;
     };
 
     namespace Oculus_2100MHz {
@@ -60,5 +62,9 @@ namespace liboculus {
 
          const float AzimuthBeamwidthDeg = 0.4;
          const float AzimuthBeamwidthRad = 0.4*M_PI/180.0;
+
+         // \todo These shouldn't be fixed, should read from Oculus.h
+         const float MaxRange = 10;  // meters
     };
+
 }  // namespace liboculus

include/liboculus/ImageData.h

@@ -81,21 +81,38 @@ class ImageData {
     return ((uint8_t *)_data)[index];
   }
 
-    // This function works for either 1- or 2-byte sonar data
-    // For 1-byte, the 8-bit value is simply cast into the 16-bit return
-    uint16_t at_uint16(unsigned int beam, unsigned int rangeBin) const {
-    if ((_data == nullptr) || (beam >= _numBeams) || (rangeBin >= _numRanges)) return 0;
+  // This function works for either 1- or 2-byte sonar data
+  // For 1-byte, the 8-bit value is simply cast into the 16-bit return
+  uint16_t at_uint16(unsigned int beam, unsigned int rangeBin) const {
+  if ((_data == nullptr) || (beam >= _numBeams) || (rangeBin >= _numRanges)) return 0;
+
+  if(_dataSize == 1) {
+      return at_uint8(beam,rangeBin);
+  } else if (_dataSize == 2) {
+      const size_t offset = (rangeBin * _stride) + (beam * _dataSize) + _offset;
+      CHECK(offset < (_imageSize-1));
+      return (_data[offset] | _data[offset+1] << 8);
+  }
+
+  return 0;
+  }
 
-    if(_dataSize == 1) {
-        return at_uint8(beam,rangeBin);
-    } else if (_dataSize == 2) {
-        const size_t offset = (rangeBin * _stride) + (beam * _dataSize) + _offset;
-        CHECK(offset < (_imageSize-1));
-        return (_data[offset] | _data[offset+1] << 8);
-    }
+  uint32_t at_uint32(unsigned int beam, unsigned int rangeBin) const {
+  if ((_data == nullptr) || (beam >= _numBeams)
+        || (rangeBin >= _numRanges)) return 0;
+
+  if (_dataSize == 1) {
+      return at_uint8(beam, rangeBin);
+  } else if (_dataSize == 2) {
+      return at_uint16(beam, rangeBin);
+  } else {
+      const size_t offset = (rangeBin * _stride) + (beam * _dataSize) + _offset;
+      CHECK(offset < (_imageSize-1));
+      return (_data[offset] | _data[offset+1] << 8 | _data[offset+2] << 16 | _data[offset+3] << 24);
+  }
 
-    return 0;
-    }
+  return 0;
+  }
 
  private:
   const uint8_t *_data;

include/liboculus/SimpleFireMessage.h

@@ -67,6 +67,8 @@ class SimpleFireMessage : public MessageHeader {
 
   void dump() const override;
 
+  float range() const;
+
  protected:
   OculusSimpleFireFlags _flags;
 };  // class SimpleFireMessage
@@ -92,7 +94,10 @@ void SimpleFireMessage<FireMsgT>::dump() const {
     LOG(DEBUG) << "   Ping rate: (unknown) " << static_cast<int>(this->fireMsg()->pingRate);
   }
 
-  LOG(DEBUG) << "   Send gain: " << (this->flags().getSendGain() ? "Yes" : "No");
+  LOG(DEBUG) << "Flags: Send gain: " << (this->flags().getSendGain() ? "Yes" : "No");
+  LOG(DEBUG) << "Flags:  Range is: " << (this->flags().getRangeAsMeters() ? "Meters" : "Percent");
+  LOG(DEBUG) << "       Range: " << range();
+  LOG(DEBUG) << "Speed of sound: " << this->fireMsg()->speedOfSound;
 }
 
 }  // namespace liboculus

include/liboculus/SimplePingResult.h

@@ -186,6 +186,12 @@ void SimplePingResult<Ping_t>::dump() const {
   LOG(DEBUG) << "Spd of Sound: " << this->ping()->speedOfSoundUsed;
   LOG(DEBUG) << "   Range res: " << this->ping()->rangeResolution << " m";
 
+  if (this->flags().getRangeAsMeters()) {
+    LOG(DEBUG) << "  Calc range: " << this->ping()->rangeResolution*this->ping()->nRanges << " m";
+  } else {
+    LOG(DEBUG) << "   Pct range: " << this->ping()->rangeResolution*this->ping()->nRanges;
+  }
+
   LOG(DEBUG) << "   Num range: " << this->ping()->nRanges;
   LOG(DEBUG) << "   Num beams: " << this->ping()->nBeams;
 

include/liboculus/SonarConfiguration.h

@@ -76,6 +76,10 @@ class OculusSimpleFireFlags {
 };
 
 // Thin OO wrapper around the OculusSimpleFireMessage.
+//
+// \todo This API is a little messy right now.  Probably *shouldn't* keep
+// the shadow copy of the SimpleFireMessage{2}, instead
+// just make it at time of serialization.
 class SonarConfiguration {
  public:
 
@@ -88,7 +92,6 @@ class SonarConfiguration {
   SonarConfiguration &setGamma(int input);
   SonarConfiguration &setPingRate(PingRateType newRate);
   SonarConfiguration &setGainPercent(double input);
-  SonarConfiguration &setRange(double input);
   SonarConfiguration &setFlags(uint8_t flags);
   SonarConfiguration &setWaterTemperature(double degC);
 
@@ -98,13 +101,23 @@ class SonarConfiguration {
   } OculusFreqMode;
 
   SonarConfiguration &setFreqMode(OculusFreqMode input);
+  OculusFreqMode getFreqMode() const {
+    if (_sfm.masterMode == 1)
+      return OCULUS_LOW_FREQ;
+    else if (_sfm.masterMode == 2)
+      return OCULUS_HIGH_FREQ;
+  }
 
   SonarConfiguration &setDataSize(DataSizeType sz);
-  DataSizeType getDataSize() const { return _dataSize; }
+  DataSizeType getDataSize() const         { return _dataSize; }
+
+  SonarConfiguration &setRange(double input);
+
+  SonarConfiguration &sendRangeAsMeters(bool v);
+  SonarConfiguration &sendRangeAsMeters()  { return sendRangeAsMeters(true); }
+  SonarConfiguration &sendRangeAsPercent() { return sendRangeAsMeters(false); }
+  bool getSendRangeAsMeters() const        { return _sendRangeAsMeters; }
 
-  SonarConfiguration &setRangeAsMeters(bool v);
-  SonarConfiguration &rangeAsMeters()  { return setRangeAsMeters(true); }
-  SonarConfiguration &rangeAsPercent() { return setRangeAsMeters(false); }
 
   SonarConfiguration &setSendGain(bool v);
   SonarConfiguration &sendGain()       { return setSendGain(true); }
@@ -122,7 +135,6 @@ class SonarConfiguration {
   bool get512Beams() const                { return _512beams;}
 
 
-  bool getRangeAsMeters() const           { return _rangeAsMeters; }
   bool getSendGain() const                { return _sendGain; }
   //bool getData16Bit() const               { return _16bitData; }
   bool getSimpleReturn() const            { return _simpleReturn; }
@@ -136,7 +148,9 @@ class SonarConfiguration {
 
   mutable OculusSimpleFireMessage2 _sfm;
 
-  bool _rangeAsMeters;
+  bool _sendRangeAsMeters;
+  float _rangeInMeters;
+
   bool _sendGain;
   bool _simpleReturn;
   bool _gainAssistance;

lib/DataRx.cpp

@@ -261,7 +261,7 @@ void DataRx::rxPacket(const boost::system::error_code& ec,
     LOG(DEBUG) << "Received " << bytes_transferred << " of LogMessage data";
     LOG(INFO) << std::string(_buffer->begin()+sizeof(OculusMessageHeader), _buffer->end());
   } else {
-    LOG(WARNING) << "Ignoring message id " << static_cast<int>(hdr.msgId());
+    LOG(WARNING) << "Ignoring " << MessageTypeToString(hdr.msgId()) << " message, id " <<static_cast<int>(hdr.msgId());
   }
 
 exit:

lib/SimpleFireMessage.cpp

@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2017-2022 University of Washington
+ * Author: Aaron Marburg <[email protected]>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of University of Washington nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#include <iomanip>
+
+#include "liboculus/SimpleFireMessage.h"
+
+namespace liboculus {
+
+// Specializations for the cases where the behavior actually varies between
+// the two SimpleFireMessage types
+
+template<>
+float SimpleFireMessage<OculusSimpleFireMessage>::range() const {
+  return fireMsg()->range;
+}
+
+template<>
+ float SimpleFireMessage<OculusSimpleFireMessage2>::range() const {
+  return fireMsg()->rangePercent;
+}
+
+
+}

lib/SonarConfiguration.cpp

@@ -33,14 +33,16 @@
 
 #include "liboculus/SonarConfiguration.h"
 #include "liboculus/DataTypes.h"
+#include "liboculus/Constants.h"
 
 #include <boost/asio.hpp>
 #include <g3log/g3log.hpp>
 
 namespace liboculus {
 
 SonarConfiguration::SonarConfiguration()
-: _rangeAsMeters(true),
+: _sendRangeAsMeters(true),
+  _rangeInMeters(5),
   _sendGain(true),
   _simpleReturn(true),
   _gainAssistance(true),
@@ -73,7 +75,7 @@ SonarConfiguration &SonarConfiguration::setRange(double input) {
   // 40 meters is the max range for the 1200d model
   // may need to use a double instead of uint8_t (depends on flags)
   if (input <= 40 && input > 0) {
-    _sfm.rangePercent = input;
+    _rangeInMeters = input;
   } else {
     LOG(WARNING) << "Requested invalid range: " << input;
   }
@@ -113,8 +115,8 @@ SonarConfiguration &SonarConfiguration::setDataSize(DataSizeType sz) {
   return *this;
 }
 
-SonarConfiguration &SonarConfiguration::setRangeAsMeters(bool v) {
-  _rangeAsMeters = v;
+SonarConfiguration &SonarConfiguration::sendRangeAsMeters(bool v) {
+  _sendRangeAsMeters = v;
   return *this;
 }
 
@@ -145,9 +147,22 @@ template <>
 std::vector<uint8_t> SonarConfiguration::serialize<OculusSimpleFireMessage2>() const {
   updateFlags();
 
+  // Corner case.  If in 32bit mode, range is always a percentage of max range
+  // if ((_dataSize == dataSize32Bit) || (!_sendRangeAsMeters)) {
+  //   const float maxRange = ((getFreqMode()==OCULUS_LOW_FREQ) ? Oculus_1200MHz::MaxRange : Oculus_2100MHz::MaxRange);
+
+  //   _sfm.rangePercent = std::min(_rangeInMeters/maxRange * 100.0,100.0);
+
+
+  //   LOG(INFO) << "In 32bit mode, setting range to " << _rangeInMeters << " which is " << _sfm.rangePercent << " percent";
+  // } else {
+    _sfm.rangePercent = _rangeInMeters;
+  //}
+
   std::vector<uint8_t> v;
   const auto ptr = reinterpret_cast<const char*>(&_sfm);
   v.insert(v.end(), ptr, ptr + sizeof(OculusSimpleFireMessage2));
+
   return v;
 }
 
@@ -179,7 +194,7 @@ void SonarConfiguration::updateFlags() const {
   if (_dataSize == dataSize32Bit) 
     _sfm.extFlags |= 0x00000200;
 
-  _sfm.flags = (_rangeAsMeters  ? FlagBits::RangeAsMeters : 0 ) |
+  _sfm.flags = (_rangeInMeters  ? FlagBits::RangeAsMeters : 0 ) |
          (((_dataSize == dataSize16Bit) || (_dataSize == dataSize32Bit)) ? FlagBits::Data16Bits : 0) |
          (_sendGain       ? FlagBits::DoSendGain : 0) |
          (_simpleReturn   ? FlagBits::SimpleReturn : 0) |
@@ -195,15 +210,15 @@ void SonarConfiguration::dump() const {
             << std::hex << std::setw(2) << std::setfill('0')
             << static_cast<unsigned int>(_sfm.flags)
             << std::setw(8)
-            << "\n         Ext flags 0x"
+            << "\n            Ext flags 0x"
             << std::setw(8) << static_cast<uint32_t>(_sfm.extFlags)
             << std::dec << std::setw(0)
-            << "\n   range is meters " << getRangeAsMeters()
-            << "\n   data size       " << DataSizeToString(getDataSize())
-            << "\n   send gain       " << getSendGain()
-            << "\n   simple return   " << getSimpleReturn()
-            << "\n   gain assistance " << getGainAssistance()
-            << "\n   use 512 beams   " << get512Beams();
+            << "\n  send range is meters " << getSendRangeAsMeters()
+            << "\n       data size       " << DataSizeToString(getDataSize())
+            << "\n       send gain       " << getSendGain()
+            << "\n       simple return   " << getSimpleReturn()
+            << "\n       gain assistance " << getGainAssistance()
+            << "\n       use 512 beams   " << get512Beams();
 }
 
 

tools/oculus_client.cpp

@@ -69,6 +69,8 @@ int main(int argc, char **argv) {
   int stopAfter = -1;
   app.add_option("-n,--frames", stopAfter, "Stop after (n) frames.");
 
+  float range = 4;
+  app.add_option("-r,--range", range, "Range in meters");
 
   CLI11_PARSE(app, argc, argv);
 
@@ -109,15 +111,14 @@ int main(int argc, char **argv) {
 
   SonarConfiguration config;
   config.setPingRate(pingRateNormal);
-  if (bitDepth == 8)
+  config.setRange(range);
+  if (bitDepth == 8) {
     config.setDataSize(dataSize8Bit);
-  else if (bitDepth == 16)
+  } else if (bitDepth == 16) {
     config.setDataSize(dataSize16Bit);
-  else if (bitDepth == 32) {
+  } else if (bitDepth == 32) {
     config.dontSendGain()
           .noGainAssistance()
-          .rangeAsPercent()
-          // .use256Beams()
           .setDataSize(dataSize32Bit);
   }
 
@@ -164,7 +165,7 @@ int main(int argc, char **argv) {
     }
 
     count++;
-    if ((stopAfter > 0) && (count >= stopAfter)) _io_thread->stop();
+    if ((stopAfter > 0) && (count >= stopAfter)) doStop=true;
   });
 
   _data_rx.setOnConnectCallback([&]() {