---------------------------------------------------------------------------
--
--                              -*- Mode: Ada -*-
-- Filename        : gyro_driver.adb
-- Description     : low level routines, including interrupt handler
--
----------------------------------------------------------------------------

----------------------------------------------------------------------------
--
-- Imports
--
----------------------------------------------------------------------------

with Ada.Real_Time;                     use Ada.Real_Time;
with Ada.Interrupts;                    use Ada.Interrupts;
with Ada.Task_Identification;           use Ada.Task_Identification;
with Ada.Exceptions;                    use Ada.Exceptions;
with Interfaces.C;
with System.OS_Interface;               use System.OS_Interface;
with Text_IO;                           use Text_IO;

with Metrics;                           use Metrics;
with Interrupt_Handler;                 use Interrupt_Handler;

----------------------------------------------------------------------------
--
-- Gyro_driver
--
----------------------------------------------------------------------------

package body Gyro_driver is

   package Int_Io is new Integer_Io (Integer); use Int_Io;
   package LongInt_Io is new Integer_Io (Long_Integer); use LongInt_Io;
   package Flo_Io is new Float_Io (Float); use Flo_Io;

----------------------------------------------------------------------------
--
-- Imports from os_connection
--
----------------------------------------------------------------------------

   function OpenSerialPort (PortName: Interfaces.C.char_array)
     return Interfaces.C.int;
   pragma Import (C, OpenSerialPort, "open_serial_port");

   procedure CloseSerialPort (FileD: Interfaces.C.int);
   pragma Import (C, CloseSerialPort, "close_serial_port");

   procedure SetupSerialPort (FileD: Interfaces.C.int;
                              PortSpeed: Interfaces.C.unsigned);
   pragma Import (C, SetupSerialPort, "setup_serial_port");

   function ReadSerialPort (FileD: Interfaces.C.int;
                            Buffer: Interfaces.C.char_array) return int;
   pragma Import (C, ReadSerialPort, "read_serial_port");

   procedure WriteSerialPort (FileD: Interfaces.C.int;
                              Buffer: Interfaces.C.char_array;
                              BytesToWrite: Interfaces.C.int);
   pragma Import (C, WriteSerialPort, "write_serial_port");

   procedure EnableSerialPortInterrupt (FileD: Interfaces.C.int);
   pragma Import
     (C, EnableSerialPortInterrupt, "enable_serial_port_interrupt");

----------------------------------------------------------------------------
--
-- Global types
--
----------------------------------------------------------------------------

   ScaledGyroDataPacketLength : constant Integer := 18;
   MaxTelegramLength          : constant Integer := ScaledGyroDataPacketLength;

   type Raw_8Bit     is mod 2**8;
   type Raw_16Bit    is mod 2**16;
   type ByteArray    is array (Integer range <>) of Raw_8Bit;
   type Telegram     is new ByteArray (1..MaxTelegramLength);
   type TelegramPart is (STX, DATA, CHKSUM);

