Commit a989fc03 authored by Delia Cellarier's avatar Delia Cellarier

The communiction between the CPU and the nRF51 chip is now re-initialized if...

The communiction between the CPU and the nRF51 chip is now re-initialized if we loose too many packets
parent da62585b
......@@ -40,6 +40,7 @@ with GNAT.Sockets;
with PolyORB_HI.Messages;
with PolyORB_HI.Output;
with system.io;
with PolyORB_HI_Generated.Transport;
......@@ -122,9 +123,7 @@ package body PolyORB_HI_Drivers_Client_BLE is
IP_Protocol_For_TCP_Level,
(No_Delay, True));
Bind_Socket
(Socket_Receive,
Address_Receive);
Bind_Socket (Socket_Receive, Address_Receive);
Listen_Socket (Socket_Receive);
-- pragma Debug (Verbose,
......@@ -163,6 +162,7 @@ package body PolyORB_HI_Drivers_Client_BLE is
Initialize_Receiver;
-- pragma Debug (Verbose, Put_Line ("Initialization of socket subsystem"
-- & " is complete"));
-- pragma Debug (Verbose, Put_Line ("Driver initialized"));
exception
when E : others =>
-- pragma Debug (PolyORB_HI.Output.Error, Put_Line
......@@ -181,6 +181,8 @@ package body PolyORB_HI_Drivers_Client_BLE is
procedure Initialize_Receiver is
Socket : Socket_Type;
Address : Sock_Addr_Type;
-- Channel : Stream_Access := Stream (Socket_Send);
begin
-- Wait for the connection of python socket
......@@ -188,9 +190,9 @@ package body PolyORB_HI_Drivers_Client_BLE is
-- ("Waiting on "
-- & Image (Nodes (My_Node).Address)));
Address := No_Sock_Addr;
Socket := No_Socket;
Accept_Socket (Socket_Receive, Socket, Address);
Socket_Receive := Socket;
-- String'Write (Channel, "Client accepted");
end Initialize_Receiver;
......@@ -204,48 +206,58 @@ package body PolyORB_HI_Drivers_Client_BLE is
SEL : AS_Message_Length_Stream;
SEA : AS_Full_Stream;
SEO : AS.Stream_Element_Offset;
begin
Main_Loop :
loop
-- Put_Line ("Using user-provided TCP/IP stack to receive");
-- pragma Debug (Verbose, Put_Line ("Check mailboxes"));
-- Receive message length
Receive_Socket (Socket_Receive, SEL, SEO);
-- Receive zero bytes means that peer is dead
if SEO = 0 then
-- pragma Debug (Verbose, Put_Line
-- ("Node " & Node_Type'Image (N)
-- & " is dead"));
-- pragma Debug (Verbose, Put_Line ("Closing Socket_Receive"));
System.IO.Put_Line ("Closing socket");
Close_Socket (Socket_Receive);
exit Main_Loop;
elsif SEO = SEL'First - 1 then
System.IO.Put_Line ("Dropped message");
goto End_Of_Loop;
end if;
SEO := AS.Stream_Element_Offset
(To_Length (To_PO_HI_Message_Length_Stream (SEL)));
-- pragma Debug (Verbose, Put_Line
-- ("received"
-- & AS.Stream_Element_Offset'Image (SEO)
-- & " bytes from node " & Node_Type'Image (N)));
-- & AS.Stream_Element_Offset'Image (SEO)
-- & " bytes"));
-- Get the message and preserve message length to keep
-- compatible with a local message delivery.
SEA (1 .. Message_Length_Size) := SEL;
Receive_Socket (Socket_Receive,
SEA (Message_Length_Size + 1 .. SEO + 1), SEO);
SEA (Message_Length_Size + 1 .. SEO + 1),
SEO);
if SEO = Message_Length_Size then
System.IO.Put_Line ("Dropped message");
goto End_Of_Loop;
end if;
-- Deliver to the peer handler
-- pragma Debug (Verbose, Put_Line ("Delivering message"));
System.IO.Put_Line ("Delivering message");
PolyORB_HI_Generated.Transport.Deliver
(Corresponding_Entity
(Unsigned_8 (SEA (Message_Length_Size + 1))),
(Unsigned_8 (SEA (Message_Length_Size + 1))),
To_PO_HI_Full_Stream (SEA)
(1 .. Stream_Element_Offset (SEO)));
(1 .. Stream_Element_Offset (SEO)));
-- pragma Debug (Verbose, Put_Line ("Delivered"));
<<End_Of_Loop>>
end loop Main_Loop;
exception
......@@ -256,7 +268,10 @@ package body PolyORB_HI_Drivers_Client_BLE is
-- pragma Debug (PolyORB_HI.Output.Error, Put_Line
-- ("Message "
-- & Ada.Exceptions.Exception_Message (E)));
null;
System.IO.Put_Line ("Exception "
& Ada.Exceptions.Exception_Name (E));
System.IO.Put_Line ("Message "
& Ada.Exceptions.Exception_Message (E));
end Receive;
----------
......
......@@ -32,20 +32,21 @@
pragma Style_Checks (off);
with Types; use Types;
with LEDS; use LEDS;
with Crazyflie_System; use Crazyflie_System;
with Radiolink; use Radiolink;
with CRTP; use CRTP;
with Syslink; use Syslink;
with CRTP; use CRTP;
with Syslink; use Syslink;
with UART_Syslink; use UART_Syslink;
with EXT_UART; use EXT_UART;
-- with Power_Management; use Power_Management;
with Tasks; use Tasks;
with CRTP_Service; use CRTP_Service;
with Ada.Interrupts; use Ada.Interrupts;
with Ada.Interrupts.Names; use Ada.Interrupts.Names;
with Interfaces;
With Ada.Exceptions;
with Ada.Unchecked_Conversion;
with Ada.Synchronous_Task_Control;
with Ada.Streams;
with PolyORB_HI.Output;
......@@ -57,7 +58,7 @@ with System; use System;
package body PolyORB_HI_Drivers_Crazyflie_BLE is
-- TO DO : add BLE_Conf_T_Acc and Nodes
-- TODO : add BLE_Conf_T_Acc and Nodes
pragma Suppress (Elaboration_Check, PolyORB_HI_Generated.Transport);
-- We do not want a pragma Elaborate_All to be implicitely
......@@ -70,8 +71,6 @@ package body PolyORB_HI_Drivers_Crazyflie_BLE is
use PolyORB_HI.Utils;
use PolyORB_HI.Output;
package STC renames Ada.Synchronous_Task_Control;
subtype AS_Message_Length_Stream is AS.Stream_Element_Array
(1 .. Message_Length_Size);
subtype Message_Length_Stream is Stream_Element_Array
......@@ -85,27 +84,60 @@ package body PolyORB_HI_Drivers_Crazyflie_BLE is
function To_PO_HI_Full_Stream is new Ada.Unchecked_Conversion
(AS_Full_Stream, Full_Stream);
Connected_To_Radio : Boolean := False;
Dropped_Packets : Integer := 0;
----------------
-- Initialize --
----------------
procedure Initialize (Name_Table : PolyORB_HI.Utils.Naming_Table_Type) is
Is_Init : Boolean;
begin
-- Initialize radio with default values (channel 80, 2 Mbps)
Initialize_EXT_UART;
Send_To_UART("[Crazyflie_BLE] Starting System initialization\n");
System_Init;
Send_To_UART("[Crazyflie_BLE] Starting System test\n");
Is_Init := System_Self_Test;
if Is_Init then
Send_To_UART("[Crazyflie_BLE] System_Init succeeded\n");
Connected_To_Radio := False;
if System_Self_Test then
Send_To_UART("[BLE] initialized");
end if;
end Initialize;
--------------------------
-- CRTP_Service_Handler --
--------------------------
procedure CRTP_Service_Handler (Packet : CRTP_Packet)
is
Command : CRTP_Service_Command;
Tx_Packet : CRTP_Packet := Packet;
Has_Succeed : Boolean;
------------------------------------------
-- CRTP_Channel_To_CRTP_Service_Command --
------------------------------------------
function CRTP_Channel_To_CRTP_Service_Command is
new Ada.Unchecked_Conversion (CRTP_Channel, CRTP_Service_Command);
begin
-- Send_To_UART("[CRTP_Service] Handling packet");
Command := CRTP_Channel_To_CRTP_Service_Command (Packet.Channel);
case Command is
when Link_Echo =>
Has_Succeed := Radiolink_Send_Packet (Tx_Packet);
when Link_Source =>
Tx_Packet.Size := CRTP_MAX_DATA_SIZE;
Has_Succeed := Radiolink_Send_Packet (Tx_Packet);
when others =>
-- Null packets
null;
end case;
end CRTP_Service_Handler;
-------------
-- Receive --
-------------
......@@ -124,34 +156,46 @@ package body PolyORB_HI_Drivers_Crazyflie_BLE is
Main_Loop : loop
-- Receive and route one Syslink packet, changed into CRTP packet
Syslink_Task_Type;
-- should uart reception be disabled ? For now, bytes are still enqueued
-- Wait until a CRTP Packet is received
Radiolink_Receive_Packet_Blocking(Rx_Packet);
-- Send_To_UART("[Receive] CRTP Packet received");
-- Extract PolyORB message
SEO := AS.Stream_Element_Offset (Rx_Packet.Data_2'Length);
for i in Rx_Packet.Data_2'Range loop
CRTP_Get_T_Uint8_Data (CRTP_Get_Handler_From_Packet (Rx_Packet),
i, Rx_Byte, Has_Succeed);
SEA (AS.Stream_Element_Offset (i)) := AS.Stream_Element (Rx_Byte);
end loop;
if Rx_Packet.Port = CRTP_PORT_LINK
then
-- Send_To_UART("[Receive] LINK");
CRTP_Service_Handler (Rx_Packet);
elsif Rx_Packet.Port = CRTP_PORT_CONSOLE
then
-- Send_To_UART("[Receive] Packet received on CONSOLE port");
-- Extract PolyORB message
SEO := AS.Stream_Element_Offset (Rx_Packet.Data_2'Length);
for i in Rx_Packet.Data_2'Range loop
CRTP_Get_T_Uint8_Data (CRTP_Get_Handler_From_Packet (Rx_Packet),
i, Rx_Byte, Has_Succeed);
SEA (AS.Stream_Element_Offset (i)) := AS.Stream_Element (Rx_Byte);
end loop;
-- Deliver to the peer handler
begin
PolyORB_HI_Generated.Transport.Deliver
(Corresponding_Entity
(Unsigned_8 (SEA (Message_Length_Size + 1))),
To_PO_HI_Full_Stream (SEA)
(1 .. Stream_Element_Offset (SEO)));
exception
when E : others =>
null; -- Put_Line (Ada.Exceptions.Exception_Information (E));
end;
-- Deliver to the peer handler
begin
PolyORB_HI_Generated.Transport.Deliver
(Corresponding_Entity
(Unsigned_8 (SEA (Message_Length_Size + 1))),
To_PO_HI_Full_Stream (SEA)
(1 .. Stream_Element_Offset (SEO)));
exception
when E : others =>
null; -- Put_Line (Ada.Exceptions.Exception_Information (E));
end;
else
null;
end if;
end loop Main_Loop;
end Receive;
......@@ -185,27 +229,60 @@ package body PolyORB_HI_Drivers_Crazyflie_BLE is
Has_Succeed : Boolean;
Free_Bytes_In_Packet : Boolean := True;
Size_Int : Integer := Integer (Ada.Streams.Stream_Element_Offset (Size));
begin
-- The message length should be less than 30 bytes to fit in 1 CRTP Packet
if Integer (Size) > CRTP_MAX_DATA_SIZE
then
Send_To_UART ("Message too long :" & Integer'Image (Size_Int));
return Error_Kind'(Error_Transport);
end if;
else
-- Send_To_UART ("Size :" & Integer'Image (Size_Int));
-- Use the Console Port to send the message to client (does not matter)
Packet_Handler := CRTP_Create_Packet (CRTP_PORT_CONSOLE, 0);
-- Use the Console Port to send the message to client
-- This port is checked by the python client
Packet_Handler := CRTP_Create_Packet (CRTP_PORT_CONSOLE, 0);
for SE of Msg loop
CRTP_Append_T_Uint8_Data (Packet_Handler, T_Uint8 (SE), Free_Bytes_In_Packet);
end loop;
Has_Succeed := Radiolink_Send_Packet
(CRTP_Get_Packet_From_Handler (Packet_Handler));
CRTP_Reset_Handler (Packet_Handler);
-- Append message to the CRTP Packet
for SE of Msg loop
CRTP_Append_T_Uint8_Data
(Packet_Handler, T_Uint8 (SE), Free_Bytes_In_Packet);
end loop;
return Error_Kind'(Error_None);
-- Note: we have no way to know there was an error here
-- Send the CRTP Packet by radio
Has_Succeed := Radiolink_Send_Packet
(CRTP_Get_Packet_From_Handler (Packet_Handler));
-- Send_To_UART ("Sent: " & Boolean'Image (Has_Succeed));
if Connected_To_Radio then
-- We are connected to a radio
if Has_Succeed = False then
Dropped_Packets := Dropped_Packets + 1;
else
Dropped_Packets := 0;
end if;
-- If too many packets are dropped,
-- reset the UART talking to the nRF51
if Dropped_Packets = 10 then
UART_Syslink_Init;
Dropped_Packets := 0;
Send_To_UART ("=== "
& "RESET COMMUNICATION"
& " ===");
end if;
else
-- We don't want to reset if the communication is not established
if Has_Succeed then
Connected_To_Radio := True;
end if;
end if;
return Error_Kind'(Error_None);
-- Note: we have no way to know there was an error here
end if;
end;
end PolyORB_HI_Drivers_Crazyflie_BLE;
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment