Browse Source

Add quadratic sign block

jeroen
Jeroen Vreeken 3 years ago
parent
commit
faad66299d
6 changed files with 129 additions and 3 deletions
  1. +1
    -1
      controller/block/block_gain_ratio_abs.c
  2. +93
    -0
      controller/block/block_quadratic_sign.c
  3. +25
    -0
      controller/block/block_quadratic_sign.test.ctrl
  4. +2
    -0
      controller/block/build.mk
  5. +4
    -2
      controller/packet/block_packet_eth.c
  6. +4
    -0
      controller/packet/packet_eth.ctrl

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

@ -54,7 +54,7 @@ static void gain_ra_calculate(struct controller_block *gain)
out = fabs((in * num)/ denom);
if (isnan(out)) {
out = 1.0;
out = 0.0;
}
priv->out = out;
}


+ 93
- 0
controller/block/block_quadratic_sign.c View File

@ -0,0 +1,93 @@
/*
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 <log/log.h>
/*
inputs outputs
nr name nr name
----------------------
| |
---| 0 in 0 out |----
| |
----------------------
*/
struct controller_block_private {
float *input0;
float out;
};
static void quadsign_calculate(struct controller_block *quadsign)
{
struct controller_block_private *priv = quadsign->private;
float in = *priv->input0;
float out = copysign(in * in, in);
priv->out = out;
}
static struct controller_block_interm_list interms[] = {
{ "in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, input0) },
{ NULL }
};
static struct controller_block_outterm_list outterms[] = {
{ "out", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, out) },
{ NULL }
};
static struct controller_block * block_quadratic_sign_create(char *name, int argc, va_list val)
{
struct controller_block *quadsign;
if (!(quadsign = controller_block_alloc("quadratic_sign", name, sizeof(struct controller_block_private))))
return NULL;
quadsign->private->out = 0.0;
if (controller_block_interm_list_init(quadsign, interms))
goto err_block;
if (controller_block_outterm_list_init(quadsign, outterms))
goto err_block;
quadsign->calculate = quadsign_calculate;
if (controller_block_add(quadsign))
goto err_block;
return quadsign;
err_block:
controller_block_free(quadsign);
return NULL;
}
BLOCK_CREATE(quadratic_sign) = {
.create = block_quadratic_sign_create,
.args = { NULL },
};

+ 25
- 0
controller/block/block_quadratic_sign.test.ctrl View File

@ -0,0 +1,25 @@
trigger {
{ "immediate" }
}
blocks (100.0, 0.0) {
{ "quadratic_sign", "quadratic_sign" }
{ "test_input_float", "test_input" }
{ "test_output_float", "test_output" }
}
links {
{ "test_input", "value", "quadratic_sign", "in", true }
{ "quadratic_sign", "out", "test_output", "value", true }
}
params {
{ "test_input", "value", 6, (float) { 0.0, 1.0, -1.0, 2.0, 0.5, -0.5 } }
{ "test_output", "value", 6,
(float) { 0.0, 1.0, -1.0, 4.0, 0.25, -0.25 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
}
set trace_server false

+ 2
- 0
controller/block/build.mk View File

@ -38,6 +38,7 @@ BLOCKS := \
multiplexer \
pid \
pid_aw \
quadratic_sign \
quadrature_decoder \
quantize \
random \
@ -123,6 +124,7 @@ CTRL_TESTS += \
$(DIR)/block_not.test.output \
$(DIR)/block_oneshot.test.output \
$(DIR)/block_pid_aw.test.output \
$(DIR)/block_quadratic_sign.test.output \
$(DIR)/block_rangecheck.test.output \
$(DIR)/block_setpoint_generator_1d.test.output \
$(DIR)/block_setpoint_generator_3d.test.output \


+ 4
- 2
controller/packet/block_packet_eth.c View File

@ -47,6 +47,8 @@ struct controller_block_private {
size_t rx_size;
size_t tx_size;
bool valid;
int sock;
struct sockaddr_ll sockaddr_ll;
};
@ -63,7 +65,7 @@ static void packet_eth_rx_calculate(struct controller_block *packet_rx)
valid = true;
} while (ret > 0);
priv->valid = true;
priv->valid = valid;
}
static void packet_eth_tx_calculate(struct controller_block *packet_tx)
@ -173,7 +175,7 @@ static struct controller_block * block_packet_eth_create(char *name, int argc,
goto err_block;
packet_tx = controller_block_alloc("packet_eth_tx", name_tx, 0);
if (controller_block_outterm_list_init(in_bool, outterms))
if (controller_block_outterm_list_init(packet, outterms))
goto err_outterm;
if (!packet)


+ 4
- 0
controller/packet/packet_eth.ctrl View File

@ -8,6 +8,10 @@ blocks (100.0, 0.0) {
{ "packet_in_float_be", "position", "packet", 0 }
{ "packet_in_float_be", "stick", "packet", 4 }
{ "packet_in_float_be", "speed", "packet", 8 }
{ "packet_in_float_be", "speedff", "packet", 12 }
{ "packet_in_float_be", "positionff", "packet", 16 }
{ "packet_in_float_be", "pid", "packet", 20 }
{ "packet_in_float_be", "motor_I", "packet", 24 }
# { "debug", "debug_position" }
# { "debug", "debug_stick" }
# { "debug", "debug_speed" }


Loading…
Cancel
Save