File : flex_driver.adb
-- -*- Mode: Ada -*-
-- Filename : flex_driver.adb
-- Description : Bidirectional driver between Linux and the flex controller
-- Author : Christfried Webers
-- Created On : Fri Oct 22 12:30:11 1999
-- Last Modified By: .
-- Last Modified On: .
-- Update Count : 0
-- Status : Experimental
-------------------------------------------------------------------------------
---
-------------------------------------------------------------------------------
---
--
-- Imports
--
-------------------------------------------------------------------------------
with Ada.Calendar; use Ada.Calendar;
with Ada.Text_IO; use Ada.Text_IO;
with Ada.Integer_Text_IO; use Ada.Integer_Text_IO;
with Ada.Float_Text_IO; use Ada.Float_Text_IO;
with Ada.Task_Identification; use Ada.Task_Identification;
with Ada.Exceptions; use Ada.Exceptions;
with Interfaces.C;
with Interrupt_Handler; use Interrupt_Handler;
with Flex_Common; use Flex_Common;
with Motor_Handler;
with Sonar_Handler;
-------------------------------------------------------------------------------
---
--
-- flex driver
--
-------------------------------------------------------------------------------
package body Flex_Driver is
----------------------------------------------------------------------------
----
--
-- 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 Interfaces.C.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
--
----------------------------------------------------------------------------
----
type StateType is (
ASCII,
ASCII_ESCAPE,
BINARY,
BINARY_ESCAPE,
BINARY_PIPE,
BINARY_PIPE_ESCAPE);
----------------------------------------------------------------------------
----
--
-- Global Constants
--
----------------------------------------------------------------------------
----
PortName : constant String := "/dev/cur0";
Baudrate115200 : constant Interfaces.C.unsigned := 8#0010002#;
-- see Linux configuration: /usr/include/termbits.h: #define B115200
--0010002
ReadTaskWaitingSleepTime : constant Duration := 0.1; -- seconds
DoNotAppendNull : constant Boolean := False;
ESCAPE : constant Raw_8Bit := 16#1B#;
PIPE : constant Raw_8Bit := 16#7C#;
Escape_Packet_Start : constant Raw_8Bit := 16#02#;
Escape_Packet_Stop : constant Raw_8Bit := 16#03#;
Escape_Stuff_Escape : constant Raw_8Bit := 16#00#;
Escape_Stuff_Pipe : constant Raw_8Bit := 16#01#;
----------------------------------------------------------------------------
---
--
-- Global Variables
--
----------------------------------------------------------------------------
---
FileDesc : Interfaces.C.int;
State : StateType := ASCII;
Buffer_Length : constant Integer := Integer (Raw_8Bit'Last);
Index : Integer range 0 .. Buffer_Length;
Buffer : ByteArray (1 .. Buffer_Length);
Checksum : Raw_8Bit;
----------------------------------------------------------------------------
---
procedure HandleContents (Packet : ByteArray) is
First : Integer := Packet'First;
PacketID : Raw_16Bit :=
Raw_16Bit (Packet (First)) * 2 ** 8 +
Raw_16Bit (Packet (First + 2));
PacketNumber : Raw_8Bit := Packet (Packet'First + 1);
begin
-- ShowContents(Packet); New_Line;
case PacketID is
when MotorDataID =>
Motor_Handler.Reader.RecordPacket
(PacketNumber,
Packet (Packet'First + 4 .. Packet'Last));
when SonarDataID =>
Sonar_Handler.Reader.RecordPacket
(PacketNumber,
Packet (Packet'First + 4 .. Packet'Last));
when DIODataID =>
-- ShowContents(Packet); New_Line;
-- Motor_Handler.BumperStatus.RecordPacket(PacketNumber,
-- Packet(Packet'First+4..Packet'La
--st));
-- Flex: 1B 02 05 FF 03 07 08 99 59 2E 40 00 01 59 1B 03
-- number of byte *
-- value * *
-- means front bumper activated
-- means rear bumper released
null;
when SonarStartStopID =>
WriteTask.Acknowledge (SonarStartStopSending);
null;
-- 04 FF 00 08 00 00 00 00 01 F1 F4 12
when MotorCommandID =>
-- ShowContents(Packet); New_Line;
if Packet (Packet'First + 12) = 0 then
WriteTask.Acknowledge (LinMotorCommand);
else
WriteTask.Acknowledge (AngMotorCommand);
end if;
-- 02 01 07 09 00 00 00 00 07 6F 50 10 00 : confirmation of motor
--command
-- 02 01 07 09 00 00 00 00 07 6F 50 10 01
when MotorStartStopID =>
WriteTask.Acknowledge (MotorStartStopSending);
-- 02 FF 22 08 00 00 00 00 07 B5 36 38
when DIOStartStopID =>
WriteTask.Acknowledge (DIOStartStopSending);
null;
when 16#0502# =>
-- Flex: 1B 02 05 FF 02 0C 00 00 00 00 08 93 67 60 00 00 00 12 7A
--1B 03
-- timestamp * * * *
-- number of packets send *
null;
when others =>
Put ("Unknown packet type: ");
PutHex (Raw_16Bit (PacketID));
Put (" Packetnumber:");
Put (Integer (PacketNumber), 0);
New_Line;
ShowContents (Packet);
New_Line;
null;
end case;
end HandleContents;
procedure ProcessData
(NewData : in Interfaces.C.char_array;
NewNoOfBytes : Integer)
is
This : Raw_8Bit;
PacketLength : Raw_8Bit;
begin
for I in 1 .. NewNoOfBytes loop
This :=
Raw_8Bit (Character'Pos
(Interfaces.C.To_Ada
(NewData (Interfaces.C.size_t (I)))));
case State is
when ASCII =>
case This is
when ESCAPE =>
State := ASCII_ESCAPE;
when others =>
null; -- do nothing with this byte
end case;
when ASCII_ESCAPE =>
if This = Escape_Packet_Start then
Checksum := 0;
Index := Buffer'First - 1;
State := BINARY;
else
State := ASCII;
end if;
when BINARY =>
case This is
when ESCAPE =>
State := BINARY_ESCAPE;
when PIPE =>
State := BINARY_PIPE;
when others =>
Index := Index + 1;
Buffer (Index) := This;
Checksum := Checksum xor This;
end case;
when BINARY_PIPE =>
case This is
when ESCAPE =>
State := BINARY_PIPE_ESCAPE;
when others =>
Index := Index + 1;
Buffer (Index) := PIPE;
Checksum := Checksum xor PIPE;
Index := Index + 1;
Buffer (Index) := This;
Checksum := Checksum xor This;
State := BINARY;
end case;
when BINARY_ESCAPE =>
case This is
when Escape_Packet_Stop =>
PacketLength := Buffer (Buffer'First + 3);
if PacketLength + 5 /= Raw_8Bit (Index) then
-- Show
Put ("Contents Length Error! ");
Put ("Length Byte:");
PutHex (PacketLength);
Put (" Real length:");
PutHex (Raw_8Bit (Index - 5));
New_Line;
ShowContents (Buffer (1 .. Index));
New_Line;
------------------
elsif Checksum /= 0 then
-- Show
Put ("Checksum error! ");
Put ("Checksum byte:");
PutHex (Buffer (Index));
Put (" Real checksum");
PutHex (Checksum xor Buffer (Index));
New_Line;
ShowContents (Buffer (1 .. Index));
New_Line;
------------------
else
HandleContents (Buffer (Buffer'First .. Index - 1));
-- strip checksum
end if;
State := ASCII;
when Escape_Packet_Start =>
-- maybe we jumped over a packet end, let's immediatly
--start new
Checksum := 0;
Index := Buffer'First - 1;
State := BINARY;
when Escape_Stuff_Escape =>
Index := Index + 1;
Buffer (Index) := ESCAPE;
Checksum := Checksum xor ESCAPE;
State := BINARY;
when others =>
Put ("Error: unknown ESCAPE sequence in packet! ");
------------------
ShowContents (Buffer (1 .. Index));
PutHex (ESCAPE);
Put (" ");
PutHex (This);
New_Line;
------------------
State := ASCII;
end case;
when BINARY_PIPE_ESCAPE =>
case This is
when Escape_Stuff_Pipe =>
Index := Index + 1;
Buffer (Index) := PIPE;
Checksum := Checksum xor PIPE;
State := BINARY;
when others =>
Put_Line ("Error: PIPE ESCAPE without following 01! ");
-----------------
ShowContents (Buffer (Buffer'First .. Index));
PutHex (PIPE);
Put (" ");
PutHex (This);
New_Line;
------------------
State := ASCII;
end case;
end case;
end loop;
end ProcessData;
----------------------------------------------------------------------------
---
--
-- ReadMonitor
--
----------------------------------------------------------------------------
---
protected ReadMonitor is
entry WaitForInterrupt;
procedure IO_Signal_Handler;
procedure NonBlocking;
private
InterruptArrived : Boolean := False;
NonBlockingActive : Boolean := False;
end ReadMonitor;
protected body ReadMonitor is
entry WaitForInterrupt when InterruptArrived or NonBlockingActive is
begin
if WaitForInterrupt'Count = 0 then
InterruptArrived := False;
end if;
end WaitForInterrupt;
procedure IO_Signal_Handler is
begin
InterruptArrived := True;
end IO_Signal_Handler;
procedure NonBlocking is
begin
NonBlockingActive := True;
end NonBlocking;
end ReadMonitor;
----------------------------------------------------------------------------
--
-- ReadTask
--
----------------------------------------------------------------------------
task ReadTask is
entry StartReading;
entry StopReading;
entry TerminateReading;
end ReadTask;
task body ReadTask 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 (FileDesc, ReadCharArray));
while BytesRead > 0 loop
-- Put (Integer(BytesRead)); Put_Line (" Bytes read");
ProcessData (ReadCharArray, BytesRead);
BytesRead :=
Integer (ReadSerialPort (FileDesc, ReadCharArray));
end loop;
ReadMonitor.WaitForInterrupt;
else
delay (ReadTaskWaitingSleepTime);
end if;
end select;
end loop;
Put_Line ("- flex_driver: ReadTask terminated");
exception
when E : others =>
Put_Line
(Current_Error,
"Task " &
Image (Current_Task) &
" terminating because of exception " &
Exception_Name (E));
end ReadTask;
----------------------------------------------------------------------------
---
--
-- SerialPort
--
----------------------------------------------------------------------------
---
protected body SerialPort is
procedure Init is
begin
if NumberOfClients = 0 then
InterruptInterface.InitIntHandler;
InterruptInterface.AddIntRoutine
(SignalIO,
ReadMonitor.IO_Signal_Handler'Access);
FileDesc := OpenSerialPort (Interfaces.C.To_C (PortName));
SetupSerialPort (FileDesc, Baudrate115200);
EnableSerialPortInterrupt (FileDesc);
ReadTask.StartReading;
WriteTask.Start;
end if;
NumberOfClients := NumberOfClients + 1;
end Init;
procedure Shutdown is
begin
NumberOfClients := NumberOfClients - 1;
if NumberOfClients = 0 then
InterruptInterface.ShutdownIntHandler;
WriteTask.TerminateTask;
ReadMonitor.NonBlocking;
ReadTask.TerminateReading;
CloseSerialPort (FileDesc);
end if;
end Shutdown;
end SerialPort;
----------------------------------------------------------------------------
--
-- WriteTask
--
----------------------------------------------------------------------------
procedure TransmitPacket (Data : ByteArray) is
Res : ByteArray (1 .. Data'Length * 3);
Index : Integer;
Checksum : Raw_8Bit := 0;
begin
Res (Res'First) := ESCAPE;
Res (Res'First + 1) := Escape_Packet_Start;
Index := Res'First + 1;
for I in Data'First .. Data'Last loop
if Data (I) = ESCAPE then
Index := Index + 1;
Res (Index) := ESCAPE;
Index := Index + 1;
Res (Index) := Escape_Stuff_Escape;
Checksum := Checksum xor ESCAPE;
elsif Data (I) = PIPE then
Index := Index + 1;
Res (Index) := PIPE;
Index := Index + 1;
Res (Index) := ESCAPE;
Index := Index + 1;
Res (Index) := Escape_Stuff_Pipe;
Checksum := Checksum xor PIPE;
else
Index := Index + 1;
Res (Index) := Data (I);
Checksum := Checksum xor Data (I);
end if;
end loop;
Index := Index + 1;
Res (Index) := Checksum;
if Checksum = ESCAPE then
Index := Index + 1;
Res (Index) := Escape_Stuff_Escape;
elsif Checksum = PIPE then
Index := Index + 1;
Res (Index) := ESCAPE;
Index := Index + 1;
Res (Index) := Escape_Stuff_Pipe;
end if;
Index := Index + 1;
Res (Index) := ESCAPE;
Index := Index + 1;
Res (Index) := Escape_Packet_Stop;
WriteSerialPort
(FileDesc,
Interfaces.C.To_C
(ByteArrayToString (Res (Res'First .. Index)),
DoNotAppendNull),
Interfaces.C.int (Index));
end TransmitPacket;
task body WriteTask is
Packets : array (SendPacketType) of ByteArrayPointer :=
(new ByteArray (MotorCommandType'Range),
new ByteArray (MotorCommandType'Range),
new ByteArray (MotorStartStopSendingType'Range),
new ByteArray (SonarStartStopSendingType'Range),
new ByteArray (DIOStartStopSendingType'Range));
type SendPacketFlags is array (SendPacketType) of Boolean;
AckWaitTime : constant Duration := 0.5;
VeryLongDuration : constant Duration := 1_000_000.0;
RequestToSend : SendPacketFlags := (others => False);
WaitingForAck : SendPacketFlags := (others => False);
NoWaitForAck : constant SendPacketFlags := (others => False);
Timeout : array (SendPacketType) of Time;
NextTimeout : Time := Clock + VeryLongDuration; -- first loop should
--not time out
TimeoutType : SendPacketType; -- undefined ok (first loop can not time
--out)
Task_Is_Active : Boolean := True;
ThisTimeout : Time;
ThisType : SendPacketType;
procedure Message (S : String; PacketType : SendPacketType) is
begin
Put (S);
case PacketType is
when LinMotorCommand =>
Put_Line ("LinMotorCommand");
when AngMotorCommand =>
Put_Line ("AngMotorCommand");
when MotorStartStopSending =>
Put_Line ("MotorStartStop");
when SonarStartStopSending =>
Put_Line ("SonarStartStop");
when DIOStartStopSending =>
Put_Line ("DIOStartStop");
when others =>
Put_Line ("Error in Acknowledge");
end case;
end Message;
begin
accept Start;
while Task_Is_Active loop
select when WaitingForAck = NoWaitForAck =>
accept TerminateTask;
Task_Is_Active := False;
or
accept SendPacket (
Packet : ByteArray;
PacketType : SendPacketType) do
ThisType := PacketType;
Packets (ThisType).all := Packet;
end SendPacket;
if not WaitingForAck (ThisType) then
TransmitPacket (Packets (ThisType).all);
Timeout (ThisType) := Clock + AckWaitTime;
RequestToSend (ThisType) := False;
WaitingForAck (ThisType) := True;
-- Message("Send - ", ThisPacketType );
else -- WaitingForAck first
RequestToSend (ThisType) := True;
-- Message("Sendrequest - ", ThisType);
end if;
or
accept Acknowledge (AckPacketType : SendPacketType) do
ThisType := AckPacketType;
end Acknowledge;
if WaitingForAck (ThisType) then
if ThisType = LinMotorCommand then
while WaitForLinMotorCommandAck'Count > 0 loop
accept WaitForLinMotorCommandAck;
-- release all blocked tasks
end loop;
elsif ThisType = AngMotorCommand then
while WaitForAngMotorCommandAck'Count > 0 loop
accept WaitForAngMotorCommandAck;
-- release all blocked tasks
end loop;
end if;
if RequestToSend (ThisType) then
TransmitPacket (Packets (ThisType).all);
Timeout (ThisType) := Clock + AckWaitTime;
RequestToSend (ThisType) := False;
--Message("Ack/Send - ", ThisPacketType);
else
WaitingForAck (ThisType) := False; -- reset
--Message("Ack - ", ThisPacketType);
end if;
else
null; -- ignore irregular acknowledgement
end if;
or
delay until NextTimeout;
Message ("Timeout - ", TimeoutType);
-- send again, possibly with updated values
TransmitPacket (Packets (TimeoutType).all);
Timeout (TimeoutType) := Clock + AckWaitTime;
RequestToSend (TimeoutType) := False;
Message ("Send Again - ", TimeoutType);
end select;
-- check for timeout
NextTimeout := Clock + VeryLongDuration;
for I in SendPacketType loop
if WaitingForAck (I) then
ThisTimeout := Timeout (I);
if ThisTimeout < NextTimeout then
-- if ThisTimeout < Clock then
-- Put_Line("ThisTimeout < Clock");
-- end if;
NextTimeout := ThisTimeout;
TimeoutType := I;
end if;
end if;
end loop;
end loop;
Put_Line ("- flex_driver: WriteTask terminated");
exception
when E : others =>
Put_Line
(Current_Error,
"Task " &
Image (Current_Task) &
" terminating because of exception " &
Exception_Name (E));
end WriteTask;
end Flex_Driver;