----------------------------------------------------------------------------
--
-- Global constants
--
----------------------------------------------------------------------------

   SerialPortName : constant String  := "/dev/cur4";

   G_Range : constant AccelG        := 10.0;
   A_Range : constant Speed_angular := 150.0;

   Baudrate9600  : constant Interfaces.C.unsigned := 13;
   Baudrate19200 : constant Interfaces.C.unsigned := 14;
   Baudrate38400 : constant Interfaces.C.unsigned := 15;

   WatchDogTimeOut          : constant Duration := 1.0;   -- seconds
   ReadTaskWaitingSleepTime : constant Duration := 0.01; -- seconds

   NoOfStoredTelegrams          : constant Integer := 500;

   DoNotAppendNull              : constant Boolean := False;
   CharArrayIsNotNullTerminated : constant Boolean := False;

   Byte_Offset   : constant Raw_16Bit := 2**8;
   Max_Signed_16 : constant Raw_16Bit := 2**15;
   NullByteArray : constant ByteArray (0..1) := (0,1);

   NUL_Byte      : constant Raw_8Bit := 16#00#;
   STX_Byte      : constant Raw_8Bit := 16#FF#;

   AngRateX_Offset : constant Integer :=  2;
   AngRateY_Offset : constant Integer :=  4;
   AngRateZ_Offset : constant Integer :=  6;
   AccelX_Offset   : constant Integer :=  8;
   AccelY_Offset   : constant Integer := 10;
   AccelZ_Offset   : constant Integer := 12;
   Temp_Offset     : constant Integer := 14;
   Time_Offset     : constant Integer := 16;

   Reset_Cmd     : constant Raw_8Bit := Character'Pos ('R');
   Scale_Cmd     : constant Raw_8Bit := Character'Pos ('c');
   Continuous_Cmd: constant Raw_8Bit := Character'Pos ('C');
   Calibrate_Cmd : constant Raw_8Bit := Character'Pos ('z');

   Reset_Rsp     : constant Raw_8Bit := Character'Pos ('H');
   Scale_Rsp     : constant Raw_8Bit := Character'Pos ('C');
   Calibrate_Rsp : constant Raw_8Bit := Character'Pos ('Z');

----------------------------------------------------------------------------
--
-- Global variables
--
----------------------------------------------------------------------------

   InterruptPointer : InterruptRoutine;

   FileD : Interfaces.C.Int;

   GyroDriverInitialized  : Boolean := False;
   InterruptArrived       : Boolean := False;

   RecentTelegrams            : array (1..NoOfStoredTelegrams) of Telegram;
   RecentTelegramsComplete    : array (1..NoOfStoredTelegrams) of Boolean;
   RecentTelegramsTimeStamp   : array (1..NoOfStoredTelegrams) of Time;
   MostRecentCompleteTelegram : Integer range 1..NoOfStoredTelegrams;
   MostRecentCompleteGyroData : Integer range 1..NoOfStoredTelegrams;
   OneOrMoreTelegramsComplete : Boolean                              := False;
   OneOrMoreGyroDataComplete  : Boolean                              := False;
   CurrentTelegram            : Integer range 1..NoOfStoredTelegrams := 1;
   CurrentTelegramByte        : Integer range 1..MaxTelegramLength   := 1;
   CurrentTelegramPart        : TelegramPart                         := STX;
   CurrentTelegramLength      : Natural := 0;

----------------------------------------------------------------------------
--
-- Utility routines
--
----------------------------------------------------------------------------

   function ByteArrayToString
     (Data: in ByteArray; NoOfBytes: in Integer) return String is

      Index      : Integer := 0;
      DataString : String (1 .. NoOfBytes);

   begin

      for Index in 1..NoOfBytes loop
         DataString (Index) :=
           Character'Val (Character'Pos (Character'First)
                          + Data (Data'First+Index-1));

      end loop;

      return (DataString);
   end ByteArrayToString;


   function StringToByteArray (Data: String) return ByteArray is

      Index     : Integer := 0;
      DataArray : ByteArray (1..Data'Length);

   begin
      for Index in Data'Range loop
         DataArray (Index) := Character'Pos (Data (Index));
      end loop;
      return (DataArray);
   end StringToByteArray;


   function CheckSum_8
     (Data: in ByteArray; NoOfBytes: in Integer) return Raw_8Bit is

      CheckSum_8 : Raw_8Bit := 0;

   begin
      for Index in 2..NoOfBytes loop -- first byte (STX) ignored
         CheckSum_8 := Raw_8Bit
           ((Raw_16Bit (CheckSum_8)
             + Raw_16Bit (Data (Data'First+Index-1)))
            mod Byte_Offset);
      end loop;

      return (CheckSum_8);
   end CheckSum_8;


   function WordToSignedInt (Word : in Raw_16Bit) return Integer is

      FullMask : constant Raw_16Bit := 2#1111111111111111#;

   begin
      if Word < Max_Signed_16 then
         return (Integer (Word));
      else
         return (- Integer (Word xor FullMask) + 1);
      end if;
   end WordToSignedInt;


   procedure TelegramToGyroReading
     (GyroTelegram: in Telegram; GyroData: out GyroReading) is

      ScalingConstant : constant Float := 1.5;

   begin
      GyroData.AngularRateX := Speed_Angular
        (Float (WordToSignedInt
                ((Byte_Offset
                  * Raw_16Bit (GyroTelegram (AngRateX_Offset)))
                 + (Raw_16Bit (GyroTelegram (AngRateX_Offset + 1)))))
         * Float (A_Range) * ScalingConstant / Float (Max_Signed_16));

      GyroData.AngularRateY := Speed_Angular
        (Float (WordToSignedInt
                ((Byte_Offset
                  * Raw_16Bit (GyroTelegram (AngRateY_Offset)))
                 + (Raw_16Bit (GyroTelegram (AngRateY_Offset + 1)))))
         * Float (A_Range) * ScalingConstant / Float (Max_Signed_16));

      GyroData.AngularRateZ := Speed_Angular
        (Float (WordToSignedInt
                ((Byte_Offset
                  * Raw_16Bit (GyroTelegram (AngRateZ_Offset)))
                 + (Raw_16Bit (GyroTelegram (AngRateZ_Offset + 1)))))
         * Float (A_Range) * ScalingConstant / Float (Max_Signed_16));

      GyroData.AccelerationX := AccelG
        (Float (WordToSignedInt
                ((Byte_Offset
                  * Raw_16Bit (GyroTelegram (AccelX_Offset)))
                 + (Raw_16Bit (GyroTelegram (AccelX_Offset + 1)))))
         * Float (G_Range) * ScalingConstant / Float (Max_Signed_16));

      GyroData.AccelerationY := AccelG
        (Float (WordToSignedInt
                ((Byte_Offset
                  * Raw_16Bit (GyroTelegram (AccelY_Offset)))
                 + (Raw_16Bit (GyroTelegram (AccelY_Offset + 1)))))
         * Float (G_Range) * ScalingConstant / Float (Max_Signed_16));

      GyroData.AccelerationZ := AccelG
        (Float (WordToSignedInt
                ((Byte_Offset
                  * Raw_16Bit (GyroTelegram (AccelZ_Offset)))
                 + (Raw_16Bit (GyroTelegram (AccelZ_Offset + 1)))))
         * Float (G_Range) * ScalingConstant / Float (Max_Signed_16));

      GyroData.Temperature := TemperatureC
        (44.4 *
         ((Float ((Byte_Offset
                   * Raw_16Bit (GyroTelegram (Temp_Offset)))
                  + (Raw_16Bit (GyroTelegram (Temp_Offset + 1))))
           * 5.0 / 4096.0)
          - 1.375));

      GyroData.GyroTicks := GyroTicksType
        ((Byte_Offset
          * Raw_16Bit (GyroTelegram (Time_Offset)))
         + (Raw_16Bit (GyroTelegram (Time_Offset + 1))));

   end TelegramToGyroReading;


   procedure CopyMostRecentGyroData (GyroData: out GyroReading) is

   begin
      TelegramToGyroReading
        (RecentTelegrams (MostRecentCompleteGyroData), GyroData);
      GyroData.TimeStamp :=
        RecentTelegramsTimeStamp (MostRecentCompleteGyroData);
   end CopyMostRecentGyroData;


   procedure CopyAllGyroDataSince (TimeStamp    :  in Time;
                                   Readings     : out GyroReadingArray;
                                   NoOfReadings : out Natural) is

      TelegramIndex, Telegramcounter  : Integer := 1;

   begin
      NoOfReadings    := 0;
      TelegramCounter := 1;
      TelegramIndex   := MostRecentCompleteGyroData;
      while TelegramCounter <= NoOfStoredTelegrams
        and then NoOfReadings <= Readings'Last - Readings'First loop
         if RecentTelegramsComplete (TelegramIndex)
           and then RecentTelegrams (TelegramIndex) (1) = STX_Byte
           and then RecentTelegramsTimeStamp (TelegramIndex) > TimeStamp then
            NoOfReadings := NoOfReadings + 1;
            TelegramToGyroReading
              (RecentTelegrams (TelegramIndex),
              Readings (NoOfReadings + Readings'First - 1));
            Readings (NoOfReadings + Readings'First - 1).TimeStamp :=
              RecentTelegramsTimeStamp (TelegramIndex);
         end if;
         TelegramCounter := TelegramCounter + 1;
         if TelegramIndex = 1 then
            TelegramIndex := NoOfStoredTelegrams;
         else
            TelegramIndex := TelegramIndex - 1;
         end if;
      end loop;
   end CopyAllGyroDataSince;


   procedure Send_Gyro_Telegram
     (Cmd: in Raw_8Bit;
      Attr: in ByteArray := NullByteArray;
      NoOfAttrBytes: in Integer := 0) is

      Telegram   : ByteArray (1..1 + NoOfAttrBytes);
      Index      : Integer := 0;

   begin
      Telegram (1) := Cmd;
      if NoOfAttrBytes > 0 then
         for Index in 1.. NoOfAttrBytes loop
           Telegram (1+Index) := Attr (Attr'First+Index-1);
         end loop;
      end if;

      WriteSerialPort
        (FileD,
         Interfaces.C.To_C (ByteArrayToString (Telegram,
                                               1 + NoOfAttrBytes),
                            DoNotAppendNull),
         Interfaces.C.int (1 + NoOfAttrBytes));

   end Send_Gyro_Telegram;


   procedure SetupGyro is

      MaxAttr : constant Integer := 1;

      GyroReactionTime : constant Duration := 0.01;  -- seconds
      DelayPerSample   : constant Duration := 0.004; -- seconds
      Attr             : ByteArray (1..MaxAttr);
      CalibSamples     : constant Raw_8Bit := 100;

      LastCompleteTelegram : Integer range 1..NoOfStoredTelegrams := 1;


      function MostRecentResponse return Raw_8Bit is

      begin
         if OneOrMoreTelegramsComplete then
            return (RecentTelegrams (MostRecentCompleteTelegram) (1));
         else
            return (NUL_Byte);
         end if;
      end MostRecentResponse;


      function ResponseTelegramReceived
        (TelegramRsp: Raw_8Bit) return Boolean is

         Trials : Integer := 1;

      begin
         while Trials <= 10
           and then ((LastCompleteTelegram = MostRecentCompleteTelegram)
                     or else (MostRecentResponse /= TelegramRsp)) loop
            delay (GyroReactionTime);
            Trials := Trials + 1;
         end loop;
         LastCompleteTelegram := MostRecentCompleteTelegram;
         return (MostRecentResponse = TelegramRsp);
      end ResponseTelegramReceived;


   begin

      if not ResponseTelegramReceived (STX_Byte) then

         loop
            Put_Line ("Send reset ...");
            Send_Gyro_Telegram (Reset_Cmd);
            exit when ResponseTelegramReceived (Reset_Rsp);
         end loop;

         Attr (1) := CalibSamples;
         loop
            Put_Line ("Calibrating ...");
            Send_Gyro_Telegram (Calibrate_Cmd, Attr, 1);
            delay (Duration (Float
                             (CalibSamples) * 10.0 * Float (DelayPerSample)));
            exit when ResponseTelegramReceived (Calibrate_Rsp);
         end loop;

         loop
            Put_Line ("Send Scale ...");
            Send_Gyro_Telegram (Scale_Cmd);
            exit when ResponseTelegramReceived (Scale_Rsp);
         end loop;


         Put_Line ("Send Continuous ...");

         Send_Gyro_Telegram (Continuous_Cmd);

      end if;

   end SetupGyro;


   procedure ProcessGyroData (NewData: in ByteArray; NewNoOfBytes: Integer) is

      NewDataIndex      : Integer := 1;
      PrintIndex        : Integer := 1;
      ExpectedByteFound : Boolean;
      TelegramComplete  : Boolean := False;

   begin
      while NewDataIndex <= NewNoOfBytes loop
         ExpectedByteFound := False;

--         Put ("Byte processed: ");
--         Put (Integer (NewData (NewDataIndex)));
--         Put (Character'Val (NewData (NewDataIndex)));
--         New_Line;

         case CurrentTelegramPart is
            when STX => begin
               while (NewDataIndex <= NewNoOfBytes)
                 and then (NewData (NewDataIndex) /= STX_Byte)
                 and then (NewData (NewDataIndex) /= Reset_Rsp)
                 and then (NewData (NewDataIndex) /= Scale_Rsp)
                 and then (NewData (NewDataIndex) /= Calibrate_Rsp)
               loop
                  NewDataIndex := NewDataIndex + 1;
               end loop;
               if NewDataIndex <= NewNoOfBytes then
                  RecentTelegramsTimeStamp (CurrentTelegram) := Clock;
                  ExpectedByteFound := True;
                  if NewData (NewDataIndex) = STX_Byte then
                     CurrentTelegramPart :=
                       TelegramPart'Succ(CurrentTelegramPart);
                  else
                     TelegramComplete := True;
                  end if;
               end if;
            end;
            when DATA => begin
               ExpectedByteFound := True;
               if CurrentTelegramByte = ScaledGyroDataPacketLength - 1 then
                  CurrentTelegramPart := CHKSUM;
               end if;
            end;
            when CHKSUM => begin
               ExpectedByteFound := True;
               TelegramComplete := True;
            end;
         end case;
         if ExpectedByteFound then
            RecentTelegrams (CurrentTelegram) (CurrentTelegramByte)
              := NewData (NewDataIndex);
            NewDataIndex := NewDataIndex + 1;
            if TelegramComplete then
               if CurrentTelegramPart = STX or else
                 CheckSum_8 (ByteArray (RecentTelegrams (CurrentTelegram)),
                         ScaledGyroDataPacketLength - 1)
                 =
                 (RecentTelegrams (CurrentTelegram)
                            (ScaledGyroDataPacketLength))
               then
                  RecentTelegramsComplete (CurrentTelegram) := True;
                  MostRecentCompleteTelegram := CurrentTelegram;
                  OneOrMoreTelegramsComplete := True;

                  if CurrentTelegramPart = CHKSUM then
                     OneOrMoreGyroDataComplete := True;
                     MostRecentCompleteGyroData := CurrentTelegram;
                     GyroMonitor.FreeTasks;

--                     Put (".");
--                      Put ("Telegram: ");
--                      for PrintIndex in 1..CurrentTelegramByte - 1 loop
--                         Put (Integer (RecentTelegrams (CurrentTelegram)
--                                       (PrintIndex)), 4);
--                      end loop;
--                      New_Line;

                  end if;

--                   Put ("Telegram: ");
--                   for PrintIndex in 1..CurrentTelegramByte - 1 loop
--                      Put (Integer (RecentTelegrams (CurrentTelegram)
--                                    (PrintIndex)), 4);
--                   end loop;
--                   New_Line;

                  if CurrentTelegram < NoOfStoredTelegrams then
                     CurrentTelegram := CurrentTelegram + 1;
                  else
                     CurrentTelegram := 1;
                  end if;
                  RecentTelegramsComplete (CurrentTelegram) := False;

               elsif CurrentTelegramPart /= STX then -- wrong CHKSUM

                  Put ("  --->>>> ignoring telegram ");
                  for PrintIndex in 1..5 loop
                     Put (Integer (RecentTelegrams (CurrentTelegram)
                                   (PrintIndex)), 4);
                  end loop;
                  Put (" - (");
                  Put (Integer (CurrentTelegramByte), 4);
                  Put_Line ("bytes), due to wrong checksum");

               end if;

               CurrentTelegramByte := 1;
               CurrentTelegramPart := STX;
               TelegramComplete := False;
            else
               CurrentTelegramByte := CurrentTelegramByte + 1;
            end if;
         else
            CurrentTelegramByte := 1;
            CurrentTelegramPart := STX;
         end if;
      end loop;
   end ProcessGyroData;

----------------------------------------------------------------------------
--
-- GyroReadMonitor
--
----------------------------------------------------------------------------

   protected GyroReadMonitor is

      entry BlockTask;
      entry BlockedTasksQueue;
      procedure IO_Signal_Handler;
--      pragma Attach_Handler (IO_Signal_Handler, SIGIO);
      procedure TriggerRead;
      procedure NonBlocking;

   private

      NewDataArrived     : Boolean := False;
      NonBlockingActive  : Boolean := False;
      TasksWaitingForData: Natural := 0;

   end GyroReadMonitor;

   protected body GyroReadMonitor is

      entry BlockTask when True is

      begin

         --         Put_Line ("Blocking task ");

         TasksWaitingForData := TasksWaitingForData + 1;
         requeue BlockedTasksQueue;
      end BlockTask;


      entry BlockedTasksQueue when NewDataArrived or NonBlockingActive is

      begin

--         Put_Line ("Freeing task ");

         TasksWaitingForData := TasksWaitingForData - 1;
         if TasksWaitingForData = 0 then
            NewDataArrived := False;
         end if;
      end BlockedTasksQueue;


      procedure IO_Signal_Handler is

      begin

--         Put_Line ("Gyro_SIGIO !!!");

         NewDataArrived := True;
         InterruptArrived := True;
      end IO_Signal_Handler;


      procedure TriggerRead is

      begin

         Put_Line ("Gyro Serial timeout !!!");

         NewDataArrived := True;
      end TriggerRead;


      procedure NonBlocking is

      begin
         NonBlockingActive := True;
      end NonBlocking;

   end GyroReadMonitor;

----------------------------------------------------------------------------
--
-- SerialWatchDog
--
----------------------------------------------------------------------------

   task SerialWatchDog is

      entry StartWatching;
      entry TerminateWatching;

   end SerialWatchDog;


   task body SerialWatchDog is

      TaskActive : Boolean := True;

   begin

      accept StartWatching;

      while TaskActive loop

         select
            accept TerminateWatching do
               TaskActive := False;
            end TerminateWatching;
         else
            InterruptArrived := False;
            delay WatchDogTimeOut;
            if not InterruptArrived then
               GyroReadMonitor.TriggerRead;
            end if;
         end select;
      end loop;

      Put_Line (" -> Gyro_driver: SerialWatchDog terminated");

   exception
      when E: others =>
         Put_Line (Current_Error,
                   "Task "
                   & Image (Current_Task)
                   & " reports: "
                   & Exception_Name (E));

   end SerialWatchDog;

----------------------------------------------------------------------------
--
-- GyroReadTask
--
----------------------------------------------------------------------------

   task ReadGyro is

      entry StartReading;
      entry StopReading;
      entry TerminateReading;

   end ReadGyro;


   task body ReadGyro is

      MaxBytesPerRead    : constant Integer := 255;
      BytesRead          : Integer;

      DoReading          : Boolean := True;
      TaskActive         : Boolean := True;
      ReadCharArray      : Interfaces.C.char_array
        (Interfaces.C.size_t (1) .. Interfaces.C.size_t (MaxBytesPerRead));
      ReadBuffer         : ByteArray (1..MaxBytesPerRead);

   begin
      accept StartReading;

      while TaskActive loop

         select
            accept StartReading do
               DoReading := True;
            end StartReading;
         or
            accept StopReading do
               DoReading := False;
            end StopReading;
         or
            accept TerminateReading do
               TaskActive := False;
            end TerminateReading;
         else
               if DoReading then

                  BytesRead :=
                    Integer (ReadSerialPort (FileD, ReadCharArray));

                  while BytesRead > 0 loop

                     ProcessGyroData (StringToByteArray
                                      (Interfaces.C.To_Ada
                                       (ReadCharArray,
                                        CharArrayIsNotNullTerminated)),
                                      BytesRead);
                     BytesRead :=
                       Integer (ReadSerialPort (FileD, ReadCharArray));
                  end loop;
                  GyroReadMonitor.BlockTask;
               else
                  delay (ReadTaskWaitingSleepTime);
               end if;
         end select;
      end loop;

      Put_Line (" -> Gyro_driver: ReadGyro terminated");

   exception
      when E: others =>
         Put_Line (Current_Error,
                   "Task "
                   & Image (Current_Task)
                   & " reports: "
                   & Exception_Name (E));

   end ReadGyro;

----------------------------------------------------------------------------
--
-- GyroDriverInterface
--
----------------------------------------------------------------------------

      protected body GyroDriverInterface is

      entry InitGyroDriver when not GyroDriverInitialized is

         Index : Integer := 1;

      begin

         InterruptInterface.InitIntHandler;

         FileD := OpenSerialPort (Interfaces.C.To_C (SerialPortName));
         SetupSerialPort (FileD, Baudrate38400);

         InterruptPointer := GyroReadMonitor.IO_Signal_Handler'Access;
         InterruptInterface.AddIntRoutine (SignalIO, InterruptPointer);

         EnableSerialPortInterrupt (FileD);

         ReadGyro.StartReading;
         SerialWatchDog.StartWatching;

         for Index in 1..NoOfStoredTelegrams loop
            RecentTelegramsComplete (Index) := False;
         end loop;

         SetupGyro;

         GyroDriverInitialized := True;
      end;


      entry ShutdownGyroDriver when GyroDriverInitialized is

      begin

         InterruptInterface.ShutdownIntHandler;

         GyroReadMonitor.NonBlocking;
         ReadGyro.TerminateReading;
         SerialWatchDog.TerminateWatching;

         CloseSerialPort (FileD);

         GyroDriverInitialized := False;
      end;


      entry SuspendGyroDriver when GyroDriverInitialized is

      begin
         ReadGyro.StopReading;
      end SuspendGyroDriver;


      entry ResumeGyroDriver when GyroDriverInitialized is

      begin
         ReadGyro.StartReading;
      end ResumeGyroDriver;


      entry GetMostRecentGyroData (GyroData: out GyroReading)
      when GyroDriverInitialized is

      begin
         if OneOrMoreGyroDataComplete then
            CopyMostRecentGyroData (GyroData);
         end if;
      end GetMostRecentGyroData;


      entry GetMostRecentGyroTimeStamp (TimeStamp: out Time)
      when GyroDriverInitialized is

      begin
         if OneOrMoreGyroDataComplete then
            TimeStamp :=
              RecentTelegramsTimeStamp (MostRecentCompleteGyroData);
         end if;
      end GetMostRecentGyroTimeStamp;


      entry GetAllGyroSince             (TimeStamp    :  in Time;
                                         Readings     : out GyroReadingArray;
                                         NoOfReadings : out Natural)
      when GyroDriverInitialized is

      begin
         if OneOrMoreGyroDataComplete then
            CopyAllGyroDataSince (TimeStamp, Readings, NoOfReadings);
         end if;
      end GetAllGyroSince;


   end GyroDriverInterface;

----------------------------------------------------------------------------
--
-- Syncronization Monitor
--
----------------------------------------------------------------------------

   protected body GyroMonitor is

      entry BlockTask when True is

      begin
         TasksWaitingForData := TasksWaitingForData + 1;
         requeue BlockedTasksQueue;
      end BlockTask;


      entry BlockedTasksQueue when NewGyroDataArrived is

      begin
         TasksWaitingForData := TasksWaitingForData - 1;
         if TasksWaitingForData = 0 then
            NewGyroDataArrived := False;
         end if;
      end BlockedTasksQueue;


      procedure FreeTasks is

      begin
         NewGyroDataArrived := True;
      end FreeTasks;

   end GyroMonitor;

end Gyro_driver;