#include <sr_edc.h>

Inheritance diagram for SrEdc:

[legend]

Public Member Functions
void	build_CAN_message (ETHERCAT_CAN_BRIDGE_DATA *message)

bool	can_data_is_ack (ETHERCAT_CAN_BRIDGE_DATA *packet)
	This function checks if the can packet in the unpackState() this_buffer is an ACK. More...

virtual void	construct (EtherCAT_SlaveHandler *sh, int &start_address, unsigned int ethercat_command_data_size, unsigned int ethercat_status_data_size, unsigned int ethercat_can_bridge_data_size, unsigned int ethercat_command_data_address, unsigned int ethercat_status_data_address, unsigned int ethercat_can_bridge_data_command_address, unsigned int ethercat_can_bridge_data_status_address)
	Construct function, run at startup to set SyncManagers and FMMUs. More...

void	erase_flash ()
	Erase the PIC18F Flash memory. More...

bool	read_flash (unsigned int offset, unsigned int baddr)
	Function that reads back 8 bytes from PIC18F program memory. More...

bool	simple_motor_flasher (sr_robot_msgs::SimpleMotorFlasher::Request &req, sr_robot_msgs::SimpleMotorFlasher::Response &res)
	ROS Service that flashes a new firmware into a SimpleMotor board. More...

	SrEdc ()
	Constructor of the SrEdc driver. More...

Public Member Functions inherited from SR0X
virtual int	initialize (hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)

Protected Types
typedef realtime_tools::RealtimePublisher< std_msgs::Int16 >	rt_pub_int16_t

Protected Types inherited from SR0X
enum	{ MODE_OFF = 0x00, MODE_ENABLE = (1 << 0), MODE_CURRENT = (1 << 1), MODE_SAFETY_RESET = (1 << 4), MODE_SAFETY_LOCKOUT = (1 << 5), MODE_UNDERVOLTAGE = (1 << 6), MODE_RESET = (1 << 7) }

enum	{ EC_PRODUCT_ID_BRIDGE = 0, EC_PRODUCT_ID_SHADOWCAN = 2, EC_PRODUCT_ID_DUALMOTOR = 3 }

Protected Member Functions
virtual void	get_board_id_and_can_bus (int board_id, int can_bus, unsigned int board_can_id)=0

virtual void	reinitialize_boards ()=0
	This function will call the reinitialization function for the boards attached to the CAN bus. More...

Protected Attributes
bool	can_packet_acked

int	counter_

std::string	device_id_

std::string	device_joint_prefix_

boost::shared_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > >	extra_analog_inputs_publisher
	Extra analog inputs real time publisher (+ accelerometer and gyroscope) More...

bool	flashing

ros::NodeHandle	nh_tilde_

ros::NodeHandle	nodehandle_

std::vector< boost::shared_ptr< rt_pub_int16_t > >	realtime_pub_

Protected Attributes inherited from SR0X
uint8_t	board_major_

uint8_t	board_minor_

int	command_base_

int	device_offset_

uint8_t	fw_major_

uint8_t	fw_minor_

int	level_

string	reason_

int	status_base_

Private Member Functions
void	find_address_range (bfd fd, unsigned int smallest_start_address, unsigned int *biggest_end_address)

bool	read_back_and_check_flash (unsigned int baddr, unsigned int total_size)

bool	read_content_from_object_file (bfd fd, bfd_byte content, unsigned int base_addr)

void	send_CAN_msg (int8u can_bus, int16u msg_id, int8u msg_length, int8u msg_data[], int timeout, bool *timedout)

bool	write_flash_data (unsigned int base_addr, unsigned int total_size)

Static Private Member Functions
static std::string	get_filename (std::string full_path)

Private Attributes
bfd_byte *	binary_content

int	can_bus_

ETHERCAT_CAN_BRIDGE_DATA	can_message_

bool	can_message_sent

unsigned int	motor_being_flashed

unsigned int	pos

pthread_mutex_t	producing

ros::ServiceServer	serviceServer

Static Private Attributes
static const unsigned int	max_retry = 20

Detailed Description

Definition at line 60 of file sr_edc.h.

Member Typedef Documentation

