Browse Source

After test at corso

jeroen
Jeroen Vreeken 3 years ago
parent
commit
0e311aef0f
36 changed files with 1550 additions and 96 deletions
  1. +2
    -0
      README
  2. +1
    -1
      buildflags.mk.in
  3. +2
    -1
      common/command/command.h
  4. +2
    -2
      configure.ac
  5. +218
    -0
      controller/atsamx70/block_atsamx70_gmac.c
  6. +2
    -0
      controller/atsamx70/block_atsamx70_pio_in.c
  7. +1
    -0
      controller/atsamx70/block_atsamx70_pio_out.c
  8. +12
    -7
      controller/atsamx70/block_atsamx70_tc.c
  9. +1
    -0
      controller/atsamx70/build.mk
  10. +9
    -1
      controller/block/block_gain_ratio_abs.c
  11. +11
    -4
      controller/block/block_i2t.c
  12. +5
    -3
      controller/block/block_motor_model_dc_v.c
  13. +10
    -4
      controller/block/block_motor_model_dc_v.test.ctrl
  14. +49
    -20
      controller/block/block_setpoint_generator_3d.c
  15. +11
    -0
      controller/block/block_setpoint_generator_3d.test.ctrl
  16. +6
    -10
      controller/block/build.mk
  17. +1
    -0
      controller/build.mk
  18. +1
    -1
      controller/controller/build.mk
  19. +9
    -1
      controller/controller/controller_command.c
  20. +1
    -1
      controller/controller/controller_load.c
  21. +10
    -9
      controller/controller/controller_sample.c
  22. +125
    -31
      controller/ctrl_embedded.ctrl
  23. +6
    -0
      controller/dt_ctrl.ctrl
  24. +5
    -0
      controller/packet/Makefile
  25. +223
    -0
      controller/packet/block_packet_eth.c
  26. +94
    -0
      controller/packet/block_packet_in_bool_byte.c
  27. +99
    -0
      controller/packet/block_packet_in_float_be.c
  28. +97
    -0
      controller/packet/block_packet_out_bool_byte.c
  29. +100
    -0
      controller/packet/block_packet_out_float_be.c
  30. +130
    -0
      controller/packet/block_packet_sim.c
  31. +38
    -0
      controller/packet/build.mk
  32. +124
    -0
      controller/packet/packet.c
  33. +50
    -0
      controller/packet/packet.h
  34. +38
    -0
      controller/packet/packet.test.ctrl
  35. +32
    -0
      controller/packet/packet_bus.h
  36. +25
    -0
      controller/packet/packet_eth.ctrl

+ 2
- 0
README View File

@ -21,3 +21,5 @@ Building the software
Just run "make". To see more targets, run "make help". You may also run make in
the individual directories.
./configure --host arm-none-eabihf CFLAGS=-specs /usr/local/arm-none-eabihf/lib/atsamx70x21_flash.specs --disable-am335x --enable-embedded --disable-test --disable-ethercat --disable-vesp

+ 1
- 1
buildflags.mk.in View File

@ -21,5 +21,5 @@ BUILD_FILEIO=@BUILD_FILEIO@
BUILD_EMBEDDED=@BUILD_EMBEDDED@
BUILD_SYSTICK=@BUILD_SYSTICK@
BUILD_CFLAGS=@CFLAGS@ @CFLAGS_PTHREAD@ @CFLAGS_TCP@ @CFLAGS_FILEIO@ @CFLAGS_EMBEDDED@ @CFLAGS_SYSTICK@
BUILD_CFLAGS=@CFLAGS@ @CFLAGS_PTHREAD@ @CFLAGS_TCP@ @CFLAGS_SEMAPHORE@ @CFLAGS_FILEIO@ @CFLAGS_EMBEDDED@ @CFLAGS_SYSTICK@

+ 2
- 1
common/command/command.h View File

@ -36,7 +36,8 @@ enum command_ptype {
COMMAND_PTYPE_SPEED = 5, /* Simple direct 1st derivative setpoint */
COMMAND_PTYPE_SETPOINT_TIME = 6, /* Setpoint to be reached @ time */
COMMAND_PTYPE_SETPOINT_TRACK = 7, /* Use setpoint from track input */
COMMAND_PTYPE_SPEED_TRACK = 8, /* Use 1st derivative from track input */
COMMAND_PTYPE_MAX, /* Always the last entry! */
};


+ 2
- 2
configure.ac View File

@ -193,8 +193,8 @@ AS_IF([test "$HAVE_TCP" = "1"],
[AC_SUBST(BUILD_TCP,[""])])
AS_IF([test "$HAVE_SEMAPHORE" = "1"],
[AC_SUBST(CFLAGS_TCP,["-DHAVE_SEMAPHORE"])],
[AC_SUBST(CFLAGS_TCP,[""])])
[AC_SUBST(CFLAGS_SEMAPHORE,["-DHAVE_SEMAPHORE"])],
[AC_SUBST(CFLAGS_SEMAPHORE,[""])])
AS_IF([test "$HAVE_HEADER_LINUX_JOYSTICK" = "1"],
[AC_SUBST(BUILD_LINUX_JOYSTICK,["yes"])],


+ 218
- 0
controller/atsamx70/block_atsamx70_gmac.c View File

@ -0,0 +1,218 @@
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2018
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 <controller/controller_bus.h>
#include <controller/controller_mem.h>
#include <controller/controller_sample.h>
#include <log/log.h>
#include <packet/packet.h>
#include <packet/packet_bus.h>
#include <samx70.h>
#include <pio/samx70.h>
#include <ioport.h>
#include <gmac.h>
#define TX_Q_SIZE 4
struct controller_block_private {
struct controller_bus *bus;
void *rx_packet;
void *tx_packet;
size_t rx_size;
size_t tx_size;
struct gmac_state gmac;
struct gmac_tx_buffer_descriptor tx_q[TX_Q_SIZE];
char tx_buffer[TX_Q_SIZE][1600];
};
static void atsamx70_gmac_tx_calculate(struct controller_block *packet_tx)
{
struct controller_block_private *priv = packet_tx->private;
((char*)priv->tx_packet)[0]++;
gmac_tx(&priv->gmac, priv->tx_packet, priv->tx_size);
}
static void packet_sample_start(void *arg)
{
struct controller_block *packet = arg;
struct controller_bus *bus = packet->private->bus;
size_t header_size = 14;
packet->private->tx_size = bus->private->tx_end_offset + header_size;
packet->private->rx_size = bus->private->rx_end_offset + header_size;
packet->private->tx_packet = controller_mem_calloc(
CONTROLLER_MEM_PERIODIC_READ | CONTROLLER_MEM_PERIODIC_WRITE,
packet->private->tx_size, 1);
packet->private->rx_packet = controller_mem_calloc(
CONTROLLER_MEM_PERIODIC_READ | CONTROLLER_MEM_PERIODIC_WRITE,
packet->private->rx_size, 1);
char *tx_start = packet->private->tx_packet;
char *rx_start = packet->private->rx_packet;
memcpy(tx_start + 0, (char[]){0xff, 0xff, 0xff, 0xff, 0xff, 0xff}, 6);
memcpy(tx_start + 6, (char[]){0x01, 0x02, 0x03, 0x04, 0x05, 0x06}, 6);
tx_start[12] = 0x44;
tx_start[13] = 0x4d;
tx_start += header_size;
rx_start += header_size;
packet_client_callbacks(bus, rx_start, tx_start);
}
static enum controller_bus_state atsamx70_gmac_poll(struct controller_bus *bus)
{
struct gmac_state *gmac = &bus->owner->private->gmac;
uint32_t nsr = gmac_network_status_get(gmac);
uint32_t tsr = gmac_transmit_status_get(gmac);
uint32_t isr = gmac_interrupt_status_get(gmac);
log_send(LOG_T_DEBUG, "%s: NSR: 0x%08"PRIx32" TSR: 0x%08"PRIx32" ISR: 0x%08"PRIx32,
bus->owner->name, nsr, tsr, isr);
if (tsr) {
gmac_transmit_status_set(gmac, tsr);
}
return CONTROLLER_BUS_STATE_OK;
}
static int atsamx70_gmac_recover(struct controller_bus *bus)
{
return 0;
}
static struct controller_block * block_atsamx70_gmac_create(char *name, int argc,
va_list val)
{
struct controller_block *packet;
struct controller_bus *bus;
char *name_tx;
struct controller_block *packet_tx;
asprintf(&name_tx, "%s_tx", name);
if (!name_tx)
return NULL;
packet = controller_block_alloc("atsamx70_gmac", name, sizeof(struct controller_block_private));
if (!packet)
goto err_block;
packet_tx = controller_block_alloc("atsamx70_gmac_tx", name_tx, 0);
if (!packet)
goto err_block_tx;
packet_tx->private = packet->private;
free(name_tx);
name_tx = NULL;
bus = packet_bus_create(name, packet);
if (!bus)
goto err_bus;
controller_bus_output_set(bus, packet_tx);
controller_bus_input_set(bus, packet);
controller_bus_poll_set(bus, atsamx70_gmac_poll, atsamx70_gmac_recover);
packet->private->bus = bus;
packet_tx->calculate = atsamx70_gmac_tx_calculate;
if (controller_block_add(packet))
goto err_add;
if (controller_block_add(packet_tx))
goto err_add;
controller_sample_start_hook(packet_sample_start, packet);
/* RMII pins:
GTXCK PD0,A
GTXEN PD1,A
GTX0 PD2,A
GTX1 PD3,A
GRXDV PD4,A
GRX0 PD5,A
GRX1 PD6,A
GRXER PD7,A
GMDC PD8,A
GMDIO PD9,A
same70 xplained:
RESET PC10
INT PA14
*/
ioport_disable_pin(PIO_GTXCK_IDX);
ioport_set_pin_mode(PIO_GTXCK_IDX, IOPORT_MODE_GTXCK);
ioport_disable_pin(PIO_GTXEN_IDX);
ioport_set_pin_mode(PIO_GTXEN_IDX, IOPORT_MODE_GTXEN);
ioport_disable_pin(PIO_GTX0_IDX);
ioport_set_pin_mode(PIO_GTX0_IDX, IOPORT_MODE_GTX0);
ioport_disable_pin(PIO_GTX1_IDX);
ioport_set_pin_mode(PIO_GTX1_IDX, IOPORT_MODE_GTX1);
ioport_disable_pin(PIO_GRXDV_IDX);
ioport_set_pin_mode(PIO_GRXDV_IDX, IOPORT_MODE_GRX0);
ioport_disable_pin(PIO_GRX0_IDX);
ioport_set_pin_mode(PIO_GRX1_IDX, IOPORT_MODE_GRX1);
ioport_disable_pin(PIO_GRXER_IDX);
ioport_set_pin_mode(PIO_GRXER_IDX, IOPORT_MODE_GRXER);
ioport_disable_pin(PIO_GMDC_IDX);
ioport_set_pin_mode(PIO_GMDC_IDX, IOPORT_MODE_GMDC);
ioport_disable_pin(PIO_GMDIO_IDX);
ioport_set_pin_mode(PIO_GMDIO_IDX, IOPORT_MODE_GMDIO);
struct gmac_state *gmac = &packet->private->gmac;
log_send(LOG_T_DEBUG, "gmac: %p, tx_q: %p", gmac, packet->private->tx_q);
int i;
for (i = 0; i < TX_Q_SIZE; i++) {
packet->private->tx_q[i].buffer = packet->private->tx_buffer[i];
}
gmac_init(gmac, packet->private->tx_q, TX_Q_SIZE);
gmac_network_configuration_set(gmac,
GMAC_NCFGR_SPD | GMAC_NCFGR_FD | GMAC_NCFGR_DBW(0) |
GMAC_NCFGR_CLK_MCK_64 | GMAC_NCFGR_MAXFS | GMAC_NCFGR_PEN |
GMAC_NCFGR_RFCS);
return packet;
err_add:
err_bus:
err_block_tx:
controller_block_free(packet);
err_block:
free(name_tx);
return NULL;
}
BLOCK_CREATE(atsamx70_gmac) = {
.create = block_atsamx70_gmac_create,
.args = { NULL },
};

+ 2
- 0
controller/atsamx70/block_atsamx70_pio_in.c View File

@ -90,7 +90,9 @@ static struct controller_block * block_atsamx70_pio_in_create(char *name, int ar
ioport_init();
ioport_enable_pin(pio->private->pin);
ioport_set_pin_mode(pio->private->pin, IOPORT_MODE_PULLUP | IOPORT_MODE_GLITCH_FILTER);
ioport_set_pin_dir(pio->private->pin, IOPORT_DIR_INPUT);
ioport_set_pin_sense_mode(pio->private->pin, IOPORT_SENSE_BOTHEDGES);
if (controller_block_add(pio))
goto err_add;


+ 1
- 0
controller/atsamx70/block_atsamx70_pio_out.c View File

@ -92,6 +92,7 @@ static struct controller_block * block_atsamx70_pio_out_create(char *name, int a
ioport_enable_pin(pio->private->pin);
ioport_set_pin_dir(pio->private->pin, IOPORT_DIR_OUTPUT);
ioport_set_pin_mode(pio->private->pin, 0);
if (controller_block_add(pio))
goto err_add;


+ 12
- 7
controller/atsamx70/block_atsamx70_tc.c View File

@ -41,10 +41,12 @@ struct controller_block_private {
uint32_t tcn;
};
#include <stdio.h>
static void tc_calculate(struct controller_block *tc)
{
struct controller_block_private *priv = tc->private;
uint16_t cnt0;
int16_t cnt0p;
int16_t cnt1;
uint16_t prevcnt0 = priv->prevcnt0;
float speed;
@ -59,17 +61,19 @@ static void tc_calculate(struct controller_block *tc)
priv->homed |= qisr & TC_QISR_IDX;
priv->homed &= !(*priv->reset);
if (cnt0 > rev)
cnt0 += rev;
cnt0p = cnt0;
// if (cnt0p > rev)
// cnt0p += rev;
int16_t diff = cnt0 - prevcnt0;
if (diff > rev/2)
diff -= rev;
if (diff < -rev/2)
diff += rev;
speed = diff * priv->speedfac;
position = (cnt1 * rev + cnt0) * priv->posfac;
speed = diff * priv->speedfac;
position = (/*cnt1 * rev + */cnt0p) * priv->posfac;
priv->prevcnt0 = cnt0;
priv->speed = speed;
@ -204,7 +208,8 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
*/
tc_init(tc->private->tcn, TC_CH0,
TC_CMR_ETRGEDG_RISING | TC_CMR_ABETRG_A | TC_CMR_TCCLKS_XC0 | TC_CMR_CPCTRG,
TC_CMR_ETRGEDG_RISING | TC_CMR_ABETRG_A | TC_CMR_TCCLKS_XC0 |
((rev * 4 < 32768) ? TC_CMR_CPCTRG : 0),
0);
tc_write_rc(tc->private->tcn, TC_CH0, rev * 4);
@ -220,7 +225,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
tc_start(tc->private->tcn, TC_CH0);
tc_start(tc->private->tcn, TC_CH1);
tc->private->speedfac = (controller_time_period_get(tc->time) * M_PI * 2) * (rev * 4);
tc->private->speedfac = (controller_time_frequency_get(tc->time) * M_PI * 2) / (rev * 4);
tc->private->posfac = (M_PI * 2) / (rev * 4);
if (controller_block_add(tc))


+ 1
- 0
controller/atsamx70/build.mk View File

@ -3,6 +3,7 @@ ATSAMX70_TARGETS := $(LIBDIR)/libatsamx70.la
ATSAMX70_BLOCKS := \
atsamx70_afec \
atsamx70_gmac \
atsamx70_pio_in \
atsamx70_pio_out \
atsamx70_pwm \


+ 9
- 1
controller/block/block_gain_ratio_abs.c View File

@ -47,8 +47,16 @@ struct controller_block_private {
static void gain_ra_calculate(struct controller_block *gain)
{
struct controller_block_private *priv = gain->private;
float num = *priv->num;
float denom = *priv->denom;
float in = *priv->in;
float out;
priv->out = fabs((*priv->in * *priv->num)/ *priv->denom);
out = fabs((in * num)/ denom);
if (isnan(out)) {
out = 1.0;
}
priv->out = out;
}
static struct controller_block_interm_list interms[] = {


+ 11
- 4
controller/block/block_i2t.c View File

@ -30,7 +30,9 @@
----------------------
| |
---| 0 I 0 Ilim |----
---| 0 I 0 Ilim |----
| |
| 1 limiting |----
| |
----------------------
@ -39,6 +41,7 @@
struct controller_block_private {
float *I;
float Ilim;
bool limiting;
float i2t;
@ -64,10 +67,13 @@ static void i2t_calculate(struct controller_block *block)
if (i2t < 0.0)
i2t = 0.0;
if (i2t >= peak2t)
if (i2t >= peak2t) {
priv->Ilim = priv->continuous;
else
priv->limiting = true;
} else {
priv->limiting = false;
priv->Ilim = sqrtf(peak2t - i2t + continuous2);
}
priv->i2t = i2t;
}
@ -128,7 +134,8 @@ static struct controller_block_interm_list interms[] = {
};
static struct controller_block_outterm_list outterms[] = {
{ "Ilim", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, Ilim) },
{ "Ilim", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, Ilim) },
{ "limiting", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, limiting) },
{ NULL }
};


+ 5
- 3
controller/block/block_motor_model_dc_v.c View File

@ -63,17 +63,19 @@ static void mmdcv_calculate(struct controller_block *mmdcv)
struct controller_block_private *priv = mmdcv->private;
float V = *priv->V;
float w = *priv->w;
float I, Eb, T;
float I, Eb, Er, T;
float kt = priv->kt;
float ke = priv->ke;
float iR = priv->iR;
Eb = V - w * ke;
I = (Eb) * iR;
Eb = w * ke;
Er = V - Eb;
I = (Er) * iR;
T = I * kt;
priv->I = I;
priv->T = T;
priv->Eb = Eb;
}
static int param_set_kt(struct controller_block *mmdcv, char *param, int argc,


+ 10
- 4
controller/block/block_motor_model_dc_v.test.ctrl View File

@ -9,13 +9,15 @@ blocks (1000.0, 0.0) {
{ "test_input_float", "test_w" }
{ "test_output_float", "test_I" }
{ "test_output_float", "test_T" }
{ "test_output_float", "test_Eb" }
}
links {
{ "test_V", "value", "mm", "V", true }
{ "test_w", "value", "mm", "w", true }
{ "mm", "I", "test_I", "value", true }
{ "mm", "T", "test_T", "value", true }
{ "test_V", "value", "mm", "V", true }
{ "test_w", "value", "mm", "w", true }
{ "mm", "I", "test_I", "value", true }
{ "mm", "T", "test_T", "value", true }
{ "mm", "Eb", "test_Eb", "value", true }
}
params {
@ -33,6 +35,10 @@ params {
(float) { 0.0, 0.025, -0.225, 0.55 },
(float) { 0.0, 0.0, 0.0001, 0.0 }
}
{ "test_Eb", "value", 4,
(float) { 0.0, 1.0, -1.0, 2.0 },
(float) { 0.0, 0.0, 0.0001, 0.0001 }
}
}
set trace_server false

+ 49
- 20
controller/block/block_setpoint_generator_3d.c View File

@ -34,25 +34,30 @@
inputs outputs
nr name nr name
--------------------------
| |
----| 0 reset_x 0 x |----
| |
----| 1 reset 1 v |----
| |
----| 2 track_x 2 a |----
| |
| 3 j |----
| |
| 4 setpoint |----
| |
--------------------------
-------------------------------
| |
----| 0 reset_x 0 x |----
| |
----| 1 reset 1 v |----
| |
----| 2 track_x 2 a |----
| |
----| 3 track_v 3 j |----
| |
----| 4 track_x_cmd 4 setpoint |----
| |
----| 5 track_v_cmd |
| |
-------------------------------
'setpoint' is the setpoint as the user requested it.
'x' is the output of the setpoint generator.
'v', 'a' and 'j' are the first, second and third
order derivatives of 'x'.
'v', 'a' and 'j' are the first, second and third order derivatives of 'x'.
While reset is active the reset_x value is taken as new setpoint value.
(Both x and setpoint are updated and are not limited by v, a, j)
track_x and track_v are the inputs for setpoint and speed tracking commands.
*/
struct spg_command {
@ -80,6 +85,9 @@ struct controller_block_private {
float *reset_x;
bool *reset;
float *track_x;
float *track_v;
bool *track_x_cmd;
bool *track_v_cmd;
/* beware: 'samples' is the time unit, not 'seconds',
all parameters and commands are scaled on block entry/exit */
@ -256,6 +264,13 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
goto set_output;
}
if (*priv->track_x_cmd) {
priv->cur_done = false;
priv->cur_command.type = COMMAND_PTYPE_SETPOINT_TRACK;
} else if (*priv->track_v_cmd) {
priv->cur_done = false;
priv->cur_command.type = COMMAND_PTYPE_SPEED_TRACK;
}
if (priv->cur_done) {
int r;
@ -289,6 +304,9 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cmd_v = priv->cur_command.value.f * controller_time_period_get(spg->time);
priv->cur_done = true;
break;
case COMMAND_PTYPE_SPEED_TRACK:
priv->cur_done = true;
break;
case COMMAND_PTYPE_SETPOINT_TIME:
if (!priv->cur_start) {
controller_trigger_time command_t;
@ -330,6 +348,12 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
ignore_x = true;
priv->cmd_x = cur_x + priv->cmd_v;
}
if (priv->cur_command.type == COMMAND_PTYPE_SPEED_TRACK) {
float track_v = *priv->track_v;
ignore_x = true;
priv->cmd_v = track_v;
priv->cmd_x = cur_x + track_v * controller_time_period_get(spg->time);
}
if (priv->cur_command.type == COMMAND_PTYPE_SETPOINT_TRACK) {
priv->cmd_x = *priv->track_x;
}
@ -809,6 +833,7 @@ static int block_setpoint_generator_command_filter(struct controller_command *co
break;
}
case COMMAND_PTYPE_SETPOINT_TRACK:
case COMMAND_PTYPE_SPEED_TRACK:
break;
default:
r = -1;
@ -942,9 +967,12 @@ static struct controller_block_param_list params[] = {
static struct controller_block_interm_list interms[] = {
{ "reset_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x) },
{ "reset", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, reset) },
{ "track_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, track_x) },
{ "reset_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x) },
{ "reset", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, reset) },
{ "track_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, track_x) },
{ "track_v", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, track_v) },
{ "track_x_cmd", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, track_x_cmd) },
{ "track_v_cmd", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, track_v_cmd) },
{ NULL }
};
@ -1033,8 +1061,9 @@ static struct controller_block * block_setpoint_generator_3d_create(char *name,
spg->private->command->value_type = COMMAND_VALUE_TYPE_FLOAT;
spg->private->command->command_types[0] = COMMAND_PTYPE_SETPOINT;
spg->private->command->command_types[1] = COMMAND_PTYPE_SPEED;
spg->private->command->command_types[2] = COMMAND_PTYPE_SETPOINT_TIME;
spg->private->command->command_types[3] = COMMAND_PTYPE_SETPOINT_TRACK;
spg->private->command->command_types[2] = COMMAND_PTYPE_SPEED_TRACK;
spg->private->command->command_types[3] = COMMAND_PTYPE_SETPOINT_TIME;
spg->private->command->command_types[4] = COMMAND_PTYPE_SETPOINT_TRACK;
spg->private->command->filter =
block_setpoint_generator_command_filter;


+ 11
- 0
controller/block/block_setpoint_generator_3d.test.ctrl View File

@ -9,6 +9,8 @@ blocks (10.0, 0.0) {
{ "test_input_bool", "reset" }
{ "test_input_float", "reset_x" }
{ "test_input_float", "track_x" }
{ "test_input_float", "track_v" }
{ "value_bool", "false" }
{ "test_output_float", "setpoint" }
{ "test_output_float", "x" }
@ -24,6 +26,9 @@ links {
{ "reset", "value", "spg", "reset", true }
{ "reset_x", "value", "spg", "reset_x", true }
{ "track_x", "value", "spg", "track_x", true }
{ "track_v", "value", "spg", "track_v", true }
{ "false", "value", "spg", "track_x_cmd", true }
{ "false", "value", "spg", "track_v_cmd", true }
{ "spg", "x", "x", "value", true }
{ "spg", "v", "v", "value", true }
@ -75,6 +80,12 @@ params {
40.0, 1337.0
}
}
{ "track_v", "value", 22,
(float) { 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0,
42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0,
40.0, 1337.0
}
}
{ "setpoint", "value", 22,
(float) { 50.0, 56.0, 62.0, 68.0, 74.0, 80.0, -100.0, 200.0, 61.0, 51.1395,


+ 6
- 10
controller/block/build.mk View File

@ -7,6 +7,9 @@ BLOCKS := \
add \
and \
bridge_pwm \
command_bool \
command_float \
command_uint32 \
controller_profile \
counter \
deadzone \
@ -38,6 +41,9 @@ BLOCKS := \
quantize \
random \
rangecheck \
setpoint_generator_1d \
setpoint_generator_3d \
servo_state \
sine \
state_machine \
subtract \
@ -51,16 +57,6 @@ BLOCKS := \
schmitt_trigger \
oneshot
ifdef BUILD_TCP
BLOCKS += \
command_bool \
command_float \
command_uint32 \
setpoint_generator_1d \
setpoint_generator_3d \
servo_state
endif
BLOCKS_IL := \
not \


+ 1
- 0
controller/build.mk View File

@ -15,6 +15,7 @@ $(eval $(call SUBDIR,trigger))
ifdef BUILD_ETHERCAT
$(eval $(call SUBDIR,ec))
endif
$(eval $(call SUBDIR,packet))
ifdef BUILD_TEST
$(eval $(call SUBDIR,test))
endif


+ 1
- 1
controller/controller/build.mk View File

@ -5,6 +5,7 @@ CONTROLLER_SRCS= \
$(DIR)/controller_block.c \
$(DIR)/controller_block_param.c \
$(DIR)/controller_bus.c \
$(DIR)/controller_command.c \
$(DIR)/controller_mem.c \
$(DIR)/controller_module.c \
$(DIR)/controller_time.c \
@ -40,7 +41,6 @@ endif
ifdef BUILD_TCP
CONTROLLER_SRCS+= \
$(DIR)/controller_block_trace.c \
$(DIR)/controller_command.c \
$(DIR)/controller_trace.c
endif


+ 9
- 1
controller/controller/controller_command.c View File

@ -22,11 +22,13 @@
#include <string.h>
#include <pthread.h>
#include <sys/types.h>
#if defined(HAVE_TCP)
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#endif
#include <signal.h>
#include <errno.h>
#include <limits.h>
@ -46,7 +48,9 @@ static bool sample_hook_installed = false;
static void controller_command_sample_hook(void *arg)
{
#if defined(HAVE_TCP)
controller_command_server_start(CTRL_COMMAND_PORT, command_list_nr * 2);
#endif
}
@ -109,6 +113,8 @@ struct controller_command *controller_command_find_by_name(char *name)
return NULL;
}
#if defined(HAVE_TCP)
struct command_hdl {
bool free;
@ -122,9 +128,10 @@ static struct command_hdl *command_hdl;
static int controller_command_server_port;
/* Thes are created when starting the server and sent to each new client */
/* These are created when starting the server and sent to each new client */
struct command_pkt *controller_command_pkt_list;
static void handler_close(struct command *command)
{
struct command_hdl *hdl = command->private;
@ -340,3 +347,4 @@ void controller_command_server_start(int portnr, int max)
pthread_create(&thread_id, &attr,
controller_command_handle, NULL);
}
#endif

+ 1
- 1
controller/controller/controller_load.c View File

@ -669,7 +669,7 @@ int controller_load_mem(char *name, char *ctrl, size_t ctrl_size)
return ret;
}
#ifdef BUILD_TCP
#ifdef HAVE_TCP
static int controller_load_shell(char *args, char *out, int *outlen)
{


+ 10
- 9
controller/controller/controller_sample.c View File

@ -336,15 +336,6 @@ void controller_sample_iteration(controller_trigger_time t_trig)
* Shell interface:
*/
static struct shell_cmd sample_shell_cmd = {
"timing", "reset/print sample timing", controller_sample_timing
};
int controller_sample_shell_add(void)
{
return shell_cmd_add(&sample_shell_cmd);
}
static void controller_sample_timing_reset(void)
{
sample_timing_init(&st_start, "start");
@ -384,6 +375,16 @@ static int controller_sample_timing(char *args, char *out, int *outlen)
return *outlen;
}
static struct shell_cmd sample_shell_cmd = {
"timing", "reset/print sample timing", controller_sample_timing
};
int controller_sample_shell_add(void)
{
return shell_cmd_add(&sample_shell_cmd);
}
#endif
void controller_sample_start_hook(void (*func)(void *arg), void *arg)


+ 125
- 31
controller/ctrl_embedded.ctrl View File

@ -9,53 +9,147 @@ trigger {
{ "systick" }
}
blocks (2.0, 0.0) {
{ "atsamx70_pio_out", "status0_LED", 0, 2 } #PA2
{ "atsamx70_pio_out", "blinky_LED", 2, 8 } #PC8
{ "not", "blinky" }
}
blocks ($(frequency), $(delay)) {
# Profiling block, may be commented out.
{ "controller_profile", "profile" }
#{ "controller_profile", "profile" }
{ "atsamx70_tc", "counter0", 0, 2500, 0 }
{ "atsamx70_tc", "counter0", 0, 2500, 0 }
{ "atsamx70_tc", "counter1", 1, 10000, 0 }
{ "atsamx70_afec", "afec0", 0 }
{ "atsamx70_pwm", "pwm0", 0, 0, 10000.0 }
# { "debug_float", "debug_counter0_p", 1.0 }
# { "debug_float", "debug_counter1_p", 1.0 }
# { "debug_bool", "debug_homed0", 1.0 }
# { "debug_bool", "debug_homed1", 1.0 }
# { "debug_float", "debug_counter0_v", 1.0 }
# { "debug_float", "debug_counter1_v", 1.0 }
# { "debug_float", "debug_afec0", 1.0 }
# { "debug_float", "debug_sample", 1.0 }
{ "atsamx70_afec", "afec0", 0 }
{ "atsamx70_pwm", "pwm0", 0, 0, 10000.0 }
{ "atsamx70_pio_in", "button0", 2, 30 } #PC30
{ "atsamx70_pio_in", "button1", 0, 17 } #PA17
{ "atsamx70_pio_in", "button2", 2, 12 } #PC12
{ "atsamx70_pio_out", "status1_LED", 0, 24 } #PA24
{ "atsamx70_pio_out", "status2_LED", 0, 6 } #PA6
{ "atsamx70_pio_out", "status3_LED", 3, 11 } #PD11
{ "atsamx70_pio_out", "status4_LED", 2, 19 } #PC19
{ "atsamx70_pio_out", "status5_LED", 3, 26 } #PD26
{ "gain", "w2V" }
{ "gain", "V2pwm" }
{ "deadzone", "joystick_deadzone" }
{ "gain", "joystick2w" }
{ "motor_model_dc_v", "motor_model" }
{ "i2t", "motor_i2t" }
{ "gain", "Ilim2Vlim" }
{ "limit_dyn", "Vlim" }
{ "not", "button0n" }
{ "not", "button1n" }
{ "and2", "track_x_cmd" }
{ "setpoint_generator_3d", "spg", "spg", "rad" }
{ "value_bool", "false" }
{ "value_float", "zero" }
}
blocks (2.0, 0.0) {
{ "atsamx70_pio_out", "blinky_LED", 2, 8 } #PC8
{ "not", "blinky" }
{ "atsamx70_gmac", "packet" }
{ "packet_out_float_be", "out_position", "packet", 0 }
{ "packet_out_float_be", "out_stick", "packet", 4 }
{ "packet_out_float_be", "out_speed", "packet", 8 }
{ "packet_out_bool_byte", "out_homed", "packet", 9 }
}
alias {
}
set w_nom 1800.0
set V_nom 24.0
set I_nom 28.0
set I_peak 95.0
set t_peak 0.5 #???
set R_peak $(V_nom)/$(I_peak)
set V_supply 24.0 #27.6?
set gear_ratio 100.0 * 19.33
links {
# { "profile", "sample", "debug_sample", "in", true }
{ "false", "value", "counter0", "reset", true }
{ "false", "value", "counter1", "reset", true }
# { "counter0", "position", "debug_counter0_p", "in", true }
# { "counter1", "position", "debug_counter1_p", "in", true }
# { "counter0", "speed", "debug_counter0_v", "in", true }
# { "counter1", "speed", "debug_counter1_v", "in", true }
# { "counter0", "homed", "debug_homed0", "in", true }
# { "counter1", "homed", "debug_homed1", "in", true }
# { "afec0", "value", "debug_afec0", "in", true }
{ "afec0", "value", "pwm0", "in", true }
{ "blinky", "output", "blinky", "input", false }
{ "blinky", "output", "blinky_LED", "value", true }
{ "button0", "value", "button0n", "input", true }
{ "button0n", "output", "counter0", "reset", true }
{ "button1", "value", "button1n", "input", true }
{ "button1n", "output", "track_x_cmd", "a", true }
{ "counter0", "homed", "track_x_cmd", "b", true }
{ "false", "value", "counter1", "reset", true }
{ "afec0", "value", "joystick_deadzone", "in", true }
{ "joystick_deadzone", "out", "joystick2w", "in", true }
{ "joystick2w", "out", "w2V", "in", true }
{ "w2V", "out", "Vlim", "in", true }
{ "Vlim", "out", "V2pwm", "in", true }
{ "V2pwm", "out", "pwm0", "in", true }
{ "counter0", "position", "spg", "reset_x", true }
{ "zero", "value", "spg", "track_x", true }
{ "joystick2w", "out", "spg", "track_v", true }
{ "track_x_cmd", "q", "spg", "track_x_cmd", true }
{ "button0n", "output", "spg", "track_v_cmd", true }
{ "false", "value", "spg", "reset", true }
{ "w2V", "out", "motor_model", "V", false }
{ "counter1", "speed", "motor_model", "w", true }
{ "motor_model", "I", "motor_i2t", "I", true }
{ "motor_i2t", "Ilim", "Ilim2Vlim", "in", true }
{ "Ilim2Vlim", "out", "Vlim", "limit", true }
# Status leds
{ "counter0", "homed", "status1_LED", "value", true }
{ "motor_i2t", "limiting", "status2_LED", "value", true }
{ "blinky", "output", "blinky", "input", false }
{ "blinky", "output", "blinky_LED", "value", true }
{ "blinky", "output", "status0_LED", "value", true }
{ "button0", "value", "status3_LED", "value", true }
{ "button1", "value", "status4_LED", "value", true }
{ "button2", "value", "status5_LED", "value", true }
# Packet output
{ "counter0", "position", "out_position", "value", true }
{ "afec0", "value", "out_stick", "value", true }
{ "counter1", "speed", "out_speed", "value", true }
{ "counter0", "homed", "out_homed", "value", true }
}
params {
{ "w2V", "gain", (float)$(gear_ratio)*$(V_nom)/$(w_nom) }
{ "V2pwm", "gain", (float)1.0/$(V_supply) }
{ "joystick2w", "gain", (float)$(w_nom)/$(gear_ratio) }
# GNM8055/4
# 24V 28A 490W
# zul. Spitzenstrom 95A
# S3 25% I.CI F
# 24/95 --> R=0.25263158 (max R)
# 672W - 490W = 182W --> R=0.23214286 (6.5V) (17.5V emf)
# 490W @1800rpm --> T = 2.6Nm
# kt = 2.6/28 = 0.092857143
{ "motor_model", "kt", (float)0.092857143 }
{ "motor_model", "ke", (float)0.092857143 }
{ "motor_model", "R", (float)0.23214286 }
{ "motor_i2t", "continuous", (float)$(I_nom) }
{ "motor_i2t", "peak", (float)$(I_peak) }
{ "motor_i2t", "duration", (float)$(t_peak) }
{ "Ilim2Vlim", "gain", (float)$(R_peak) }
{ "joystick_deadzone", "deadzone", (float)0.1 }
{ "joystick_deadzone", "intercept", (float)1.0 }
}

+ 6
- 0
controller/dt_ctrl.ctrl View File

@ -133,6 +133,9 @@ links {
{ "azimuth_servo_state", "reset", "azimuth_spg", "reset" , false }
{ $<Azimuth_Position>, "azimuth_spg", "reset_x" , true }
{ "zero", "value", "azimuth_spg", "track_x" , true }
{ "zero", "value", "azimuth_spg", "track_v" , true }
{ "false", "value", "azimuth_spg", "track_x_cmd" , true }
{ "false", "value", "azimuth_spg", "track_v_cmd" , true }
{ "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 }
@ -179,6 +182,9 @@ links {
{ "elevation_servo_state", "reset", "elevation_spg", "reset" , false }
{ $<Elevation_Position>, "elevation_spg", "reset_x" , true }
{ "zero", "value", "elevation_spg", "track_x" , true }
{ "zero", "value", "elevation_spg", "track_v" , true }
{ "false", "value", "elevation_spg", "track_x_cmd" , true }
{ "false", "value", "elevation_spg", "track_v_cmd" , true }
{ "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 }


+ 5
- 0
controller/packet/Makefile View File

@ -0,0 +1,5 @@
all:
@$(MAKE) --no-print-directory -C ../.. targets_controller/packet
$(MAKECMDGOALS):
@$(MAKE) --no-print-directory -C ../.. $(MAKECMDGOALS)

+ 223
- 0
controller/packet/block_packet_eth.c View File

@ -0,0 +1,223 @@
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2018
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 <controller/controller_bus.h>
#include <controller/controller_mem.h>
#include <controller/controller_sample.h>
#include <log/log.h>
#include <packet/packet.h>
#include <packet/packet_bus.h>
#include <net/if.h>
#include <arpa/inet.h>
#include <netpacket/packet.h>
#include <net/ethernet.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
struct controller_block_private {
struct controller_bus *bus;
void *rx_packet;
void *tx_packet;
size_t rx_size;
size_t tx_size;
int sock;
struct sockaddr_ll sockaddr_ll;
};
static void packet_eth_rx_calculate(struct controller_block *packet_rx)
{
struct controller_block_private *priv = packet_rx->private;
ssize_t ret;
bool valid = false;
do {
ret = recv(priv->sock, priv->rx_packet, priv->rx_size, 0);
if (ret == priv->rx_size)
valid = true;
} while (ret > 0);
priv->valid = true;
}
static void packet_eth_tx_calculate(struct controller_block *packet_tx)
{
struct controller_block_private *priv = packet_tx->private;
if (priv->tx_size)
sendto(priv->sock, priv->tx_packet, priv->tx_size,
MSG_DONTWAIT,
(struct sockaddr *)&priv->sockaddr_ll, sizeof(priv->sockaddr_ll));
}
static void packet_sample_start(void *arg)
{
struct controller_block *packet = arg;
struct controller_bus *bus = packet->private->bus;
packet->private->tx_size = bus->private->tx_end_offset;
packet->private->rx_size = bus->private->rx_end_offset;
packet->private->tx_packet = controller_mem_calloc(
CONTROLLER_MEM_PERIODIC_READ | CONTROLLER_MEM_PERIODIC_WRITE,
packet->private->tx_size, 1);
packet->private->rx_packet = controller_mem_calloc(
CONTROLLER_MEM_PERIODIC_READ | CONTROLLER_MEM_PERIODIC_WRITE,
packet->private->rx_size, 1);
packet_client_callbacks(bus, packet->private->rx_packet, packet->private->tx_packet);
}
#define ETH_P_DM 0x444d
static int open_eth(struct controller_block_private *priv, char *ifname)
{
struct ifreq ifreq;
int ret;
priv->sock = socket(AF_PACKET, SOCK_DGRAM, htons(ETH_P_DM));
if (priv->sock < 0)
goto err;
strncpy(ifreq.ifr_name, ifname, IFNAMSIZ-1);
ret = ioctl(priv->sock, SIOCGIFINDEX, &ifreq);
if (ret < 0) {
log_send(LOG_T_ERROR, "ioctl SIOCGIFINDEX: %s", strerror(errno));
goto err_socket;
}
log_send(LOG_T_DEBUG, "Interface %s index nr found: %d",
ifname, ifreq.ifr_ifindex);
priv->sockaddr_ll.sll_family = AF_PACKET;
priv->sockaddr_ll.sll_protocol = htons(ETH_P_DM);
priv->sockaddr_ll.sll_ifindex = ifreq.ifr_ifindex;
ret = bind(priv->sock, (struct sockaddr*)&priv->sockaddr_ll, sizeof(priv->sockaddr_ll));
if (ret < 0) {
log_send(LOG_T_ERROR, "bind(): %s", strerror(errno));
goto err_socket;
}
log_send(LOG_T_DEBUG, "Bind to interface and protocol 0x%04x done",
ETH_P_DM);
priv->sockaddr_ll.sll_addr[0] = 0xff;
priv->sockaddr_ll.sll_addr[1] = 0xff;
priv->sockaddr_ll.sll_addr[2] = 0xff;
priv->sockaddr_ll.sll_addr[3] = 0xff;
priv->sockaddr_ll.sll_addr[4] = 0xff;
priv->sockaddr_ll.sll_addr[5] = 0xff;
priv->sockaddr_ll.sll_halen = ETH_ALEN;
/* Using select: set to non-blocking (1)
Using SO_SNDTIMEO: set to blocking (0) */
ioctl(priv->sock, FIONBIO, &(int){1});
return 0;
err_socket:
close(priv->sock);
err:
priv->sock = -1;
return -1;
}
static struct controller_block_outterm_list outterms[] = {
{ "valid", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, valid) },
{ NULL }
};
static struct controller_block * block_packet_eth_create(char *name, int argc,
va_list val)
{
struct controller_block *packet;
struct controller_bus *bus;
char *name_tx;
struct controller_block *packet_tx;
char *ifname;
ifname = va_arg(val, char *);
asprintf(&name_tx, "%s_tx", name);
if (!name_tx)
return NULL;
packet = controller_block_alloc("packet_eth", name, sizeof(struct controller_block));
if (!packet)
goto err_block;
packet_tx = controller_block_alloc("packet_eth_tx", name_tx, 0);
if (controller_block_outterm_list_init(in_bool, outterms))
goto err_outterm;
if (!packet)
goto err_block_tx;
packet_tx->private = packet->private;
free(name_tx);
name_tx = NULL;
bus = packet_bus_create(name, packet);
if (!bus)
goto err_bus;
controller_bus_output_set(bus, packet_tx);
controller_bus_input_set(bus, packet);
packet->private->bus = bus;
packet->calculate = packet_eth_rx_calculate;
packet_tx->calculate = packet_eth_tx_calculate;
if (open_eth(packet->private, ifname))
goto err_open;
if (controller_block_add(packet))
goto err_add;
if (controller_block_add(packet_tx))
goto err_add;
controller_sample_start_hook(packet_sample_start, packet);
return packet;
err_add:
err_open:
err_bus:
err_block_tx:
err_outterm:
controller_block_free(packet);
err_block:
free(name_tx);
return NULL;
}
BLOCK_CREATE(packet_eth) = {
.create = block_packet_eth_create,
.args = { "char*", NULL },
};

+ 94
- 0
controller/packet/block_packet_in_bool_byte.c View File

@ -0,0 +1,94 @@
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2018
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 <controller/controller_bus.h>
#include <log/log.h>
#include <packet/packet.h>
struct controller_block_private {
bool value;
uint8_t *data;
struct controller_bus *bus;
};
static void calculate(struct controller_block *in_bool)
{
struct controller_block_private *priv = in_bool->private;
priv->value = *priv->data;
}
static void rx_callback(struct controller_block *block, void *data)
{
block->private->data = data;
}
static struct controller_block_outterm_list outterms[] = {
{ "value", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, value) },
{ NULL }
};
static struct controller_block * block_packet_in_bool_byte_create(char *name,
int argc, va_list ap)
{
struct controller_block *in_bool;
char *busname = va_arg(ap, char *);
int offset = va_arg(ap, int);
if (!(in_bool = controller_block_alloc("packet_in_bool_byte", name, sizeof(struct controller_block_private))))
return NULL;
in_bool->calculate = calculate;
if (controller_block_outterm_list_init(in_bool, outterms))
goto err_block;
in_bool->private->bus = controller_bus_find(busname);
if (!in_bool->private->bus)
goto err_bus;
if (packet_bus_add_input(in_bool, in_bool->private->bus,
offset, 4, rx_callback))
goto err_bus_input;
if (controller_block_add(in_bool))
goto err_add;
return in_bool;
err_add:
err_bus_input:
err_bus:
err_block:
controller_block_free(in_bool);
return NULL;
}
BLOCK_CREATE(packet_in_bool_byte) = {
.create = block_packet_in_bool_byte_create,
.args = { "char*,int", NULL },
};

+ 99
- 0
controller/packet/block_packet_in_float_be.c View File

@ -0,0 +1,99 @@
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2018
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 <controller/controller_bus.h>
#include <log/log.h>
#include <packet/packet.h>
struct controller_block_private {
float value;
uint32_t *data __attribute__((packed));
struct controller_bus *bus;
};
static void calculate(struct controller_block *in_float)
{
struct controller_block_private *priv = in_float->private;
union {
float f;
uint32_t u32;
} f2u;
f2u.u32 = ntohl(*priv->data);
priv->value = f2u.f;
}
static void rx_callback(struct controller_block *block, void *data)
{
block->private->data = data;
}
static struct controller_block_outterm_list outterms[] = {
{ "value", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, value) },
{ NULL }
};
static struct controller_block * block_packet_in_float_be_create(char *name,
int argc, va_list ap)
{
struct controller_block *in_float;
char *busname = va_arg(ap, char *);
int offset = va_arg(ap, int);
if (!(in_float = controller_block_alloc("packet_in_float_be", name, sizeof(struct controller_block_private))))
return NULL;
in_float->calculate = calculate;
if (controller_block_outterm_list_init(in_float, outterms))
goto err_block;
in_float->private->bus = controller_bus_find(busname);
if (!in_float->private->bus)
goto err_bus;
if (packet_bus_add_input(in_float, in_float->private->bus,
offset, 4, rx_callback))
goto err_bus_input;
if (controller_block_add(in_float))
goto err_add;
return in_float;
err_add:
err_bus_input:
err_bus:
err_block:
controller_block_free(in_float);
return NULL;
}
BLOCK_CREATE(packet_in_float_be) = {
.create = block_packet_in_float_be_create,
.args = { "char*,int", NULL },
};

+ 97
- 0
controller/packet/block_packet_out_bool_byte.c View File

@ -0,0 +1,97 @@
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2018
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 <controller/controller_bus.h>
#include <log/log.h>
#include <packet/packet.h>
struct controller_block_private {
bool *value;
uint8_t *data __attribute__((packed));
struct controller_bus *bus;
};
static void calculate(struct controller_block *out_bool)
{
struct controller_block_private *priv = out_bool->private;
*priv->data = *priv->value;
}
static void tx_callback(struct controller_block *block, void *data)
{
block->private->data = data;
}
static struct controller_block_interm_list interms[] = {
{ "value", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, value) },
{ NULL }
};
static struct controller_block * block_packet_out_bool_byte_create(char *name,
int argc, va_list ap)
{
struct controller_block *out_bool;
char *busname = va_arg(ap, char *);
int offset = va_arg(ap, int);
if (!(out_bool = controller_block_alloc("packet_out_bool_byte", name, sizeof(struct controller_block_private))))
return NULL;
out_bool->calculate = calculate;
log_send(LOG_T_DEBUG, "b1");
if (controller_block_interm_list_init(out_bool, interms))
goto err_block;
out_bool->private->bus = controller_bus_find(busname);
if (!out_bool->private->bus) {
log_send(LOG_T_ERROR, "bus %s could not be found", busname);
goto err_bus;
}
if (packet_bus_add_output(out_bool, out_bool->private->bus,
offset, 1, tx_callback))
goto err_bus_output;
if (controller_block_add(out_bool))
goto err_add;
return out_bool;
err_add:
err_bus_output:
err_bus:
err_block:
controller_block_free(out_bool);
return NULL;
}
BLOCK_CREATE(packet_out_bool_byte) = {
.create = block_packet_out_bool_byte_create,
.args = { "char*,int", NULL },
};

+ 100
- 0
controller/packet/block_packet_out_float_be.c View File