Skip to content

Commit 50498e4

Browse files
BsAtHomeBsAtHome
committed
Update dependencies with PktUART update.
Fix crash bugs and warnings in modbus template.
1 parent 86974fe commit 50498e4

2 files changed

Lines changed: 111 additions & 51 deletions

File tree

‎src/hal/components/mesa_pktgyro_test.comp‎

Lines changed: 20 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@ Check linuxcnc.log for debug output.
3939
author "Boris Skegin";
4040
license "GPL";
4141

42+
include "hal/drivers/mesa-hostmot2/hostmot2-serial.h";
4243
include "hal/drivers/mesa-hostmot2/hostmot2.h";
4344

4445

@@ -155,7 +156,22 @@ EXTRA_SETUP(){ // the names parameters are passed in 'prefix'.
155156
Then we read out whatever is in the buffer, send the DISABLE STREAM
156157
datagram and only then set RXEnable bit.
157158
*/
158-
int retval=hm2_pktuart_setup(name, BAUDRATE , 0x0ff20, 0x007f00,1,1);
159+
hm2_pktuart_config_t cfgrx = {
160+
.baudrate = BAUDRATE,
161+
.filterrate = 0,
162+
.drivedelay = 0,
163+
.ifdelay = 127,
164+
.flags = HM2_PKTUART_CONFIG_FLUSH
165+
};
166+
hm2_pktuart_config_t cfgtx = {
167+
.baudrate = BAUDRATE,
168+
.filterrate = 0,
169+
.drivedelay = 0,
170+
.ifdelay = 255,
171+
.flags = HM2_PKTUART_CONFIG_DRIVEAUTO | HM2_PKTUART_CONFIG_FLUSH
172+
};
173+
int retval = hm2_pktuart_config(name, &cfgrx, &cfgtx, 0);
174+
// int retval=hm2_pktuart_setup(name, BAUDRATE , 0x0ff20, 0x007f00,1,1);
159175
if (retval<0)
160176
{
161177
rtapi_print_msg(1, "PktUART for gyro setup problem: %d\n", retval);
@@ -215,7 +231,9 @@ EXTRA_SETUP(){ // the names parameters are passed in 'prefix'.
215231
}
216232

217233
// Now we set RxEnable bit and clear Rx/Tx registers
218-
retval=hm2_pktuart_setup(name, BAUDRATE , 0x0ff20, 0x007f08,1,1);
234+
cfgrx.flags |= HM2_PKTUART_CONFIG_RXEN;
235+
retval = hm2_pktuart_config(name, &cfgrx, &cfgtx, 0);
236+
//retval=hm2_pktuart_setup(name, BAUDRATE , 0x0ff20, 0x007f08,1,1);
219237
if (retval<0)
220238
{
221239
rtapi_print_msg(1, "PktUART for gyro setup problem: %d\n", retval);

‎src/hal/drivers/mesa-hostmot2/modbus/mesa_modbus.c.tmpl‎

Lines changed: 91 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -38,10 +38,21 @@ MODULE_LICENSE("GPL");
3838

3939
#define MAX_CHAN 8
4040

41-
char error_codes[12][30]={"NULL", "Illegal Function", "Illegal Data Address",
42-
"Illegal Data Value", "Server Device Failure", "Acknowledge",
43-
"Server Device Busy", "Negative Acknowledge", "Memory Parity Error",
44-
"Gateway Path Unavailable", "Gateway Failed to Respond", "Comm Timeout"};
41+
static const char *error_codes[]={
42+
"Invalid exception code 0",
43+
"Illegal Function",
44+
"Illegal Data Address",
45+
"Illegal Data Value",
46+
"Server Device Failure",
47+
"Acknowledge",
48+
"Server Device Busy",
49+
"Negative Acknowledge",
50+
"Memory Parity Error",
51+
"Unknown exception code 9"
52+
"Gateway Path Unavailable",
53+
"Gateway Failed to Respond",
54+
"Comm Timeout"
55+
};
4556

4657
// This is needed by the header file
4758
typedef struct {
@@ -78,6 +89,17 @@ typedef struct {
7889
#define DEBUG 1
7990
#endif
8091

92+
// Values within +/-EPSILON are considered zero
93+
#define EPSILON (1e-12)
94+
95+
// Rate is the update-hz pin frequency. There is no point in slower than once
96+
// every 1000 seconds or faster than 10000 times per second. The slowness of
97+
// the RS485/Modbus protocol prevents any real fast communication.
98+
// Anyway, the current implementation does not honor the frequency correctly.
99+
// This limit will at least prevent a divide-by-zero.
100+
#define RATE_MIN (1e-3)
101+
#define RATE_MAX (1e4)
102+
81103
enum {
82104
START,
83105
WAIT_FOR_SEND_BEGIN,
@@ -98,12 +120,12 @@ typedef struct {
98120
rtapi_s64 *buff;
99121
hal_bit_t *fault;
100122
hal_u32_t *last_err;
101-
hal_u32_t address;
102-
hal_u32_t baudrate;
103-
hal_u32_t parity;
104-
hal_u32_t txdelay;
105-
hal_u32_t rxdelay;
106-
hal_u32_t drive_delay;
123+
hal_s32_t address;
124+
hal_s32_t baudrate;
125+
hal_s32_t parity;
126+
hal_s32_t txdelay;
127+
hal_s32_t rxdelay;
128+
hal_s32_t drive_delay;
107129
hal_float_t rate;
108130
} hm2_modbus_hal_t;
109131

@@ -146,13 +168,12 @@ typedef struct {
146168
static int comp_id;
147169
static hm2_modbus_t *m;
148170
static void process(void *arg, long period);
149-
int parse_data_frame(hm2_modbus_inst_t *inst);
150-
int build_data_frame(hm2_modbus_inst_t *inst);
151-
int do_setup(hm2_modbus_inst_t *inst);
171+
static int parse_data_frame(hm2_modbus_inst_t *inst);
172+
static int build_data_frame(hm2_modbus_inst_t *inst);
173+
static int do_setup(hm2_modbus_inst_t *inst);
152174
static uint16_t RTU_CRC(rtapi_u8* buf, int len);
153-
static int clocklow = 0;
154175

155-
char *ports[MAX_CHAN];
176+
static char *ports[MAX_CHAN];
156177
RTAPI_MP_ARRAY_STRING(ports, MAX_CHAN, "PktUART names");
157178

158179
int rtapi_app_main(void){
@@ -161,7 +182,6 @@ int rtapi_app_main(void){
161182
int c; // channel loop
162183
int p; // pin loops
163184
int j; // generic loops
164-
char hal_name[HAL_NAME_LEN];
165185

166186
rtapi_set_msg_level(DEBUG);
167187

@@ -207,11 +227,8 @@ int rtapi_app_main(void){
207227
inst->hal->buff = (rtapi_s64 *) hal_malloc(inst->num_pins * sizeof(rtapi_s64));
208228

209229
rtapi_strlcpy(inst->port, ports[i], HAL_NAME_LEN);
210-
retval = rtapi_snprintf(hal_name, HAL_NAME_LEN, COMP_NAME".%02i", i);
211-
if (retval >= HAL_NAME_LEN) {
212-
goto fail0;
213-
}
214-
retval = hal_export_funct(hal_name, process, inst, 1, 0, comp_id);
230+
231+
retval = hal_export_functf(process, inst, 1, 0, comp_id, COMP_NAME".%02i", i);
215232
if (retval < 0) {
216233
rtapi_print_msg(RTAPI_MSG_ERR, COMP_NAME" ERROR: function export failed\n");
217234
goto fail0;
@@ -262,7 +279,7 @@ int rtapi_app_main(void){
262279
" for function 5, resetting\n");
263280
ch->count = 1;
264281
}
265-
// deliberate fall-through
282+
/* Fallthrough */
266283
case 15: // write multiple coils
267284
if (ch->count > 8 * MAX_MSG_LEN){
268285
ch-> count = 8 * MAX_MSG_LEN;
@@ -273,7 +290,7 @@ int rtapi_app_main(void){
273290
ch->count);
274291
}
275292
dir = HAL_IN;
276-
// deliberate fall-through
293+
/* Fallthrough */
277294
case 1: // read coils
278295
case 2: // read inputs
279296
if (ch->count > 1){
@@ -296,7 +313,7 @@ int rtapi_app_main(void){
296313
" for function %i, resetting\n", ch->func);
297314
ch->count = 1;
298315
}
299-
// deliberate fall-through
316+
/* Fallthrough */
300317
case 16: // write multiple registers
301318
if (ch->count > MAX_MSG_LEN / 2){
302319
ch-> count = MAX_MSG_LEN/2;
@@ -307,7 +324,7 @@ int rtapi_app_main(void){
307324
ch->count);
308325
}
309326
dir = HAL_IN;
310-
// deliberate fall-through
327+
/* Fallthrough */
311328
case 3: // read holding registers
312329
case 4: // read input registers
313330
for (j = 0; j < ch->count; j++){
@@ -414,9 +431,8 @@ int rtapi_app_main(void){
414431

415432
}
416433

417-
int do_setup(hm2_modbus_inst_t *inst){
434+
static int do_setup(hm2_modbus_inst_t *inst){
418435

419-
int txmode, rxmode, filter;
420436
int retval;
421437

422438
if (inst->baudrate == inst->hal->baudrate
@@ -432,10 +448,25 @@ int do_setup(hm2_modbus_inst_t *inst){
432448
inst->txdelay = inst->hal->txdelay;
433449
inst->rxdelay = inst->hal->rxdelay;
434450
inst->drive_delay = inst->hal->drive_delay;
435-
436-
437-
retval = hm2_pktuart_setup_rx(inst->port, inst->baudrate, inst->baudrate *2, inst->parity, inst->rxdelay, 1, 1);
438-
retval += hm2_pktuart_setup_tx(inst->port, inst->baudrate, inst->parity, inst->txdelay, 1, 1, inst->drive_delay);
451+
452+
unsigned flg;
453+
flg = inst->parity != 0 ? HM2_PKTUART_CONFIG_PARITYEN : 0;
454+
flg |= inst->parity == 1 ? HM2_PKTUART_CONFIG_PARITYODD : 0;
455+
hm2_pktuart_config_t cfgrx = {
456+
.baudrate = inst->baudrate,
457+
.filterrate = inst->baudrate * 2,
458+
.drivedelay = 0,
459+
.ifdelay = inst->rxdelay,
460+
.flags = flg | HM2_PKTUART_CONFIG_RXEN | HM2_PKTUART_CONFIG_RXMASKEN
461+
};
462+
hm2_pktuart_config_t cfgtx = {
463+
.baudrate = inst->baudrate,
464+
.filterrate = 0,
465+
.drivedelay = inst->drive_delay,
466+
.ifdelay = inst->txdelay,
467+
.flags = flg | HM2_PKTUART_CONFIG_DRIVEEN | HM2_PKTUART_CONFIG_DRIVEAUTO
468+
};
469+
retval = hm2_pktuart_config(inst->port, &cfgrx, &cfgtx, 0);
439470

440471
if (retval<0)
441472
{
@@ -446,7 +477,7 @@ int do_setup(hm2_modbus_inst_t *inst){
446477
return 0;
447478
}
448479

449-
int send_modbus_pkt(hm2_modbus_inst_t *inst){
480+
static int send_modbus_pkt(hm2_modbus_inst_t *inst){
450481
hm2_modbus_channel_t *ch = &(inst->chans[inst->index]);
451482
rtapi_u16 checksum;
452483
rtapi_u16 fsizes[1];
@@ -467,7 +498,7 @@ int send_modbus_pkt(hm2_modbus_inst_t *inst){
467498
return 0;
468499
}
469500

470-
void do_timeout(hm2_modbus_inst_t *inst){
501+
static void do_timeout(hm2_modbus_inst_t *inst){
471502
if (inst->state != inst->old_state){
472503
inst->iter = 0;
473504
inst->old_state = inst->state;
@@ -484,10 +515,10 @@ void do_timeout(hm2_modbus_inst_t *inst){
484515
rtapi_print_msg(RTAPI_MSG_INFO, "%i timeout %i\r", inst->iter, inst->state);
485516
}
486517

487-
void process(void *arg, long period) {
518+
static void process(void *arg, long period) {
488519
static long timer = 0;
489520
hm2_modbus_inst_t *inst = arg;
490-
521+
real_t rate;
491522
int r;
492523

493524
rtapi_u32 rxstatus = hm2_pktuart_get_rx_status(inst->port);
@@ -496,7 +527,8 @@ void process(void *arg, long period) {
496527
switch (inst->state) {
497528
case START:
498529

499-
if (inst->hal->rate > 0 && (timer -= period) > 0) break;
530+
rate = inst->hal->rate;
531+
if (rate >= RATE_MIN && rate <= RATE_MAX && (timer -= period) > 0) break;
500532

501533
rtapi_print_msg(RTAPI_MSG_INFO, "START txstatus = %08X rxstatus = %08X\n", txstatus, rxstatus);
502534

@@ -517,7 +549,8 @@ void process(void *arg, long period) {
517549
if (build_data_frame(inst)){ // if data has changed
518550
r = send_modbus_pkt(inst);
519551
inst->state = WAIT_FOR_SEND_BEGIN;
520-
timer = 1e9 / inst->hal->rate;
552+
if(rate >= RATE_MIN && rate <= RATE_MAX)
553+
timer = 1e9 / rate;
521554
}
522555

523556
break;
@@ -600,34 +633,34 @@ void process(void *arg, long period) {
600633

601634
}
602635

603-
int ch_append8(hm2_modbus_channel_t *ch, rtapi_u8 v){
636+
static int ch_append8(hm2_modbus_channel_t *ch, rtapi_u8 v){
604637
int r = 0;
605638
if (ch->ptr++ > MAX_MSG_LEN) return -MAX_MSG_LEN;
606639
if (ch->data[ch->ptr] != v) r = 1; // flag data changed
607640
ch->data[ch->ptr] = v;
608641
return r;
609642
}
610643

611-
int ch_append16(hm2_modbus_channel_t *ch, rtapi_u16 v){
644+
static int ch_append16(hm2_modbus_channel_t *ch, rtapi_u16 v){
612645
int r = 0;
613646
r = ch_append8(ch, (rtapi_u8)(v >> 8));
614647
r += ch_append8(ch, (rtapi_u8)(v & 0xFF));
615648
return r;
616649
}
617650

618-
int ch_init(hm2_modbus_channel_t *ch, hm2_modbus_hal_t hal){
651+
static int ch_init(hm2_modbus_channel_t *ch, hm2_modbus_hal_t hal){
619652
int r;
620653
ch->ptr = -1;
621654
r = ch_append8(ch, hal.address);
622655
r = ch_append8(ch, ch->func);
623656
return r;
624657
}
625658

626-
int build_data_frame(hm2_modbus_inst_t *inst){
659+
static int build_data_frame(hm2_modbus_inst_t *inst){
627660
hm2_modbus_channel_t *ch = &(inst->chans[inst->index]);
628661
hm2_modbus_hal_t hal = *inst->hal;
629662
rtapi_u8 acc = 0;
630-
int r;
663+
int r = 0;
631664
int p = ch->start_pin;
632665
int i;
633666

@@ -663,7 +696,7 @@ int build_data_frame(hm2_modbus_inst_t *inst){
663696
break;
664697
case HAL_FLOAT:
665698
rtapi_print_msg(RTAPI_MSG_INFO, "671 %f %f %f \n", hal.pins[p]->f, hal.pin2[p]->f, *hal.scale[p]);
666-
if (*hal.scale[p] != 0){
699+
if (*hal.scale[p] <= -EPSILON || *hal.scale[p] >= +EPSILON){
667700
r+= ch_append16(ch, (rtapi_u16)((hal.pins[p]->f - hal.pin2[p]->f) / *hal.scale[p])) ;
668701
}
669702
rtapi_print_msg(RTAPI_MSG_INFO, "678\n");
@@ -697,8 +730,11 @@ int build_data_frame(hm2_modbus_inst_t *inst){
697730
r += ch_append16(ch, (rtapi_s16)hal.pins[p]->s);
698731
break;
699732
case HAL_FLOAT:
700-
if (*(hal.scale[p]) != 0){
733+
if (*hal.scale[p] <= -EPSILON || *hal.scale[p] >= +EPSILON){
701734
r+= ch_append16(ch, (rtapi_u16)((hal.pins[p]->f - hal.pin2[p]->f) / *hal.scale[p])) ;
735+
} else {
736+
// Prevent (near) divide-by-zero and assume scale 1.0
737+
r+= ch_append16(ch, (rtapi_u16)(hal.pins[p]->f - hal.pin2[p]->f));
702738
}
703739
break;
704740
default:
@@ -713,7 +749,7 @@ int build_data_frame(hm2_modbus_inst_t *inst){
713749
return r;
714750
}
715751

716-
int parse_data_frame(hm2_modbus_inst_t *inst){
752+
static int parse_data_frame(hm2_modbus_inst_t *inst){
717753
hm2_modbus_channel_t *ch = &(inst->chans[inst->index]);
718754
hm2_modbus_hal_t hal = *inst->hal;
719755
rtapi_u32 *data = inst->rxdata;
@@ -723,7 +759,7 @@ int parse_data_frame(hm2_modbus_inst_t *inst){
723759
int b = 0;
724760
int p;
725761
int tmp;
726-
rtapi_u8 bytes[MAX_MSG_LEN + 4];
762+
rtapi_u8 bytes[MAX_MSG_LEN + 4] = {};
727763
rtapi_u16 checksum;
728764

729765
rtapi_print_msg(RTAPI_MSG_INFO, "Return packet is ");
@@ -808,8 +844,13 @@ int parse_data_frame(hm2_modbus_inst_t *inst){
808844
case 143: // 15 + error bit
809845
case 144: // 16 + error bit
810846
ch->data[1] = 0xFF; // Write invalid data to force re-send
811-
rtapi_print_msg(RTAPI_MSG_ERR, "Modbus error response function %i error %s\n",
812-
bytes[1] & 0x8F, error_codes[bytes[2]]);
847+
if(bytes[2] < sizeof(error_codes) / sizeof(*error_codes)) {
848+
rtapi_print_msg(RTAPI_MSG_ERR, "Modbus error response function %i error %s\n",
849+
bytes[1] & 0x7F, error_codes[bytes[2]]);
850+
} else {
851+
rtapi_print_msg(RTAPI_MSG_ERR, "Modbus error response function %i returned invalid exception code %u\n",
852+
bytes[1] & 0x7F, (unsigned)bytes[2]);
853+
}
813854
break;
814855
default:
815856
rtapi_print_msg(RTAPI_MSG_ERR, "Unknown or unsupported Modbus function code\n");
@@ -828,7 +869,7 @@ if (m != NULL) rtapi_kfree(m);
828869
hal_exit(comp_id);
829870
}
830871

831-
uint16_t RTU_CRC(rtapi_u8* buf, int len){
872+
static uint16_t RTU_CRC(rtapi_u8* buf, int len){
832873
uint16_t crc = 0xFFFF;
833874
int pos;
834875
int i;
@@ -849,3 +890,4 @@ uint16_t RTU_CRC(rtapi_u8* buf, int len){
849890
return crc;
850891
}
851892

893+
// vim: syn=c

0 commit comments

Comments
 (0)