xan/hexagram
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

That's better, I think

Browse source
This commit is contained in:
XANTRONIX Development 2023-12-27 13:59:55 -05:00
parent 23514f6155
commit b6e552e178
3 changed files with 26 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
bin/dash2can.c
View file

@ -42,14 +42,14 @@ static int ev_handler(struct can_frame *frame, void *ctx) {
return hexagram_can_if_write(can_if, frame); return hexagram_can_if_write(can_if, frame);
} }
static hexagram_schedule_slot table[4] = { static hexagram_schedule_table_entry table[4] = {
{ 16670, { .can_id = 0x280, .can_dlc = 8 }, ev_handler }, { 16670, { .can_id = 0x280, .can_dlc = 8 }, ev_handler },
{ 16670, { .can_id = 0x288, .can_dlc = 8 }, ev_handler }, { 16670, { .can_id = 0x288, .can_dlc = 8 }, ev_handler },
{ 16670, { .can_id = 0x320, .can_dlc = 8 }, ev_handler }, { 16670, { .can_id = 0x320, .can_dlc = 8 }, ev_handler },
{ 16670, { .can_id = 0x5a0, .can_dlc = 8 }, ev_handler } { 16670, { .can_id = 0x5a0, .can_dlc = 8 }, ev_handler }
}; };
static void table_update(hexagram_schedule_slot *table, static void table_update(hexagram_schedule_table_entry *table,
hexagram_telemetry_dash_packet *packet) { hexagram_telemetry_dash_packet *packet) {
float speed_ms = sqrt(powf(packet->velocity.x, 2) float speed_ms = sqrt(powf(packet->velocity.x, 2)
+ powf(packet->velocity.z, 2)); + powf(packet->velocity.z, 2));
@ -67,6 +67,7 @@ static void table_update(hexagram_schedule_slot *table,
} }
int hexagram_main_dash2can(int argc, char **argv) { int hexagram_main_dash2can(int argc, char **argv) {
size_t i, count = 4;
int sock; int sock;
struct sockaddr_in sockaddr; struct sockaddr_in sockaddr;
@ -100,7 +101,11 @@ int hexagram_main_dash2can(int argc, char **argv) {
goto error_can_if_open; goto error_can_if_open;
} }
if ((schedule = hexagram_schedule_create(table, 4, can_if)) == NULL) { for (i=0; i<count; i++) {
memset(table[i].frame.data, '\0', sizeof(table[i].frame.data));
}
if ((schedule = hexagram_schedule_create(table, count, can_if)) == NULL) {
goto error_schedule_create; goto error_schedule_create;
} }

14
include/hexagram/schedule.h
View file

@ -1,19 +1,25 @@
#ifndef _HEXAGRAM_SCHEDULE_H #ifndef _HEXAGRAM_SCHEDULE_H
#define _HEXAGRAM_SCHEDULE_H #define _HEXAGRAM_SCHEDULE_H
#include <time.h> #include <stdint.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/time.h> #include <sys/time.h>
#include <time.h>
#include <signal.h> #include <signal.h>
#include <hexagram/can.h> #include <hexagram/can.h>
typedef int (*hexagram_schedule_handler)(struct can_frame *, void *); typedef int (*hexagram_schedule_handler)(struct can_frame *, void *);
typedef struct _hexagram_schedule_slot { typedef struct _hexagram_schedule_table_entry {
time_t interval_us; time_t interval_us;
struct can_frame frame; struct can_frame frame;
hexagram_schedule_handler handler; hexagram_schedule_handler handler;
} hexagram_schedule_table_entry;
typedef struct _hexagram_schedule_slot {
time_t interval_us;
hexagram_schedule_handler handler;
} hexagram_schedule_slot; } hexagram_schedule_slot;
typedef struct _hexagram_schedule { typedef struct _hexagram_schedule {
@ -24,11 +30,13 @@ typedef struct _hexagram_schedule {
size_t current, size_t current,
count; count;
hexagram_schedule_table_entry *table;
void *ctx; void *ctx;
int error; int error;
} hexagram_schedule; } hexagram_schedule;
hexagram_schedule *hexagram_schedule_create(hexagram_schedule_slot *table, hexagram_schedule *hexagram_schedule_create(hexagram_schedule_table_entry *table,
size_t count, size_t count,
void *ctx); void *ctx);

12
src/schedule.c
View file

@ -42,7 +42,8 @@ static void _ev_notify(union sigval sv) {
hexagram_schedule_slot *slot = _slot(schedule, schedule->current); hexagram_schedule_slot *slot = _slot(schedule, schedule->current);
time_t delay = slot->interval_us; time_t delay = slot->interval_us;
if (slot->handler(&slot->frame, schedule->ctx) < 0) { if (slot->handler(&(schedule->table[schedule->current].frame),
schedule->ctx) < 0) {
schedule->error = errno; schedule->error = errno;
break; break;
@ -65,7 +66,7 @@ static void _ev_notify(union sigval sv) {
} }
static int _schedule_init(hexagram_schedule *schedule, static int _schedule_init(hexagram_schedule *schedule,
hexagram_schedule_slot *table, hexagram_schedule_table_entry *table,
size_t count, size_t count,
void *ctx) { void *ctx) {
size_t i; size_t i;
@ -75,6 +76,7 @@ static int _schedule_init(hexagram_schedule *schedule,
schedule->current = 0; schedule->current = 0;
schedule->error = 0; schedule->error = 0;
schedule->count = count; schedule->count = count;
schedule->table = table;
schedule->ctx = ctx; schedule->ctx = ctx;
schedule->ev.sigev_notify = SIGEV_THREAD; schedule->ev.sigev_notify = SIGEV_THREAD;
@ -90,8 +92,8 @@ static int _schedule_init(hexagram_schedule *schedule,
for (i=0; i<count; i++) { for (i=0; i<count; i++) {
hexagram_schedule_slot *slot = &((hexagram_schedule_slot *)(schedule + 1))[i]; hexagram_schedule_slot *slot = &((hexagram_schedule_slot *)(schedule + 1))[i];
memset(slot->frame.data, '\0', sizeof(*(slot->frame.data))); slot->interval_us = table[i].interval_us;
memcpy(slot, &table[i], sizeof(hexagram_schedule_slot)); slot->handler = table[i].handler;
} }
qsort(schedule + 1, count, sizeof(hexagram_schedule_slot), _slot_cmp); qsort(schedule + 1, count, sizeof(hexagram_schedule_slot), _slot_cmp);
@ -102,7 +104,7 @@ error_timer_create:
return -1; return -1;
} }
hexagram_schedule *hexagram_schedule_create(hexagram_schedule_slot *table, hexagram_schedule *hexagram_schedule_create(hexagram_schedule_table_entry *table,
size_t count, size_t count,
void *ctx) { void *ctx) {
hexagram_schedule *schedule; hexagram_schedule *schedule;