Skip to content
GitLab
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
7a2c1190
Commit
7a2c1190
authored
Dec 08, 2013
by
Jeroen Vreeken
Browse files
ethercat status
parent
5edaf3bc
Changes
9
Hide whitespace changes
Inline
Side-by-side
controller/block/block_servo_state.c
View file @
7a2c1190
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2011
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2011
, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2011
This program is free software: you can redistribute it and/or modify
...
...
@@ -71,56 +71,56 @@ struct controller_block_private {
bool
override
;
};
static
void
calculate
(
struct
controller_block
*
servo_state
)
static
void
servo_state_
calculate
(
struct
controller_block
*
servo_state
)
{
bool
safe
=
*
servo_state
->
private
->
safe
;
struct
controller_block_private
*
priv
=
servo_state
->
private
;
bool
safe
=
*
priv
->
safe
;
bool
unsafe_disable
=
false
;
if
(
servo_state
->
priv
ate
->
emergency
)
{
servo_state
->
priv
ate
->
emergency
=
false
;
if
(
priv
->
emergency
)
{
priv
->
emergency
=
false
;
safe
=
false
;
log_send
(
LOG_T_ERROR
,
"%s: External emergency triggered"
,
servo_state
->
name
);
}
if
(
!
safe
)
{
servo_state
->
private
->
enable_param
=
false
;
if
(
priv
->
enable_param
)
unsafe_disable
=
true
;
priv
->
enable_param
=
false
;
}
if
(
servo_state
->
priv
ate
->
override
)
{
servo_state
->
priv
ate
->
out_x
=
*
servo_state
->
priv
ate
->
spg_x
;
servo_state
->
priv
ate
->
out_v
=
*
servo_state
->
priv
ate
->
spg_v
;
servo_state
->
priv
ate
->
out_a
=
*
servo_state
->
priv
ate
->
spg_a
;
servo_state
->
priv
ate
->
enable
=
true
;
servo_state
->
priv
ate
->
reset
=
false
;
if
(
priv
->
override
)
{
priv
->
out_x
=
*
priv
->
spg_x
;
priv
->
out_v
=
*
priv
->
spg_v
;
priv
->
out_a
=
*
priv
->
spg_a
;
priv
->
enable
=
true
;
priv
->
reset
=
false
;
return
;
}
switch
(
servo_state
->
priv
ate
->
state
)
{
switch
(
priv
->
state
)
{
case
SERVO_STATE_ENABLED
:
servo_state
->
private
->
out_x
=
*
servo_state
->
private
->
spg_x
;
servo_state
->
private
->
out_v
=
*
servo_state
->
private
->
spg_v
;
servo_state
->
private
->
out_a
=
*
servo_state
->
private
->
spg_a
;
priv
->
out_x
=
*
priv
->
spg_x
;
priv
->
out_v
=
*
priv
->
spg_v
;
priv
->
out_a
=
*
priv
->
spg_a
;
if
(
!
safe
)
{
servo_state
->
private
->
enable
=
false
;
servo_state
->
private
->
reset
=
true
;
servo_state
->
private
->
state
=
SERVO_STATE_DISABLED
;
priv
->
enable
=
false
;
priv
->
reset
=
true
;
priv
->
state
=
SERVO_STATE_DISABLED
;
log_send
(
LOG_T_ERROR
,
"%s: Unsafe, going to state DISABLED"
,
servo_state
->
name
);
break
;
}
servo_state
->
priv
ate
->
enable
=
true
;
servo_state
->
priv
ate
->
reset
=
false
;
priv
->
enable
=
true
;
priv
->
reset
=
false
;
if
(
servo_state
->
priv
ate
->
enable_param
)
{
if
(
priv
->
enable_param
)
{
break
;
}
else
{
servo_state
->
priv
ate
->
state
=
priv
->
state
=
SERVO_STATE_DISABLING
;
log_send
(
LOG_T_INFO
,
"%s: Going to state DISABLING"
,
...
...
@@ -128,16 +128,16 @@ static void calculate(struct controller_block *servo_state)
}
/* Fallthrough if disabling */
case
SERVO_STATE_DISABLING
:
{
float
out_x
=
servo_state
->
priv
ate
->
out_x
;
float
out_v
=
servo_state
->
priv
ate
->
out_v
;
float
out_x
=
priv
->
out_x
;
float
out_v
=
priv
->
out_v
;
float
out_a
=
0
.
0
;
float
t
=
controller_sample_period
();
if
(
!
safe
)
{
servo_state
->
priv
ate
->
enable
=
false
;
servo_state
->
priv
ate
->
reset
=
true
;
servo_state
->
priv
ate
->
state
=
priv
->
enable
=
false
;
priv
->
reset
=
true
;
priv
->
state
=
SERVO_STATE_DISABLED
;
log_send
(
LOG_T_ERROR
,
"%s: Unsafe, going to state DISABLED"
,
...
...
@@ -146,14 +146,14 @@ static void calculate(struct controller_block *servo_state)
}
if
(
out_v
>
0
.
0
)
{
out_a
=
-
servo_state
->
priv
ate
->
max_a
;
out_a
=
-
priv
->
max_a
;
out_v
+=
out_a
*
t
;
if
(
out_v
<
0
.
0
)
{
out_a
+=
out_v
;
out_v
=
0
.
0
;
}
}
else
if
(
out_v
<
0
.
0
)
{
out_a
=
servo_state
->
priv
ate
->
max_a
;
out_a
=
priv
->
max_a
;
out_v
+=
out_a
*
t
;
if
(
out_v
>
0
.
0
)
{
out_a
-=
out_v
;
...
...
@@ -162,7 +162,7 @@ static void calculate(struct controller_block *servo_state)
}
if
(
out_v
==
0
.
0
)
{
servo_state
->
priv
ate
->
state
=
priv
->
state
=
SERVO_STATE_DISABLED
;
log_send
(
LOG_T_INFO
,
"%s: Going to state DISABLED"
,
...
...
@@ -171,36 +171,39 @@ static void calculate(struct controller_block *servo_state)
out_x
+=
out_v
*
t
;
if
(
out_x
>
servo_state
->
priv
ate
->
max_x
)
out_x
=
servo_state
->
priv
ate
->
max_x
;
if
(
out_x
<
servo_state
->
priv
ate
->
min_x
)
out_x
=
servo_state
->
priv
ate
->
min_x
;
if
(
out_x
>
priv
->
max_x
)
out_x
=
priv
->
max_x
;
if
(
out_x
<
priv
->
min_x
)
out_x
=
priv
->
min_x
;
servo_state
->
priv
ate
->
out_x
=
out_x
;
servo_state
->
priv
ate
->
out_v
=
out_v
;
servo_state
->
priv
ate
->
out_a
=
out_a
;
priv
->
out_x
=
out_x
;
priv
->
out_v
=
out_v
;
priv
->
out_a
=
out_a
;
servo_state
->
priv
ate
->
enable
=
true
;
servo_state
->
priv
ate
->
reset
=
true
;
priv
->
enable
=
true
;
priv
->
reset
=
true
;
break
;
}
case
SERVO_STATE_DISABLED
:
servo_state
->
private
->
out_x
=
*
servo_state
->
private
->
spg_x
;
servo_state
->
private
->
out_v
=
0
.
0
;
servo_state
->
private
->
out_a
=
0
.
0
;
priv
->
out_x
=
*
priv
->
spg_x
;
priv
->
out_v
=
0
.
0
;
priv
->
out_a
=
0
.
0
;
servo_state
->
priv
ate
->
enable
=
false
;
servo_state
->
priv
ate
->
reset
=
true
;
priv
->
enable
=
false
;
priv
->
reset
=
true
;
if
(
servo_state
->
private
->
enable_param
)
{
servo_state
->
private
->
state
=
SERVO_STATE_ENABLED
;
if
(
priv
->
enable_param
)
{
priv
->
state
=
SERVO_STATE_ENABLED
;
log_send
(
LOG_T_INFO
,
"%s: Going to state ENABLED"
,
servo_state
->
name
);
}
if
(
unsafe_disable
)
{
log_send
(
LOG_T_WARNING
,
"%s: Unsafe, enabling is currently not allowed"
,
servo_state
->
name
);
}
break
;
}
}
...
...
@@ -303,19 +306,9 @@ struct controller_block * block_servo_state_create(char *name)
{
struct
controller_block
*
servo_state
;
servo_state
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
servo_state
)
if
(
!
(
servo_state
=
controller_block_alloc
(
"servo_state"
,
name
,
sizeof
(
struct
controller_block_private
))))
return
NULL
;
servo_state
->
type
=
"servo_state"
;
servo_state
->
name
=
malloc
(
strlen
(
name
)
+
1
);
if
(
!
servo_state
->
name
)
goto
err_servo_state
;
strcpy
(
servo_state
->
name
,
name
);
servo_state
->
private
=
malloc
(
sizeof
(
struct
controller_block_private
));
if
(
!
servo_state
->
private
)
goto
err_name
;
servo_state
->
private
->
out_x
=
0
.
0
;
servo_state
->
private
->
out_v
=
0
.
0
;
servo_state
->
private
->
out_a
=
0
.
0
;
...
...
@@ -327,15 +320,15 @@ struct controller_block * block_servo_state_create(char *name)
servo_state
->
private
->
override
=
false
;
if
(
controller_block_interm_list_init
(
servo_state
,
interms
))
goto
err_
private
;
goto
err_
block
;
if
(
controller_block_outterm_list_init
(
servo_state
,
outterms
))
goto
err_
input
;
goto
err_
block
;
servo_state
->
calculate
=
calculate
;
servo_state
->
calculate
=
servo_state_
calculate
;
if
(
controller_block_param_list_init
(
servo_state
,
params
))
goto
err_
output
;
goto
err_
block
;
servo_state
->
param_get
=
param_get
;
servo_state
->
param_set
=
param_set
;
...
...
@@ -343,15 +336,7 @@ struct controller_block * block_servo_state_create(char *name)
controller_block_add
(
servo_state
);
return
servo_state
;
err_output:
free
(
servo_state
->
output
);
err_input:
free
(
servo_state
->
input
);
err_private:
free
(
servo_state
->
private
);
err_name:
free
(
servo_state
->
name
);
err_servo_state:
free
(
servo_state
);
err_block:
controller_block_free
(
servo_state
);
return
NULL
;
}
controller/dt_ctrl.ctrl
View file @
7a2c1190
...
...
@@ -14,6 +14,7 @@ frequency 250
# dt_ctrl_el_sim.ctrl Elevation with simulated network
# dt_ctrl_az.ctrl Azimuth with real servo drives
# dt_ctrl_az_sim.ctrl Azimuth with simulated network
# dt_ctrl_ec_sim.ctrl Ethercat simulation block
#
# Uncomment either the real network, or the sim network.
# But never both!
...
...
@@ -24,10 +25,12 @@ include "dt_ctrl_el_sim.ctrl"
#include "dt_ctrl_az.ctrl"
include "dt_ctrl_az_sim.ctrl"
include "dt_ctrl_ec_sim.ctrl"
blocks {
{ "setpoint_generator", "azimuth_spg", "Azimuth_Setpoint", "rad" }
{ "servo_state", "azimuth_servo_state" }
{ "and2", "azimuth_safe_and" }
{ "subtract", "azimuth_setpoint_error" }
{ "subtract", "azimuth_error" }
{ "pid_aw", "azimuth_pid" }
...
...
@@ -45,6 +48,7 @@ blocks {
{ "matrix_2x2", "elevation_input_matrix" }
{ "setpoint_generator", "elevation_spg", "Elevation_Setpoint", "rad" }
{ "servo_state", "elevation_servo_state" }
{ "and2", "elevation_safe_and" }
{ "setpoint_generator", "elevation_torsion_spg", "Elevation_Torsion_Setpoint", "rad" }
{ "subtract", "elevation_setpoint_error" }
{ "subtract", "elevation_error" }
...
...
@@ -81,7 +85,9 @@ links {
{ "azimuth_spg", "x", "azimuth_servo_state", "spg_x" , true }
{ "azimuth_spg", "v", "azimuth_servo_state", "spg_v" , true }
{ "azimuth_spg", "a", "azimuth_servo_state", "spg_a" , true }
{ "azimuth_safety", "safe_out", "azimuth_servo_state", "safe" , false }
{ "azimuth_safety", "safe_out", "azimuth_safe_and", "a" , false }
{ "ethercat", "pdo_data", "azimuth_safe_and", "b" , true }
{ "azimuth_safe_and", "q", "azimuth_servo_state", "safe" , true }
{ "azimuth_servo_state", "out_x", "azimuth_error", "positive" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_setpoint_error", "negative" , true }
{ "azimuth_spg", "setpoint", "azimuth_setpoint_error", "positive" , true }
...
...
@@ -111,7 +117,9 @@ links {
{ "elevation_spg", "x", "elevation_servo_state", "spg_x" , true }
{ "elevation_spg", "v", "elevation_servo_state", "spg_v" , true }
{ "elevation_spg", "a", "elevation_servo_state", "spg_a" , true }
{ "elevation_safety", "safe_out", "elevation_servo_state", "safe" , false }
{ "elevation_safety", "safe_out", "elevation_safe_and", "a" , false }
{ "ethercat", "pdo_data", "elevation_safe_and", "b" , true }
{ "elevation_safe_and", "q", "elevation_servo_state", "safe" , true }
{ "elevation_servo_state", "out_x", "elevation_error", "positive" , true }
{ "elevation_servo_state", "out_v", "elevation_speed_ff", "in0" , true }
{ "dt_el_r", "position", "elevation_position_offset_r_sum", "in0" , true }
...
...
controller/dt_ctrl_ec_sim.ctrl
0 → 100644
View file @
7a2c1190
blocks {
{ "ec_sim", "ethercat" }
}
controller/ec/Makefile
View file @
7a2c1190
...
...
@@ -12,7 +12,7 @@ endif
LIBSRCS
=
ec.c
\
esc.c esc_coe.c esc_esi.c esc_mailbox.c esc_device.c esc_watchdog.c
\
esc_dc.c canopen.c
block_ec.c
esc_sm.c esc_fmmu.c
esc_dc.c canopen.c esc_sm.c esc_fmmu.c
ifeq
($(OS), FreeBSD)
LIBSRCS
+=
eth_bsd.c
...
...
@@ -31,7 +31,9 @@ BLOCKSRCS= \
block_beckhoff_el5001.c
\
block_beckhoff_el5101.c
\
block_beckhoff_el7031.c
\
block_stoeber.c
block_stoeber.c
\
block_ec.c
\
block_ec_sim.c
# block_beckhoff_ax5xxx.c \
...
...
controller/ec/block_ec.c
View file @
7a2c1190
...
...
@@ -36,8 +36,6 @@ struct controller_block_private {
static
void
calculate
(
struct
controller_block
*
ec
)
{
if
(
ec_rx_pdo
())
{
ec
->
private
->
pdo_data
=
false
;
}
else
{
ec
->
private
->
pdo_data
=
true
;
}
}
...
...
@@ -48,7 +46,7 @@ static void calculate_tx(struct controller_block *ec)
ec_datagram_sample
();
}
static
void
param_set
(
struct
controller_block
*
spg
,
int
param
,
va_list
val
)
static
void
param_set
(
struct
controller_block
*
ec
,
int
param
,
va_list
val
)
{
int
enabled
=
va_arg
(
val
,
int
);
...
...
@@ -87,26 +85,11 @@ struct controller_block * block_ec_create(char *name, va_list ap)
nr_slaves
=
va_arg
(
ap
,
int
);
single_cycle
=
va_arg
(
ap
,
int
);
ret
=
ec_init
(
ifname
,
single_cycle
);
ret
=
ec_init
(
ifname
,
nr_slaves
,
single_cycle
);
if
(
ret
!=
0
)
return
NULL
;
while
((
ret
=
ec_slave_count
())
!=
nr_slaves
)
{
if
(
ret
==
-
1
)
{
log_send
(
LOG_T_ERROR
,
"Error counting slaves"
);
}
else
if
(
ret
<
nr_slaves
)
{
log_send
(
LOG_T_ERROR
,
"Found only %d slaves, waiting for %d."
,
ret
,
nr_slaves
);
}
else
{
log_send
(
LOG_T_WARNING
,
"More slaves found (%d) than expected (%d)."
,
ret
,
nr_slaves
);
break
;
}
sleep
(
10
);
}
sprintf
(
name_tx
,
"%s_pdo_tx"
,
name
);
ec
=
controller_block_alloc
(
"ec"
,
name
,
...
...
controller/ec/block_ec_sim.c
0 → 100644
View file @
7a2c1190
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2013
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
<stdio.h>
#include
<stdlib.h>
#include
<string.h>
#include
<controller/controller_block.h>
#include
"log.h"
struct
controller_block
*
ec_block
;
struct
controller_block_private
{
bool
pdo_data
;
};
static
void
param_set
(
struct
controller_block
*
ec
,
int
param
,
va_list
val
)
{
int
enabled
=
va_arg
(
val
,
int
);
ec
->
private
->
pdo_data
=
enabled
;
}
static
struct
controller_block_param_list
params
[]
=
{
{
"pdo_enabled"
,
true
},
{
NULL
},
};
static
struct
controller_block_outterm_list
outterms
[]
=
{
{
"pdo_data"
,
CONTROLLER_BLOCK_TERM_BOOL
,
offsetof
(
struct
controller_block_private
,
pdo_data
)
},
{
NULL
},
};
struct
controller_block
*
block_ec_sim_create
(
char
*
name
,
va_list
ap
)
{
struct
controller_block
*
ec
;
/* For now we only support a single ethercat interface,
but we allow multiple inits
*/
if
(
ec_block
)
{
return
ec_block
;
}
ec
=
controller_block_alloc
(
"ec_sim"
,
name
,
sizeof
(
struct
controller_block_private
));
if
(
!
ec
)
goto
err
;
ec
->
private
->
pdo_data
=
true
;
if
(
controller_block_outterm_list_init
(
ec
,
outterms
))
goto
err_ec
;
if
(
controller_block_param_list_init
(
ec
,
params
))
goto
err_ec
;
ec
->
param_get
=
NULL
;
ec
->
param_set
=
param_set
;
controller_block_add
(
ec
);
ec_block
=
ec
;
return
ec
;
err_ec:
controller_block_free
(
ec
);
err:
return
NULL
;
}
controller/ec/ec.c
View file @
7a2c1190
...
...
@@ -850,10 +850,11 @@ bool ec_initialized(void)
return
init_done
;
}
int
ec_init
(
char
*
ifname
,
bool
single_cycle
)
int
ec_init
(
char
*
ifname
,
int
nr_slaves
,
bool
single_cycle
)
{
struct
timeval
timeout
;
double
timeout_sec
;
int
ret
;
if
(
init_done
)
return
0
;
...
...
@@ -881,10 +882,22 @@ int ec_init(char *ifname, bool single_cycle)
eth_timeout_set
(
ec_sock
,
&
timeout
,
&
timeout
);
if
(
ec_slave_count
()
>=
0
)
{
log_send
(
LOG_T_DEBUG
,
"Found ethercat slaves"
);
}
else
{
log_send
(
LOG_T_WARNING
,
"No ethercat slaves found"
);
while
((
ret
=
ec_slave_count
())
!=
nr_slaves
)
{
if
(
ret
==
-
1
)
{
log_send
(
LOG_T_ERROR
,
"Error counting ethercat slaves"
);
}
else
if
(
nr_slaves
>
0
)
{
if
(
ret
<
nr_slaves
)
{
log_send
(
LOG_T_ERROR
,
"Found only %d ethercat slaves, waiting for %d."
,
ret
,
nr_slaves
);
}
else
{
log_send
(
LOG_T_WARNING
,
"More ethercat slaves found (%d) than expected (%d)."
,
ret
,
nr_slaves
);
break
;
}
}
sleep
(
10
);
}
log_send
(
LOG_T_DEBUG
,
"Successfull init, %d slaves found."
,
ec_slave_count
());
...
...
controller/ec/ec.h
View file @
7a2c1190
...
...
@@ -36,7 +36,7 @@
#include
"canopen.h"
int
ec_init
(
char
*
ifname
,
bool
single_cycle
);
int
ec_init
(
char
*
ifname
,
int
nr_slaves
,
bool
single_cycle
);
bool
ec_initialized
(
void
);
int
ec_slave_count
(
void
);
...
...
controller/ec/ec_enum.c
View file @
7a2c1190
...
...
@@ -300,7 +300,7 @@ int main(int argc, char **argv)
return
-
1
;
}
if
(
ec_init
(
argv
[
1
],
0
))
{
if
(
ec_init
(
argv
[
1
],
0
,
0
))
{
printf
(
"EtherCat init failed
\n
"
);
return
-
1
;
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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