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'Last));
-- 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;