File : laser.adb

----------------------------------------------------------------------------
--
--                              -*- Mode: Ada -*-
--
-- Filename        : Laser.adb
-- Description     : All you need to read the laser range finder on the B14
-- Author          : Uwe R. Zimmer
-- Created On      : Wed Oct 15 16:04:34 1997
-- Last Modified By:
-- Last Modified On:
-- Update Count    :
-- Status          :
--
----------------------------------------------------------------------------

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

with Ada.Real_Time; use Ada.Real_Time;
with Interfaces.C;  use Interfaces.C;
with Laser_driver;  use Laser_driver;

----------------------------------------------------------------------------
--
-- Package Laser
--
----------------------------------------------------------------------------

package body Laser is

   ----------------------------------------------------------------------------
   --
   -- Variables global for package Laser
   --
   ----------------------------------------------------------------------------

   LaserInitialized : Boolean := False;

   NoOfClients  : Natural := 0;
   NoOfSuspends : Natural := 0;

   ----------------------------------------------------------------------------
   --
   -- Protected Monitor LaserInterface;
   --
   ----------------------------------------------------------------------------

   protected body LaserInterface is

      procedure InitLaser is

         LaserIndex : Positive range FirstLaserReading .. LastLaserReading :=
           FirstLaserReading;

      begin
         if not LaserInitialized then

            for LaserIndex in LaserReadingRange loop
               ActualLaserAngles (LaserIndex) :=
                 Degrees (Float (FirstLaserAngle) +
                          Float (LaserIndex - LaserReadingRange'First) *
                          (Float (LastLaserAngle - FirstLaserAngle) /
                           Float (LaserReadingRange'Last -
                                  LaserReadingRange'First)));
            end loop;

            LaserDriverInterface.InitLaserDriver;
            LaserMonitor.BlockTask;
            LaserInitialized := True;
         end if;
         NoOfClients := NoOfClients + 1;
      end InitLaser;

      entry ShutdownLaser when LaserInitialized is
      begin
         NoOfClients := NoOfClients - 1;
         if NoOfClients = 0 then
            LaserDriverInterface.ShutdownLaserDriver;
            LaserInitialized := False;
         end if;
      end ShutdownLaser;

      entry SuspendSensing when LaserInitialized is
      begin
         NoOfSuspends := NoOfSuspends + 1;
         if NoOfSuspends >= NoOfClients then
            LaserDriverInterface.SuspendLaserDriver;
         end if;
      end SuspendSensing;

      entry ResumeSensing when LaserInitialized is
      begin
         NoOfSuspends := NoOfSuspends - 1;
         if NoOfSuspends < NoOfClients then
            LaserDriverInterface.ResumeLaserDriver;
         end if;
      end ResumeSensing;

      entry GetCurrentLaserStatus (Readings : out LaserStatus) when
           LaserInitialized is
      begin
         LaserDriverInterface.GetMostRecentLaserScan (Readings);
      end GetCurrentLaserStatus;

      entry GetCurrentLaserTimeStamp (TimeStamp : out Time) when
           LaserInitialized is
      begin
         LaserDriverInterface.GetMostRecentLaserTimeStamp (TimeStamp);
      end GetCurrentLaserTimeStamp;

      entry WaitforNewLaserData when LaserInitialized is
      begin
         requeue LaserMonitor.BlockTask;
      end WaitforNewLaserData;

   end LaserInterface;

end Laser;