Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Tammo Jan Dijkema
dt_ctrl
Commits
2d8d46db
Commit
2d8d46db
authored
Jan 07, 2013
by
Jeroen Vreeken
Committed by
Michel Roelofs
Jan 07, 2013
Browse files
Ethercat updates
parent
f2fb2315
Changes
5
Hide whitespace changes
Inline
Side-by-side
ec/Makefile
View file @
2d8d46db
...
...
@@ -13,7 +13,8 @@ BLOCKSRCS= \
block_beckhoff_el3xxx.c
\
block_beckhoff_el4xxx.c
\
block_beckhoff_el5001.c
\
block_beckhoff_el5101.c
block_beckhoff_el5101.c
\
block_stoeber.c
BLOCKS
=
$(BLOCKSRCS:.c=.o)
SRCS
=
$(BLOCKSRCS)
$(LIBSRCS)
...
...
ec/block
0 → 100644
View file @
2d8d46db
This source diff could not be displayed because it is too large. You can
view the blob
instead.
ec/block_stoeber.c
0 → 100644
View file @
2d8d46db
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2012
Copyright Stichting C.A. Muller Radioastronomiestation, 2012
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include
<string.h>
#include
<stdio.h>
#include
"controller_block.h"
#include
"esc.h"
#include
"ec_stoeber.h"
struct
controller_block_private
{
struct
ec_stoeber
stbr
;
bool
enabled
;
float
output_position
;
float
output_speed
;
float
output_torque
;
bool
output_be1
;
bool
output_be2
;
bool
output_be3
;
bool
output_be4
;
bool
output_be5
;
float
output_ae1
;
float
*
input_speed
;
float
*
input_torque
;
bool
*
input_enable
;
};
static
void
calculate_tx
(
struct
controller_block
*
tx
)
{
struct
controller_block_private
*
private
=
tx
->
private
;
struct
ec_stoeber
*
stbr
=
&
private
->
stbr
;
bool
enable
;
bool
enabled
=
private
->
enabled
;
float
sp
;
float
torque
;
int
ret
;
enable
=
*
private
->
input_enable
;
if
(
enable
&&
!
enabled
)
{
esc_al_state_set
(
&
stbr
->
addr
,
ESC_AL_STATE_OPERATIONAL
);
enabled
=
true
;
}
*
stbr
->
tx_A180
=
enabled
;
*
stbr
->
tx_F210
=
0
;
*
stbr
->
tx_I210
=
htole16
(
le16toh
(
*
stbr
->
tx_I210
)
^
1
);
if
(
enabled
)
sp
=
*
private
->
input_speed
*
16384
.
0
/
RPM2RADS
(
3000
.
0
);
else
sp
=
0
;
if
(
sp
>
32767
)
sp
=
32767
;
if
(
sp
<
-
32768
)
sp
=
-
32768
;
*
stbr
->
tx_I215
=
htole16
((
int16_t
)
sp
);
torque
=
*
private
->
input_torque
/
stbr
->
standstill_torque
*
16384
;
*
stbr
->
tx_C230
=
htole16
((
int16_t
)
torque
);
ret
=
ec_tx_pdo
(
controller_samplenr
);
if
(
ret
&&
enabled
)
{
enabled
=
false
;
}
private
->
enabled
=
enabled
;
}
static
void
calculate_rx
(
struct
controller_block
*
rx
)
{
struct
controller_block_private
*
private
=
rx
->
private
;
struct
ec_stoeber
*
stbr
=
&
private
->
stbr
;
int
ret
;
float
speed
,
torque
,
position
,
ae1
;
bool
enabled
,
be1
,
be2
,
be3
,
be4
,
be5
;
uint16_t
E19
;
enabled
=
private
->
enabled
;
ret
=
ec_rx_pdo
(
controller_samplenr
);
if
(
ret
==
0
)
{
position
=
(
int32_t
)
le32toh
(
*
stbr
->
rx_I80
);
position
*=
2
.
0
*
M_PI
/
16384
;
speed
=
(
int32_t
)
le32toh
(
*
stbr
->
rx_E91
);
speed
*=
1
.
0
/
16384
;
speed
=
RPM2RADS
(
speed
);
torque
=
(
int16_t
)
le16toh
(
*
stbr
->
rx_E90
);
torque
*=
1
.
0
/
1200
;
ae1
=
(
int16_t
)
le16toh
(
*
stbr
->
rx_E10
);
ae1
*=
20
.
0
/
32767
.
0
;
E19
=
le16toh
(
*
stbr
->
rx_E19
);
be1
=
E19
&
0x01
;
be2
=
E19
&
0x02
;
be3
=
E19
&
0x04
;
be4
=
E19
&
0x08
;
be5
=
E19
&
0x10
;
}
else
{
enabled
=
false
;
speed
=
0
;
torque
=
0
;
position
=
0
;
be1
=
false
;
be2
=
false
;
be3
=
false
;
be4
=
false
;
be5
=
false
;
ae1
=
0
;
}
private
->
output_position
=
position
;
private
->
output_speed
=
speed
;
private
->
output_torque
=
torque
;
private
->
enabled
=
enabled
;
private
->
output_be1
=
be1
;
private
->
output_be2
=
be2
;
private
->
output_be3
=
be3
;
private
->
output_be4
=
be4
;
private
->
output_be5
=
be5
;
private
->
output_ae1
=
ae1
;
}
struct
controller_block
*
block_stoeber_create
(
char
*
name
,
va_list
ap
)
{
struct
controller_block
*
stoeber
;
struct
controller_block
*
stoeber_tx
;
struct
controller_block
*
stoeber_rx
;
struct
controller_block_private
*
private
;
int
ec_pos
;
int
i
;
double
max_torque
,
brake_resistor
,
brake_power
;
ec_pos
=
va_arg
(
ap
,
int
);
max_torque
=
va_arg
(
ap
,
double
);
brake_resistor
=
va_arg
(
ap
,
double
);
brake_power
=
va_arg
(
ap
,
double
);
stoeber
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
stoeber
)
goto
err_malloc
;
stoeber_tx
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
stoeber_tx
)
goto
err_malloc_tx
;
stoeber_rx
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
stoeber_rx
)
goto
err_malloc_rx
;
stoeber
->
type
=
"stoeber"
;
stoeber_tx
->
type
=
"stoeber_tx"
;
stoeber_rx
->
type
=
"stoeber_rx"
;
stoeber
->
name
=
strdup
(
name
);
if
(
!
stoeber
->
name
)
goto
err_name
;
stoeber_rx
->
name
=
malloc
(
strlen
(
name
)
+
4
);
if
(
!
stoeber_rx
->
name
)
goto
err_stoeber_rx_name
;
sprintf
(
stoeber_rx
->
name
,
"%s_rx"
,
name
);
stoeber_tx
->
name
=
malloc
(
strlen
(
name
)
+
4
);
if
(
!
stoeber_tx
->
name
)
goto
err_stoeber_tx_name
;
sprintf
(
stoeber_tx
->
name
,
"%s_tx"
,
name
);
private
=
calloc
(
1
,
sizeof
(
struct
controller_block_private
));
if
(
!
private
)
goto
err_private
;
stoeber
->
private
=
private
;
stoeber_rx
->
private
=
private
;
stoeber_tx
->
private
=
private
;
ec_addr_set_auto_inc_nr
(
&
private
->
stbr
.
addr
,
ec_pos
);
private
->
stbr
.
max_speed
=
RPM2RADS
(
3000
);
private
->
stbr
.
max_accel
=
3000
*
360
/
60
*
10
;
private
->
stbr
.
max_torque
=
max_torque
;
private
->
stbr
.
brake_resistor
=
brake_resistor
;
private
->
stbr
.
brake_power
=
brake_power
;
if
(
ec_stoeber_init
(
&
private
->
stbr
))
goto
err_stoeber_init
;
stoeber_rx
->
outputs
=
10
;
stoeber_rx
->
output
=
calloc
(
stoeber_rx
->
outputs
,
sizeof
(
struct
controller_block_outterm
));
if
(
!
stoeber_rx
->
output
)
goto
err_rx_output
;
stoeber_rx
->
output
[
0
].
name
=
"position"
;
stoeber_rx
->
output
[
0
].
value
.
f
=
&
private
->
output_position
;
stoeber_rx
->
output
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
stoeber_rx
->
output
[
0
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
0
].
sourceterm
=
&
stoeber_rx
->
output
[
0
];
stoeber_rx
->
output
[
1
].
name
=
"speed"
;
stoeber_rx
->
output
[
1
].
value
.
f
=
&
private
->
output_speed
;
stoeber_rx
->
output
[
1
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
stoeber_rx
->
output
[
1
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
1
].
sourceterm
=
&
stoeber_rx
->
output
[
1
];
stoeber_rx
->
output
[
2
].
name
=
"torque"
;
stoeber_rx
->
output
[
2
].
value
.
f
=
&
private
->
output_torque
;
stoeber_rx
->
output
[
2
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
stoeber_rx
->
output
[
2
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
2
].
sourceterm
=
&
stoeber_rx
->
output
[
2
];
stoeber_rx
->
output
[
3
].
name
=
"enabled"
;
stoeber_rx
->
output
[
3
].
value
.
b
=
&
private
->
enabled
;
stoeber_rx
->
output
[
3
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
stoeber_rx
->
output
[
3
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
3
].
sourceterm
=
&
stoeber_rx
->
output
[
3
];
stoeber_rx
->
output
[
4
].
name
=
"be1"
;
stoeber_rx
->
output
[
4
].
value
.
b
=
&
private
->
output_be1
;
stoeber_rx
->
output
[
4
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
stoeber_rx
->
output
[
4
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
4
].
sourceterm
=
&
stoeber_rx
->
output
[
4
];
stoeber_rx
->
output
[
5
].
name
=
"be2"
;
stoeber_rx
->
output
[
5
].
value
.
b
=
&
private
->
output_be2
;
stoeber_rx
->
output
[
5
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
stoeber_rx
->
output
[
5
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
5
].
sourceterm
=
&
stoeber_rx
->
output
[
5
];
stoeber_rx
->
output
[
6
].
name
=
"be3"
;
stoeber_rx
->
output
[
6
].
value
.
b
=
&
private
->
output_be3
;
stoeber_rx
->
output
[
6
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
stoeber_rx
->
output
[
6
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
6
].
sourceterm
=
&
stoeber_rx
->
output
[
6
];
stoeber_rx
->
output
[
7
].
name
=
"be4"
;
stoeber_rx
->
output
[
7
].
value
.
b
=
&
private
->
output_be4
;
stoeber_rx
->
output
[
7
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
stoeber_rx
->
output
[
7
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
7
].
sourceterm
=
&
stoeber_rx
->
output
[
7
];
stoeber_rx
->
output
[
8
].
name
=
"be5"
;
stoeber_rx
->
output
[
8
].
value
.
b
=
&
private
->
output_be5
;
stoeber_rx
->
output
[
8
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
stoeber_rx
->
output
[
8
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
8
].
sourceterm
=
&
stoeber_rx
->
output
[
8
];
stoeber_rx
->
output
[
9
].
name
=
"ae1"
;
stoeber_rx
->
output
[
9
].
value
.
f
=
&
private
->
output_ae1
;
stoeber_rx
->
output
[
9
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
stoeber_rx
->
output
[
9
].
source
=
stoeber_rx
;
stoeber_rx
->
output
[
9
].
sourceterm
=
&
stoeber_rx
->
output
[
9
];
/* stoeber also exports these outputs we will re-use the same struct
this is possible as both source and sourceterm point to the
stoeber_tx structs alread. */
stoeber
->
outputs
=
stoeber_rx
->
outputs
;
stoeber
->
output
=
stoeber_rx
->
output
;
stoeber_tx
->
outputs
=
0
;
stoeber_tx
->
output
=
NULL
;
stoeber_tx
->
inputs
=
3
;
stoeber_tx
->
input
=
calloc
(
stoeber_tx
->
inputs
,
sizeof
(
struct
controller_block_interm
));
if
(
!
stoeber_tx
->
input
)
goto
err_tx_input
;
stoeber_tx
->
input
[
0
].
name
=
"speed"
;
stoeber_tx
->
input
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
stoeber_tx
->
input
[
0
].
value
.
f
=
&
private
->
input_speed
;
stoeber_tx
->
input
[
0
].
ghostof
=
NULL
;
stoeber_tx
->
input
[
1
].
name
=
"torque"
;
stoeber_tx
->
input
[
1
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
stoeber_tx
->
input
[
1
].
value
.
f
=
&
private
->
input_torque
;
stoeber_tx
->
input
[
1
].
ghostof
=
NULL
;
stoeber_tx
->
input
[
2
].
name
=
"enable"
;
stoeber_tx
->
input
[
2
].
type
=
CONTROLLER_BLOCK_TERM_BOOL
;
stoeber_tx
->
input
[
2
].
value
.
b
=
&
private
->
input_enable
;
stoeber_tx
->
input
[
2
].
ghostof
=
NULL
;
stoeber
->
inputs
=
stoeber_tx
->
inputs
;
stoeber
->
input
=
calloc
(
stoeber
->
inputs
,
sizeof
(
struct
controller_block_interm
));
if
(
!
stoeber
->
input
)
goto
err_input
;
for
(
i
=
0
;
i
<
stoeber
->
inputs
;
i
++
)
{
stoeber
->
input
[
i
].
name
=
stoeber_tx
->
input
[
i
].
name
;
stoeber
->
input
[
i
].
type
=
stoeber_tx
->
input
[
i
].
type
;
stoeber
->
input
[
i
].
value
=
stoeber_tx
->
input
[
i
].
value
;
stoeber
->
input
[
i
].
ghostof
=
&
stoeber_tx
->
input
[
i
];
}
stoeber
->
calculate
=
NULL
;
stoeber_tx
->
calculate
=
calculate_tx
;
stoeber_rx
->
calculate
=
calculate_rx
;
controller_block_add
(
stoeber_tx
);
controller_block_add
(
stoeber_rx
);
controller_block_add
(
stoeber
);
return
stoeber
;
err_input:
free
(
stoeber_tx
->
input
);
err_tx_input:
free
(
stoeber
->
output
);
err_rx_output:
/* no way to un-init the device? */
err_stoeber_init:
free
(
private
);
err_private:
free
(
stoeber_tx
->
name
);
err_stoeber_tx_name:
free
(
stoeber_rx
->
name
);
err_stoeber_rx_name:
free
(
stoeber
->
name
);
err_name:
free
(
stoeber_rx
);
err_malloc_rx:
free
(
stoeber_tx
);
err_malloc_tx:
free
(
stoeber
);
err_malloc:
return
NULL
;
}
ec/ec.c
View file @
2d8d46db
...
...
@@ -123,6 +123,19 @@ int ec_rx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len)
struct
ec_dgram_hdr
*
prev
;
uint16_t
*
wkc
;
for
(
prev
=
(
struct
ec_dgram_hdr
*
)(
rx_pdo
.
tx_frame
+
sizeof
(
struct
ec_frame_hdr
));
(
size_t
)
prev
<
(
size_t
)
rx_pdo
.
tx_frame
+
rx_pdo
.
flen
;
prev
=
((
void
*
)
prev
)
+
EC_DGRAM_HDR_LEN
(
prev
)
+
sizeof
(
struct
ec_dgram_hdr
)
+
2
)
{
if
(
addr
->
type
==
EC_DGRAM_ADDR_AUTO_INC
&&
prev
->
cmd
==
EC_DGRAM_HDR_CMD_APRD
&&
prev
->
addr
.
position
.
pos
==
addr
->
addr
.
position
.
pos
&&
prev
->
addr
.
position
.
off
==
addr
->
addr
.
position
.
off
)
{
printf
(
"Double registration for rx PDO"
);
*
data
=
(
unsigned
char
*
)
prev
+
sizeof
(
struct
ec_dgram_hdr
);
return
0
;
}
}
dgram
=
(
struct
ec_dgram_hdr
*
)(
rx_pdo
.
tx_frame
+
rx_pdo
.
flen
);
*
data
=
rx_pdo
.
rx_frame
+
rx_pdo
.
flen
+
sizeof
(
struct
ec_dgram_hdr
);
...
...
@@ -170,6 +183,19 @@ int ec_tx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len)
struct
ec_dgram_hdr
*
dgram
;
struct
ec_dgram_hdr
*
prev
;
uint16_t
*
wkc
;
for
(
prev
=
(
struct
ec_dgram_hdr
*
)(
tx_pdo
->
tx_frame
+
sizeof
(
struct
ec_frame_hdr
));
(
size_t
)
prev
<
(
size_t
)
tx_pdo
->
tx_frame
+
tx_pdo
->
flen
;
prev
=
((
void
*
)
prev
)
+
EC_DGRAM_HDR_LEN
(
prev
)
+
sizeof
(
struct
ec_dgram_hdr
)
+
2
)
{
if
(
addr
->
type
==
EC_DGRAM_ADDR_AUTO_INC
&&
prev
->
cmd
==
EC_DGRAM_HDR_CMD_APWR
&&
prev
->
addr
.
position
.
pos
==
addr
->
addr
.
position
.
pos
&&
prev
->
addr
.
position
.
off
==
addr
->
addr
.
position
.
off
)
{
printf
(
"Double registration for tx PDO"
);
*
data
=
(
unsigned
char
*
)
prev
+
sizeof
(
struct
ec_dgram_hdr
);
return
0
;
}
}
dgram
=
(
struct
ec_dgram_hdr
*
)(
tx_pdo
->
tx_frame
+
tx_pdo
->
flen
);
*
data
=
tx_pdo
->
tx_frame
+
tx_pdo
->
flen
+
sizeof
(
struct
ec_dgram_hdr
);
...
...
ec/ec_test.c
View file @
2d8d46db
...
...
@@ -399,7 +399,7 @@ while(1) {
// }
// if (le32toh(*(uint32_t*)&buffer[1]) != prev) {
// prev = le32toh(*(uint32_t*)&buffer[1]);
fprintf
(
stderr
,
"%d %d
\n
"
,
speed
,
le32toh
(
*
(
uint32_t
*
)
&
buffer
[
1
]));
//
fprintf(stderr, "%d %d\n", speed, le32toh(*(uint32_t*)&buffer[1]));
// i++;
// }
// printf(" %d\n", ret);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment