File : laser_driver.adb
---------------------------------------------------------------------------
--
-- -*- Mode: Ada -*-
-- Filename : laser_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;
----------------------------------------------------------------------------
--
-- Laser_driver
--
----------------------------------------------------------------------------
package body Laser_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
--
----------------------------------------------------------------------------
MaxTelegramLength : constant Integer := 1024;
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_ACK_NAK,
ADR,
LENL,
LENH,
CMD,
ATTR,
CRCL,
CRCH);
----------------------------------------------------------------------------
--
-- Global constants
--
----------------------------------------------------------------------------
SerialPortName : constant String := "/dev/cur2";
Resetting_Laser : constant Boolean := False;
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.1; -- seconds
NoOfStoredTelegrams : constant Integer := 50;
TelegramFrameLength : constant Integer := 6;
TelegramHeaderLength : constant Integer := 4;
TelegramCommandLength : constant Integer := 1;
DoNotAppendNull : constant Boolean := False;
CharArrayIsNotNullTerminated : constant Boolean := False;
Byte_Offset : constant Raw_16Bit := 2 ** 8;
NullByteArray : constant ByteArray (0 .. 1) := (0, 1);
STX_Offset : constant Integer := 0;
ADR_Offset : constant Integer := 1;
LENL_Offset : constant Integer := 2;
LENH_Offset : constant Integer := 3;
CMD_Offset : constant Integer := 4;
CRCL_Offset : constant Integer := 5;
CRCH_Offset : constant Integer := 6;
NUL_Byte : constant Raw_8Bit := 16#00#;
STX_Byte : constant Raw_8Bit := 16#02#;
ACK_Byte : constant Raw_8Bit := 16#06#;
DLE_Byte : constant Raw_8Bit := 16#10#;
NAK_Byte : constant Raw_8Bit := 16#15#;
ADR_Byte : constant Raw_8Bit := 16#80#;
LMS_ADR_Byte : constant Raw_8Bit := 16#00#;
INIT_TGM_Cmd : constant Raw_8Bit := 16#10#;
BM_Cmd : constant Raw_8Bit := 16#20#;
MWANF_Cmd : constant Raw_8Bit := 16#30#;
SSANF_Cmd : constant Raw_8Bit := 16#31#;
ERRANF_Cmd : constant Raw_8Bit := 16#32#;
MMWANF_Cmd : constant Raw_8Bit := 16#36#;
MWPANF_Cmd : constant Raw_8Bit := 16#37#;
TYPANF_Cmd : constant Raw_8Bit := 16#3A#;
MMWPANF_Cmd : constant Raw_8Bit := 16#3F#;
BRPERMDEF_Cmd : constant Raw_8Bit := 16#66#;
LASER_Cmd : constant Raw_8Bit := 16#68#;
PWON_Tgm : constant Raw_8Bit := 16#90#;
INIT_ACK_Tgm : constant Raw_8Bit := 16#91#;
NACK_Tgm : constant Raw_8Bit := 16#92#;
BMACK_Tgm : constant Raw_8Bit := 16#A0#;
MW_Tgm : constant Raw_8Bit := 16#B0#;
ERR_Tgm : constant Raw_8Bit := 16#B2#;
BM_Cmd_Installation : constant Raw_8Bit := 16#00#;
BM_Cmd_Calibration : constant Raw_8Bit := 16#01#;
BM_Cmd_ContMeasure : constant Raw_8Bit := 16#24#;
BM_Cmd_38400 : constant Raw_8Bit := 16#40#;
BM_Cmd_9600 : constant Raw_8Bit := 16#42#;
Scan_Tgm_LENL : constant Raw_8Bit := 16#D6#;
Scan_Tgm_LENH : constant Raw_8Bit := 16#02#;
----------------------------------------------------------------------------
--
-- Global variables
--
----------------------------------------------------------------------------
FileD : Interfaces.C.int;
LaserDriverInitialized : Boolean := False;
InterruptArrived : Boolean := False;
RecentTelegrams : array (1 .. NoOfStoredTelegrams) of Telegram;
RecentTelegramsComplete : array (1 .. NoOfStoredTelegrams) of Boolean;
RecentTelegramsTimeStamp : array (1 .. NoOfStoredTelegrams) of Time;
RecentTelegramsLength : array (1 .. NoOfStoredTelegrams) of Integer;
MostRecentCompleteTelegram : Integer range 1 .. NoOfStoredTelegrams;
MostRecentCompleteScan : Integer range 1 .. NoOfStoredTelegrams;
OneOrMoreTelegramsComplete : Boolean :=
False;
OneOrMoreScansComplete : Boolean :=
False;
CurrentTelegram : Integer range 1 .. NoOfStoredTelegrams := 1;
CurrentTelegramByte : Integer range 1 .. MaxTelegramLength := 1;
CurrentTelegramPart : TelegramPart :=
STX_ACK_NAK;
CurrentTelegramLength : Natural := 0;
ParseForTelegramsOnly : Boolean := False;
ParseForScanTelegramOnly : Boolean := False;
----------------------------------------------------------------------------
--
-- Utility routines
--
----------------------------------------------------------------------------
function ByteArrayToString
(Data : in ByteArray;
NoOfBytes : in Integer)
return String
is
Index : Integer := 0;
DataString : String (1 .. NoOfBytes);
begin
-- Put ("Command: ");
for Index in 1 .. NoOfBytes loop
DataString (Index) :=
Character'Val
(Character'Pos (Character'First) +
Data (Data'First + Index - 1));
-- Put (Integer (Data (Data'First+Index-1)), 4);
end loop;
-- New_Line;
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 CRC_16
(Data : in ByteArray;
NoOfBytes : in Integer)
return Raw_16Bit
is
Generator_Polynom : constant Raw_16Bit := 2 ** 15 + 2 ** 2 + 2 ** 0;
Highest_Bit : constant Raw_16Bit := 2 ** 15;
Byte_Offset : constant Raw_16Bit := 2 ** 8;
CRC_16 : Raw_16Bit := 0;
Lower, Higher : Raw_8Bit := 0;
Index : Integer := 0;
begin
for Index in 1 .. NoOfBytes loop
Higher := Lower;
Lower := Data (Data'First + Index - 1);
if CRC_16 >= Highest_Bit then
CRC_16 := (2 * (CRC_16 - Highest_Bit)) xor Generator_Polynom;
else
CRC_16 := 2 * CRC_16;
end if;
CRC_16 := CRC_16 xor
(Raw_16Bit (Lower) + Byte_Offset * Raw_16Bit (Higher));
end loop;
return (CRC_16);
end CRC_16;
procedure TelegramToLaserScan
(ScanTelegram : in Telegram;
LaserScan : out LaserStatus)
is
LaserIndex : Positive range FirstLaserReading .. LastLaserReading :=
FirstLaserReading;
ByteIndex : Integer;
ValueMask : constant Raw_16Bit := 2#1111111111111000#;
DazzleBit : constant Raw_16Bit := 2#0000000000000100#;
TelegramElement : Raw_16Bit;
begin
ByteIndex := 4 + CMD_Offset;
for LaserIndex in LaserReadingRange loop
TelegramElement := Raw_16Bit (ScanTelegram (ByteIndex)) +
(Byte_Offset *
Raw_16Bit (ScanTelegram (ByteIndex + 1)));
LaserScan.Readings (LaserIndex).Distance :=
mm (TelegramElement and ValueMask);
LaserScan.Readings (LaserIndex).Dazzled :=
(TelegramElement and DazzleBit) = 1;
ByteIndex := ByteIndex + 2;
end loop;
end TelegramToLaserScan;
procedure CopyMostRecentLaserScan (LaserScan : out LaserStatus) is
begin
TelegramToLaserScan
(RecentTelegrams (MostRecentCompleteScan),
LaserScan);
LaserScan.TimeStamp :=
RecentTelegramsTimeStamp (MostRecentCompleteScan);
end CopyMostRecentLaserScan;
procedure CopyAllAvailableLaserScans (LaserScans : out LaserStatusArray) is
NoOfLaserScans : Natural := 0;
TelegramIndex : Integer := 0;
ScanIndex : Natural := 1;
begin
for TelegramIndex in 1 .. NoOfStoredTelegrams loop
if RecentTelegramsComplete (TelegramIndex)
and then RecentTelegrams (TelegramIndex) (5) = MW_Tgm
then
NoOfLaserScans := NoOfLaserScans + 1;
end if;
end loop;
declare
LocalLaserScans : LaserStatusArray (1 .. NoOfLaserScans);
begin
for TelegramIndex in 1 .. NoOfStoredTelegrams loop
if RecentTelegramsComplete (TelegramIndex)
and then RecentTelegrams (TelegramIndex) (5) = MW_Tgm
then
TelegramToLaserScan
(RecentTelegrams (TelegramIndex),
LocalLaserScans (ScanIndex));
ScanIndex := ScanIndex + 1;
end if;
end loop;
LaserScans := LocalLaserScans;
end;
end CopyAllAvailableLaserScans;
procedure Send_Laser_Telegram
(Cmd : in Raw_8Bit;
Attr : in ByteArray := NullByteArray;
NoOfAttrBytes : in Integer := 0)
is
Telegram : ByteArray (
1 .. 1 + TelegramFrameLength + NoOfAttrBytes);
TelegramLength, Index : Integer := 0;
CRC : Raw_16Bit;
begin
Telegram (1 + STX_Offset) := STX_Byte;
Telegram (1 + ADR_Offset) := LMS_ADR_Byte;
Telegram (1 + LENL_Offset) := Raw_8Bit ((1 + NoOfAttrBytes) mod 256);
Telegram (1 + LENH_Offset) := Raw_8Bit ((1 + NoOfAttrBytes) / 256);
Telegram (1 + CMD_Offset) := Cmd;
if NoOfAttrBytes > 0 then
for Index in 1 .. NoOfAttrBytes loop
Telegram (1 + CMD_Offset + Index) :=
Attr (Attr'First + Index - 1);
end loop;
end if;
CRC :=
CRC_16 (Telegram, 1 + CMD_Offset + NoOfAttrBytes);
Telegram (1 + CRCL_Offset + NoOfAttrBytes) := Raw_8Bit (CRC mod 256);
Telegram (1 + CRCH_Offset + NoOfAttrBytes) := Raw_8Bit (CRC / 256);
WriteSerialPort
(FileD,
Interfaces.C.To_C
(ByteArrayToString
(Telegram,
1 + TelegramFrameLength + NoOfAttrBytes),
DoNotAppendNull),
Interfaces.C.int (1 + TelegramFrameLength + NoOfAttrBytes));
end Send_Laser_Telegram;
procedure SetupLaser is
MaxAttr : constant Integer := 24;
LaserReactionTime : constant Duration := 0.01; -- seconds
Attr : ByteArray (1 .. MaxAttr);
LastCompleteTelegram : Integer range 1 .. NoOfStoredTelegrams := 1;
TimeStamp : Time;
function MostRecentResponse return Raw_8Bit is
TelegramStartByte : Raw_8Bit;
begin
if OneOrMoreTelegramsComplete then
TelegramStartByte :=
RecentTelegrams (MostRecentCompleteTelegram) (1);
if TelegramStartByte = ACK_Byte
or else TelegramStartByte = NAK_Byte
then
return (TelegramStartByte);
elsif TelegramStartByte = STX_Byte then
return (RecentTelegrams (MostRecentCompleteTelegram) (5));
else
return (NUL_Byte);
end if;
else
return (NUL_Byte);
end if;
end MostRecentResponse;
function ResponseTelegramReceived
(TelegramCmd : Raw_8Bit)
return Boolean
is
Trials : Integer := 1;
begin
while Trials <= 10
and then ((LastCompleteTelegram = MostRecentCompleteTelegram)
or else MostRecentResponse /= TelegramCmd)
loop
delay (LaserReactionTime);
Trials := Trials + 1;
end loop;
LastCompleteTelegram := MostRecentCompleteTelegram;
return (MostRecentResponse = TelegramCmd);
end ResponseTelegramReceived;
begin
-- Put_Line ("setting up laser start");
if Resetting_Laser then
PowerUp_Loop : loop
Reset_Loop : loop
SetupSerialPort (FileD, Baudrate9600);
-- Put_Line ("send INIT 9600 ...");
Send_Laser_Telegram (INIT_TGM_Cmd);
exit Reset_Loop when ResponseTelegramReceived (INIT_ACK_Tgm);
SetupSerialPort (FileD, Baudrate38400);
-- Put_Line ("send INIT 38400 ...");
Send_Laser_Telegram (INIT_TGM_Cmd);
exit Reset_Loop when ResponseTelegramReceived (INIT_ACK_Tgm);
end loop Reset_Loop;
SetupSerialPort (FileD, Baudrate9600);
TimeStamp := Clock;
while (Clock - TimeStamp) < To_Time_Span (Duration (10.0))
and then not ResponseTelegramReceived (PWON_Tgm)
loop
null;
end loop;
exit PowerUp_Loop when (Clock - TimeStamp) <
To_Time_Span (Duration (10.0));
end loop PowerUp_Loop;
end if;
Attr (1) := BM_Cmd_Installation;
Attr (2) := Character'Pos ('S');
Attr (3) := Character'Pos ('I');
Attr (4) := Character'Pos ('C');
Attr (5) := Character'Pos ('K');
Attr (6) := Character'Pos ('_');
Attr (7) := Character'Pos ('L');
Attr (8) := Character'Pos ('M');
Attr (9) := Character'Pos ('S');
Init_Loop : loop
SetupSerialPort (FileD, Baudrate9600);
Send_Laser_Telegram (BM_Cmd, Attr, 9);
if ResponseTelegramReceived (BMACK_Tgm) then
Attr (1) := BM_Cmd_38400;
loop
Send_Laser_Telegram (BM_Cmd, Attr, 1);
exit when ResponseTelegramReceived (BMACK_Tgm);
end loop;
SetupSerialPort (FileD, Baudrate38400);
exit Init_Loop;
end if;
SetupSerialPort (FileD, Baudrate38400);
Send_Laser_Telegram (BM_Cmd, Attr, 9);
exit Init_Loop when ResponseTelegramReceived (BMACK_Tgm);
end loop Init_Loop;
ParseForTelegramsOnly := True;
ParseForScanTelegramOnly := True;
Attr (1) := BM_Cmd_ContMeasure;
loop
Send_Laser_Telegram (BM_Cmd, Attr, 1);
exit when ResponseTelegramReceived (BMACK_Tgm);
end loop;
end SetupLaser;
procedure ProcessLaserData
(NewData : in ByteArray;
NewNoOfBytes : Integer)
is
NewDataIndex : Integer := 1;
PrintIndex : Integer := 1;
ExpectedByteFound : Boolean;
TelegramComplete : Boolean := False;
begin
while NewDataIndex <= NewNoOfBytes loop
ExpectedByteFound := False;
case CurrentTelegramPart is
when STX_ACK_NAK =>
begin
while (NewDataIndex <= NewNoOfBytes)
and then (NewData (NewDataIndex) /= STX_Byte)
and then (ParseForTelegramsOnly
or else ((NewData (NewDataIndex) /= ACK_Byte)
and then (NewData (NewDataIndex) /=
NAK_Byte)))
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 ADR =>
begin
if NewData (NewDataIndex) = ADR_Byte then
ExpectedByteFound := True;
CurrentTelegramPart :=
TelegramPart'Succ (CurrentTelegramPart);
end if;
end;
when LENL =>
begin
if not ParseForScanTelegramOnly
or else NewData (NewDataIndex) = Scan_Tgm_LENL
then
ExpectedByteFound := True;
CurrentTelegramPart :=
TelegramPart'Succ (CurrentTelegramPart);
end if;
end;
when LENH =>
begin
CurrentTelegramLength :=
Natural (RecentTelegrams (CurrentTelegram) (
CurrentTelegramByte -
1)) +
(Natural (Byte_Offset) *
Natural (NewData (NewDataIndex)));
if (CurrentTelegramLength <=
MaxTelegramLength - TelegramFrameLength)
and then (not ParseForScanTelegramOnly
or else NewData (NewDataIndex) = Scan_Tgm_LENH)
then
ExpectedByteFound := True;
CurrentTelegramPart :=
TelegramPart'Succ (CurrentTelegramPart);
else
CurrentTelegramLength := 0;
end if;
end;
when CMD =>
begin
ExpectedByteFound := True;
if CurrentTelegramLength > TelegramCommandLength then
CurrentTelegramPart := ATTR;
else
CurrentTelegramPart := CRCL;
end if;
end;
when ATTR =>
begin
ExpectedByteFound := True;
if CurrentTelegramByte =
CurrentTelegramLength + TelegramHeaderLength
then
CurrentTelegramPart := CRCL;
end if;
end;
when CRCL =>
begin
ExpectedByteFound := True;
CurrentTelegramPart :=
TelegramPart'Succ (CurrentTelegramPart);
end;
when CRCH =>
begin
ExpectedByteFound := True;
TelegramComplete := True;
end;
end case;
if ExpectedByteFound then
RecentTelegrams (CurrentTelegram) (CurrentTelegramByte) :=
NewData (NewDataIndex);
CurrentTelegramByte :=
CurrentTelegramByte + 1;
NewDataIndex :=
NewDataIndex + 1;
if TelegramComplete then
if CurrentTelegramPart = STX_ACK_NAK
or else CRC_16
(ByteArray (RecentTelegrams (CurrentTelegram)),
CurrentTelegramLength + TelegramHeaderLength) =
Raw_16Bit (RecentTelegrams (CurrentTelegram) (
CurrentTelegramLength + TelegramHeaderLength +
1)) +
(Byte_Offset *
Raw_16Bit (RecentTelegrams (CurrentTelegram) (
CurrentTelegramLength + TelegramHeaderLength +
2)))
then
RecentTelegramsComplete (CurrentTelegram) := True;
RecentTelegramsLength (CurrentTelegram) :=
CurrentTelegramByte - 1;
MostRecentCompleteTelegram :=
CurrentTelegram;
OneOrMoreTelegramsComplete := True;
if RecentTelegrams (MostRecentCompleteTelegram) (5) =
MW_Tgm
then
OneOrMoreScansComplete := True;
MostRecentCompleteScan := CurrentTelegram;
LaserMonitor.FreeTasks;
-- Put (".");
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_ACK_NAK then -- wrong CRC
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 CRC");
end if;
CurrentTelegramByte := 1;
CurrentTelegramPart := STX_ACK_NAK;
TelegramComplete := False;
end if;
else
-- Put (" --->>>> ignoring ");
-- Put (Integer (CurrentTelegramByte), 4);
-- Put_Line ("bytes, due to misexpectation");
CurrentTelegramByte := 1;
CurrentTelegramPart := STX_ACK_NAK;
end if;
end loop;
end ProcessLaserData;
----------------------------------------------------------------------------
--
-- LaserReadMonitor
--
----------------------------------------------------------------------------
protected LaserReadMonitor 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 LaserReadMonitor;
protected body LaserReadMonitor is
entry BlockTask when True is
begin
TasksWaitingForData := TasksWaitingForData + 1;
requeue BlockedTasksQueue;
end BlockTask;
entry BlockedTasksQueue when NewDataArrived or NonBlockingActive is
begin
TasksWaitingForData := TasksWaitingForData - 1;
if TasksWaitingForData = 0 then
NewDataArrived := False;
end if;
end BlockedTasksQueue;
procedure IO_Signal_Handler is
begin
-- Put_Line ("Laser_SIGIO !!!");
NewDataArrived := True;
InterruptArrived := True;
end IO_Signal_Handler;
procedure TriggerRead is
begin
Put_Line ("Laser Serial timeout !!!");
NewDataArrived := True;
end TriggerRead;
procedure NonBlocking is
begin
NonBlockingActive := True;
end NonBlocking;
end LaserReadMonitor;
----------------------------------------------------------------------------
--
-- 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
LaserReadMonitor.TriggerRead;
end if;
end select;
end loop;
Put_Line (" -> Laser_driver: SerialWatchDog terminated");
exception
when E : others =>
Put_Line
(Current_Error,
"Task " &
Image (Current_Task) &
" reports: " &
Exception_Name (E));
end SerialWatchDog;
----------------------------------------------------------------------------
--
-- LaserReadTask
--
----------------------------------------------------------------------------
task ReadLaser is
entry StartReading;
entry StopReading;
entry TerminateReading;
end ReadLaser;
task body ReadLaser 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
ProcessLaserData
(StringToByteArray
(Interfaces.C.To_Ada
(ReadCharArray,
CharArrayIsNotNullTerminated)),
BytesRead);
BytesRead :=
Integer (ReadSerialPort (FileD, ReadCharArray));
end loop;
LaserReadMonitor.BlockTask;
else
delay (ReadTaskWaitingSleepTime);
end if;
end select;
end loop;
Put_Line (" -> ReadLaser terminated");
exception
when E : others =>
Put_Line
(Current_Error,
"Task " &
Image (Current_Task) &
" reports: " &
Exception_Name (E));
end ReadLaser;
----------------------------------------------------------------------------
--
-- LaserDriverInterface
--
----------------------------------------------------------------------------
protected body LaserDriverInterface is
entry InitLaserDriver when not LaserDriverInitialized is
Index : Integer := 1;
begin
InterruptInterface.InitIntHandler;
InterruptInterface.AddIntRoutine
(SignalIO,
LaserReadMonitor.IO_Signal_Handler'Access);
FileD := OpenSerialPort (Interfaces.C.To_C (SerialPortName));
SetupSerialPort (FileD, Baudrate9600);
EnableSerialPortInterrupt (FileD);
ReadLaser.StartReading;
SerialWatchDog.StartWatching;
for Index in 1 .. NoOfStoredTelegrams loop
RecentTelegramsComplete (Index) := False;
end loop;
SetupLaser;
LaserDriverInitialized := True;
end InitLaserDriver;
entry ShutdownLaserDriver when LaserDriverInitialized is
begin
InterruptInterface.ShutdownIntHandler;
LaserReadMonitor.NonBlocking;
ReadLaser.TerminateReading;
SerialWatchDog.TerminateWatching;
CloseSerialPort (FileD);
LaserDriverInitialized := False;
end ShutdownLaserDriver;
entry SuspendLaserDriver when LaserDriverInitialized is
begin
ReadLaser.StopReading;
end SuspendLaserDriver;
entry ResumeLaserDriver when LaserDriverInitialized is
begin
ReadLaser.StartReading;
end ResumeLaserDriver;
entry GetMostRecentLaserScan (LaserScan : out LaserStatus) when
LaserDriverInitialized is
begin
if OneOrMoreScansComplete then
CopyMostRecentLaserScan (LaserScan);
end if;
end GetMostRecentLaserScan;
entry GetMostRecentLaserTimeStamp (LaserTimeStamp : out Time) when
LaserDriverInitialized is
begin
if OneOrMoreScansComplete then
LaserTimeStamp :=
RecentTelegramsTimeStamp (MostRecentCompleteScan);
end if;
end GetMostRecentLaserTimeStamp;
entry GetAllAvailableLaserScans (LaserScans : out LaserStatusArray) when
LaserDriverInitialized is
begin
if OneOrMoreScansComplete then
CopyAllAvailableLaserScans (LaserScans);
end if;
end GetAllAvailableLaserScans;
end LaserDriverInterface;
----------------------------------------------------------------------------
--
-- Syncronization Monitor
--
----------------------------------------------------------------------------
protected body LaserMonitor is
entry BlockTask when True is
begin
TasksWaitingForData := TasksWaitingForData + 1;
requeue BlockedTasksQueue;
end BlockTask;
entry BlockedTasksQueue when NewScanArrived is
begin
TasksWaitingForData := TasksWaitingForData - 1;
if TasksWaitingForData = 0 then
NewScanArrived := False;
end if;
end BlockedTasksQueue;
procedure FreeTasks is
begin
NewScanArrived := True;
end FreeTasks;
end LaserMonitor;
end Laser_driver;