◆ rt_pub_int16_t

typedef realtime_tools::RealtimePublisher<std_msgs::Int16> SrEdc::rt_pub_int16_t

protected

Definition at line 89 of file sr_edc.h.

Constructor & Destructor Documentation

◆ SrEdc()

SrEdc::SrEdc ( )

Constructor of the SrEdc driver.

This is the Constructor of the driver. We initialize a few boolean values, a mutex and create the Bootloading service.

Definition at line 120 of file sr_edc.cpp.

Member Function Documentation

◆ build_CAN_message()

void SrEdc::build_CAN_message ( ETHERCAT_CAN_BRIDGE_DATA * message )

Definition at line 615 of file sr_edc.cpp.

◆ can_data_is_ack()

bool SrEdc::can_data_is_ack ( ETHERCAT_CAN_BRIDGE_DATA * packet )

This function checks if the can packet in the unpackState() this_buffer is an ACK.

This function checks several things on the can packet in this_buffer, it compares it with the can_message_ private member in several ways (SID, length, data) to check if it's an ACK.

Parameters

packet The packet from this_buffer of unpackState() that we want to check if it's an ACK

Returns: Returns true if packet is an ACK of can_message_ packet.

Definition at line 668 of file sr_edc.cpp.

◆ construct()

void SrEdc::construct	(	EtherCAT_SlaveHandler *	sh,
		int &	start_address,
		unsigned int	ethercat_command_data_size,
		unsigned int	ethercat_status_data_size,
		unsigned int	ethercat_can_bridge_data_size,
		unsigned int	ethercat_command_data_address,
		unsigned int	ethercat_status_data_address,
		unsigned int	ethercat_can_bridge_data_command_address,
		unsigned int	ethercat_can_bridge_data_status_address
	)

virtual

Construct function, run at startup to set SyncManagers and FMMUs.

The role of this function is to setup the SyncManagers and the FMMUs used by this EtherCAT slave. This slave is using two Mailboxes on two different memory areas.

Here we are setting up the way of communicating between ROS and the PIC32 using the EtherCAT protocol.

We communicate using Shared Memory areas.

The FMMUs are usefull to map the logical memory used by ROS to the Physical memory of the EtherCAT slave chip (ET1200 chip). So that the chip receiving the packet will know that the data at address 0x10000 is in reality to be written at physical address 0x1000 of the chip memory for example. It is the mapping between the EtherCAT bus address space and each slave's chip own memory address space.

The SyncManagers are usefull to give a safe way of accessing this Shared Memory, using a consumer / producer model. There are features like interrupts to tell the consumer that there is something to consume or to tell the producer that the Mailbox is empty and then ready to receive a new message.

One Mailbox contains the commands, written by ROS, read by the PIC32
One Mailbox contains the status, written back by the PIC32, read by ROS

That's basically one Mailbox for upstream and one Mailbox for downstream.

The first Mailbox contains in fact two commands, one is the torque demand, the other is a CAN command used in CAN_DIRECT_MODE to communicate with the SimpleMotor for test purposes, or to reflash a new firmware in bootloading mode. This Mailbox is at logicial address 0x10000 and mapped via a FMMU to physical address 0x1000 (the first address of user memory)
The second Mailbox contains in fact two status, they are the response of the two previously described commands. One is the status containing the joints data, sensor data, finger tips data and motor data. The other is the can command response in CAN_DIRECT_MODE. When doing a flashing in bootloading mode this is usually an acknowledgment from the bootloader. This Mailbox is at logical address 0x10038 and mapped via a FMMU to physical address 0x1038.

This function sets the two private members command_size_ and status_size_ to be the size of each Mailbox. It is important for these numbers to be accurate since they are used by the EthercatHardware class when manipulating the buffers. If you need to have several commands like in this SrEdc driver, put the sum of the size, same thing for the status.

Definition at line 167 of file sr_edc.cpp.

◆ erase_flash()

void SrEdc::erase_flash ( void )

Erase the PIC18F Flash memory.

This function fills the can_message_ struct with a CAN message which tells the bootloader of the PIC18F to erase its Flash memory

Definition at line 308 of file sr_edc.cpp.

◆ find_address_range()

void SrEdc::find_address_range	(	bfd *	fd,
		unsigned int *	smallest_start_address,
		unsigned int *	biggest_end_address
	)

private

Look for the start and end address of every section in the hex file, to detect the lowest and highest address of the data we need to write in the PIC's flash. The sections starting at an address higher than 0x7fff will be ignored as they are not proper "code memory" firmware (they can contain the CONFIG bits of the microcontroller, which we don't want to write here) To understand the structure (sections) of the object file containing the firmware (usually a .hex) the following commands can be useful:

objdump -x simplemotor.hex

objdump -s simplemotor.hex

Parameters

fd	pointer to a bfd file structure
smallest_start_address	the lowest address found is returned through this pointer
biggest_end_address	the highest address found is returned through this pointer

Definition at line 830 of file sr_edc.cpp.

◆ get_board_id_and_can_bus()

virtual void SrEdc::get_board_id_and_can_bus	(	int	board_id,
		int *	can_bus,
		unsigned int *	board_can_id
	)

protectedpure virtual

Given the identifier for a certain board (motor board/ muscle driver) determines the right value for the CAN bus and the ID of the board in that CAN bus.

Parameters

board_id	the unique identifier for the board
can_bus	pointer to the can bus number we want to determine
board_can_id	pointer to the board id we want to determine

Implemented in SR09, SR06, SR08, and SrEdcMuscle.

◆ get_filename()

static std::string SrEdc::get_filename ( std::string full_path )

inlinestaticprivate

Extract the filename from the full path.

Parameters

full_path The full path.

Returns: the filename.

Definition at line 202 of file sr_edc.h.

◆ read_back_and_check_flash()

bool SrEdc::read_back_and_check_flash	(	unsigned int	baddr,
		unsigned int	total_size
	)

private

Read back the firmware from the flash of the PIC, and checks it against the data read from the object (.hex) file

Parameters

baddr	the base address of the code (the lowest address to be written on the flash)
total_size	the size in bytes of the code to write

Returns: true if both are identical

Definition at line 797 of file sr_edc.cpp.

◆ read_content_from_object_file()

bool SrEdc::read_content_from_object_file	(	bfd *	fd,
		bfd_byte *	content,
		unsigned int	base_addr
	)

private

Reads the content from the object (.hex) file and stores it in a previously reserved memory space

Parameters

fd	pointer to a bfd file structure
content	a pointer to the memory space where we want to store the firmware
base_addr	the base address of the code (the lowest address to be written on the flash)

Returns: true if the reading succeeds

Definition at line 867 of file sr_edc.cpp.

◆ read_flash()

bool SrEdc::read_flash	(	unsigned int	offset,
		unsigned int	baddr
	)

Function that reads back 8 bytes from PIC18F program memory.

Flash memory is the program memory on the PIC18F This function is here to read back what we flashed into the PIC18F To check that there was no flashing transmission or writting error during the Flashing process. 8 bytes will be read, from address baddr + offset This function will fill the can_message_ structure with the correct value to allow the packCommand() function to send the correct etherCAT message to the PIC32 which will then send the correct CAN message to the PIC18F

Parameters

offset	The position of the 8 bytes we want to read, relative to the base address
baddr	the base address

Returns: Returns true if the command has timed out, false if the command was properly acknowledged

Definition at line 375 of file sr_edc.cpp.

◆ reinitialize_boards()

virtual void SrEdc::reinitialize_boards ( )

protectedpure virtual

This function will call the reinitialization function for the boards attached to the CAN bus.

Implemented in SR09, SR06, SR08, and SrEdcMuscle.

◆ send_CAN_msg()

void SrEdc::send_CAN_msg	(	int8u	can_bus,
		int16u	msg_id,
		int8u	msg_length,
		int8u	msg_data[],
		int	timeout,
		bool *	timedout
	)

private

Contains the common procedure to send a CAN message (i.e. write it in the global struct from which it is read, added to the next ethercat frame, and sent) and wait for the can_packet_acked flag to be set

Parameters

