File : sonar_handler.adb

--                              -*- Mode: Ada -*-
-- Filename        : sonar_handler.ads
-- Description     : synchronization for the sonar readings
-- Author          : Christfried Webers
-- Created On      : Mon Nov  8 20:47:28 1999
-- Last Modified By: .
-- Last Modified On: .
-- Update Count    : 0
-- Status          : Experimental
-------------------------------------------------------------------
with Text_IO;             use Text_IO;
with Ada.Integer_Text_IO; use Ada.Integer_Text_IO;

with Ada.Numerics.Elementary_Functions; use Ada.Numerics.Elementary_Functions;
with Metrics;                           use Metrics;

package body Sonar_Handler is

   InvalidSonarData : constant Raw_16Bit := 16#7FFE#;
   Scale            : constant Float     := 16#05A5.0# / 1000.0;

   AllValuesNew : SonarFlags := (others => True);
   AllValuesOld : SonarFlags := (others => False);

   type Orientation is record
      X, Y  : mm;
      Angle : Degrees;
   end record;

   Orientations : array (Sonar.Index) of Orientation :=
     ((X => -305, Y => 150, Angle => 180.0),
      (X => -270, Y => 180, Angle => 90.0),
      (X => 70, Y => 180, Angle => 90.0),
      (X => 110, Y => 175, Angle => 75.0),
      (X => 165, Y => 175, Angle => 60.0),
      (X => 220, Y => 170, Angle => 45.0),
      (X => 290, Y => 110, Angle => 30.0),
      (X => 295, Y => 55, Angle => 15.0),
      (X => 300, Y => 0, Angle => 0.0),
      (X => 295, Y => -55, Angle => -15.0),
      (X => 290, Y => -110, Angle => -30.0),
      (X => 220, Y => -170, Angle => -45.0),
      (X => 165, Y => -175, Angle => -60.0),
      (X => 110, Y => -175, Angle => -75.0),
      (X => 70, Y => -180, Angle => -90.0),
      (X => -270, Y => -180, Angle => -90.0),
      (X => -305, Y => -150, Angle => -180.0));

   function ClusterIndexToSonarIndex
     (ClusterIndex : Raw_8Bit)
      return         Sonar.Index
   is
      First : Integer := Sonar.Index'Pos (Sonar.Index'First);
   begin
      case ClusterIndex is
      when 16#00# =>
         return Sonar.Index'Val (First);
      when 16#01# =>
         return Sonar.Index'Val (First + 1);
      when 16#02# =>
         return Sonar.Index'Val (First + 2);
      when 16#03# =>
         return Sonar.Index'Val (First + 3);
      when 16#04# =>
         return Sonar.Index'Val (First + 4);
      when 16#05# =>
         return Sonar.Index'Val (First + 5);

      when 16#10# =>
         return Sonar.Index'Val (First + 6);
      when 16#11# =>
         return Sonar.Index'Val (First + 7);
      when 16#12# =>
         return Sonar.Index'Val (First + 8);
      when 16#13# =>
         return Sonar.Index'Val (First + 9);
      when 16#14# =>
         return Sonar.Index'Val (First + 10);

      when 16#20# =>
         return Sonar.Index'Val (First + 16);
      when 16#21# =>
         return Sonar.Index'Val (First + 15);
      when 16#22# =>
         return Sonar.Index'Val (First + 14);
      when 16#23# =>
         return Sonar.Index'Val (First + 13);
      when 16#24# =>
         return Sonar.Index'Val (First + 12);
      when 16#25# =>
         return Sonar.Index'Val (First + 11);
      when others =>
         Put ("Wrong ClusterIndex in packet!");
         return Sonar.Index'First;
      end case;
   end ClusterIndexToSonarIndex;

   protected body Reader is
      procedure RecordPacket (PacketNumber : Raw_8Bit; Data : ByteArray) is
         -- Data = 00 00 00 02
         --        <4 Byte TimeStamp>
         --        <1 Byte Cluster> <2 Byte Data>
         --        <1 Byte Cluster> <2 Byte Data>
         --        <1 Byte Cluster> <2 Byte Data>
         -- sometimes more clusters

         DataIndex            : Integer;
         ClusterIndex         : Raw_8Bit;
         SonarIndex           : Sonar.Index;
         RawData              : Raw_16Bit;
         DistanceFromSensor   : Float;
         SensorAngle          : Float;
         Xposition, Yposition : Float;
      begin
         DataIndex := Integer (Data'First + 8);
         while DataIndex <= Data'Last - 2 loop
            ClusterIndex := Data (DataIndex);
            SonarIndex   := ClusterIndexToSonarIndex (ClusterIndex);
            RawData      :=
               TwoBytes_To_Raw_16Bit (Data (DataIndex + 1 .. DataIndex + 2));
            if RawData = InvalidSonarData then
               NewReadings (SonarIndex).Valid := False;
            else
               NewReadings (SonarIndex).Valid    := True;
               DistanceFromSensor                := Float (RawData) / Scale;
               SensorAngle                       :=
                 Float (Orientations (SonarIndex).Angle);
               Xposition                         :=
                 Float (Orientations (SonarIndex).X) +
                 DistanceFromSensor * Cos (SensorAngle, 360.0);
               Yposition                         :=
                 Float (Orientations (SonarIndex).Y) +
                 DistanceFromSensor * Sin (SensorAngle, 360.0);
               NewReadings (SonarIndex).Distance :=
                 mm (Sqrt (Xposition * Xposition + Yposition * Yposition));
               NewReadings (SonarIndex).Angle    :=
                 Degrees (Arctan (Yposition, Xposition, 360.0));
            end if;

            NewValueFlag (SonarIndex) := True;
            DataIndex                 := DataIndex + 3;
         end loop;

         if NewValueFlag = AllValuesNew then
            LastStatus.Sonar     := NewReadings;
            LastStatus.TimeStamp := Ada.Real_Time.Clock;
            NewDataArrived       := True;
            FirstDataArrived     := True;
            NewValueFlag         := AllValuesOld;
         end if;
      end RecordPacket;

      entry GetCurrentStatus (SonarStatus : out Sonar.Status) when
           FirstDataArrived is
      begin
         SonarStatus := LastStatus;
      end GetCurrentStatus;

      entry WaitForNewData when NewDataArrived or NonBlockingActive is
      begin
         if WaitForNewData'Count = 0 then
            NewDataArrived := False;
         end if;
      end WaitForNewData;

      procedure NonBlocking is
      begin
         NonBlockingActive := True;
      end NonBlocking;

   end Reader;

   protected body Commander is

      procedure StartReading is
      begin
         Flex_Driver.WriteTask.SendPacket
           (ByteArray (SonarStartSendingPacket),
            SonarStartStopSending);
      end StartReading;

      procedure StopReading is
      begin
         Flex_Driver.WriteTask.SendPacket
           (ByteArray (SonarStopSendingPacket),
            SonarStartStopSending);
      end StopReading;

   end Commander;

end Sonar_Handler;