can_bus	which of the 2 CAN buses (0 or 1) wil be used
msg_id	id of the CAN message
msg_length	the length in bytes of msg_data
msg_data	data of the CAN message
timeout	time max (in ms) to wait for the can_packet_acked flag to be set
timedout	if true, the can_packet_acked flag wasn't set before the timeout

Definition at line 750 of file sr_edc.cpp.

◆ simple_motor_flasher()

bool SrEdc::simple_motor_flasher	(	sr_robot_msgs::SimpleMotorFlasher::Request &	req,
		sr_robot_msgs::SimpleMotorFlasher::Response &	res
	)

ROS Service that flashes a new firmware into a SimpleMotor board.

This function is a ROS Service, aimed at flashing a new firmware into the PIC18F of a SimpleMotor board through a CAN bootloader protocol.

The CAN bootloader allows for several commands to be executed : read_flash, erase_flash, write_flash, reboot, read_version

This service will fill a can_message_ structure and then switch a few boolean values to "false" in order to trigger the message to be sent by the SRXX::packCommand() function. And then repeat the process for another message, and so on.

This service will first read all the sections of the firmware using libbfd and find out the lowest and highest addresses containing code. Then it will allocate an array to contain the firmware's code. The size is (highest_addr - lowest_addr). The bfd library provides functions to manage object files more easily. To better understand some of the concepts used below, the following link can be useful: http://www.delorie.com/gnu/docs/binutils/ld_7.html The use of the following commands can also help to understand the structure of the object file containing the firmware

objdump -x simplemotor.hex

objdump -s simplemotor.hex

Then it will send a MAGIC PACKET command to the PIC18F which will make it reboot in bootloader mode (regardless of whether it was already in bootloader mode or whether it was running the SimpleMotor code)
Then it will send an ERASE_FLASH command to the PIC18F.
Then it will send a WRITE_FLASH_ADDRESS_COMMAND to tell the PIC18F where we wanna write, and then 4 WRITE_FLASH_DATA commands (we write by blocks of 32 bytes). This process is repeated untill we've written all the firmware code, padding with 0x00 bytes in the end if the size is not a multiple of 32 bytes. The process starts at address (lowest_addr) and ends at (hiest_addr) + a few padding bytes if necessary.

You can call this service using this command :

rosservice call SimpleMotorFlasher "/home/hand/simplemotor.hex" 8

This will flash the "simplemotor.hex" firmware to the motor 8

Parameters

req	The Request, contains the ID of the motor we want to flash via req.motor_id, and the path of the firmware to flash in req.firmware
res	The Response, it is always SUCCESS for now.

Returns: This returns always true, the real return value is in the res parameter

Definition at line 458 of file sr_edc.cpp.

◆ write_flash_data()

bool SrEdc::write_flash_data	(	unsigned int	base_addr,
		unsigned int	total_size
	)

private

Writes the code previously read from the hex file to the flash memory of the PIC

Parameters

base_addr	the base address of the code (the lowest address to be written on the flash)
total_size	the size in bytes of the code to write

Returns: true if the writing process succeeds

protected

Definition at line 99 of file sr_edc.h.

◆ extra_analog_inputs_publisher

boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> > SrEdc::extra_analog_inputs_publisher

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Private Member Functions

Static Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Member Typedef Documentation

◆ rt_pub_int16_t

Constructor & Destructor Documentation

◆ SrEdc()

Member Function Documentation

◆ build_CAN_message()

◆ can_data_is_ack()

◆ construct()

◆ erase_flash()

◆ find_address_range()

◆ get_board_id_and_can_bus()

◆ get_filename()

◆ read_back_and_check_flash()

◆ read_content_from_object_file()

◆ read_flash()

◆ reinitialize_boards()

◆ send_CAN_msg()

◆ simple_motor_flasher()

◆ write_flash_data()

Member Data Documentation

◆ binary_content

◆ can_bus_

◆ can_message_

◆ can_message_sent

◆ can_packet_acked

◆ counter_

◆ device_id_

◆ device_joint_prefix_

◆ extra_analog_inputs_publisher

◆ flashing

◆ max_retry

◆ motor_being_flashed

◆ nh_tilde_

◆ nodehandle_

◆ pos

◆ producing

◆ realtime_pub_

◆ serviceServer