mirror of
https://github.com/eddyem/zeiss_utils.git
synced 2025-12-06 02:35:15 +03:00
initial commit
This commit is contained in:
eddyem
parent
82e167bb74
commit
cbec6c1f63
55
.gitignore
vendored
55
.gitignore
vendored
@ -1,52 +1,3 @@
|
||||
# Prerequisites
|
||||
*.d
|
||||
|
||||
# Object files
|
||||
*.o
|
||||
*.ko
|
||||
*.obj
|
||||
*.elf
|
||||
|
||||
# Linker output
|
||||
*.ilk
|
||||
*.map
|
||||
*.exp
|
||||
|
||||
# Precompiled Headers
|
||||
*.gch
|
||||
*.pch
|
||||
|
||||
# Libraries
|
||||
*.lib
|
||||
*.a
|
||||
*.la
|
||||
*.lo
|
||||
|
||||
# Shared objects (inc. Windows DLLs)
|
||||
*.dll
|
||||
*.so
|
||||
*.so.*
|
||||
*.dylib
|
||||
|
||||
# Executables
|
||||
*.exe
|
||||
*.out
|
||||
*.app
|
||||
*.i*86
|
||||
*.x86_64
|
||||
*.hex
|
||||
|
||||
# Debug files
|
||||
*.dSYM/
|
||||
*.su
|
||||
*.idb
|
||||
*.pdb
|
||||
|
||||
# Kernel Module Compile Results
|
||||
*.mod*
|
||||
*.cmd
|
||||
.tmp_versions/
|
||||
modules.order
|
||||
Module.symvers
|
||||
Mkfile.old
|
||||
dkms.conf
|
||||
mk/*
|
||||
Zphocus.*
|
||||
can_phocus
|
||||
|
||||
68
Z1000_focus/DS406_canopen.h
Normal file
68
Z1000_focus/DS406_canopen.h
Normal file
@ -0,0 +1,68 @@
|
||||
/**
|
||||
Commands & other data for absolute rotary encoder DS406
|
||||
**/
|
||||
#pragma once
|
||||
#ifndef DS406_CANOPEN_H__
|
||||
#define DS406_CANOPEN_H__
|
||||
|
||||
#define DS406_DEVTYPE 0x1000
|
||||
#define DS406_ERRORREG 0x1001
|
||||
#define DS406_MANUFSTATUS 0x1002
|
||||
#define DS406_COBID_SYNC 0x1005
|
||||
#define DS406_MANDEVNAME 0x1008
|
||||
#define DS406_MANHW_VERS 0x1009
|
||||
#define DS406_MANSW_VERS 0x100A
|
||||
#define DS406_STORE_PARAMS 0x1010
|
||||
#define DS406_RESTORE_DEF 0x1011
|
||||
#define DS406_COBID_EMERG 0x1014
|
||||
#define DS406_INHIB_TM_EMERG 0x1015
|
||||
#define DS406_PROD_HEARTB_TM 0x1017
|
||||
#define DS406_IDENT_OBJ 0x1018
|
||||
#define DS406_ERROR_BEHAVIOUR 0x1029
|
||||
|
||||
#define DS406_PDO1 0x1800
|
||||
#define DS406_PDO2 0x1801
|
||||
#define DS406_PDO1_MAPPED 0x1A00
|
||||
#define DS406_PDO2_MAPPED 0x1A01
|
||||
#define DS406_PDO3_MAPPED 0x1A02
|
||||
#define DS406_PDO4_MAPPED 0x1A03
|
||||
|
||||
#define DS406_BAUDRATE 0x2100
|
||||
#define DS406_NODE_NUMBER 0x2101
|
||||
#define DS406_TERMINATOR 0x2102
|
||||
#define DS406_NMT_AUTOSTART 0x2104
|
||||
#define DS406_PDO_TRIG_THRES 0x2105
|
||||
#define DS406_FILTER_CONFIG 0x2106
|
||||
#define DS406_CUSTOMER_MEMRY 0x2110
|
||||
#define DS406_SENSOR_AMPLITUDE 0x2200
|
||||
#define DS406_TARG_FREQ_DEVIAT 0x2201
|
||||
|
||||
#define DS406_OPER_PARAMS 0x6000
|
||||
#define DS406_MEAS_UNITS_PERREV 0x6001
|
||||
#define DS406_TOT_MEAS_RANGE 0x6002
|
||||
#define DS406_PRESET_VAL 0x6003
|
||||
#define DS406_POSITION_VAL 0x6004
|
||||
#define DS406_PDO_IS_POSITION 0x60040020
|
||||
#define DS406_POS_RAW_VAL 0x600C
|
||||
#define DS406_SPEED_VAL 0x6030
|
||||
#define DS406_PDO_IS_SPEED 0x60300110
|
||||
#define DS406_CYCLE_TIMER 0x6200
|
||||
#define DS406_WORK_AREA_STATE 0x6400
|
||||
#define DS406_WORK_AREA_LIM_LO 0x6401
|
||||
#define DS406_WORK_AREA_LIM_HI 0x6401
|
||||
#define DS406_OPER_STATUS 0x6500
|
||||
#define DS406_TURN_RESOLUT 0x6501
|
||||
#define DS406_REVOL_NUMBER 0x6502
|
||||
#define DS406_ALARMS 0x6503
|
||||
#define DS406_SUPP_ALARMS 0x6504
|
||||
#define DS406_WARNINGS 0x6505
|
||||
#define DS406_SUPP_WARNINGS 0x6506
|
||||
#define DS406_PROF_SW_VERS 0x6507
|
||||
#define DS406_OFFSET_VAL 0x6509
|
||||
#define DS406_MODULE_ID 0x650A
|
||||
#define DS406_SERIAL_NUMBER 0x650B
|
||||
|
||||
|
||||
|
||||
|
||||
#endif // DS406_CANOPEN_H__
|
||||
70
Z1000_focus/HW_dependent.h
Normal file
70
Z1000_focus/HW_dependent.h
Normal file
@ -0,0 +1,70 @@
|
||||
/*
|
||||
* This file is part of the Zphocus project.
|
||||
* Copyright 2019 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef HW_DEPENDENT__
|
||||
#define HW_DEPENDENT__
|
||||
|
||||
#include "motor_cancodes.h"
|
||||
|
||||
// amount of encoder's counts when stop = rawspeed^2/STOPPING_COEFF
|
||||
#define STOPPING_COEFF (117500L)
|
||||
|
||||
// direction of motor rotation positive to encoder (1 - negative)
|
||||
#define MOTOR_REVERSE (0)
|
||||
|
||||
// End switches (lowest bit - DI00)
|
||||
// clockwise: DI4
|
||||
#define ESW_CW (1<<4)
|
||||
// counterclockwise: DI5
|
||||
#define ESW_CCW (1<<5)
|
||||
// parameter indexes of esw:
|
||||
#define PAR_CW_IDX PAR_DI04_IDX
|
||||
#define PAR_CCW_IDX PAR_DI05_IDX
|
||||
|
||||
// constants
|
||||
// translate raw values to revolutions per minute
|
||||
#define REVMIN(x) (x/5)
|
||||
// rev/min to raw speed value
|
||||
#define RAWSPEED(x) (x*5)
|
||||
// max/min speed (rev/min)
|
||||
#define MAXSPEED 1200
|
||||
#define MINSPEED 130
|
||||
// moving timeout (*0.01s)
|
||||
#define MOVING_TIMEOUT 5000
|
||||
|
||||
// raw position precision
|
||||
#define RAWPOS_TOLERANCE 20
|
||||
// raw dF value for accurate focussing
|
||||
#define dF0 100
|
||||
// minimal & maximal focus positions (should be >min+dF0 & <max-dF0)
|
||||
#define FOCMIN 200
|
||||
#define FOCMAX 320000
|
||||
// the same in mm
|
||||
#define FOCMIN_MM 0.1
|
||||
#define FOCMAX_MM 76.0
|
||||
// permitted distance to end-switch
|
||||
#define ESW_DIST_ALLOW 1.0
|
||||
|
||||
// constants for focus conversion: foc_mm = (foc_raw - FOCRAW_MM0) / FOCSCALE_MM
|
||||
#define FOCSCALE_MM 4096.
|
||||
#define FOCRAW_MM0 0
|
||||
#define FOC_RAW2MM(x) ((x-FOCRAW_MM0)/FOCSCALE_MM)
|
||||
#define FOC_MM2RAW(x) (FOCRAW_MM0+x*FOCSCALE_MM)
|
||||
|
||||
#endif // HW_DEPENDENT__
|
||||
42
Z1000_focus/Makefile
Normal file
42
Z1000_focus/Makefile
Normal file
@ -0,0 +1,42 @@
|
||||
# run `make DEF="-D... -D..."` to add extra defines
|
||||
PROGRAM := can_phocus
|
||||
LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all -pthread
|
||||
SRCS := $(wildcard *.c)
|
||||
DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
|
||||
OBJDIR := mk
|
||||
CFLAGS += -O2 -Wall -Werror -Wextra -Wno-trampolines -std=gnu99
|
||||
OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o))
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
CC ?= gcc
|
||||
$(info GCC is: ${CC})
|
||||
#CXX = g++
|
||||
|
||||
all : $(OBJDIR) $(PROGRAM)
|
||||
|
||||
$(PROGRAM) : $(OBJS)
|
||||
@echo -e "\t\tLD $(PROGRAM)"
|
||||
$(CC) $(LDFLAGS) $(OBJS) -o $(PROGRAM)
|
||||
|
||||
$(OBJDIR):
|
||||
mkdir $(OBJDIR)
|
||||
|
||||
ifneq ($(MAKECMDGOALS),clean)
|
||||
-include $(DEPS)
|
||||
endif
|
||||
|
||||
$(OBJDIR)/%.o: %.c
|
||||
@echo -e "\t\tCC $<"
|
||||
$(CC) -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@ $<
|
||||
|
||||
clean:
|
||||
@echo -e "\t\tCLEAN"
|
||||
@rm -f $(OBJS) $(DEPS)
|
||||
@rmdir $(OBJDIR) 2>/dev/null || true
|
||||
|
||||
xclean: clean
|
||||
@rm -f $(PROGRAM)
|
||||
|
||||
gentags:
|
||||
CFLAGS="$(CFLAGS) $(DEFINES)" geany -g $(PROGRAM).c.tags *[hc] 2>/dev/null
|
||||
|
||||
.PHONY: gentags clean xclean
|
||||
86
Z1000_focus/Readme_SEW
Normal file
86
Z1000_focus/Readme_SEW
Normal file
@ -0,0 +1,86 @@
|
||||
======= Идентификатор устройства: =======
|
||||
10 - reserved
|
||||
9 - 0 - данные процесса, 1 - данные параметров
|
||||
8 - \
|
||||
7 - |
|
||||
6 - | address
|
||||
5 - |
|
||||
4 - |
|
||||
3 - /
|
||||
2 - \
|
||||
1 - | function
|
||||
0 - /
|
||||
|
||||
арес задается в настройках привода
|
||||
|
||||
функция:
|
||||
3 - выходные данные процесса (PO)
|
||||
4 - входные данные процесса (PI)
|
||||
5 - синхронизация
|
||||
6 - (для группового адреса) - групповое PO
|
||||
|
||||
если данные параметров (9-й бит == 1), то функция:
|
||||
3 - запрос данных параметров
|
||||
4 - ответ
|
||||
|
||||
|
||||
======= Контрольные слова =======
|
||||
|
||||
Первые 8 бит CW1&CW2 (со звездочкой - то, что используется):
|
||||
7 - резерв
|
||||
6 - *сброс ошибки
|
||||
5 - набор параметров
|
||||
4 - генератор темпа
|
||||
3 - резерв
|
||||
2 - *разрешение/стоп
|
||||
1 - *разрешение/быстрый стоп
|
||||
0 - *блокировка/разрешение
|
||||
|
||||
Старшие 8 бит у CW1 не использую, у CW2 старшие 8 бит - виртуальные клеммы (цифровые входы)
|
||||
|
||||
|
||||
======= Слова состояния =======
|
||||
|
||||
Младшие 8 бит обоих:
|
||||
7 - резерв
|
||||
6 - резерв
|
||||
5 - *неисправность/предупреждение
|
||||
4 - набор параметров 2/1
|
||||
3 - генератор темпа 2/1
|
||||
2 - *PO разблокированы/заблокированы
|
||||
1 - *готов/не готов
|
||||
0 - *выход разблокирован/заблокирован
|
||||
|
||||
Биты 1+7:
|
||||
0+0 - не готов
|
||||
0+1 - ошибка
|
||||
1+0 - готов
|
||||
1+1 - предупреждение
|
||||
|
||||
Старшие 8 бит CW1 зависит от бита 5 младшего слова:
|
||||
1 - код ошибки
|
||||
0 - состояние
|
||||
|
||||
Коды состояний:
|
||||
0 - не готов
|
||||
1 - блокировка
|
||||
2 - нет разрешения
|
||||
3 - ток удержания
|
||||
4 - разрешение
|
||||
5 - регулирование
|
||||
8 - заводские настройки
|
||||
13 - захват
|
||||
16 - ожидание данных
|
||||
17 - безопасный останов
|
||||
|
||||
|
||||
======= Чтение/запись парметров =======
|
||||
Индекс и субиндекс параметров можно посмотреть в сплывающей подсказке конфигуратора.
|
||||
Идентификатор - с 1 в девятом бите.
|
||||
Посылка и ответ состоят из восьми байт. Первые четыре байта:
|
||||
0 - команда (0x31 для считывания параметра и 0x32 для записи)
|
||||
1 - субиндекс
|
||||
2 - индекс, старший байт
|
||||
3 - индекс, младший байт
|
||||
|
||||
Четыре младших байта - данные.
|
||||
716
Z1000_focus/can_encoder.c
Normal file
716
Z1000_focus/can_encoder.c
Normal file
@ -0,0 +1,716 @@
|
||||
/*
|
||||
* This file is part of the Zphocus project.
|
||||
* Copyright 2019 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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 <string.h> // memcpy
|
||||
#include <math.h> // fabs
|
||||
#include "canopen.h"
|
||||
#include "can_encoder.h"
|
||||
#include "usefull_macros.h"
|
||||
#include "DS406_canopen.h"
|
||||
#include "motor_cancodes.h"
|
||||
#include "HW_dependent.h"
|
||||
#include "socket.h"
|
||||
|
||||
// printf when -v
|
||||
extern int verbose(const char *fmt, ...);
|
||||
|
||||
// CAN bus IDs: for motor's functions (PI ID [F=4] == PO ID[F=3] + 1) and parameters
|
||||
static unsigned long motor_id = 0, motor_p_id = 0;//, bcast_id = 1;
|
||||
// current motor position
|
||||
static unsigned long curposition = 0;
|
||||
// encoder's node number
|
||||
static int encnodenum = 0;
|
||||
|
||||
static canstatus can_write_par(uint8_t subidx, uint16_t idx, uint32_t *parval);
|
||||
static canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval);
|
||||
static int move(unsigned long targposition, int16_t rawspeed);
|
||||
|
||||
/**
|
||||
* @brief init_encoder - encoder's interface initialisation
|
||||
* @param encnode - encoder's node number
|
||||
* @param reset - reset node before operating
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int init_encoder(int encnode, int reset){
|
||||
unsigned long lval;
|
||||
encnodenum = encnode;
|
||||
verbose("cur node: %d\n", encnodenum);
|
||||
if(!initNode(encnodenum)){
|
||||
ERRX("Can't find device with Node address %d!", encnodenum);
|
||||
return 1;
|
||||
}
|
||||
if(reset){
|
||||
verbose("Reset node %d... ", encnodenum);
|
||||
if(resetNode(encnodenum))
|
||||
verbose("Ok!\n");
|
||||
else
|
||||
verbose("Failed.\n");
|
||||
}
|
||||
if(getLong(encnodenum, DS406_DEVTYPE, 0, &lval)){
|
||||
int prof = lval&0xffff;
|
||||
int type = lval>>16;
|
||||
verbose("Found absolute %s-turn rotary encoder DS%d\n", (type==2)?"multi":"single", prof);
|
||||
if(prof != 406 || type != 2){
|
||||
WARNX("The device on node %d isn't a multi-turn encoder!", encnodenum);
|
||||
return 1;
|
||||
}
|
||||
}else{
|
||||
WARNX("Can't get encoder device type");
|
||||
return 1;
|
||||
}
|
||||
/*
|
||||
//setByte(encnodenum, 0x3010, 1, 1); // Posital's encoders specific! (Could not be saved!)
|
||||
if(G->verbose){
|
||||
if(getLong(encnodenum, DS406_TURN_RESOLUT, 0, &lval)) printf("TURN_RESOLUT: %08lx (%ld)\n",lval, lval);
|
||||
if(getShort(encnodenum, DS406_REVOL_NUMBER, 0, &sval)) printf("REVOL_NUMBER: %04x (%d)\n",sval, sval);
|
||||
if(getLong(encnodenum, DS406_MODULE_ID, 1, &lval)) printf("OFFSET VALUE: %08lx (%ld)\n",lval, lval);
|
||||
if(getLong(encnodenum, DS406_MODULE_ID, 2, &lval)) printf("MIN POS: %08lx (%ld)\n",lval, lval);
|
||||
if(getLong(encnodenum, DS406_MODULE_ID, 3, &lval)) printf("MAX POS: %08lx (%ld)\n",lval, lval);
|
||||
}
|
||||
*/
|
||||
verbose("Set operational... ");
|
||||
startNode(encnodenum);
|
||||
int n, i = recvNextPDO(0.1, &n, &lval);
|
||||
int state = getNodeState(encnodenum);
|
||||
verbose("State=%02x\n", state);
|
||||
if(state == NodeOperational) verbose("Ok!\n");
|
||||
else{
|
||||
WARNX("Failed to start node!");
|
||||
returnPreOper();
|
||||
return 1;
|
||||
}
|
||||
if(i > 0) verbose("Node%d PDO%d %08lx\n",n,i,lval);
|
||||
do{
|
||||
int node[4], pdo_n[4];
|
||||
unsigned long pdo_v[4];
|
||||
verbose("Send SYNC...\n");
|
||||
clean_recv();
|
||||
sendSync();
|
||||
can_dsleep(0.01);
|
||||
if((n = recvPDOs(0.5, 4, node, pdo_n, pdo_v))==0)
|
||||
WARNX("No PDO received on SYNC");
|
||||
else for(i=0;i<n;i++)
|
||||
verbose("Node%d PDO%d %08lx (%ld)\n",node[i],pdo_n[i],pdo_v[i],pdo_v[n]);
|
||||
}while(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief go_out_from_ESW - check ESW roles, check ESW states and try to go out if stay on ESW
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int go_out_from_ESW(){
|
||||
uint32_t cw, ccw, parval;
|
||||
// check esw roles
|
||||
if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CW_IDX, &cw)) goto bad;
|
||||
if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &ccw)) goto bad;
|
||||
parval = DI_ENSTOP;
|
||||
if(cw != DI_ENSTOP || ccw != DI_ENSTOP){ // activate enable/stop
|
||||
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CW_IDX, &parval)) goto bad;
|
||||
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &parval)) goto bad;
|
||||
}
|
||||
// check esw
|
||||
eswstate e;
|
||||
if(CAN_NOERR != get_endswitches(&e)){
|
||||
WARNX("Can't read end-switches state");
|
||||
return 1;
|
||||
}
|
||||
if(e == ESW_BOTH_ACTIVE){ // error situation!
|
||||
WARNX("Error: both end-switches are active!");
|
||||
return 1;
|
||||
}
|
||||
if(e == ESW_INACTIVE){
|
||||
DBG("Esw inactive");
|
||||
return 0;
|
||||
}
|
||||
// try to move from esw
|
||||
parval = DI_NOFUNC;
|
||||
uint16_t idx = (e == ESW_CW_ACTIVE) ? PAR_CW_IDX : PAR_CCW_IDX;
|
||||
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, idx, &parval)) goto bad; // turn off motor stopping @esw
|
||||
int16_t speed = (e == ESW_CW_ACTIVE) ? -RAWSPEED(MINSPEED) : RAWSPEED(MINSPEED);
|
||||
for(int i = 0; i < 5; ++i){ // 5 tries to move out
|
||||
getPos(NULL);
|
||||
DBG("try %d, pos: %lu, E=%d", i, curposition, e);
|
||||
unsigned long targ = (e == ESW_CW_ACTIVE) ? curposition - (double)FOCSCALE_MM*0.2 : curposition + (double)FOCSCALE_MM*0.2;
|
||||
|
||||
if(move(targ, speed)) continue;
|
||||
get_endswitches(&e);
|
||||
if(e == ESW_INACTIVE) break;
|
||||
}
|
||||
// return esw state
|
||||
parval = DI_ENSTOP;
|
||||
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CW_IDX, &parval)) goto bad;
|
||||
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &parval)) goto bad;
|
||||
if(e != ESW_INACTIVE){
|
||||
WARNX("Can't move out of end-switch");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
bad:
|
||||
WARNX("Can't get/set esw parameters");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief init_motor_ids - convert motor address into CAN IDs (PO & data)
|
||||
* run this function before any working with motor
|
||||
* @param addr - motor driver address
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int init_motor_ids(int addr){
|
||||
if(addr < 0 || addr > 0x3f){
|
||||
WARNX("Wrong motor address, should be from 0 to 63");
|
||||
return 1;
|
||||
}
|
||||
motor_id = MOTOR_PO_ID(addr);
|
||||
motor_p_id = MOTOR_PAR_ID(addr);
|
||||
DBG("motor POid=%lu, motor_PROCid=%lu", motor_id, motor_p_id);
|
||||
// check esw roles & end-switches state
|
||||
if(go_out_from_ESW()) return 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief getPos - get current encoder's position
|
||||
* @param pos (o) - position value (in mm)
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int getPos(double *pos){
|
||||
int r = !(getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition));
|
||||
if(pos) *pos = FOC_RAW2MM(curposition);
|
||||
return r;
|
||||
}
|
||||
|
||||
// just return value of curposition in mm
|
||||
double curPos(){
|
||||
return FOC_RAW2MM(curposition);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief returnPreOper - return encoder into pre-operational state
|
||||
*/
|
||||
void returnPreOper(){
|
||||
verbose("Return to Pre-operational... ");
|
||||
setPreOper(encnodenum);
|
||||
int state=getNodeState(encnodenum);
|
||||
verbose("State=0x%02x - ", state);
|
||||
if(state == NodePreOperational){
|
||||
verbose("Ok!\n");
|
||||
}else{
|
||||
verbose("Failed!\n");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief fix_targspeed - fix raw speed value if it is greater MAXSPEED or less than MINSPEED
|
||||
* @param targspd (io) - raw target speed
|
||||
*/
|
||||
static void fix_targspeed(int16_t *targspd){
|
||||
if(!targspd) return;
|
||||
DBG("cur spd: %d", *targspd);
|
||||
int16_t sign = (*targspd < 0) ? -1 : 1;
|
||||
int16_t absspd = abs(*targspd);
|
||||
if(absspd < MINSPEED) *targspd = sign * MINSPEED;
|
||||
else if(absspd > MAXSPEED) *targspd = sign * MAXSPEED;
|
||||
DBG("become spd: %d", *targspd);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief can_send_chk - send CAN frame to motor & check answer
|
||||
* @param buf (i) - PO data frame (6 bytes)
|
||||
* @param obuf (o) - received PI data frame
|
||||
* @return status
|
||||
*/
|
||||
static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){
|
||||
const int l = 6; // frame length
|
||||
/*if(G->verbose){
|
||||
printf("Send frame to ID=%lu: ", motor_id);
|
||||
for(int i = 0; i < l; ++i)
|
||||
printf(" %02x", buf[i]);
|
||||
printf("\n");
|
||||
}*/
|
||||
if(can_send_frame(motor_id, l, buf) <= 0){
|
||||
WARNX("Error sending CAN frame (len %d)", l);
|
||||
return CAN_CANTSEND;
|
||||
}
|
||||
int I, rxpnt, idr, dlen;
|
||||
double rxtime;
|
||||
unsigned char rdata[8];
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
for(I = 0; I < 50; ++I){
|
||||
if(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata) && (idr&0x1fffffff) == motor_id+1) break;
|
||||
can_dsleep(0.01);
|
||||
}
|
||||
if(I == 50){
|
||||
WARNX("Error getting answer");
|
||||
return CAN_NOANSWER;
|
||||
}
|
||||
if(obuf) memcpy(obuf, rdata, l);
|
||||
/*if(G->verbose){
|
||||
printf("Got answer with ID=%d: ", idr&0x1fffffff);
|
||||
for(int i = 0; i < dlen; ++i)
|
||||
printf(" %02x", rdata[i]);
|
||||
printf("\n");
|
||||
}*/
|
||||
if((rdata[0] & (SW_B_MAILFUN|SW_B_READY)) == SW_B_MAILFUN){ // error
|
||||
WARNX("Mailfunction, error code: %d", rdata[1]);
|
||||
return CAN_ERROR; // error
|
||||
}else if((rdata[0] & (SW_B_MAILFUN|SW_B_READY)) == (SW_B_MAILFUN|SW_B_READY)){ // warning
|
||||
WARNX("Warning, code: %d", rdata[1]);
|
||||
buf[1] |= CW_B_CLERR; // try to clear warnings
|
||||
can_send_frame(motor_id, l, buf);
|
||||
return CAN_WARNING; // warning
|
||||
}
|
||||
//verbose("Got motor state: %d\n", rdata[0]);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief can_send_parag - send/get motor parameters
|
||||
* @param buf (i) - parameter out data frame (8 bytes)
|
||||
* @param obuf (o) - parameter in data frame (8 bytes)
|
||||
* @return status
|
||||
*/
|
||||
static canstatus can_send_param(unsigned char *buf, unsigned char *obuf){
|
||||
const int l = 8; // frame length
|
||||
int I, rxpnt, idr, dlen;
|
||||
double rxtime;
|
||||
unsigned char rdata[8];
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
if(can_send_frame(motor_p_id, l, buf) <= 0){
|
||||
WARNX("Error sending CAN frame (len %d)", l);
|
||||
return CAN_CANTSEND;
|
||||
}
|
||||
/*
|
||||
green("Sent param: ");
|
||||
for(int i=0; i<l; ++i) printf("0x%02x ", buf[i]);
|
||||
printf("\n");
|
||||
*/
|
||||
for(I = 0; I < 50; ++I){
|
||||
if(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata) && (idr&0x1fffffff) == motor_p_id+1) break;
|
||||
can_dsleep(0.01);
|
||||
}
|
||||
if(I == 50){
|
||||
WARNX("Error getting answer");
|
||||
return CAN_NOANSWER;
|
||||
}
|
||||
/*
|
||||
green("Received param: ");
|
||||
for(int i=0; i<dlen; ++i) printf("0x%02x ", rdata[i]);
|
||||
printf("\n");
|
||||
*/
|
||||
if(obuf) memcpy(obuf, rdata, dlen);
|
||||
if(obuf[0] & CAN_PAR_ERRFLAG){
|
||||
WARNX("Wrong parameter idx/subidx or other error");
|
||||
return CAN_WARNING;
|
||||
}
|
||||
if(*((uint32_t*)buf) != *((uint32_t*)obuf)){
|
||||
WARNX("Got wrong answer for parameter request");
|
||||
return CAN_WARNING;
|
||||
}
|
||||
return CAN_NOERR;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief can_read_par - read motor parameter
|
||||
* @param subidx - parameter subindex
|
||||
* @param idx - parameter index
|
||||
* @param parval (o) - parameter value
|
||||
* @return status
|
||||
*/
|
||||
static canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
|
||||
if(!parval) return CAN_WARNING;
|
||||
uint8_t buf[8] = {CAN_READPAR_CMD, subidx}, obuf[8];
|
||||
buf[2] = idx >> 8;
|
||||
buf[3] = idx & 0xff;
|
||||
canstatus s = can_send_param(buf, obuf);
|
||||
if(s != CAN_NOERR) return s;
|
||||
*parval = obuf[4]<<24 | obuf[5]<<16 | obuf[6]<<8 | obuf[7];
|
||||
return CAN_NOERR;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief can_write_par - write motor parameter
|
||||
* @param subidx - parameter subindex
|
||||
* @param idx - parameter index
|
||||
* @param parval (i) - new parameter value (NULL for 0)
|
||||
* @return status
|
||||
*/
|
||||
static canstatus can_write_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
|
||||
uint8_t buf[8] = {CAN_WRITEPAR_CMD, subidx,0}, obuf[8];
|
||||
buf[2] = idx >> 8;
|
||||
buf[3] = idx & 0xff;
|
||||
if(parval){
|
||||
uint32_t par = *parval;
|
||||
obuf[4] = (par >> 24) & 0xff;
|
||||
obuf[5] = (par >> 16) & 0xff;
|
||||
obuf[6] = (par >> 8) & 0xff;
|
||||
obuf[7] = par & 0xff;
|
||||
}
|
||||
return can_send_param(buf, obuf);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief get_motor_speed
|
||||
* @param motstatus (o) - status (if !NULL)
|
||||
* @return speed in rev/min
|
||||
*/
|
||||
canstatus get_motor_speed(double *spd){
|
||||
if(!spd) return CAN_WARNING;
|
||||
union{
|
||||
uint32_t u;
|
||||
int32_t i;
|
||||
} speed;
|
||||
canstatus s = can_read_par(PAR_SPD_SUBIDX, PAR_SPD_IDX, &speed.u);
|
||||
if(s != CAN_NOERR){
|
||||
return s;
|
||||
}
|
||||
*spd = (double)speed.i / 1000.;
|
||||
/*
|
||||
union{
|
||||
int16_t i16;
|
||||
uint8_t c8[2];
|
||||
} mspd;
|
||||
mspd.c8[0] = rdata[3];
|
||||
mspd.c8[1] = rdata[2];
|
||||
*/
|
||||
return CAN_NOERR;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief get_endswitches - get state of end-switches
|
||||
* @param Esw (o) - end-switches state
|
||||
* @return
|
||||
*/
|
||||
canstatus get_endswitches(eswstate *Esw){
|
||||
uint32_t val = 0;
|
||||
canstatus s = can_read_par(PAR_DI_SUBIDX, PAR_DIST_IDX, &val);
|
||||
if(Esw){
|
||||
int v = 0;
|
||||
if(!(val & ESW_CW)){ // + pressed
|
||||
v |= ESW_CW_ACTIVE;
|
||||
}
|
||||
if(!(val & ESW_CCW)){ // - pressed
|
||||
v |= ESW_CCW_ACTIVE;
|
||||
}
|
||||
*Esw = v;
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief stop - stop motor
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int stop(){
|
||||
unsigned char buf[6] = {0, CW_STOP,0,};
|
||||
if(can_send_chk(buf, NULL)){
|
||||
WARNX("Can't stop motor!");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief movewconstspeed - move with constant speed
|
||||
* @param rawspd - given speed
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int movewconstspeed(int spd){
|
||||
int16_t targspd = RAWSPEED(spd);
|
||||
unsigned char buf[8] = {0,};
|
||||
buf[1] = CW_ENABLE;
|
||||
if(MOTOR_REVERSE) spd = -spd;
|
||||
buf[2] = (targspd >> 8) & 0xff;
|
||||
buf[3] = targspd & 0xff;
|
||||
if(can_send_chk(buf, NULL)){
|
||||
WARNX("Can't move motor!");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief move - move focuser from current position to approximately `targposition` with speed `rawspeed`
|
||||
* @param targposition - target position in raw value
|
||||
* @param rawspeed - speed in raw value
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
static int move(unsigned long targposition, int16_t rawspeed){
|
||||
if(abs(targposition - curposition) < RAWPOS_TOLERANCE){
|
||||
verbose("Already at position\n");
|
||||
return 0;
|
||||
}
|
||||
unsigned char buf[6] = {0,};
|
||||
DBG("Start moving with speed %d", rawspeed);
|
||||
buf[1] = CW_ENABLE;
|
||||
if(MOTOR_REVERSE) rawspeed = -rawspeed;
|
||||
buf[2] = (rawspeed >> 8) & 0xff;
|
||||
buf[3] = rawspeed & 0xff;
|
||||
DBG("\tBUF: %d, %d, %d, %d", buf[0], buf[1], buf[2], buf[3]);
|
||||
if(can_send_chk(buf, NULL)){
|
||||
WARNX("Can't move motor!");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
#ifdef EBUG
|
||||
double t0 = can_dtime();
|
||||
#endif
|
||||
long olddiffr = targposition - curposition;
|
||||
long corrvalue = (long)rawspeed*(long)rawspeed / STOPPING_COEFF; // correction due to stopping ramp
|
||||
DBG("start-> curpos: %ld, difference: %ld, corrval: %ld, tm=%g",
|
||||
curposition, olddiffr, corrvalue, can_dtime()-t0);
|
||||
int i, zerctr = 0;
|
||||
for(i = 0; i < MOVING_TIMEOUT; ++i){
|
||||
can_dsleep(0.01);
|
||||
//uint16_t motstat;
|
||||
double speed;
|
||||
eswstate esw;
|
||||
if(emerg_stop){ // emergency stop activated
|
||||
WARNX("Activated stop while moving");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
if(get_motor_speed(&speed) != CAN_NOERR){ // WTF?
|
||||
WARNX("Unknown situation: can't get speed of moving motor");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
if(get_endswitches(&esw) != CAN_NOERR){
|
||||
WARNX("Can't get endswitches state, stopping");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
if(esw != ESW_INACTIVE){ // OOps, something wrong
|
||||
if(esw == ESW_BOTH_ACTIVE){ // error!
|
||||
WARNX("Check end-switches, both active!");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
//TODO: check wrong end-switches!
|
||||
if(rawspeed > 0){ // moving CW, don't care for CCW esw state
|
||||
if(esw == ESW_CW_ACTIVE){
|
||||
WARNX("CW mowing: end-switch!");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
}else{ // movig CCW
|
||||
if(esw == ESW_CCW_ACTIVE){
|
||||
WARNX("CCW mowing: end-switch!");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(fabs(speed) < 0.1){ // || (motstat & (SW_B_UNBLOCK|SW_B_READY|SW_B_POUNBLOCK)) != (SW_B_UNBLOCK|SW_B_READY|SW_B_POUNBLOCK)){
|
||||
if(++zerctr == 10){
|
||||
WARNX("Motor stopped while moving!");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
}else zerctr = 0;
|
||||
if(!getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition)) continue;
|
||||
long diffr = targposition - curposition;
|
||||
//DBG("Speed: %g, curpos: %ld, diff: %ld", speed, curposition, diffr);
|
||||
if(abs(diffr) < corrvalue || abs(diffr) - RAWPOS_TOLERANCE > abs(olddiffr)){
|
||||
DBG("OK! almost reach: olddif=%ld, diff=%ld, corrval=%ld, tm=%g", olddiffr, diffr, corrvalue, can_dtime()-t0);
|
||||
olddiffr = diffr;
|
||||
break;
|
||||
}
|
||||
olddiffr = diffr;
|
||||
}
|
||||
if(i == MOVING_TIMEOUT){
|
||||
WARNX("Error: timeout, but motor still not @ position! STOP!");
|
||||
stop();
|
||||
return 1;
|
||||
}
|
||||
DBG("end-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
|
||||
long oldposition;
|
||||
int r = stop();
|
||||
if(r) r = stop();
|
||||
if(!r) do{ // wait till stop
|
||||
oldposition = curposition;
|
||||
can_dsleep(0.1);
|
||||
getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition);
|
||||
//DBG("wait-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
|
||||
}while((long)curposition != oldposition);
|
||||
if(abs(targposition - curposition) > RAWPOS_TOLERANCE)
|
||||
verbose("Current (%ld) position is too far from target (%ld)\n", curposition, targposition);
|
||||
DBG("stop-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
|
||||
return r;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief move2pos - accurate focus moving to target position (in encoder's units)
|
||||
* @param target - position 2 move (in mm)
|
||||
* @return 0 if all OK
|
||||
*/
|
||||
int move2pos(double target){
|
||||
double cur;
|
||||
if(getPos(&cur)){
|
||||
WARNX("Can't get current position!");
|
||||
return 1;
|
||||
}
|
||||
unsigned long targposition = FOC_MM2RAW(target);
|
||||
DBG("Raw target position: %lu", targposition);
|
||||
if(target > FOCMAX_MM || target < FOCMIN_MM || targposition < FOCMIN || targposition > FOCMAX){
|
||||
WARNX("Target focus position over the available range!");
|
||||
return 1;
|
||||
}
|
||||
if(abs(targposition - curposition) < RAWPOS_TOLERANCE){
|
||||
verbose("Already at position\n");
|
||||
return 0;
|
||||
}
|
||||
long targ0pos = (long)targposition + (long)dF0;
|
||||
long spd = (targ0pos - (long)curposition) / 2L;
|
||||
int16_t targspd = (int16_t) spd;
|
||||
if(spd > INT16_MAX) targspd = INT16_MAX;
|
||||
else if(spd < INT16_MIN) targspd = INT16_MIN;
|
||||
fix_targspeed(&targspd);
|
||||
// check moving direction: thin focus correction always should run to negative!
|
||||
if(targposition < curposition){ // we are from the right
|
||||
if(targspd < -MINSPEED*3/2){ // omit rough moving to focus value if there's too little distance towards target
|
||||
// rough moving
|
||||
DBG("1) ROUGH move to the LEFT: curpos=%ld, difference=%ld\n", curposition, targ0pos - (long)curposition);
|
||||
if(move(targ0pos, RAWSPEED(targspd))){
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}else{ // we are from the left - move to the point @ right side of target
|
||||
DBG("1) ROUGH move to the RIGHT: curpos=%ld, difference=%ld\n", curposition, targ0pos - (long)curposition);
|
||||
if(move(targ0pos, RAWSPEED(targspd))){
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
// now move precisely
|
||||
if(!getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition)){
|
||||
WARNX("Can't get current position");
|
||||
return 1;
|
||||
}
|
||||
if(abs(targposition - curposition) < RAWPOS_TOLERANCE){
|
||||
verbose("Catch the position @ rough movint\n");
|
||||
return 0;
|
||||
}
|
||||
if(curposition < targposition){
|
||||
WARNX("Error in current position!");
|
||||
return 1;
|
||||
}
|
||||
DBG("2) curpos: %ld, difference: %ld\n", curposition, (long)targposition - (long)curposition);
|
||||
// now make an accurate moving
|
||||
if(move(targposition, -(RAWSPEED(MINSPEED)))){
|
||||
WARNX("Can't catch focus precisely!");
|
||||
return 1;
|
||||
}
|
||||
if(!getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition)){
|
||||
WARNX("Can't get current position");
|
||||
return 1;
|
||||
}
|
||||
if(abs(targposition - curposition) > RAWPOS_TOLERANCE){
|
||||
verbose("Stopped over the accuracy range\n");
|
||||
return 1;
|
||||
}
|
||||
return stop();
|
||||
}
|
||||
|
||||
// return 0 if all OK
|
||||
static int get_pos_speed(unsigned long *pos, double *speed){
|
||||
int ret = 0;
|
||||
if(pos){
|
||||
if(!getLong(encnodenum, DS406_POSITION_VAL, 0, pos)) ret = 1;
|
||||
}
|
||||
if(speed){
|
||||
if(get_motor_speed(speed) != CAN_NOERR){
|
||||
*speed = 0.;
|
||||
ret = 1;
|
||||
}else if(MOTOR_REVERSE) *speed = -*speed;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void movewithmon(double spd){
|
||||
unsigned char buf[6] = {0,};
|
||||
if(fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED){
|
||||
WARNX("Target speed should be be from %d to %d (rev/min)", MINSPEED, MAXSPEED);
|
||||
return;
|
||||
}
|
||||
unsigned long pos, oldpos = 0, startpos;
|
||||
double speed;
|
||||
get_pos_speed(&startpos, NULL); // starting position
|
||||
int16_t targspd = RAWSPEED(spd);
|
||||
buf[1] = CW_ENABLE;
|
||||
if(MOTOR_REVERSE) targspd = -targspd;
|
||||
buf[2] = (targspd >> 8) & 0xff;
|
||||
buf[3] = targspd & 0xff;
|
||||
if(can_send_chk(buf, NULL)){
|
||||
WARNX("Can't move motor!");
|
||||
return;
|
||||
}
|
||||
double t0 = can_dtime(), tlast = t0;
|
||||
green("\nAcceleration with monitoring not longer than for 4 seconds\n\n");
|
||||
#define PRINT() do{printf("t=%g, pos=%lu (%.3fmm), spd=%g, Dpos=%.0f\n", tcur - t0, pos, \
|
||||
FOC_RAW2MM(pos), speed, oldpos ? ((double)pos - oldpos)/(tcur - tlast) : 0);}while(0)
|
||||
while(can_dtime() - t0 < 4.){
|
||||
if(get_pos_speed(&pos, &speed)){ // can't get speed? WTF?
|
||||
WARNX("Strange things are going here...");
|
||||
break;
|
||||
}
|
||||
double tcur = can_dtime();
|
||||
PRINT();
|
||||
oldpos = pos;
|
||||
if(fabs(speed - spd) < 1.){
|
||||
green("\tTarget speed reached for %.2fs, DPOS=%ld\n", tlast - t0, pos - startpos);
|
||||
break;
|
||||
}
|
||||
tlast = tcur;
|
||||
can_dsleep(0.03);
|
||||
}
|
||||
green("\nMove for 3 seconds with constant speed\n\n");
|
||||
get_pos_speed(&startpos, NULL);
|
||||
t0 = can_dtime();
|
||||
do{
|
||||
can_dsleep(0.5);
|
||||
if(get_pos_speed(&pos, &speed)){ // can't get speed? WTF?
|
||||
WARNX("Strange things are going there...");
|
||||
break;
|
||||
}
|
||||
double tcur = can_dtime();
|
||||
PRINT();
|
||||
oldpos = pos;
|
||||
tlast = tcur;
|
||||
}while(can_dtime() - t0 < 3.);
|
||||
double meanspd = ((double)pos - startpos) / (tlast - t0);
|
||||
green("\tMean pos speed: %.0f (%g mm/s)\n", meanspd, FOC_RAW2MM(meanspd));
|
||||
green("\nStop with monitoring not longer than for 4 seconds\n\n");
|
||||
get_pos_speed(&startpos, NULL);
|
||||
t0 = can_dtime();
|
||||
for(int i = 0; i < 100 && stop(); ++i);
|
||||
while(can_dtime() - t0 < 4.){
|
||||
get_pos_speed(&pos, NULL);
|
||||
double tcur = can_dtime();
|
||||
PRINT();
|
||||
if(oldpos == pos){
|
||||
green("\tStopped for %.2fs, DPOS=%ld\n", tlast - t0, pos - startpos);
|
||||
break;
|
||||
}
|
||||
oldpos = pos;
|
||||
tlast = tcur;
|
||||
can_dsleep(0.03);
|
||||
}
|
||||
#undef PRINT
|
||||
}
|
||||
53
Z1000_focus/can_encoder.h
Normal file
53
Z1000_focus/can_encoder.h
Normal file
@ -0,0 +1,53 @@
|
||||
/*
|
||||
* This file is part of the Zphocus project.
|
||||
* Copyright 2019 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef CAN_ENCODER_H__
|
||||
#define CAN_ENCODER_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
typedef enum{
|
||||
CAN_NOERR = 0,
|
||||
CAN_CANTSEND,
|
||||
CAN_NOANSWER,
|
||||
CAN_WARNING,
|
||||
CAN_ERROR
|
||||
} canstatus;
|
||||
|
||||
typedef enum{
|
||||
ESW_INACTIVE = 0,
|
||||
ESW_CW_ACTIVE = 1,
|
||||
ESW_CCW_ACTIVE = 2,
|
||||
ESW_BOTH_ACTIVE = 3
|
||||
} eswstate;
|
||||
|
||||
int init_encoder(int encnode, int reset);
|
||||
void returnPreOper();
|
||||
int getPos(double *pos);
|
||||
double curPos();
|
||||
int init_motor_ids(int addr);
|
||||
void movewithmon(double spd);
|
||||
canstatus get_motor_speed(double *spd);
|
||||
canstatus get_endswitches(eswstate *Esw);
|
||||
int move2pos(double target);
|
||||
int stop();
|
||||
int movewconstspeed(int spd);
|
||||
int go_out_from_ESW();
|
||||
|
||||
#endif // CAN_ENCODER_H__
|
||||
566
Z1000_focus/can_io.c
Normal file
566
Z1000_focus/can_io.c
Normal file
@ -0,0 +1,566 @@
|
||||
/* CAN I/O library (to use as a process)
|
||||
* usage:
|
||||
* first: fork() + start_can_io(NULL) - start CAN Rx-buffering process
|
||||
* then: fork() + Control_1(....) - start process that uses recv/send functions
|
||||
* ...........................
|
||||
* then: fork() + Control_N(....)
|
||||
*
|
||||
* note: use init_can_io() at the begining of every Control process
|
||||
* BUT DON't USE it in main() before Control process start
|
||||
* ^^^^^^^^^^^^^
|
||||
* (c) vsher@sao.ru
|
||||
*/
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <signal.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/time.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/file.h>
|
||||
#include <sys/shm.h>
|
||||
#include <sys/ipc.h>
|
||||
#include <sys/msg.h>
|
||||
|
||||
|
||||
#include "canmsg.h"
|
||||
#include "can_io.h"
|
||||
|
||||
char can_dev[40] = "/dev/can0";
|
||||
int can_fd=-1;
|
||||
char can_lck[40] = "/tmp/dev_can0.lock";
|
||||
int can_lk=-1;
|
||||
static int server_mode=0;
|
||||
static int my_uid;
|
||||
|
||||
#define CAN_SHM_SIZE ((sizeof(int)*4)+CAN_CTLR_SIZE+(CAN_RX_SIZE*sizeof(canmsg_t)))
|
||||
|
||||
union ShMkey {
|
||||
char name[5];
|
||||
key_t code;
|
||||
} can_shm_key;
|
||||
|
||||
int can_shm_id=-1;
|
||||
char *can_shm_addr = NULL;
|
||||
|
||||
#define can_pid (*(((int *)can_shm_addr)+0)) /* PID of CAN I/O process */
|
||||
#define can_open (*(((int *)can_shm_addr)+1)) /* file descr.of CAN-driver */
|
||||
#define rx_buff_pntr (*(((int *)can_shm_addr)+2)) /* from 0 till CAN_RX_SIZE-1 */
|
||||
#define can_mode (*(((int *)can_shm_addr)+3)) /* CAN server/client mode flags */
|
||||
#define CAN_SEND_SERVER 1 /* clients should try to send frames through CAN-server or directly to CAN-driver otherwise */
|
||||
|
||||
void *can_ctrl_addr = NULL; /* shm area reserved for control process purpose*/
|
||||
canmsg_t *rx_buff; /* rx ring buffer: CAN_RX_SIZE*sizeof(canmsg_t)*/
|
||||
|
||||
struct CMD_Queue { /* ÏÐÉÓÁÎÉÅ ÏÞÅÒÅÄÉ (ËÁÎÁÌÁ) ËÏÍÁÎÄ */
|
||||
union{
|
||||
char name[5]; /* ËÌÀÞ ÉÄÅÎÔÅÆÉËÁÃÉÉ ÏÞÅÒÅÄÉ */
|
||||
key_t code;
|
||||
} key;
|
||||
int mode; /* ÒÅÖÉÍ ÄÏÓÔÕÐÁ (rwxrwxrwx) */
|
||||
int side; /* ÔÉÐ ÐÏÄÓÏÅÄÉÎÅÎÉÑ: ëÌÉÅÎÔ/óÅÒ×ÅÒ (Sender/Receiver)*/
|
||||
int id; /* ÄÅÓËÒÉÐÔÏÒ ÐÏÄÓÏÅÄÉÎÅÎÉÑ */
|
||||
unsigned int acckey; /* ËÌÀÞ ÄÏÓÔÕÐÁ (ÄÌÑ ÐÅÒÅÄÁÞÉ ëÌÉÅÎÔ->óÅÒ×ÅÒ) */
|
||||
};
|
||||
|
||||
/* ËÁÎÁÌ ËÏÍÁÎÄ ÉÓÐÏÌØÚÕÅÍ ÄÌÑ ÐÅÒÅÄÁÞÉ CAN-ÆÒÅÊÍÏ× */
|
||||
static struct CMD_Queue canout = {.key.name = {'C','A','N',0,0},0200,0,-1,0};
|
||||
|
||||
/* ÓÔÒÕËÔÕÒÁ ÓÏÏÂÝÅÎÉÑ */
|
||||
struct my_msgbuf {
|
||||
unsigned long mtype; /* type of message */
|
||||
unsigned long acckey; /* ËÌÀÞ ÄÏÓÔÕÐÁ ËÌÉÅÎÔÁ */
|
||||
unsigned long src_pid; /* ÎÏÍÅÒ ÐÒÏÃÅÓÓÁ ÉÓÔÏÞÎÉËÁ */
|
||||
unsigned long src_ip; /* IP-ÁÄÒ. ÉÓÔÏÞÎÉËÁ, =0 - ÌÏËÁÌØÎÁÑ ËÏÍÁÎÄÁ */
|
||||
char mtext[100]; /* message text */
|
||||
};
|
||||
|
||||
static void can_abort(int sig);
|
||||
|
||||
void set_server_mode(int mode) {server_mode=mode;}
|
||||
int can_server() {return(server_mode);}
|
||||
void set_sending_mode(int to_server) {
|
||||
if(to_server) can_mode |= CAN_SEND_SERVER;
|
||||
else can_mode &= ~CAN_SEND_SERVER;
|
||||
}
|
||||
int can_sending_mode() {return(can_mode&CAN_SEND_SERVER);}
|
||||
int can_card() {return(can_fd>0);}
|
||||
int can_gate() {return(0);}
|
||||
double can_gate_time_offset() {return(0.0);}
|
||||
|
||||
void setup_can_net(_U_ unsigned long ipaddr, _U_ int port, _U_ unsigned long acckey) {return;}
|
||||
|
||||
unsigned long get_acckey() {return(0);}
|
||||
|
||||
static int shm_created=0;
|
||||
|
||||
/* to use _AFTER_ process forking */
|
||||
void *init_can_io() { /* returns shared area addr. for client control process*/
|
||||
int new_shm=0;
|
||||
char *p, msg[256];
|
||||
|
||||
my_uid=geteuid();
|
||||
if(!can_shm_addr){
|
||||
if((p = strrchr(can_dev,'/'))){
|
||||
memcpy(&can_lck[9], p+1, 4);
|
||||
memcpy(can_shm_key.name, p+1, 4);
|
||||
can_shm_key.name[4]='\0';
|
||||
}else{
|
||||
fprintf(stderr,"Wrong CAN device name: %s\n", can_dev);
|
||||
exit(1);
|
||||
}
|
||||
can_shm_id = shmget(can_shm_key.code, CAN_SHM_SIZE, 0644);
|
||||
if(can_shm_id<0 && errno==EACCES)
|
||||
can_shm_id = shmget(can_shm_key.code, CAN_SHM_SIZE, 0444);
|
||||
if(can_shm_id<0 && errno==ENOENT && server_mode) {
|
||||
can_shm_id = shmget(can_shm_key.code, CAN_SHM_SIZE, IPC_CREAT|IPC_EXCL|0644);
|
||||
new_shm = shm_created = 1;
|
||||
}
|
||||
if(can_shm_id<0){
|
||||
can_prtime(stderr);
|
||||
if(new_shm)
|
||||
sprintf(msg,"Can't create shm CAN buffer '%s'",can_shm_key.name);
|
||||
else if(server_mode)
|
||||
sprintf(msg,"CAN-I/O: Can't find shm segment for CAN buffer '%s'",can_shm_key.name);
|
||||
else
|
||||
snprintf(msg, 256, "Can't find shm segment for CAN buffer '%s' (maybe no CAN-I/O process?)",can_shm_key.name);
|
||||
perror(msg);
|
||||
exit(errno);
|
||||
}
|
||||
can_shm_addr = shmat(can_shm_id, NULL, 0);
|
||||
if(can_shm_addr == (void*)-1 && errno == EACCES)
|
||||
can_shm_addr = shmat(can_shm_id, NULL, SHM_RDONLY);
|
||||
if(can_shm_addr == (void*)-1){
|
||||
sprintf(msg,"Can't attach shm CAN buffer '%s'",can_shm_key.name);
|
||||
perror(msg);
|
||||
shmctl(can_shm_id, IPC_RMID, NULL);
|
||||
exit(errno);
|
||||
}
|
||||
}
|
||||
can_ctrl_addr = (canmsg_t *)(can_shm_addr+sizeof(int)*4);
|
||||
rx_buff = (canmsg_t *)(can_ctrl_addr+CAN_CTLR_SIZE);
|
||||
|
||||
if(can_fd < 0 && canout.id < 0){
|
||||
int flags = (server_mode)? O_RDWR : O_WRONLY;
|
||||
if(server_mode){
|
||||
if(( can_fd = open(can_dev, flags)) < 0 ){
|
||||
sprintf(msg,"CAN-I/O: Error opening CAN device %s", can_dev);
|
||||
can_prtime(stderr);
|
||||
perror(msg);
|
||||
shmctl(can_shm_id, IPC_RMID, NULL);
|
||||
exit(errno);
|
||||
}
|
||||
}else{
|
||||
can_fd = open(can_dev, flags);
|
||||
canout.id = msgget(canout.key.code, canout.mode);
|
||||
if(can_fd < 0 && canout.id < 0) {
|
||||
snprintf(msg, 256, "Error opening CAN device(%s) or CAN output queue '%s' (maybe no CANqueue server process?)",can_dev,canout.key.name);
|
||||
perror(msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
if(can_lk > 0) close(can_lk);
|
||||
if(( can_lk = open(can_lck, O_RDWR|O_CREAT, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH )) < 0 ){
|
||||
sprintf(msg,"Error opening CAN device lock-file %s", can_lck);
|
||||
perror(msg);
|
||||
shmctl(can_shm_id, IPC_RMID, NULL);
|
||||
close(can_fd);
|
||||
exit(errno);
|
||||
}
|
||||
fchmod(can_lk, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH);
|
||||
if(new_shm){
|
||||
struct timeval tmv;
|
||||
struct timezone tz;
|
||||
gettimeofday(&tmv,&tz);
|
||||
if(flock(can_lk, LOCK_EX)<0) perror("locking CAN");
|
||||
can_pid = 0;
|
||||
can_open = -1;
|
||||
rx_buff_pntr = 0;
|
||||
for(int i = 0; i < CAN_RX_SIZE; ++i){
|
||||
rx_buff[i].id = 0;
|
||||
rx_buff[i].timestamp = tmv;
|
||||
}
|
||||
if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN");
|
||||
}
|
||||
signal(SIGHUP, can_exit);
|
||||
signal(SIGINT, can_exit);
|
||||
signal(SIGQUIT,can_exit);
|
||||
signal(SIGTERM,can_exit);
|
||||
return(can_ctrl_addr);
|
||||
}
|
||||
|
||||
/* CAN "Rx to buff" process */
|
||||
void *start_can_io(_U_ void *arg){
|
||||
set_server_mode(1);
|
||||
init_can_io();
|
||||
if(can_io_ok()){
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"CAN I/O process(%d) already running!\n",can_pid);
|
||||
sleep(1);
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"New CAN I/O process(%d) exiting...!\n",getpid());
|
||||
exit(0);
|
||||
}
|
||||
if(can_fd < 0){
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"Error opening CAN device %s\n", can_dev);
|
||||
shmctl(can_shm_id, IPC_RMID, NULL);
|
||||
exit(0);
|
||||
}
|
||||
can_pid = getpid();
|
||||
can_open = can_fd;
|
||||
can_mode = 0;
|
||||
|
||||
signal(SIGHUP, can_abort);
|
||||
signal(SIGINT, can_abort);
|
||||
signal(SIGQUIT,can_abort);
|
||||
signal(SIGFPE, can_abort);
|
||||
signal(SIGPIPE,can_abort);
|
||||
signal(SIGSEGV,can_abort);
|
||||
signal(SIGALRM, SIG_IGN);
|
||||
signal(SIGTERM,can_abort);
|
||||
|
||||
if(shmctl(can_shm_id, SHM_LOCK, NULL) < 0)
|
||||
perror("CAN I/O: can't prevents swapping of Rx-buffer area");
|
||||
|
||||
while(1){
|
||||
int n;
|
||||
canmsg_t rx;
|
||||
|
||||
if(!can_io_shm_ok()){can_delay(0.3); continue;}
|
||||
|
||||
n = can_wait(can_fd, 0.3);
|
||||
if(n < 0) sleep(1);
|
||||
if(n <= 0) continue;
|
||||
|
||||
do{
|
||||
//static struct timeval tm = {0,0};
|
||||
n = read(can_fd, &rx, sizeof(struct canmsg_t));
|
||||
if(n < 0){
|
||||
perror("CAN Rx error");
|
||||
} else if(n > 0) {
|
||||
/* work around the timestamp bug in old driver version
|
||||
while((double)rx.timestamp.tv_sec+(double)rx.timestamp.tv_usec/1e6 < (double)tm.tv_sec+(double)tm.tv_usec/1e6) {
|
||||
rx.timestamp.tv_usec += 10000;
|
||||
if(rx.timestamp.tv_usec > 1000000) {
|
||||
rx.timestamp.tv_sec++;
|
||||
rx.timestamp.tv_usec -= 1000000;
|
||||
}
|
||||
}*/
|
||||
if(flock(can_lk, LOCK_EX)<0) perror("locking CAN");
|
||||
rx_buff[rx_buff_pntr] = rx;
|
||||
rx_buff_pntr = (rx_buff_pntr + 1) % CAN_RX_SIZE;
|
||||
if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN");
|
||||
//fprintf(stderr,"%d read(id=%02x,len=%d)\n",rx_buff_pntr,rx.id,rx.length);fflush(stderr);
|
||||
/*fprintf(stderr,"reading CAN: 1 frame\n");*/
|
||||
}/*else {
|
||||
* fprintf(stderr,"reading CAN: nothing\n");fflush(stderr);
|
||||
} */
|
||||
} while(n>0);
|
||||
}
|
||||
}
|
||||
|
||||
/* put CAN-frame to recv-buffer */
|
||||
void can_put_buff_frame(double rtime, int id, int length, unsigned char data[]) {
|
||||
int i;
|
||||
canmsg_t rx;
|
||||
int sec = (int)rtime;
|
||||
if(!server_mode) return;
|
||||
if(length<0) length=0;
|
||||
if(length>8) length=8;
|
||||
rx.id = id&((id&CAN_EXT_FLAG)? 0x1fffffff : 0x7ff);
|
||||
rx.cob=0;
|
||||
rx.flags = ((id&CAN_RTR_FLAG)?MSG_RTR:0)|((id&CAN_EXT_FLAG)?MSG_EXT:0);
|
||||
rx.length=length;
|
||||
for(i=0; i<length; i++) rx.data[i]=data[i];
|
||||
rx.timestamp.tv_sec = sec;
|
||||
rx.timestamp.tv_usec = (int)((rtime-sec)*1000000.);
|
||||
if(flock(can_lk, LOCK_EX)<0) perror("locking CAN");
|
||||
rx_buff[rx_buff_pntr] = rx;
|
||||
rx_buff_pntr = (rx_buff_pntr + 1) % CAN_RX_SIZE;
|
||||
if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN");
|
||||
//fprintf(stderr,"put_buf(id=%02x,flag=%1x,len=%d)\n",rx.id,rx.flags,rx.length);fflush(stderr);
|
||||
}
|
||||
|
||||
/* ÷ÓÅ ÎÏÒÍÁÌØÎÏ Ó SHM-ÂÕÆÅÒÏÍ CAN-I/O ÐÒÏÃÅÓÓÁ */
|
||||
int can_io_shm_ok(){
|
||||
return(can_pid>0 && can_open>0);
|
||||
}
|
||||
|
||||
|
||||
/* ÷ÓÅ ÎÏÒÍÁÌØÎÏ Ó CAN-I/O ÐÒÏÃÅÓÓÏÍ */
|
||||
/* (ÎÏ ÎÁÄÏ ÂÙÔØ ÓÕÐÅÒ-ÀÚÅÒÏÍ!) */
|
||||
int can_io_ok(){
|
||||
return(can_io_shm_ok() && (my_uid!=0||kill(can_pid, 0)==0));
|
||||
}
|
||||
|
||||
|
||||
/* ÷ÏÚÍÏÖÎÁ ÒÁÂÏÔÁ c CAN ÄÌÑ ËÌÉÅÎÔÁ */
|
||||
int can_ok() {
|
||||
return(can_io_shm_ok());
|
||||
}
|
||||
|
||||
|
||||
/* wait for CAN-frame */
|
||||
int can_wait(int fd, double tout){
|
||||
int nfd,width;
|
||||
struct timeval tv;
|
||||
fd_set readfds;
|
||||
|
||||
if(fd==0 && tout>=0.01){
|
||||
double dt = can_dsleep(tout);
|
||||
if(dt>0.) can_dsleep(dt);
|
||||
return(0);
|
||||
}
|
||||
if(fd<0) fd=can_fd;
|
||||
if(fd>0){
|
||||
FD_ZERO(&readfds);
|
||||
FD_SET(fd, &readfds);
|
||||
width = fd+1;
|
||||
} else width = 0;
|
||||
tv.tv_sec = (int)tout;
|
||||
tv.tv_usec = (int)((tout - tv.tv_sec)*1000000.+0.9);
|
||||
slipping:
|
||||
if(fd>0 && can_fd>0)
|
||||
nfd = select(width, &readfds, (fd_set *)NULL, (fd_set *)NULL, &tv);
|
||||
else
|
||||
nfd = select(0, (fd_set *)NULL, (fd_set *)NULL, (fd_set *)NULL, &tv);
|
||||
if(nfd < 0){
|
||||
if(errno == EINTR)
|
||||
goto slipping;
|
||||
perror("Error in can_wait(){ select() }");
|
||||
return(-1);
|
||||
} else if(nfd == 0) /* timeout! */
|
||||
return(0);
|
||||
if(fd>0 && FD_ISSET(fd, &readfds)) /* Rx frame! */
|
||||
return(1);
|
||||
return(0);
|
||||
}
|
||||
|
||||
/* cleanup recv-buffer in client process */
|
||||
void can_clean_recv(int *pbuf, double *rtime) {
|
||||
struct timeval tmv;
|
||||
struct timezone tz;
|
||||
gettimeofday(&tmv,&tz);
|
||||
*pbuf = rx_buff_pntr;
|
||||
*rtime = tmv.tv_sec + (double)tmv.tv_usec/1000000.;
|
||||
}
|
||||
|
||||
/* find next rx-frame in recv-buffer for client process */
|
||||
int can_recv_frame(int *pbuf, double *rtime,
|
||||
int *id, int *length, unsigned char data[]){
|
||||
return(can_get_buff_frame(pbuf, rtime, id, length, data));
|
||||
}
|
||||
int can_get_buff_frame(int *pbuf, double *rtime,
|
||||
int *id, int *length, unsigned char data[]) {
|
||||
while(*pbuf != rx_buff_pntr){
|
||||
canmsg_t *rx = &rx_buff[*pbuf];
|
||||
struct timeval *tv = &rx->timestamp;
|
||||
double t_rx;
|
||||
|
||||
if(flock(can_lk, LOCK_EX)<0) perror("locking CAN");
|
||||
|
||||
t_rx = tv->tv_sec + (double)tv->tv_usec/1000000.;
|
||||
if(t_rx+1. >= *rtime){
|
||||
int i;
|
||||
*id = rx->id | ((rx->flags&MSG_RTR)? CAN_RTR_FLAG:0);
|
||||
*length = rx->length;
|
||||
for(i = 0; i < *length; i++)
|
||||
data[i] = rx->data[i];
|
||||
*rtime = t_rx;
|
||||
*pbuf = (*pbuf + 1) % CAN_RX_SIZE;
|
||||
if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN");
|
||||
return(1);
|
||||
}
|
||||
*pbuf = (*pbuf + 1) % CAN_RX_SIZE;
|
||||
|
||||
if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN");
|
||||
}
|
||||
return(0);
|
||||
}
|
||||
|
||||
/* send tx-frame from client process */
|
||||
/* to CAN-driver or to output queue */
|
||||
int can_send_frame(unsigned long id, int length, unsigned char data[]) {
|
||||
int i, ret=1;
|
||||
if(can_fd<0 && canout.id<0)
|
||||
return(0);
|
||||
if(length>8) length=8;
|
||||
if(length<0) length=0;
|
||||
if(!server_mode && (can_mode&CAN_SEND_SERVER) && canout.id>=0 )
|
||||
goto send2server;
|
||||
if(can_fd >= 0){
|
||||
canmsg_t tx;
|
||||
tx.id = id&((id&CAN_EXT_FLAG)? 0x1fffffff : 0x7ff);
|
||||
tx.cob=0;
|
||||
tx.flags = ((id&CAN_RTR_FLAG)?MSG_RTR:0)|((id&CAN_EXT_FLAG)?MSG_EXT:0);
|
||||
tx.length=length;
|
||||
for(i=0;i<length;i++) tx.data[i]=data[i];
|
||||
if(flock(can_lk, LOCK_EX)<0) perror("locking CAN");
|
||||
//fprintf(stderr,"write(id=%02x,flag=%1x,len=%d)\n",tx.id,tx.flags,tx.length);fflush(stderr);
|
||||
ret = write(can_fd, &tx, sizeof(struct canmsg_t));
|
||||
if(flock(can_lk, LOCK_UN)<0) perror("unlocking CAN");
|
||||
if(server_mode)
|
||||
/* copy tx CAN-frame back to recv-buffer */
|
||||
can_put_buff_frame(can_dtime(), id, length, data);
|
||||
} else if(canout.id >= 0){
|
||||
struct my_msgbuf mbuf;
|
||||
send2server:
|
||||
mbuf.src_pid = getpid();
|
||||
mbuf.src_ip = 0;
|
||||
mbuf.acckey = canout.acckey;
|
||||
mbuf.mtype = id+1;
|
||||
for(i=0;i<length;i++) mbuf.mtext[i]=data[i];
|
||||
msgsnd( canout.id, (struct msgbuf *)&mbuf, length+12, IPC_NOWAIT);
|
||||
}
|
||||
return(ret);
|
||||
}
|
||||
|
||||
static void can_abort(int sig) {
|
||||
char ss[10];
|
||||
struct shmid_ds buf;
|
||||
|
||||
if(sig) signal(sig,SIG_IGN);
|
||||
if(!server_mode) can_exit(sig);
|
||||
switch(sig){
|
||||
case 0 : strcpy(ss," "); break;
|
||||
case SIGHUP : strcpy(ss,"SIGHUP -"); break;
|
||||
case SIGINT : strcpy(ss,"SIGINT -"); break;
|
||||
case SIGQUIT: strcpy(ss,"SIGQUIT -"); break;
|
||||
case SIGFPE : strcpy(ss,"SIGFPE -"); break;
|
||||
case SIGPIPE: strcpy(ss,"SIGPIPE -"); break;
|
||||
case SIGSEGV: strcpy(ss,"SIGSEGV -"); break;
|
||||
case SIGTERM: strcpy(ss,"SIGTERM -"); break;
|
||||
default: sprintf(ss,"SIG_%d -",sig); break;
|
||||
}
|
||||
switch(sig){
|
||||
default:
|
||||
case SIGHUP :
|
||||
case SIGINT :
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"CAN I/O: %s Ignore .....\n",ss);
|
||||
fflush(stderr);
|
||||
signal(sig, can_abort);
|
||||
return;
|
||||
case SIGPIPE:
|
||||
case SIGQUIT:
|
||||
case SIGFPE :
|
||||
case SIGSEGV:
|
||||
case SIGTERM:
|
||||
signal(SIGALRM, can_abort);
|
||||
alarm(2);
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"CAN I/O: %s process should stop after 2sec delay...\n",ss);
|
||||
fflush(stderr);
|
||||
close(can_fd);
|
||||
can_fd = can_open = -1;
|
||||
return;
|
||||
case SIGALRM:
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"CAN I/O: process stop!\n");
|
||||
fflush(stderr);
|
||||
close(can_lk);
|
||||
can_lk = -1;
|
||||
can_pid = 0;
|
||||
shmdt(can_shm_addr);
|
||||
shmctl(can_shm_id, IPC_STAT, &buf);
|
||||
if(buf.shm_nattch == 0){
|
||||
shmctl(can_shm_id, SHM_UNLOCK, NULL);
|
||||
shmctl(can_shm_id, IPC_RMID, NULL);
|
||||
}
|
||||
exit(sig);
|
||||
}
|
||||
}
|
||||
|
||||
void can_exit(int sig) {
|
||||
char ss[16];
|
||||
|
||||
struct shmid_ds buf;
|
||||
if(sig) signal(sig,SIG_IGN);
|
||||
if(server_mode) can_abort(sig);
|
||||
switch (sig) {
|
||||
case 0 : strcpy(ss,"Exiting - "); break;
|
||||
case SIGHUP : strcpy(ss,"SIGHUP -"); break;
|
||||
case SIGINT : strcpy(ss,"SIGINT -"); break;
|
||||
case SIGQUIT: strcpy(ss,"SIGQUIT -"); break;
|
||||
case SIGFPE : strcpy(ss,"SIGFPE -"); break;
|
||||
case SIGPIPE: strcpy(ss,"SIGPIPE -"); break;
|
||||
case SIGSEGV: strcpy(ss,"SIGSEGV -"); break;
|
||||
case SIGTERM: strcpy(ss,"SIGTERM -"); break;
|
||||
default: sprintf(ss,"SIG_%d -",sig); break;
|
||||
}
|
||||
switch (sig) {
|
||||
default:
|
||||
case SIGHUP :
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"%s Ignore .....\n", ss);
|
||||
fflush(stderr);
|
||||
signal(sig, can_exit);
|
||||
return;
|
||||
case 0:
|
||||
case SIGINT :
|
||||
case SIGPIPE:
|
||||
case SIGQUIT:
|
||||
case SIGFPE :
|
||||
case SIGSEGV:
|
||||
case SIGTERM:
|
||||
if(can_fd>=0) close(can_fd);
|
||||
can_prtime(stderr);
|
||||
fprintf(stderr,"%s process stop!\n", ss);
|
||||
fflush(stderr);
|
||||
close(can_lk);
|
||||
shmdt(can_shm_addr);
|
||||
shmctl(can_shm_id, IPC_STAT, &buf);
|
||||
if(buf.shm_nattch == 0)
|
||||
shmctl(can_shm_id, IPC_RMID, NULL);
|
||||
exit(sig);
|
||||
}
|
||||
}
|
||||
|
||||
char *time2asc(double t){
|
||||
static char stmp[10][20];
|
||||
static int itmp=0;
|
||||
char *lin = stmp[itmp];
|
||||
int h, min;
|
||||
double sec;
|
||||
h = (int)(t/3600.);
|
||||
min = (int)((t - (double)h*3600.)/60.);
|
||||
sec = t - (double)h*3600. - (double)min*60.;
|
||||
h %= 24;
|
||||
sprintf(lin, "%02d:%02d:%09.6f", h,min,sec);
|
||||
itmp = (itmp+1)%10;
|
||||
return lin;
|
||||
}
|
||||
|
||||
double can_dsleep(double dt) {
|
||||
struct timespec ts,tsr;
|
||||
ts.tv_sec = (time_t)dt;
|
||||
ts.tv_nsec = (long)((dt-ts.tv_sec)*1e9);
|
||||
nanosleep(&ts,&tsr);
|
||||
return((double)ts.tv_sec + (double)ts.tv_nsec/1e9);
|
||||
}
|
||||
|
||||
double can_dtime() {
|
||||
struct timeval ct;
|
||||
struct timezone tz;
|
||||
gettimeofday(&ct, &tz);
|
||||
return ((double)ct.tv_sec + (double)ct.tv_usec/1e6);
|
||||
}
|
||||
|
||||
char *can_atime() {return(time2asc(can_dtime()));}
|
||||
|
||||
void can_prtime(FILE *fd) {
|
||||
static double otime=0.0;
|
||||
double ntime=can_dtime();
|
||||
time_t itime = (int)ntime;
|
||||
if(otime==0.0) tzset();
|
||||
ntime -= (double)timezone;
|
||||
if((((int)ntime)%(24*3600) < ((int)otime)%(24*3600)) || otime==0.0)
|
||||
fprintf(fd,"========================\n%s",ctime(&itime));
|
||||
fprintf(fd,"%s ",time2asc(ntime));
|
||||
otime=ntime;
|
||||
}
|
||||
45
Z1000_focus/can_io.h
Normal file
45
Z1000_focus/can_io.h
Normal file
@ -0,0 +1,45 @@
|
||||
// (c) vsher@sao.ru
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifndef CAN_IO_H__
|
||||
#define CAN_IO_H__
|
||||
|
||||
#define _U_ __attribute__((__unused__))
|
||||
|
||||
|
||||
#define CAN_CTLR_SIZE 1024 /* size of client process shared area */
|
||||
#define CAN_RX_SIZE 1000 /* max. # frames in Rx-buffer */
|
||||
#define CAN_RTR_FLAG 0x20000000 /* send frame as Remote Transmission Request */
|
||||
#define CAN_EXT_FLAG 0x40000000 /* send frame with extended 29-bit ID, 11-bit otherwise */
|
||||
|
||||
int can_wait(int fd, double tout);
|
||||
#define can_delay(Tout) can_wait(0, Tout)
|
||||
void set_server_mode(int mode);
|
||||
int can_server();
|
||||
void set_sending_mode(int to_server);
|
||||
int can_sending_mode();
|
||||
int can_card();
|
||||
int can_gate();
|
||||
double can_gate_time_offset();
|
||||
void setup_can_net(unsigned long ipaddr, int port, unsigned long acckey);
|
||||
unsigned long get_acckey();
|
||||
void *init_can_io();
|
||||
void *start_can_io(void *arg);
|
||||
void can_put_buff_frame(double rtime, int id, int length, unsigned char data[]);
|
||||
int can_io_ok();
|
||||
int can_io_shm_ok();
|
||||
int can_ok();
|
||||
void can_clean_recv(int *pbuf, double *rtime);
|
||||
int can_get_buff_frame(int *pbuf, double *rtime,
|
||||
int *id, int *length, unsigned char data[]);
|
||||
int can_recv_frame(int *pbuf, double *rtime,
|
||||
int *id, int *length, unsigned char data[]);
|
||||
int can_send_frame(unsigned long id, int length, unsigned char data[]);
|
||||
void can_exit(int sig);
|
||||
char *time2asc(double t);
|
||||
double can_dsleep(double dt);
|
||||
double can_dtime();
|
||||
char *can_atime();
|
||||
void can_prtime(FILE *fd);
|
||||
#endif // CAN_IO_H__
|
||||
136
Z1000_focus/canmsg.h
Normal file
136
Z1000_focus/canmsg.h
Normal file
@ -0,0 +1,136 @@
|
||||
/* canmsg.h - common kernel-space and user-space CAN message structure
|
||||
* Linux CAN-bus device driver.
|
||||
* Written by Pavel Pisa - OCERA team member
|
||||
* email:pisa@cmp.felk.cvut.cz
|
||||
* This software is released under the GPL-License.
|
||||
* Version lincan-0.3 17 Jun 2004
|
||||
*/
|
||||
|
||||
#ifndef _CANMSG_T_H
|
||||
#define _CANMSG_T_H
|
||||
|
||||
#ifdef __KERNEL__
|
||||
|
||||
#include <linux/time.h>
|
||||
#include <linux/types.h>
|
||||
|
||||
#else /* __KERNEL__ */
|
||||
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#endif /* __KERNEL__ */
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/*
|
||||
* CAN_MSG_VERSION_2 enables new canmsg_t layout compatible with
|
||||
* can4linux project from http://www.port.de/
|
||||
*
|
||||
*/
|
||||
#define CAN_MSG_VERSION_2
|
||||
|
||||
/* Number of data bytes in one CAN message */
|
||||
#define CAN_MSG_LENGTH 8
|
||||
|
||||
#ifdef CAN_MSG_VERSION_2
|
||||
|
||||
typedef struct timeval canmsg_tstamp_t ;
|
||||
|
||||
typedef unsigned long canmsg_id_t;
|
||||
|
||||
/**
|
||||
* struct canmsg_t - structure representing CAN message
|
||||
* @flags: message flags
|
||||
* %MSG_RTR .. message is Remote Transmission Request,
|
||||
* %MSG_EXT .. message with extended ID,
|
||||
* %MSG_OVR .. indication of queue overflow condition,
|
||||
* %MSG_LOCAL .. message originates from this node.
|
||||
* @cob: communication object number (not used)
|
||||
* @id: ID of CAN message
|
||||
* @timestamp: not used
|
||||
* @length: length of used data
|
||||
* @data: data bytes buffer
|
||||
*
|
||||
* Header: canmsg.h
|
||||
*/
|
||||
struct canmsg_t {
|
||||
int flags;
|
||||
int cob;
|
||||
canmsg_id_t id;
|
||||
canmsg_tstamp_t timestamp;
|
||||
unsigned short length;
|
||||
unsigned char data[CAN_MSG_LENGTH];
|
||||
};
|
||||
|
||||
#else /*CAN_MSG_VERSION_2*/
|
||||
#ifndef PACKED
|
||||
#define PACKED __attribute__((packed))
|
||||
#endif
|
||||
/* Old, deprecated version of canmsg_t structure */
|
||||
struct canmsg_t {
|
||||
short flags;
|
||||
int cob;
|
||||
canmsg_id_t id;
|
||||
unsigned long timestamp;
|
||||
unsigned int length;
|
||||
unsigned char data[CAN_MSG_LENGTH];
|
||||
} PACKED;
|
||||
#endif /*CAN_MSG_VERSION_2*/
|
||||
|
||||
typedef struct canmsg_t canmsg_t;
|
||||
|
||||
/**
|
||||
* struct canfilt_t - structure for acceptance filter setup
|
||||
* @flags: message flags
|
||||
* %MSG_RTR .. message is Remote Transmission Request,
|
||||
* %MSG_EXT .. message with extended ID,
|
||||
* %MSG_OVR .. indication of queue overflow condition,
|
||||
* %MSG_LOCAL .. message originates from this node.
|
||||
* there are corresponding mask bits
|
||||
* %MSG_RTR_MASK, %MSG_EXT_MASK, %MSG_LOCAL_MASK.
|
||||
* %MSG_PROCESSLOCAL enables local messages processing in the
|
||||
* combination with global setting
|
||||
* @queid: CAN queue identification in the case of the multiple
|
||||
* queues per one user (open instance)
|
||||
* @cob: communication object number (not used)
|
||||
* @id: selected required value of cared ID id bits
|
||||
* @mask: select bits significand for the comparation;
|
||||
* 1 .. take care about corresponding ID bit, 0 .. don't care
|
||||
*
|
||||
* Header: canmsg.h
|
||||
*/
|
||||
struct canfilt_t {
|
||||
int flags;
|
||||
int queid;
|
||||
int cob;
|
||||
canmsg_id_t id;
|
||||
canmsg_id_t mask;
|
||||
};
|
||||
|
||||
typedef struct canfilt_t canfilt_t;
|
||||
|
||||
/* Definitions to use for canmsg_t and canfilt_t flags */
|
||||
#define MSG_RTR (1<<0)
|
||||
#define MSG_OVR (1<<1)
|
||||
#define MSG_EXT (1<<2)
|
||||
#define MSG_LOCAL (1<<3)
|
||||
/* If you change above lines, check canque_filtid2internal function */
|
||||
|
||||
/* Additional definitions used for canfilt_t only */
|
||||
#define MSG_FILT_MASK_SHIFT 8
|
||||
#define MSG_RTR_MASK (MSG_RTR<<MSG_FILT_MASK_SHIFT)
|
||||
#define MSG_EXT_MASK (MSG_EXT<<MSG_FILT_MASK_SHIFT)
|
||||
#define MSG_LOCAL_MASK (MSG_LOCAL<<MSG_FILT_MASK_SHIFT)
|
||||
#define MSG_PROCESSLOCAL (MSG_OVR<<MSG_FILT_MASK_SHIFT)
|
||||
|
||||
/* Can message ID mask */
|
||||
#define MSG_ID_MASK ((1l<<29)-1)
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C"*/
|
||||
#endif
|
||||
|
||||
#endif /*_CANMSG_T_H*/
|
||||
314
Z1000_focus/canopen.c
Normal file
314
Z1000_focus/canopen.c
Normal file
@ -0,0 +1,314 @@
|
||||
// (c) vsher@sao.ru
|
||||
#include "canopen.h"
|
||||
#include "sdo_abort_codes.h"
|
||||
|
||||
|
||||
static int rxpnt=-1;
|
||||
static double rxtime=0.;
|
||||
|
||||
int sendNMT(int node, int icode){
|
||||
unsigned long idt=0;
|
||||
int dlen=2;
|
||||
unsigned char tdata[2] = {0};
|
||||
tdata[0] = icode&0xff;
|
||||
tdata[1] = node&0x7f;
|
||||
return (can_send_frame(idt, dlen, tdata) > 0);
|
||||
}
|
||||
|
||||
int resetNode2(int oldnode, int newnode){
|
||||
int idr,dlen,ntout=50;
|
||||
unsigned char rdata[8];
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
if(!sendNMT(oldnode, 0x81)) return 0;
|
||||
for(int i=0; i < ntout; ++i){
|
||||
can_dsleep(0.01);
|
||||
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){
|
||||
if(idr == (0x700|newnode) && dlen==1 && rdata[0] == 0) return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int resetNode(int node){return resetNode2(node,node);}
|
||||
|
||||
int getNodeState(int node){
|
||||
/* use Node Guarding Protocol */
|
||||
unsigned long idt = (0x700 | (node&0x7f) | CAN_RTR_FLAG);
|
||||
int idr = 0, dlen = 0, ntout = 15;
|
||||
unsigned char rdata[8];
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
if(can_send_frame(idt, dlen, rdata)<=0) return 0;
|
||||
for(int i=0; i < ntout; ++i){
|
||||
can_dsleep(0.01);
|
||||
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)) {
|
||||
if(idr == (0x700|node) && dlen == 1) return rdata[0]&0x7f;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int initNode(int node){
|
||||
int state;
|
||||
if(rxpnt<0){
|
||||
init_can_io();
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
}
|
||||
if(!can_ok()) return 0;
|
||||
if((state = getNodeState(node)) == 0) return 0;
|
||||
if(state != NodePreOperational) setPreOper(node);
|
||||
return 1;
|
||||
}
|
||||
|
||||
int sendSDOdata(int node, int func, int object, int subindex, unsigned char data[]){
|
||||
unsigned long idt = 0x600 | (node&0x7f);
|
||||
int dlen = 8;
|
||||
unsigned char tdata[8] = {0};
|
||||
func &= 0x7f;
|
||||
tdata[0] = func;
|
||||
tdata[1] = object&0xff;
|
||||
tdata[2] = (object&0xff00)>>8;
|
||||
tdata[3] = subindex&0xff;
|
||||
switch(func){
|
||||
case 0x22:
|
||||
case 0x23:
|
||||
case 0x43: tdata[7]=data[3];
|
||||
// FALLTHRU
|
||||
case 0x27:
|
||||
case 0x47: tdata[6]=data[2];
|
||||
// FALLTHRU
|
||||
case 0x2b:
|
||||
case 0x4b: tdata[5]=data[1];
|
||||
// FALLTHRU
|
||||
case 0x2f:
|
||||
case 0x4f: tdata[4]=data[0];
|
||||
// FALLTHRU
|
||||
case 0x40: break;
|
||||
default: return 0;
|
||||
}
|
||||
return (can_send_frame(idt, dlen, tdata) > 0);
|
||||
}
|
||||
|
||||
int sendSDOreq(int node, int object, int subindex){
|
||||
unsigned char dummy[1];
|
||||
return sendSDOdata(node, 0x40, object, subindex, dummy);
|
||||
}
|
||||
|
||||
int recvSDOresp(int node, int t_func, int t_object, int t_subindex, unsigned char data[]){
|
||||
int idt = 0x580|(node&0x7f);
|
||||
int idr = 0, dlen = 0;
|
||||
unsigned char rdata[8] = {0};
|
||||
int ntout = (t_object == 0x1010||t_object == 0x1011)? 50 : 15;
|
||||
for(int i = 0; i < ntout; ++i){
|
||||
can_dsleep(0.01);
|
||||
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)) if(idr==idt){
|
||||
int r_func, r_object, r_subindex;
|
||||
if(dlen < 4){
|
||||
fprintf(stderr,"Too short SDO response from Node%d\n",node&0x7f);
|
||||
continue;
|
||||
}
|
||||
r_func = rdata[0];
|
||||
r_object = (rdata[2]<<8)|rdata[1];
|
||||
r_subindex = rdata[3];
|
||||
if(r_func == 0x80){ // got SDO error code
|
||||
unsigned long ercode = (rdata[7]<<24)|(rdata[6]<<16)|(rdata[5]<<8)|rdata[4];
|
||||
fprintf(stderr,"SDO error %08lx from Node%d (object %04x/%d) \n",ercode,node&0x7f,r_object,r_subindex);
|
||||
fprintf(stderr,"(%s)\n",sdo_abort_text(ercode));
|
||||
return 0;
|
||||
}
|
||||
if(r_object!=t_object || r_subindex != t_subindex){
|
||||
fprintf(stderr,"Got SDO response with a stranger object (%04x/%d instead of %04x/%d) from Node%d\n",r_object,r_subindex,t_object,t_subindex,node&0x7f);
|
||||
continue;
|
||||
}
|
||||
if((t_func&0xf0) == 0x20 && r_func == 0x60) return 1;
|
||||
if(t_func == 0x40 && (r_func&0xf0) == 0x40){
|
||||
dlen = 0;
|
||||
switch (r_func & 0x7f){
|
||||
default:
|
||||
case 0x43: data[3] = rdata[7]; dlen++;
|
||||
// FALLTHRU
|
||||
case 0x47: data[2] = rdata[6]; dlen++;
|
||||
// FALLTHRU
|
||||
case 0x4b: data[1] = rdata[5]; dlen++;
|
||||
// FALLTHRU
|
||||
case 0x4f: data[0] = rdata[4]; dlen++;
|
||||
break;
|
||||
}
|
||||
return dlen;
|
||||
}
|
||||
fprintf(stderr,"Suspicious SDO response from Node%d (func %02x object %04x/%d)\n",node&0x7f,r_func,r_object,r_subindex);
|
||||
}
|
||||
}
|
||||
fprintf(stderr,"Can't get SDO response from Node%d! Timeout?\n",node&0x7f);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int doSDOdownload(int node, int object, int subindex, unsigned char data[], int dlen){
|
||||
int func = 0x22;
|
||||
switch(dlen){
|
||||
default:func = 0x22; break;
|
||||
case 4: func = 0x23; break;
|
||||
case 3: func = 0x27; break;
|
||||
case 2: func = 0x2b; break;
|
||||
case 1: func = 0x2f; break;
|
||||
}
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
if(!sendSDOdata(node, func, object, subindex, data)) return 0;
|
||||
return recvSDOresp(node, func, object, subindex, data);
|
||||
}
|
||||
|
||||
int doSDOupload(int node, int object, int subindex, unsigned char data[]){
|
||||
int func = 0x40;
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
if(!sendSDOdata(node, func, object, subindex, data)) return 0;
|
||||
return recvSDOresp(node, func, object, subindex, data);
|
||||
}
|
||||
|
||||
int setLong(int node, int object, int subindex, unsigned long value){
|
||||
unsigned char data[4] = {0};
|
||||
data[0] = value&0xff;
|
||||
data[1] = (value>>8)&0xff;
|
||||
data[2] = (value>>16)&0xff;
|
||||
data[3] = (value>>24)&0xff;
|
||||
return doSDOdownload(node, object, subindex, data, 4);
|
||||
}
|
||||
|
||||
int setShort(int node, int object, int subindex, unsigned short value){
|
||||
unsigned char data[4] = {0};
|
||||
data[0] = value&0xff;
|
||||
data[1] = (value>>8)&0xff;
|
||||
return doSDOdownload(node, object, subindex, data, 2);
|
||||
}
|
||||
|
||||
int setByte(int node, int object, int subindex, unsigned char value){
|
||||
unsigned char data[4] = {0};
|
||||
data[0] = value;
|
||||
return doSDOdownload(node, object, subindex, data, 1);
|
||||
}
|
||||
|
||||
int saveObjects(int node){
|
||||
unsigned char data[4] = {'s','a','v','e'};
|
||||
return doSDOdownload(node, 0x1010, 1, data, 0);
|
||||
}
|
||||
|
||||
static unsigned char sdata[5] = {'\0'};
|
||||
char *getString(int node, int object, int subindex){
|
||||
int dlen = doSDOupload(node, object, subindex, sdata);
|
||||
if(dlen == 0) return NULL;
|
||||
sdata[4] = '\0';
|
||||
return (char*)sdata;
|
||||
}
|
||||
|
||||
int getLong(int node, int object, int subindex, unsigned long *value){
|
||||
unsigned char data[4] = {0};
|
||||
int dlen = doSDOupload(node, object, subindex, data);
|
||||
if(dlen == 0) return 0;
|
||||
if(dlen != 4)
|
||||
fprintf(stderr,"Warning! Got only %d bytes for Long value from Node%d/%04x/%d\n",dlen,node,object,subindex);
|
||||
*value = (data[3]<<24)|(data[2]<<16)|(data[1]<<8)|data[0];
|
||||
return 1;
|
||||
}
|
||||
|
||||
int getShort(int node, int object, int subindex, unsigned short *value){
|
||||
unsigned char data[4] = {0};
|
||||
int dlen = doSDOupload(node, object, subindex, data);
|
||||
if(dlen == 0) return 0;
|
||||
if(dlen != 2)
|
||||
fprintf(stderr,"Warning! Got %d bytes for Short value from Node%d/%04x/%d\n",dlen,node,object,subindex);
|
||||
*value = (data[1]<<8)|data[0];
|
||||
return 1;
|
||||
}
|
||||
|
||||
int getByte(int node, int object, int subindex, unsigned char *value){
|
||||
unsigned char data[4] = {0};
|
||||
int dlen = doSDOupload(node, object, subindex, data);
|
||||
if(dlen == 0) return 0;
|
||||
if(dlen != 1)
|
||||
fprintf(stderr,"Warning! Got %d bytes for Byte value from Node%d/%04x/%d\n",dlen,node,object,subindex);
|
||||
*value = data[0];
|
||||
return 1;
|
||||
}
|
||||
|
||||
int sendSync(){
|
||||
// send broadcasting SYNC telegram
|
||||
unsigned long idt=0x80;
|
||||
int dlen=0;
|
||||
unsigned char tdata[1] = {0};
|
||||
return (can_send_frame(idt, dlen, tdata) > 0);
|
||||
}
|
||||
|
||||
int recvNextPDO(double tout, int *node, unsigned long *value){
|
||||
// wait up to 'tout' sec. for the one next PDO
|
||||
// if ok - return 1 for PDO1 or 2 for PDO2; else 0 if timeout
|
||||
double te = can_dtime()+tout;
|
||||
int idr=0, dlen=0, pdon = 0;
|
||||
unsigned char rdata[8] = {0};
|
||||
do{
|
||||
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){
|
||||
if(idr&CAN_RTR_FLAG) continue;
|
||||
if((idr&0xf80) == 0x180) pdon=1;
|
||||
else if((idr&0xf80) == 0x280) pdon=2;
|
||||
if(pdon){
|
||||
*node = idr&0x7f;
|
||||
*value = (rdata[3]<<24)|(rdata[2]<<16)|(rdata[1]<<8)|rdata[0];
|
||||
return pdon;
|
||||
}
|
||||
}
|
||||
can_dsleep(0.02);
|
||||
} while(can_dtime() < te);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int recvPDOs(double tout, int maxpdo, int node[], int pdo_n[], unsigned long value[]){
|
||||
// wait up to 'tout' sec. for the array of 'maxpdo' PDOs
|
||||
// if ok - return number of PDO really received; else 0 if timeout
|
||||
double te = can_dtime()+tout;
|
||||
int npdo=0, idr=0, dlen=0, pdon = 0;
|
||||
unsigned char rdata[8] = {0};
|
||||
do{
|
||||
while(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata)){
|
||||
if(idr&CAN_RTR_FLAG) continue;
|
||||
if((idr&0xf80) == 0x180) pdon=1;
|
||||
else if((idr&0xf80) == 0x280) pdon=2;
|
||||
else pdon = 0;
|
||||
if(pdon){
|
||||
node[npdo] = idr&0x7f;
|
||||
pdo_n[npdo] = pdon;
|
||||
switch(dlen){
|
||||
case 1:
|
||||
value[npdo] = rdata[0];
|
||||
break;
|
||||
case 2:
|
||||
value[npdo] = (rdata[1]<<8)|rdata[0];
|
||||
break;
|
||||
case 3:
|
||||
value[npdo] = (rdata[2]<<16)|(rdata[1]<<8)|rdata[0];
|
||||
break;
|
||||
default:
|
||||
value[npdo] = (rdata[3]<<24)|(rdata[2]<<16)|(rdata[1]<<8)|rdata[0];
|
||||
break;
|
||||
}
|
||||
npdo++;
|
||||
if(npdo>=maxpdo) return npdo;
|
||||
}
|
||||
}
|
||||
can_dsleep(0.02);
|
||||
} while(can_dtime() < te && npdo < maxpdo);
|
||||
return npdo;
|
||||
}
|
||||
|
||||
int requestPDO(double tout, int node, int pdon, unsigned long *value){
|
||||
// send request for PDO and wait up to 'tout' sec. while the node respond to
|
||||
// if ok - return 1; else 0
|
||||
unsigned long idt = (((pdon==1)?0x180:0x280)|(node&0x7f)|CAN_RTR_FLAG);
|
||||
int dlen=0, rnode;
|
||||
unsigned char dummy[1];
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
if(can_send_frame(idt, dlen, dummy) <= 0) return 0;
|
||||
can_dsleep(0.01);
|
||||
if(recvNextPDO(tout, &rnode, value) == pdon) return 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void clean_recv(){
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
}
|
||||
47
Z1000_focus/canopen.h
Normal file
47
Z1000_focus/canopen.h
Normal file
@ -0,0 +1,47 @@
|
||||
// (c) vsher@sao.ru
|
||||
#pragma once
|
||||
#ifndef CANOPEN_H__
|
||||
#define CANOPEN_H__
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <signal.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include "can_io.h"
|
||||
|
||||
#define startNode(Node) sendNMT(Node,1) // put node in "Operational" mode
|
||||
#define stopNode(Node) sendNMT(Node,2) // put node in "Stop" mode
|
||||
#define setPreOper(Node) sendNMT(Node,0x80) // return node in "Pre-Operational" mode
|
||||
|
||||
#define NodeStopped 4
|
||||
#define NodeOperational 5
|
||||
#define NodePreOperational 0x7f
|
||||
|
||||
void clean_recv();
|
||||
int initNode(int node);
|
||||
int sendNMT(int node, int icode);
|
||||
int resetNode2(int oldnode, int newnode);
|
||||
int resetNode(int node);
|
||||
int getNodeState(int node);
|
||||
int initNode(int node);
|
||||
int sendSDOdata(int node, int func, int object, int subindex, unsigned char data[]);
|
||||
int sendSDOreq(int node, int object, int subindex);
|
||||
int recvSDOresp(int node, int t_func, int t_object, int t_subindex, unsigned char data[]);
|
||||
int doSDOdownload(int node, int object, int subindex, unsigned char data[], int dlen);
|
||||
int doSDOupload(int node, int object, int subindex, unsigned char data[]);
|
||||
int setLong(int node, int object, int subindex, unsigned long value);
|
||||
int setShort(int node, int object, int subindex, unsigned short value);
|
||||
int setByte(int node, int object, int subindex, unsigned char value);
|
||||
int saveObjects(int node);
|
||||
char *getString(int node, int object, int subindex);
|
||||
int getLong(int node, int object, int subindex, unsigned long *value);
|
||||
int getShort(int node, int object, int subindex, unsigned short *value);
|
||||
int getByte(int node, int object, int subindex, unsigned char *value);
|
||||
int sendSync();
|
||||
int recvNextPDO(double tout, int *node, unsigned long *value);
|
||||
int recvPDOs(double tout, int maxpdo, int node[], int pdo_n[], unsigned long value[]);
|
||||
int requestPDO(double tout, int node, int pdon, unsigned long *value);
|
||||
|
||||
#endif // CANOPEN_H__
|
||||
86
Z1000_focus/cmdlnopts.c
Normal file
86
Z1000_focus/cmdlnopts.c
Normal file
@ -0,0 +1,86 @@
|
||||
/*
|
||||
* cmdlnopts.c - the only function that parse cmdln args and returns glob parameters
|
||||
*
|
||||
* Copyright 2013 Edward V. Emelianoff <eddy@sao.ru>
|
||||
*
|
||||
* 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 2 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, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301, USA.
|
||||
*/
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <strings.h>
|
||||
#include <math.h>
|
||||
#include "cmdlnopts.h"
|
||||
#include "usefull_macros.h"
|
||||
#include "socket.h"
|
||||
|
||||
/*
|
||||
* here are global parameters initialisation
|
||||
*/
|
||||
int help;
|
||||
glob_pars G;
|
||||
|
||||
// DEFAULTS
|
||||
// default global parameters
|
||||
glob_pars const Gdefault = {
|
||||
.nodenum = 3,
|
||||
.motorID = 12,
|
||||
.gotopos = NAN,
|
||||
.port = DEFPORT
|
||||
};
|
||||
|
||||
/*
|
||||
* Define command line options by filling structure:
|
||||
* name has_arg flag val type argptr help
|
||||
*/
|
||||
myoption cmdlnopts[] = {
|
||||
// set 1 to param despite of its repeating number:
|
||||
{"help", NO_ARGS, NULL, 'h', arg_none, APTR(&help), "show this help"},
|
||||
{"node", NEED_ARG, NULL, 'n', arg_int, APTR(&G.nodenum), "encoder node number"},
|
||||
{"reset", NO_ARGS, NULL, 'r', arg_none, APTR(&G.reset), "reset encoder"},
|
||||
{"verbose", NO_ARGS, NULL, 'v', arg_int, APTR(&G.verbose), "show more info"},
|
||||
{"motorid", NEED_ARG, NULL, 'i', arg_int, APTR(&G.motorID), "motor controller address"},
|
||||
//{"bcastid", NEED_ARG, NULL, 'b', arg_int, APTR(&G.motorID), "motor controller broadcast address"},
|
||||
{"gotopos", NEED_ARG, NULL, 'g', arg_double, APTR(&G.gotopos), "target focus position"},
|
||||
{"targspeed",NEED_ARG, NULL, 't', arg_double, APTR(&G.targspeed), "move motor with constant speed (rev/min)"},
|
||||
{"stop", NO_ARGS, NULL, 's', arg_none, APTR(&G.stop), "stop motor"},
|
||||
{"monitor", NEED_ARG, NULL, 'm', arg_double, APTR(&G.monitspd), "move a little with given speed with monitoring"},
|
||||
{"eswstate",NO_ARGS, NULL, 'e', arg_none, APTR(&G.showesw), "show end-switches state"},
|
||||
{"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logname), "logfile name and path"},
|
||||
{"server", NO_ARGS, NULL, 'S', arg_none, APTR(&G.server), "work as server"},
|
||||
{"port", NEED_ARG, NULL, 'p', arg_string, APTR(&G.port), "server port number"},
|
||||
{"host", NEED_ARG, NULL, 'H', arg_string, APTR(&G.host), "host to connect (default: localhost)"},
|
||||
{"standalone",NO_ARGS, NULL, 'A', arg_none, APTR(&G.standalone),"run as standalone application"},
|
||||
end_option
|
||||
};
|
||||
|
||||
/**
|
||||
* Parse command line options and return dynamically allocated structure
|
||||
* to global parameters
|
||||
* @param argc - copy of argc from main
|
||||
* @param argv - copy of argv from main
|
||||
* @return allocated structure with global parameters
|
||||
*/
|
||||
glob_pars *parse_args(int argc, char **argv){
|
||||
void *ptr = memcpy(&G, &Gdefault, sizeof(G)); assert(ptr);
|
||||
// format of help: "Usage: progname [args]\n"
|
||||
change_helpstring("Usage: %s [args]\n\n\tWhere args are:\n");
|
||||
// parse arguments
|
||||
parseargs(&argc, &argv, cmdlnopts);
|
||||
if(help) showhelp(-1, cmdlnopts);
|
||||
return &G;
|
||||
}
|
||||
|
||||
50
Z1000_focus/cmdlnopts.h
Normal file
50
Z1000_focus/cmdlnopts.h
Normal file
@ -0,0 +1,50 @@
|
||||
/*
|
||||
* cmdlnopts.h - comand line options for parceargs
|
||||
*
|
||||
* Copyright 2013 Edward V. Emelianoff <eddy@sao.ru>
|
||||
*
|
||||
* 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 2 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, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301, USA.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef __CMDLNOPTS_H__
|
||||
#define __CMDLNOPTS_H__
|
||||
|
||||
#include "parseargs.h"
|
||||
|
||||
|
||||
typedef struct{
|
||||
int nodenum; // encoder's node number
|
||||
int reset; // reset encoder
|
||||
int verbose; // more messages
|
||||
int motorID; // motor address (from controller's settings)
|
||||
int bcastID; // broadcast motor address
|
||||
double gotopos; // move focus to given position
|
||||
double targspeed; // just rotate motor with given speed
|
||||
int stop; // stop motor
|
||||
double monitspd; // start speed monitoring (for dynamics)
|
||||
int showesw; // show end-switches state
|
||||
char *logname; // logfile name & path
|
||||
int server; // work as server
|
||||
char *port; // port number for server or client
|
||||
char *host; // host to connect (in client mode)
|
||||
int standalone; // run standalone
|
||||
} glob_pars;
|
||||
|
||||
|
||||
glob_pars *parse_args(int argc, char **argv);
|
||||
#endif // __CMDLNOPTS_H__
|
||||
|
||||
191
Z1000_focus/main.c
Normal file
191
Z1000_focus/main.c
Normal file
@ -0,0 +1,191 @@
|
||||
/*
|
||||
* This file is part of the Zphocus project.
|
||||
* Copyright 2019 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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 <math.h> // fabs
|
||||
#include <sys/wait.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <signal.h>
|
||||
#include "can_encoder.h"
|
||||
#include "canopen.h"
|
||||
#include "cmdlnopts.h"
|
||||
#include "HW_dependent.h"
|
||||
#include "socket.h"
|
||||
#include "usefull_macros.h"
|
||||
|
||||
static glob_pars *G;
|
||||
|
||||
/**
|
||||
* @brief verbose - printf when parameter `verbose` set
|
||||
* @param fmt - format & other attrs
|
||||
* @return amount of printed characters
|
||||
*/
|
||||
int verbose(const char *fmt, ...){
|
||||
if(!G || !G->verbose) return 0;
|
||||
va_list ar; int i;
|
||||
va_start(ar, fmt);
|
||||
i = vprintf(fmt, ar);
|
||||
va_end(ar);
|
||||
return i;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief signals - signal handler (also called by functions ERR/ERRX)
|
||||
* @param signo - signal number
|
||||
*/
|
||||
void signals(int signo){
|
||||
WARNX("Received signal %d", signo);
|
||||
exit(signo);
|
||||
}
|
||||
|
||||
static void cmdparser(){
|
||||
#define BUFL 128
|
||||
char buf[BUFL] = {0,};
|
||||
if(G->stop) sprintf(buf, S_CMD_STOP);
|
||||
else if(fabs(G->targspeed) > DBL_EPSILON) snprintf(buf, BUFL, S_CMD_TARGSPEED "=%g", G->targspeed);
|
||||
else if(!isnan(G->gotopos)) snprintf(buf, BUFL, S_CMD_GOTO "=%g", G->gotopos);
|
||||
else sprintf(buf, S_CMD_FOCUS);
|
||||
sock_send_data(G->host, G->port, buf);
|
||||
#undef BUFL
|
||||
}
|
||||
|
||||
//extern char can_dev[40];
|
||||
|
||||
int main (int argc, char *argv[]){
|
||||
int ret = 0;
|
||||
double curposition = 0;
|
||||
initial_setup();
|
||||
G = parse_args(argc, argv);
|
||||
|
||||
if(fabs(G->targspeed) > DBL_EPSILON && !isnan(G->gotopos))
|
||||
ERRX("Arguments \"target speed\" and \"target position\" can't meet together!");
|
||||
if(fabs(G->targspeed) > DBL_EPSILON){
|
||||
if(fabs(G->targspeed) < MINSPEED || fabs(G->targspeed) > MAXSPEED){
|
||||
WARNX("Target speed should be be from %d to %d (rev/min)", MINSPEED, MAXSPEED);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
if(!isnan(G->gotopos)){
|
||||
if(G->gotopos > FOCMAX_MM || G->gotopos < FOCMIN_MM){
|
||||
WARNX("Focal distance may be from %g to %g mm", FOCMIN_MM, FOCMAX_MM);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
signal(SIGTERM, signals);
|
||||
signal(SIGTSTP, SIG_IGN);
|
||||
signal(SIGHUP, SIG_IGN);
|
||||
//can_dev[8] = '1';
|
||||
|
||||
if(G->server || G->standalone){ // init hardware
|
||||
if(G->logname){
|
||||
openlogfile(G->logname);
|
||||
}
|
||||
if(init_encoder(G->nodenum, G->reset)) ERRX("Encoder not found");
|
||||
|
||||
if(getPos(&curposition)){
|
||||
WARNX("Can't read current position");
|
||||
ret = 1;
|
||||
goto Oldcond;
|
||||
}else verbose("Position @ start: %.2fmm\n", curposition);
|
||||
|
||||
if(init_motor_ids(G->motorID)){
|
||||
ret = 1;
|
||||
goto Oldcond;
|
||||
}
|
||||
}
|
||||
|
||||
if(G->server){ // daemonize & run server
|
||||
#if !defined EBUG
|
||||
if(daemon(1, 0)){
|
||||
ERR("daemon()");
|
||||
}
|
||||
#endif
|
||||
while(1){ // guard for dead processes
|
||||
pid_t childpid = fork();
|
||||
if(childpid){
|
||||
putlog("create child with PID %d\n", childpid);
|
||||
DBG("Created child with PID %d\n", childpid);
|
||||
wait(NULL);
|
||||
putlog("child %d died\n", childpid);
|
||||
WARNX("Child %d died\n", childpid);
|
||||
sleep(1);
|
||||
}else{
|
||||
prctl(PR_SET_PDEATHSIG, SIGTERM); // send SIGTERM to child when parent dies
|
||||
daemonize(G->port);
|
||||
}
|
||||
}
|
||||
}else if(!G->standalone){
|
||||
cmdparser();
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(fabs(G->monitspd) > DBL_EPSILON){
|
||||
movewithmon(G->monitspd);
|
||||
goto Oldcond;
|
||||
}
|
||||
|
||||
if(G->stop){ // Stop motor
|
||||
if(stop()) ret = 1;
|
||||
goto Oldcond;
|
||||
}
|
||||
|
||||
if(fabs(G->targspeed) > DBL_EPSILON){ // move with constant speed
|
||||
verbose("Try to move with %g revolutions per minute\n", G->targspeed);
|
||||
if(movewconstspeed(G->targspeed)){
|
||||
ret = 1;
|
||||
goto Oldcond;
|
||||
}
|
||||
}
|
||||
|
||||
if(!isnan(G->gotopos)){ // move to given position
|
||||
verbose("Try to move to position %g\n", G->gotopos);
|
||||
ret = move2pos(G->gotopos);
|
||||
goto Oldcond;
|
||||
}
|
||||
|
||||
Oldcond:
|
||||
if(getPos(&curposition)) WARNX("Can't read current position");
|
||||
else{
|
||||
if(G->verbose) printf("pos=%.2fmm, ", curposition);
|
||||
else printf("%.2f\n", curposition);
|
||||
}
|
||||
if(G->showesw){
|
||||
eswstate e;
|
||||
if(CAN_NOERR != get_endswitches(&e)) WARNX("Can't read end-switches state");
|
||||
else switch(e){
|
||||
case ESW_INACTIVE:
|
||||
green("End-switches inactive\n");
|
||||
break;
|
||||
case ESW_CW_ACTIVE:
|
||||
red("Active CW end-switch\n");
|
||||
break;
|
||||
case ESW_CCW_ACTIVE:
|
||||
red("Active CCW end-switch\n");
|
||||
break;
|
||||
case ESW_BOTH_ACTIVE:
|
||||
default:
|
||||
red("ERROR: both end-switches active\n");
|
||||
}
|
||||
}
|
||||
double spd;
|
||||
if(get_motor_speed(&spd) == CAN_NOERR) verbose("speed=%d\n", spd);
|
||||
else WARNX("Can't read speed");
|
||||
|
||||
returnPreOper();
|
||||
return ret;
|
||||
}
|
||||
98
Z1000_focus/motor_cancodes.h
Normal file
98
Z1000_focus/motor_cancodes.h
Normal file
@ -0,0 +1,98 @@
|
||||
/*
|
||||
* This file is part of the Zphocus project.
|
||||
* Copyright 2019 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
#pragma once
|
||||
#ifndef MOTOR_CANCODES_H__
|
||||
#define MOTOR_CANCODES_H__
|
||||
|
||||
// functions PO/PI/etc
|
||||
#define PO_FNO 3
|
||||
#define PI_FNO 4
|
||||
#define SYNC_FNO 5
|
||||
#define GR_FNO 6
|
||||
#define PARAMDATA 512
|
||||
|
||||
// calculation of ID from address
|
||||
#define MOTOR_PO_ID(addr) ((addr<<3) + PO_FNO)
|
||||
#define MOTOR_PAR_ID(addr) (PARAMDATA + (addr<<3) + PO_FNO)
|
||||
//#define MOTOR_BCAST_PO_ID(addr) ((addr<<3) + GR_FNO)
|
||||
|
||||
// zero's (command) byte of parameter request
|
||||
// read & write parameters
|
||||
#define CAN_READPAR_CMD 0x31
|
||||
#define CAN_WRITEPAR_CMD 0x32
|
||||
// error flag in answer
|
||||
#define CAN_PAR_ERRFLAG 0x80
|
||||
|
||||
// control word bits & bit combination
|
||||
#define CW_RAPIDSTOP 0
|
||||
#define CW_INHIBIT 1
|
||||
#define CW_STOP 2
|
||||
#define CW_ENABLE 6
|
||||
#define CW_B_BLOCK (1<<0)
|
||||
#define CW_B_ENRAPID (1<<1)
|
||||
#define CW_B_ENSTOP (1<<2)
|
||||
#define CW_B_TEMPO (1<<4)
|
||||
#define CW_B_PARSET (1<<5)
|
||||
#define CW_B_CLERR (1<<6)
|
||||
|
||||
// status word
|
||||
#define SW_B_UNBLOCK (1<<0)
|
||||
#define SW_B_READY (1<<1)
|
||||
#define SW_B_POUNBLOCK (1<<2)
|
||||
#define SW_B_TEMPO21 (1<<3)
|
||||
#define SW_B_PARAM21 (1<<4)
|
||||
#define SW_B_MAILFUN (1<<5)
|
||||
// artifical status bit (instead of reserved) for error getting speed
|
||||
#define SW_B_CANTGETSPD (1<<6)
|
||||
#define SW_ENABLE 4
|
||||
#define SW_NOTENABLE 2
|
||||
#define SW_INHIBIT 1
|
||||
|
||||
// state codes (when SW_B_MAILFUN==0)
|
||||
#define STATE_NOTREADY 0
|
||||
#define STATE_BLOCK 1
|
||||
#define STATE_NOPERMIT 2
|
||||
#define STATE_DETENT 3
|
||||
#define STATE_PERMIS 4
|
||||
#define STATE_REGUL 5
|
||||
#define STATE_FACTORYST 8
|
||||
#define STATE_CAPTURE 13
|
||||
#define STATE_W4DATA 16
|
||||
#define STATE_SAFESTOP 17
|
||||
|
||||
// Some parameters: indexes & subindexes
|
||||
// Digital inputs
|
||||
#define PAR_DI_SUBIDX 0
|
||||
// inputs state (lowest bit is DI00)
|
||||
#define PAR_DIST_IDX 8334
|
||||
// Speed & current
|
||||
#define PAR_SPD_SUBIDX 0
|
||||
#define PAR_CRNT_SUBIDX 0
|
||||
#define PAR_SPD_IDX 8318
|
||||
#define PAR_CRNT_IDX 8326
|
||||
// inputs role
|
||||
#define PAR_DI00_IDX 8844
|
||||
#define PAR_DI02_IDX 8336
|
||||
#define PAR_DI03_IDX 8337
|
||||
#define PAR_DI04_IDX 8338
|
||||
#define PAR_DI05_IDX 8339
|
||||
// roles:
|
||||
#define DI_NOFUNC 0
|
||||
#define DI_ENSTOP 1
|
||||
|
||||
#endif // MOTOR_CANCODES_H__
|
||||
497
Z1000_focus/parseargs.c
Normal file
497
Z1000_focus/parseargs.c
Normal file
@ -0,0 +1,497 @@
|
||||
/* geany_encoding=koi8-r
|
||||
* parseargs.c - parsing command line arguments & print help
|
||||
*
|
||||
* Copyright 2013 Edward V. Emelianoff <eddy@sao.ru>
|
||||
*
|
||||
* 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 2 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, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301, USA.
|
||||
*/
|
||||
|
||||
#include <stdio.h> // printf
|
||||
#include <getopt.h> // getopt_long
|
||||
#include <stdlib.h> // calloc, exit, strtoll
|
||||
#include <assert.h> // assert
|
||||
#include <string.h> // strdup, strchr, strlen
|
||||
#include <strings.h>// strcasecmp
|
||||
#include <limits.h> // INT_MAX & so on
|
||||
#include <libintl.h>// gettext
|
||||
#include <ctype.h> // isalpha
|
||||
#include "parseargs.h"
|
||||
#include "usefull_macros.h"
|
||||
|
||||
char *helpstring = "%s\n";
|
||||
|
||||
/**
|
||||
* Change standard help header
|
||||
* MAY consist ONE "%s" for progname
|
||||
* @param str (i) - new format
|
||||
*/
|
||||
void change_helpstring(char *s){
|
||||
int pcount = 0, scount = 0;
|
||||
char *str = s;
|
||||
// check `helpstring` and set it to default in case of error
|
||||
for(; pcount < 2; str += 2){
|
||||
if(!(str = strchr(str, '%'))) break;
|
||||
if(str[1] != '%') pcount++; // increment '%' counter if it isn't "%%"
|
||||
else{
|
||||
str += 2; // pass next '%'
|
||||
continue;
|
||||
}
|
||||
if(str[1] == 's') scount++; // increment "%s" counter
|
||||
};
|
||||
if(pcount > 1 || pcount != scount){ // amount of pcount and/or scount wrong
|
||||
/// "îÅÐÒÁ×ÉÌØÎÙÊ ÆÏÒÍÁÔ ÓÔÒÏËÉ ÐÏÍÏÝÉ"
|
||||
ERRX(_("Wrong helpstring!"));
|
||||
}
|
||||
helpstring = s;
|
||||
}
|
||||
|
||||
/**
|
||||
* Carefull atoll/atoi
|
||||
* @param num (o) - returning value (or NULL if you wish only check number) - allocated by user
|
||||
* @param str (i) - string with number must not be NULL
|
||||
* @param t (i) - T_INT for integer or T_LLONG for long long (if argtype would be wided, may add more)
|
||||
* @return TRUE if conversion sone without errors, FALSE otherwise
|
||||
*/
|
||||
static bool myatoll(void *num, char *str, argtype t){
|
||||
long long tmp, *llptr;
|
||||
int *iptr;
|
||||
char *endptr;
|
||||
assert(str);
|
||||
assert(num);
|
||||
tmp = strtoll(str, &endptr, 0);
|
||||
if(endptr == str || *str == '\0' || *endptr != '\0')
|
||||
return FALSE;
|
||||
switch(t){
|
||||
case arg_longlong:
|
||||
llptr = (long long*) num;
|
||||
*llptr = tmp;
|
||||
break;
|
||||
case arg_int:
|
||||
default:
|
||||
if(tmp < INT_MIN || tmp > INT_MAX){
|
||||
/// "ãÅÌÏÅ ×ÎÅ ÄÏÐÕÓÔÉÍÏÇÏ ÄÉÁÐÁÚÏÎÁ"
|
||||
WARNX(_("Integer out of range"));
|
||||
return FALSE;
|
||||
}
|
||||
iptr = (int*)num;
|
||||
*iptr = (int)tmp;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// the same as myatoll but for double
|
||||
// There's no NAN & INF checking here (what if they would be needed?)
|
||||
static bool myatod(void *num, const char *str, argtype t){
|
||||
double tmp, *dptr;
|
||||
float *fptr;
|
||||
char *endptr;
|
||||
assert(str);
|
||||
tmp = strtod(str, &endptr);
|
||||
if(endptr == str || *str == '\0' || *endptr != '\0')
|
||||
return FALSE;
|
||||
switch(t){
|
||||
case arg_double:
|
||||
dptr = (double *) num;
|
||||
*dptr = tmp;
|
||||
break;
|
||||
case arg_float:
|
||||
default:
|
||||
fptr = (float *) num;
|
||||
*fptr = (float)tmp;
|
||||
break;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get index of current option in array options
|
||||
* @param opt (i) - returning val of getopt_long
|
||||
* @param options (i) - array of options
|
||||
* @return index in array
|
||||
*/
|
||||
static int get_optind(int opt, myoption *options){
|
||||
int oind;
|
||||
myoption *opts = options;
|
||||
assert(opts);
|
||||
for(oind = 0; opts->name && opts->val != opt; oind++, opts++);
|
||||
if(!opts->name || opts->val != opt) // no such parameter
|
||||
showhelp(-1, options);
|
||||
return oind;
|
||||
}
|
||||
|
||||
/**
|
||||
* reallocate new value in array of multiple repeating arguments
|
||||
* @arg paptr - address of pointer to array (**void)
|
||||
* @arg type - its type (for realloc)
|
||||
* @return pointer to new (next) value
|
||||
*/
|
||||
void *get_aptr(void *paptr, argtype type){
|
||||
int i = 1;
|
||||
void **aptr = *((void***)paptr);
|
||||
if(aptr){ // there's something in array
|
||||
void **p = aptr;
|
||||
while(*p++) ++i;
|
||||
}
|
||||
size_t sz = 0;
|
||||
switch(type){
|
||||
default:
|
||||
case arg_none:
|
||||
/// "îÅ ÍÏÇÕ ÉÓÐÏÌØÚÏ×ÁÔØ ÎÅÓËÏÌØËÏ ÐÁÒÁÍÅÔÒÏ× ÂÅÚ ÁÒÇÕÍÅÎÔÏ×!"
|
||||
ERRX("Can't use multiple args with arg_none!");
|
||||
break;
|
||||
case arg_int:
|
||||
sz = sizeof(int);
|
||||
break;
|
||||
case arg_longlong:
|
||||
sz = sizeof(long long);
|
||||
break;
|
||||
case arg_double:
|
||||
sz = sizeof(double);
|
||||
break;
|
||||
case arg_float:
|
||||
sz = sizeof(float);
|
||||
break;
|
||||
case arg_string:
|
||||
sz = 0;
|
||||
break;
|
||||
/* case arg_function:
|
||||
sz = sizeof(argfn *);
|
||||
break;*/
|
||||
}
|
||||
aptr = realloc(aptr, (i + 1) * sizeof(void*));
|
||||
*((void***)paptr) = aptr;
|
||||
aptr[i] = NULL;
|
||||
if(sz){
|
||||
aptr[i - 1] = malloc(sz);
|
||||
}else
|
||||
aptr[i - 1] = &aptr[i - 1];
|
||||
return aptr[i - 1];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Parse command line arguments
|
||||
* ! If arg is string, then value will be strdup'ed!
|
||||
*
|
||||
* @param argc (io) - address of argc of main(), return value of argc stay after `getopt`
|
||||
* @param argv (io) - address of argv of main(), return pointer to argv stay after `getopt`
|
||||
* BE CAREFUL! if you wanna use full argc & argv, save their original values before
|
||||
* calling this function
|
||||
* @param options (i) - array of `myoption` for arguments parcing
|
||||
*
|
||||
* @exit: in case of error this function show help & make `exit(-1)`
|
||||
*/
|
||||
void parseargs(int *argc, char ***argv, myoption *options){
|
||||
char *short_options, *soptr;
|
||||
struct option *long_options, *loptr;
|
||||
size_t optsize, i;
|
||||
myoption *opts = options;
|
||||
// check whether there is at least one options
|
||||
assert(opts);
|
||||
assert(opts[0].name);
|
||||
// first we count how much values are in opts
|
||||
for(optsize = 0; opts->name; optsize++, opts++);
|
||||
// now we can allocate memory
|
||||
short_options = calloc(optsize * 3 + 1, 1); // multiply by three for '::' in case of args in opts
|
||||
long_options = calloc(optsize + 1, sizeof(struct option));
|
||||
opts = options; loptr = long_options; soptr = short_options;
|
||||
// in debug mode check the parameters are not repeated
|
||||
#ifdef EBUG
|
||||
char **longlist = MALLOC(char*, optsize);
|
||||
char *shortlist = MALLOC(char, optsize);
|
||||
#endif
|
||||
// fill short/long parameters and make a simple checking
|
||||
for(i = 0; i < optsize; i++, loptr++, opts++){
|
||||
// check
|
||||
assert(opts->name); // check name
|
||||
#ifdef EBUG
|
||||
longlist[i] = strdup(opts->name);
|
||||
#endif
|
||||
if(opts->has_arg){
|
||||
assert(opts->type != arg_none); // check error with arg type
|
||||
assert(opts->argptr); // check pointer
|
||||
}
|
||||
if(opts->type != arg_none) // if there is a flag without arg, check its pointer
|
||||
assert(opts->argptr);
|
||||
// fill long_options
|
||||
// don't do memcmp: what if there would be different alignment?
|
||||
loptr->name = opts->name;
|
||||
loptr->has_arg = (opts->has_arg < MULT_PAR) ? opts->has_arg : 1;
|
||||
loptr->flag = opts->flag;
|
||||
loptr->val = opts->val;
|
||||
// fill short options if they are:
|
||||
if(!opts->flag && opts->val){
|
||||
#ifdef EBUG
|
||||
shortlist[i] = (char) opts->val;
|
||||
#endif
|
||||
*soptr++ = opts->val;
|
||||
if(loptr->has_arg) // add ':' if option has required argument
|
||||
*soptr++ = ':';
|
||||
if(loptr->has_arg == 2) // add '::' if option has optional argument
|
||||
*soptr++ = ':';
|
||||
}
|
||||
}
|
||||
// sort all lists & check for repeating
|
||||
#ifdef EBUG
|
||||
int cmpstringp(const void *p1, const void *p2){
|
||||
return strcmp(* (char * const *) p1, * (char * const *) p2);
|
||||
}
|
||||
int cmpcharp(const void *p1, const void *p2){
|
||||
return (int)(*(char * const)p1 - *(char *const)p2);
|
||||
}
|
||||
qsort(longlist, optsize, sizeof(char *), cmpstringp);
|
||||
qsort(shortlist,optsize, sizeof(char), cmpcharp);
|
||||
char *prevl = longlist[0], prevshrt = shortlist[0];
|
||||
for(i = 1; i < optsize; ++i){
|
||||
if(longlist[i]){
|
||||
if(prevl){
|
||||
if(strcmp(prevl, longlist[i]) == 0) ERRX("double long arguments: --%s", prevl);
|
||||
}
|
||||
prevl = longlist[i];
|
||||
}
|
||||
if(shortlist[i]){
|
||||
if(prevshrt){
|
||||
if(prevshrt == shortlist[i]) ERRX("double short arguments: -%c", prevshrt);
|
||||
}
|
||||
prevshrt = shortlist[i];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// now we have both long_options & short_options and can parse `getopt_long`
|
||||
while(1){
|
||||
int opt;
|
||||
int oindex = 0, optind = 0; // oindex - number of option in argv, optind - number in options[]
|
||||
if((opt = getopt_long(*argc, *argv, short_options, long_options, &oindex)) == -1) break;
|
||||
if(opt == '?'){
|
||||
opt = optopt;
|
||||
optind = get_optind(opt, options);
|
||||
if(options[optind].has_arg == NEED_ARG || options[optind].has_arg == MULT_PAR)
|
||||
showhelp(optind, options); // need argument
|
||||
}
|
||||
else{
|
||||
if(opt == 0 || oindex > 0) optind = oindex;
|
||||
else optind = get_optind(opt, options);
|
||||
}
|
||||
opts = &options[optind];
|
||||
// if(opt == 0 && opts->has_arg == NO_ARGS) continue; // only long option changing integer flag
|
||||
// now check option
|
||||
if(opts->has_arg == NEED_ARG || opts->has_arg == MULT_PAR)
|
||||
if(!optarg) showhelp(optind, options); // need argument
|
||||
void *aptr;
|
||||
if(opts->has_arg == MULT_PAR){
|
||||
aptr = get_aptr(opts->argptr, opts->type);
|
||||
}else
|
||||
aptr = opts->argptr;
|
||||
bool result = TRUE;
|
||||
// even if there is no argument, but argptr != NULL, think that optarg = "1"
|
||||
if(!optarg) optarg = "1";
|
||||
switch(opts->type){
|
||||
default:
|
||||
case arg_none:
|
||||
if(opts->argptr) *((int*)aptr) += 1; // increment value
|
||||
break;
|
||||
case arg_int:
|
||||
result = myatoll(aptr, optarg, arg_int);
|
||||
break;
|
||||
case arg_longlong:
|
||||
result = myatoll(aptr, optarg, arg_longlong);
|
||||
break;
|
||||
case arg_double:
|
||||
result = myatod(aptr, optarg, arg_double);
|
||||
break;
|
||||
case arg_float:
|
||||
result = myatod(aptr, optarg, arg_float);
|
||||
break;
|
||||
case arg_string:
|
||||
result = (*((void**)aptr) = (void*)strdup(optarg));
|
||||
break;
|
||||
case arg_function:
|
||||
result = ((argfn)aptr)(optarg);
|
||||
break;
|
||||
}
|
||||
if(!result){
|
||||
showhelp(optind, options);
|
||||
}
|
||||
}
|
||||
*argc -= optind;
|
||||
*argv += optind;
|
||||
}
|
||||
|
||||
/**
|
||||
* compare function for qsort
|
||||
* first - sort by short options; second - sort arguments without sort opts (by long options)
|
||||
*/
|
||||
static int argsort(const void *a1, const void *a2){
|
||||
const myoption *o1 = (myoption*)a1, *o2 = (myoption*)a2;
|
||||
const char *l1 = o1->name, *l2 = o2->name;
|
||||
int s1 = o1->val, s2 = o2->val;
|
||||
int *f1 = o1->flag, *f2 = o2->flag;
|
||||
// check if both options has short arg
|
||||
if(f1 == NULL && f2 == NULL && s1 && s2){ // both have short arg
|
||||
return (s1 - s2);
|
||||
}else if((f1 != NULL || !s1) && (f2 != NULL || !s2)){ // both don't have short arg - sort by long
|
||||
return strcmp(l1, l2);
|
||||
}else{ // only one have short arg -- return it
|
||||
if(f2 || !s2) return -1; // a1 have short - it is 'lesser'
|
||||
else return 1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Show help information based on myoption->help values
|
||||
* @param oindex (i) - if non-negative, show only help by myoption[oindex].help
|
||||
* @param options (i) - array of `myoption`
|
||||
*
|
||||
* @exit: run `exit(-1)` !!!
|
||||
*/
|
||||
void showhelp(int oindex, myoption *options){
|
||||
int max_opt_len = 0; // max len of options substring - for right indentation
|
||||
const int bufsz = 255;
|
||||
char buf[bufsz+1];
|
||||
myoption *opts = options;
|
||||
assert(opts);
|
||||
assert(opts[0].name); // check whether there is at least one options
|
||||
if(oindex > -1){ // print only one message
|
||||
opts = &options[oindex];
|
||||
printf(" ");
|
||||
if(!opts->flag && isalpha(opts->val)) printf("-%c, ", opts->val);
|
||||
printf("--%s", opts->name);
|
||||
if(opts->has_arg == 1) printf("=arg");
|
||||
else if(opts->has_arg == 2) printf("[=arg]");
|
||||
printf(" %s\n", _(opts->help));
|
||||
exit(-1);
|
||||
}
|
||||
// header, by default is just "progname\n"
|
||||
printf("\n");
|
||||
if(strstr(helpstring, "%s")) // print progname
|
||||
printf(helpstring, __progname);
|
||||
else // only text
|
||||
printf("%s", helpstring);
|
||||
printf("\n");
|
||||
// count max_opt_len
|
||||
do{
|
||||
int L = strlen(opts->name);
|
||||
if(max_opt_len < L) max_opt_len = L;
|
||||
}while((++opts)->name);
|
||||
max_opt_len += 14; // format: '-S , --long[=arg]' - get addition 13 symbols
|
||||
opts = options;
|
||||
// count amount of options
|
||||
int N; for(N = 0; opts->name; ++N, ++opts);
|
||||
if(N == 0) exit(-2);
|
||||
// Now print all help (sorted)
|
||||
opts = options;
|
||||
qsort(opts, N, sizeof(myoption), argsort);
|
||||
do{
|
||||
int p = sprintf(buf, " "); // a little indent
|
||||
if(!opts->flag && opts->val) // .val is short argument
|
||||
p += snprintf(buf+p, bufsz-p, "-%c, ", opts->val);
|
||||
p += snprintf(buf+p, bufsz-p, "--%s", opts->name);
|
||||
if(opts->has_arg == 1) // required argument
|
||||
p += snprintf(buf+p, bufsz-p, "=arg");
|
||||
else if(opts->has_arg == 2) // optional argument
|
||||
p += snprintf(buf+p, bufsz-p, "[=arg]");
|
||||
assert(p < max_opt_len); // there would be magic if p >= max_opt_len
|
||||
printf("%-*s%s\n", max_opt_len+1, buf, _(opts->help)); // write options & at least 2 spaces after
|
||||
++opts;
|
||||
}while(--N);
|
||||
printf("\n\n");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
/**
|
||||
* get suboptions from parameter string
|
||||
* @param str - parameter string
|
||||
* @param opt - pointer to suboptions structure
|
||||
* @return TRUE if all OK
|
||||
*/
|
||||
bool get_suboption(char *str, mysuboption *opt){
|
||||
int findsubopt(char *par, mysuboption *so){
|
||||
int idx = 0;
|
||||
if(!par) return -1;
|
||||
while(so[idx].name){
|
||||
if(strcasecmp(par, so[idx].name) == 0) return idx;
|
||||
++idx;
|
||||
}
|
||||
return -1; // badarg
|
||||
}
|
||||
bool opt_setarg(mysuboption *so, int idx, char *val){
|
||||
mysuboption *soptr = &so[idx];
|
||||
bool result = FALSE;
|
||||
void *aptr = soptr->argptr;
|
||||
switch(soptr->type){
|
||||
default:
|
||||
case arg_none:
|
||||
if(soptr->argptr) *((int*)aptr) += 1; // increment value
|
||||
result = TRUE;
|
||||
break;
|
||||
case arg_int:
|
||||
result = myatoll(aptr, val, arg_int);
|
||||
break;
|
||||
case arg_longlong:
|
||||
result = myatoll(aptr, val, arg_longlong);
|
||||
break;
|
||||
case arg_double:
|
||||
result = myatod(aptr, val, arg_double);
|
||||
break;
|
||||
case arg_float:
|
||||
result = myatod(aptr, val, arg_float);
|
||||
break;
|
||||
case arg_string:
|
||||
result = (*((void**)aptr) = (void*)strdup(val));
|
||||
break;
|
||||
case arg_function:
|
||||
result = ((argfn)aptr)(val);
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
char *tok;
|
||||
bool ret = FALSE;
|
||||
char *tmpbuf;
|
||||
tok = strtok_r(str, ":,", &tmpbuf);
|
||||
do{
|
||||
char *val = strchr(tok, '=');
|
||||
int noarg = 0;
|
||||
if(val == NULL){ // no args
|
||||
val = "1";
|
||||
noarg = 1;
|
||||
}else{
|
||||
*val++ = '\0';
|
||||
if(!*val || *val == ':' || *val == ','){ // no argument - delimeter after =
|
||||
val = "1"; noarg = 1;
|
||||
}
|
||||
}
|
||||
int idx = findsubopt(tok, opt);
|
||||
if(idx < 0){
|
||||
/// "îÅÐÒÁ×ÉÌØÎÙÊ ÐÁÒÁÍÅÔÒ: %s"
|
||||
WARNX(_("Wrong parameter: %s"), tok);
|
||||
goto returning;
|
||||
}
|
||||
if(noarg && opt[idx].has_arg == NEED_ARG){
|
||||
/// "%s: ÎÅÏÂÈÏÄÉÍ ÁÒÇÕÍÅÎÔ!"
|
||||
WARNX(_("%s: argument needed!"), tok);
|
||||
goto returning;
|
||||
}
|
||||
if(!opt_setarg(opt, idx, val)){
|
||||
/// "îÅÐÒÁ×ÉÌØÎÙÊ ÁÒÇÕÍÅÎÔ \"%s\" ÐÁÒÁÍÅÔÒÁ \"%s\""
|
||||
WARNX(_("Wrong argument \"%s\" of parameter \"%s\""), val, tok);
|
||||
goto returning;
|
||||
}
|
||||
}while((tok = strtok_r(NULL, ":,", &tmpbuf)));
|
||||
ret = TRUE;
|
||||
returning:
|
||||
return ret;
|
||||
}
|
||||
124
Z1000_focus/parseargs.h
Normal file
124
Z1000_focus/parseargs.h
Normal file
@ -0,0 +1,124 @@
|
||||
/*
|
||||
* parseargs.h - headers for parsing command line arguments
|
||||
*
|
||||
* Copyright 2013 Edward V. Emelianoff <eddy@sao.ru>
|
||||
*
|
||||
* 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 2 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, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301, USA.
|
||||
*/
|
||||
#pragma once
|
||||
#ifndef __PARSEARGS_H__
|
||||
#define __PARSEARGS_H__
|
||||
|
||||
#include <stdbool.h>// bool
|
||||
#include <stdlib.h>
|
||||
|
||||
#ifndef TRUE
|
||||
#define TRUE true
|
||||
#endif
|
||||
|
||||
#ifndef FALSE
|
||||
#define FALSE false
|
||||
#endif
|
||||
|
||||
// macro for argptr
|
||||
#define APTR(x) ((void*)x)
|
||||
|
||||
// if argptr is a function:
|
||||
typedef bool(*argfn)(void *arg);
|
||||
|
||||
/*
|
||||
* type of getopt's argument
|
||||
* WARNING!
|
||||
* My function change value of flags by pointer, so if you want to use another type
|
||||
* make a latter conversion, example:
|
||||
* char charg;
|
||||
* int iarg;
|
||||
* myoption opts[] = {
|
||||
* {"value", 1, NULL, 'v', arg_int, &iarg, "char val"}, ..., end_option};
|
||||
* ..(parse args)..
|
||||
* charg = (char) iarg;
|
||||
*/
|
||||
typedef enum {
|
||||
arg_none = 0, // no arg
|
||||
arg_int, // integer
|
||||
arg_longlong, // long long
|
||||
arg_double, // double
|
||||
arg_float, // float
|
||||
arg_string, // char *
|
||||
arg_function // parse_args will run function `bool (*fn)(char *optarg, int N)`
|
||||
} argtype;
|
||||
|
||||
/*
|
||||
* Structure for getopt_long & help
|
||||
* BE CAREFUL: .argptr is pointer to data or pointer to function,
|
||||
* conversion depends on .type
|
||||
*
|
||||
* ATTENTION: string `help` prints through macro PRNT(), bu default it is gettext,
|
||||
* but you can redefine it before `#include "parseargs.h"`
|
||||
*
|
||||
* if arg is string, then value wil be strdup'ed like that:
|
||||
* char *str;
|
||||
* myoption opts[] = {{"string", 1, NULL, 's', arg_string, &str, "string val"}, ..., end_option};
|
||||
* *(opts[1].str) = strdup(optarg);
|
||||
* in other cases argptr should be address of some variable (or pointer to allocated memory)
|
||||
*
|
||||
* NON-NULL argptr should be written inside macro APTR(argptr) or directly: (void*)argptr
|
||||
*
|
||||
* !!!LAST VALUE OF ARRAY SHOULD BE `end_option` or ZEROS !!!
|
||||
*
|
||||
*/
|
||||
typedef enum{
|
||||
NO_ARGS = 0, // first three are the same as in getopt_long
|
||||
NEED_ARG = 1,
|
||||
OPT_ARG = 2,
|
||||
MULT_PAR
|
||||
} hasarg;
|
||||
|
||||
typedef struct{
|
||||
// these are from struct option:
|
||||
const char *name; // long option's name
|
||||
hasarg has_arg; // 0 - no args, 1 - nesessary arg, 2 - optionally arg, 4 - need arg & key can repeat (args are stored in null-terminated array)
|
||||
int *flag; // NULL to return val, pointer to int - to set its value of val (function returns 0)
|
||||
int val; // short opt name (if flag == NULL) or flag's value
|
||||
// and these are mine:
|
||||
argtype type; // type of argument
|
||||
void *argptr; // pointer to variable to assign optarg value or function `bool (*fn)(char *optarg, int N)`
|
||||
const char *help; // help string which would be shown in function `showhelp` or NULL
|
||||
} myoption;
|
||||
|
||||
/*
|
||||
* Suboptions structure, almost the same like myoption
|
||||
* used in parse_subopts()
|
||||
*/
|
||||
typedef struct{
|
||||
const char *name;
|
||||
hasarg has_arg;
|
||||
argtype type;
|
||||
void *argptr;
|
||||
} mysuboption;
|
||||
|
||||
// last string of array (all zeros)
|
||||
#define end_option {0,0,0,0,0,0,0}
|
||||
#define end_suboption {0,0,0,0}
|
||||
|
||||
extern const char *__progname;
|
||||
|
||||
void showhelp(int oindex, myoption *options);
|
||||
void parseargs(int *argc, char ***argv, myoption *options);
|
||||
void change_helpstring(char *s);
|
||||
bool get_suboption(char *str, mysuboption *opt);
|
||||
|
||||
#endif // __PARSEARGS_H__
|
||||
50
Z1000_focus/sdo_abort_codes.h
Normal file
50
Z1000_focus/sdo_abort_codes.h
Normal file
@ -0,0 +1,50 @@
|
||||
// (c) vsher@sao.ru
|
||||
#pragma once
|
||||
#ifndef SDO_ABORT_CODES_H__
|
||||
#define SDO_ABORT_CODES_H__
|
||||
|
||||
/* CANopen SDO response Abort codes*/
|
||||
#define SDO_MAX_ERR 30
|
||||
struct SDO_Abort_Codes {
|
||||
unsigned long code;
|
||||
char *text;
|
||||
} sdo_error[SDO_MAX_ERR] = {
|
||||
{0x05030000, "Toggle bit not alternated"},
|
||||
{0x05040000, "SDO protocol timed out"},
|
||||
{0x05040001, "Client/server command specifier not valid or unknown"},
|
||||
{0x05040002, "Invalid block size (block mode only)"},
|
||||
{0x05040003, "Invalid sequence number (block mode only)"},
|
||||
{0x05040004, "CRC error (block mode only)"},
|
||||
{0x05040005, "Out of memory"},
|
||||
{0x06010000, "Unsupported access to an object"},
|
||||
{0x06010001, "Attempt to read a write only object"},
|
||||
{0x06010002, "Attempt to write a read only object"},
|
||||
{0x06020000, "Object does not exist in the object dictionary"},
|
||||
{0x06040041, "Object cannot be mapped to the PDO"},
|
||||
{0x06040042, "The number and length of the objects to be mapped whould exeed PDO length"},
|
||||
{0x06040043, "General parameter incompatibility reason"},
|
||||
{0x06040047, "General internal incompatibility in the device"},
|
||||
{0x06060000, "Access failed due to a hardware error"},
|
||||
{0x06070010, "Data type does not match, length of service parameter does not match"},
|
||||
{0x06070012, "Data type does not match, length of service parameter too hight"},
|
||||
{0x06070013, "Data type does not match, length of service parameter too low"},
|
||||
{0x06090011, "Sub-index does not exist."},
|
||||
{0x06090030, "Value range of parameter exceeded (only for write access)"},
|
||||
{0x06090031, "Value of parameter written too hight"},
|
||||
{0x06090032, "Value of parameter written too low"},
|
||||
{0x06090036, "Maximum value is less than minimum value"},
|
||||
{0x08000000, "General error"},
|
||||
{0x08000020, "Data cannot be transferred or stored to the application"},
|
||||
{0x08000021, "Data cannot be transferred or stored to the application because of local control"},
|
||||
{0x08000022, "Data cannot be transferred or stored to the application because ofthe present device state"},
|
||||
{0x08000023, "Object dictionary dynamic generation fails or no object dictionary is present."},
|
||||
{0,NULL}
|
||||
};
|
||||
static inline char *sdo_abort_text(unsigned long code) {
|
||||
int i;
|
||||
for(i=0; i<SDO_MAX_ERR && sdo_error[i].text!=NULL; i++)
|
||||
if(code==sdo_error[i].code) return sdo_error[i].text;
|
||||
return "SDO error of unknown type";
|
||||
}
|
||||
|
||||
#endif // SDO_ABORT_CODES_H__
|
||||
401
Z1000_focus/socket.c
Normal file
401
Z1000_focus/socket.c
Normal file
@ -0,0 +1,401 @@
|
||||
/*
|
||||
* geany_encoding=koi8-r
|
||||
* socket.c - socket IO
|
||||
*
|
||||
* Copyright 2018 Edward V. Emelianov <eddy@sao.ru, edward.emelianoff@gmail.com>
|
||||
*
|
||||
* 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 2 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, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301, USA.
|
||||
*
|
||||
*/
|
||||
#include "can_encoder.h"
|
||||
#include "HW_dependent.h"
|
||||
#include "usefull_macros.h"
|
||||
#include "socket.h"
|
||||
#include <netdb.h> // addrinfo
|
||||
#include <arpa/inet.h> // inet_ntop
|
||||
#include <pthread.h>
|
||||
#include <limits.h> // INT_xxx
|
||||
#include <math.h> // fabs
|
||||
#include <signal.h> // pthread_kill
|
||||
#include <unistd.h> // daemon
|
||||
#include <sys/syscall.h> // syscall
|
||||
|
||||
#include "cmdlnopts.h" // glob_pars
|
||||
|
||||
#define BUFLEN (10240)
|
||||
// Max amount of connections
|
||||
#define BACKLOG (30)
|
||||
|
||||
extern glob_pars *G;
|
||||
|
||||
/**************** COMMON FUNCTIONS ****************/
|
||||
/**
|
||||
* wait for answer from socket
|
||||
* @param sock - socket fd
|
||||
* @return 0 in case of error or timeout, 1 in case of socket ready
|
||||
*/
|
||||
static int waittoread(int sock){
|
||||
fd_set fds;
|
||||
struct timeval timeout;
|
||||
int rc;
|
||||
timeout.tv_sec = 1; // wait not more than 1 second
|
||||
timeout.tv_usec = 0;
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(sock, &fds);
|
||||
do{
|
||||
rc = select(sock+1, &fds, NULL, NULL, &timeout);
|
||||
if(rc < 0){
|
||||
if(errno != EINTR){
|
||||
WARN("select()");
|
||||
return 0;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
}while(1);
|
||||
if(FD_ISSET(sock, &fds)) return 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**************** SERVER FUNCTIONS ****************/
|
||||
// `canbus_mutex` used to exclude simultaneous CAN messages
|
||||
// `moving_mutex` used to block simultaneous attempts to move motor
|
||||
static pthread_mutex_t canbus_mutex = PTHREAD_MUTEX_INITIALIZER, moving_mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
bool emerg_stop = FALSE;
|
||||
|
||||
/**
|
||||
* Send data over socket
|
||||
* @param sock - socket fd
|
||||
* @param webquery - ==1 if this is web query
|
||||
* @param buf - buffer with data (zero-terminated)
|
||||
* @return 1 if all OK
|
||||
*/
|
||||
static int send_data(int sock, int webquery, char *buf){
|
||||
if(!buf) return 0;
|
||||
ssize_t L, Len = strlen(buf);
|
||||
DBG("buf: %s, Len: %zd", buf, Len);
|
||||
if(Len < 1) return 0;
|
||||
char tbuf[BUFLEN];
|
||||
// OK buffer ready, prepare to send it
|
||||
if(webquery){
|
||||
L = snprintf(tbuf, BUFLEN,
|
||||
"HTTP/2.0 200 OK\r\n"
|
||||
"Access-Control-Allow-Origin: *\r\n"
|
||||
"Access-Control-Allow-Methods: GET, POST\r\n"
|
||||
"Access-Control-Allow-Credentials: true\r\n"
|
||||
"Content-type: text/plain\r\nContent-Length: %zd\r\n\r\n", Len);
|
||||
if(L < 0){
|
||||
WARN("sprintf()");
|
||||
return 0;
|
||||
}
|
||||
if(L != write(sock, tbuf, L)){
|
||||
WARN("write");
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
if(Len != write(sock, buf, Len)){
|
||||
WARN("write()");
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
// search a first word after needle without spaces
|
||||
static char* stringscan(char *str, char *needle){
|
||||
char *a, *e;
|
||||
char *end = str + strlen(str);
|
||||
a = strstr(str, needle);
|
||||
if(!a) return NULL;
|
||||
a += strlen(needle);
|
||||
while (a < end && (*a == ' ' || *a == '\r' || *a == '\t' || *a == '\r')) a++;
|
||||
if(a >= end) return NULL;
|
||||
e = strchr(a, ' ');
|
||||
if(e) *e = 0;
|
||||
return a;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief move_focus - separate thread moving focus to given position
|
||||
* @param targpos - target position
|
||||
*/
|
||||
static void *move_focus(void *targpos){
|
||||
double pos = *((double*)targpos);
|
||||
DBG("MOVE FOCUS: %g", pos);
|
||||
pthread_mutex_lock(&canbus_mutex);
|
||||
// in any error case we should check end-switches and move out of them!
|
||||
if(move2pos(pos)) go_out_from_ESW();
|
||||
pthread_mutex_unlock(&moving_mutex);
|
||||
pthread_mutex_unlock(&canbus_mutex);
|
||||
pthread_exit(NULL);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static const char *startmoving(double pos){
|
||||
static double sp;
|
||||
if(pthread_mutex_trylock(&moving_mutex)) return S_ANS_MOVING;
|
||||
pthread_t m_thread;
|
||||
DBG("startmoving: %g", pos);
|
||||
sp = pos;
|
||||
if(pthread_create(&m_thread, NULL, move_focus, (void*) &sp)){
|
||||
WARN("pthread_create()");
|
||||
pthread_mutex_unlock(&moving_mutex);
|
||||
return S_ANS_ERR;
|
||||
}else{
|
||||
DBG("Thread created, detouch");
|
||||
pthread_detach(m_thread); // don't care about thread state
|
||||
}
|
||||
return S_ANS_OK;
|
||||
}
|
||||
|
||||
static void *handle_socket(void *asock){
|
||||
#define getparam(x) (strncmp(found, x, sizeof(x)-1) == 0)
|
||||
//putlog("handle_socket(): getpid: %d, pthread_self: %lu, tid: %lu",getpid(), pthread_self(), syscall(SYS_gettid));
|
||||
int sock = *((int*)asock);
|
||||
int webquery = 0; // whether query is web or regular
|
||||
char buff[BUFLEN];
|
||||
ssize_t rd;
|
||||
double t0 = dtime();
|
||||
while(dtime() - t0 < SOCKET_TIMEOUT){
|
||||
if(!waittoread(sock)){ // no data incoming
|
||||
DBG("no incoming data");
|
||||
continue;
|
||||
}
|
||||
if((rd = read(sock, buff, BUFLEN-1)) < 1){
|
||||
DBG("socket closed. Exit");
|
||||
break;
|
||||
}
|
||||
DBG("Got %zd bytes", rd);
|
||||
// add trailing zero to be on the safe side
|
||||
buff[rd] = 0;
|
||||
// now we should check what do user want
|
||||
char *got, *found = buff;
|
||||
if((got = stringscan(buff, "GET")) || (got = stringscan(buff, "POST"))){ // web query
|
||||
webquery = 1;
|
||||
char *slash = strchr(got, '/');
|
||||
if(slash) found = slash + 1;
|
||||
// web query have format GET /some.resource
|
||||
}
|
||||
// here we can process user data
|
||||
DBG("user send: %s%s\n", buff, webquery ? ", web" : "");
|
||||
// empty request == focus request
|
||||
if(strlen(found) < 1 || getparam(S_CMD_FOCUS)){
|
||||
DBG("position request");
|
||||
snprintf(buff, BUFLEN, "%.3f", curPos());
|
||||
}else if(getparam(S_CMD_STOP)){
|
||||
DBG("Stop request");
|
||||
emerg_stop = TRUE;
|
||||
pthread_mutex_lock(&canbus_mutex);
|
||||
pthread_mutex_lock(&moving_mutex);
|
||||
if(stop()) sprintf(buff, S_ANS_ERR);
|
||||
else sprintf(buff, S_ANS_OK);
|
||||
emerg_stop = FALSE;
|
||||
pthread_mutex_unlock(&moving_mutex);
|
||||
pthread_mutex_unlock(&canbus_mutex);
|
||||
}else if(getparam(S_CMD_TARGSPEED)){
|
||||
char *ch = strchr(found, '=');
|
||||
double spd;
|
||||
if(pthread_mutex_trylock(&moving_mutex)) sprintf(buff, S_ANS_MOVING);
|
||||
else{
|
||||
pthread_mutex_lock(&canbus_mutex);
|
||||
if(!ch || !str2double(&spd, ch+1) || fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED || movewconstspeed(spd)) sprintf(buff, S_ANS_ERR);
|
||||
else{
|
||||
DBG("Move with constant speed %g request", spd);
|
||||
sprintf(buff, S_ANS_OK);
|
||||
}
|
||||
pthread_mutex_unlock(&canbus_mutex);
|
||||
pthread_mutex_unlock(&moving_mutex);
|
||||
}
|
||||
}else if(getparam(S_CMD_GOTO)){
|
||||
char *ch = strchr(found, '=');
|
||||
double pos;
|
||||
if(!ch || !str2double(&pos, ch+1) || pos < FOCMIN_MM || pos > FOCMAX_MM) sprintf(buff, S_ANS_ERR);
|
||||
else{
|
||||
DBG("Move to position %g request", pos);
|
||||
sprintf(buff, startmoving(pos));
|
||||
}
|
||||
}else sprintf(buff, S_ANS_ERR);
|
||||
if(!send_data(sock, webquery, buff)){
|
||||
WARNX("can't send data, some error occured");
|
||||
}
|
||||
}
|
||||
close(sock);
|
||||
DBG("closed");
|
||||
//putlog("socket closed, exit");
|
||||
pthread_exit(NULL);
|
||||
return NULL;
|
||||
#undef getparam
|
||||
}
|
||||
|
||||
// main socket server
|
||||
static void *server(void *asock){
|
||||
putlog("server(): getpid: %d, pthread_self: %lu, tid: %lu",getpid(), pthread_self(), syscall(SYS_gettid));
|
||||
int sock = *((int*)asock);
|
||||
if(listen(sock, BACKLOG) == -1){
|
||||
putlog("listen() failed");
|
||||
WARN("listen");
|
||||
return NULL;
|
||||
}
|
||||
while(1){
|
||||
socklen_t size = sizeof(struct sockaddr_in);
|
||||
struct sockaddr_in their_addr;
|
||||
int newsock;
|
||||
if(!waittoread(sock)) continue;
|
||||
newsock = accept(sock, (struct sockaddr*)&their_addr, &size);
|
||||
if(newsock <= 0){
|
||||
putlog("accept() failed");
|
||||
WARN("accept()");
|
||||
continue;
|
||||
}
|
||||
struct sockaddr_in* pV4Addr = (struct sockaddr_in*)&their_addr;
|
||||
struct in_addr ipAddr = pV4Addr->sin_addr;
|
||||
char str[INET_ADDRSTRLEN];
|
||||
inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN);
|
||||
//putlog("get connection from %s", str);
|
||||
DBG("Got connection from %s", str);
|
||||
pthread_t handler_thread;
|
||||
if(pthread_create(&handler_thread, NULL, handle_socket, (void*) &newsock)){
|
||||
putlog("server(): pthread_create() failed");
|
||||
WARN("pthread_create()");
|
||||
}else{
|
||||
DBG("Thread created, detouch");
|
||||
pthread_detach(handler_thread); // don't care about thread state
|
||||
}
|
||||
}
|
||||
putlog("server(): UNREACHABLE CODE REACHED!");
|
||||
}
|
||||
|
||||
// data gathering & socket management
|
||||
static void daemon_(int sock){
|
||||
if(sock < 0) return;
|
||||
pthread_t sock_thread;
|
||||
if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){
|
||||
putlog("daemon_(): pthread_create() failed");
|
||||
ERR("pthread_create()");
|
||||
}
|
||||
do{
|
||||
if(pthread_kill(sock_thread, 0) == ESRCH){ // died
|
||||
WARNX("Sockets thread died");
|
||||
putlog("Sockets thread died");
|
||||
pthread_join(sock_thread, NULL);
|
||||
if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){
|
||||
putlog("daemon_(): new pthread_create() failed");
|
||||
ERR("pthread_create()");
|
||||
}
|
||||
}
|
||||
usleep(500000); // sleep a little or thread's won't be able to lock mutex
|
||||
// get current position
|
||||
pthread_mutex_lock(&canbus_mutex);
|
||||
getPos(NULL);
|
||||
pthread_mutex_unlock(&canbus_mutex);
|
||||
}while(1);
|
||||
putlog("daemon_(): UNREACHABLE CODE REACHED!");
|
||||
}
|
||||
|
||||
/**
|
||||
* Run daemon service
|
||||
*/
|
||||
void daemonize(const char *port){
|
||||
int sock = -1;
|
||||
struct addrinfo hints, *res, *p;
|
||||
memset(&hints, 0, sizeof(hints));
|
||||
hints.ai_family = AF_INET;
|
||||
hints.ai_socktype = SOCK_STREAM;
|
||||
hints.ai_flags = AI_PASSIVE;
|
||||
if(getaddrinfo(NULL, port, &hints, &res) != 0){
|
||||
ERR("getaddrinfo");
|
||||
}
|
||||
struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr;
|
||||
char str[INET_ADDRSTRLEN];
|
||||
inet_ntop(AF_INET, &(ia->sin_addr), str, INET_ADDRSTRLEN);
|
||||
DBG("canonname: %s, port: %u, addr: %s\n", res->ai_canonname, ntohs(ia->sin_port), str);
|
||||
// loop through all the results and bind to the first we can
|
||||
for(p = res; p != NULL; p = p->ai_next){
|
||||
if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){
|
||||
WARN("socket");
|
||||
continue;
|
||||
}
|
||||
int reuseaddr = 1;
|
||||
if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) == -1){
|
||||
ERR("setsockopt");
|
||||
}
|
||||
if(bind(sock, p->ai_addr, p->ai_addrlen) == -1){
|
||||
close(sock);
|
||||
WARN("bind");
|
||||
continue;
|
||||
}
|
||||
break; // if we get here, we have a successfull connection
|
||||
}
|
||||
if(p == NULL){
|
||||
putlog("failed to bind socket, exit");
|
||||
// looped off the end of the list with no successful bind
|
||||
ERRX("failed to bind socket");
|
||||
}
|
||||
freeaddrinfo(res);
|
||||
daemon_(sock);
|
||||
close(sock);
|
||||
putlog("socket closed, exit");
|
||||
signals(0);
|
||||
}
|
||||
|
||||
/**************** CLIENT FUNCTIONS ****************/
|
||||
|
||||
/**
|
||||
* @brief sock_send_data - send data to a socket
|
||||
*/
|
||||
void sock_send_data(const char *host, const char *port, const char *data){
|
||||
int sock = 0;
|
||||
struct addrinfo hints, *res, *p;
|
||||
memset(&hints, 0, sizeof(hints));
|
||||
hints.ai_family = AF_INET;
|
||||
hints.ai_socktype = SOCK_STREAM;
|
||||
hints.ai_flags = AI_PASSIVE;
|
||||
if(getaddrinfo(host, port, &hints, &res) != 0){
|
||||
ERR("getaddrinfo");
|
||||
}
|
||||
struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr;
|
||||
char str[INET_ADDRSTRLEN];
|
||||
inet_ntop(AF_INET, &(ia->sin_addr), str, INET_ADDRSTRLEN);
|
||||
DBG("canonname: %s, port: %u, addr: %s\n", res->ai_canonname, ntohs(ia->sin_port), str);
|
||||
// loop through all the results and bind to the first we can
|
||||
for(p = res; p != NULL; p = p->ai_next){
|
||||
if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){
|
||||
WARN("socket");
|
||||
continue;
|
||||
}
|
||||
if(connect(sock, p->ai_addr, p->ai_addrlen) == -1){
|
||||
WARN("connect()");
|
||||
close(sock);
|
||||
continue;
|
||||
}
|
||||
break; // if we get here, we have a successfull connection
|
||||
}
|
||||
if(p == NULL) ERRX("failed to connect to server");
|
||||
size_t L = strlen(data);
|
||||
if(send(sock, data, L, 0) != (ssize_t)L){ WARN("send"); return;}
|
||||
double t0 = dtime();
|
||||
while(dtime() - t0 < SOCKET_TIMEOUT){
|
||||
if(!waittoread(sock)) continue;
|
||||
char buff[32];
|
||||
int n = read(sock, buff, 31);
|
||||
if(n > 0){
|
||||
buff[n] = 0;
|
||||
printf("%s\n", buff);
|
||||
close(sock);
|
||||
return;
|
||||
}
|
||||
}
|
||||
WARN("No answer!");
|
||||
}
|
||||
50
Z1000_focus/socket.h
Normal file
50
Z1000_focus/socket.h
Normal file
@ -0,0 +1,50 @@
|
||||
/*
|
||||
* geany_encoding=koi8-r
|
||||
* socket.h
|
||||
*
|
||||
* Copyright 2017 Edward V. Emelianov <eddy@sao.ru, edward.emelianoff@gmail.com>
|
||||
*
|
||||
* 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 2 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, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301, USA.
|
||||
*
|
||||
*/
|
||||
#pragma once
|
||||
#ifndef __SOCKET_H__
|
||||
#define __SOCKET_H__
|
||||
|
||||
#include "stdbool.h"
|
||||
|
||||
// timeout for socket closing
|
||||
#define SOCKET_TIMEOUT (5.0)
|
||||
// default port number (strinig)
|
||||
#define DEFPORT "4444"
|
||||
|
||||
// commands through the socket
|
||||
#define S_CMD_STOP "stop"
|
||||
#define S_CMD_FOCUS "focus"
|
||||
#define S_CMD_TARGSPEED "targspeed"
|
||||
#define S_CMD_GOTO "goto"
|
||||
|
||||
// answers through the socket
|
||||
#define S_ANS_ERR "error"
|
||||
#define S_ANS_OK "OK"
|
||||
#define S_ANS_MOVING "moving"
|
||||
|
||||
bool emerg_stop;
|
||||
|
||||
void daemonize(const char *port);
|
||||
void sock_send_data(const char *host, const char *port, const char *data);
|
||||
|
||||
#endif // __SOCKET_H__
|
||||
434
Z1000_focus/usefull_macros.c
Normal file
434
Z1000_focus/usefull_macros.c
Normal file
@ -0,0 +1,434 @@
|
||||
/*
|
||||
* usefull_macros.h - a set of usefull functions: memory, color etc
|
||||
*
|
||||
* Copyright 2013 Edward V. Emelianoff <eddy@sao.ru>
|
||||
*
|
||||
* 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 2 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, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301, USA.
|
||||
*/
|
||||
|
||||
#include "usefull_macros.h"
|
||||
#include <time.h>
|
||||
#include <linux/limits.h> // PATH_MAX
|
||||
|
||||
/**
|
||||
* function for different purposes that need to know time intervals
|
||||
* @return double value: time in seconds
|
||||
*/
|
||||
double dtime(){
|
||||
double t;
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, NULL);
|
||||
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
|
||||
return t;
|
||||
}
|
||||
|
||||
/******************************************************************************\
|
||||
* Coloured terminal
|
||||
\******************************************************************************/
|
||||
int globErr = 0; // errno for WARN/ERR
|
||||
|
||||
// pointers to coloured output printf
|
||||
int (*red)(const char *fmt, ...);
|
||||
int (*green)(const char *fmt, ...);
|
||||
int (*_WARN)(const char *fmt, ...);
|
||||
|
||||
/*
|
||||
* format red / green messages
|
||||
* name: r_pr_, g_pr_
|
||||
* @param fmt ... - printf-like format
|
||||
* @return number of printed symbols
|
||||
*/
|
||||
int r_pr_(const char *fmt, ...){
|
||||
va_list ar; int i;
|
||||
printf(RED);
|
||||
va_start(ar, fmt);
|
||||
i = vprintf(fmt, ar);
|
||||
va_end(ar);
|
||||
printf(OLDCOLOR);
|
||||
return i;
|
||||
}
|
||||
int g_pr_(const char *fmt, ...){
|
||||
va_list ar; int i;
|
||||
printf(GREEN);
|
||||
va_start(ar, fmt);
|
||||
i = vprintf(fmt, ar);
|
||||
va_end(ar);
|
||||
printf(OLDCOLOR);
|
||||
return i;
|
||||
}
|
||||
/*
|
||||
* print red error/warning messages (if output is a tty)
|
||||
* @param fmt ... - printf-like format
|
||||
* @return number of printed symbols
|
||||
*/
|
||||
int r_WARN(const char *fmt, ...){
|
||||
va_list ar; int i = 1;
|
||||
fprintf(stderr, RED);
|
||||
va_start(ar, fmt);
|
||||
if(globErr){
|
||||
errno = globErr;
|
||||
vwarn(fmt, ar);
|
||||
errno = 0;
|
||||
}else
|
||||
i = vfprintf(stderr, fmt, ar);
|
||||
va_end(ar);
|
||||
i++;
|
||||
fprintf(stderr, OLDCOLOR "\n");
|
||||
return i;
|
||||
}
|
||||
|
||||
static const char stars[] = "****************************************";
|
||||
/*
|
||||
* notty variants of coloured printf
|
||||
* name: s_WARN, r_pr_notty
|
||||
* @param fmt ... - printf-like format
|
||||
* @return number of printed symbols
|
||||
*/
|
||||
int s_WARN(const char *fmt, ...){
|
||||
va_list ar; int i;
|
||||
i = fprintf(stderr, "\n%s\n", stars);
|
||||
va_start(ar, fmt);
|
||||
if(globErr){
|
||||
errno = globErr;
|
||||
vwarn(fmt, ar);
|
||||
errno = 0;
|
||||
}else
|
||||
i = +vfprintf(stderr, fmt, ar);
|
||||
va_end(ar);
|
||||
i += fprintf(stderr, "\n%s\n", stars);
|
||||
i += fprintf(stderr, "\n");
|
||||
return i;
|
||||
}
|
||||
int r_pr_notty(const char *fmt, ...){
|
||||
va_list ar; int i;
|
||||
i = printf("\n%s\n", stars);
|
||||
va_start(ar, fmt);
|
||||
i += vprintf(fmt, ar);
|
||||
va_end(ar);
|
||||
i += printf("\n%s\n", stars);
|
||||
return i;
|
||||
}
|
||||
|
||||
/**
|
||||
* Run this function in the beginning of main() to setup locale & coloured output
|
||||
*/
|
||||
void initial_setup(){
|
||||
// setup coloured output
|
||||
if(isatty(STDOUT_FILENO)){ // make color output in tty
|
||||
red = r_pr_; green = g_pr_;
|
||||
}else{ // no colors in case of pipe
|
||||
red = r_pr_notty; green = printf;
|
||||
}
|
||||
if(isatty(STDERR_FILENO)) _WARN = r_WARN;
|
||||
else _WARN = s_WARN;
|
||||
// Setup locale
|
||||
setlocale(LC_ALL, "");
|
||||
setlocale(LC_NUMERIC, "C");
|
||||
#if defined GETTEXT_PACKAGE && defined LOCALEDIR
|
||||
bindtextdomain(GETTEXT_PACKAGE, LOCALEDIR);
|
||||
textdomain(GETTEXT_PACKAGE);
|
||||
#endif
|
||||
}
|
||||
|
||||
/******************************************************************************\
|
||||
* Memory
|
||||
\******************************************************************************/
|
||||
/*
|
||||
* safe memory allocation for macro ALLOC
|
||||
* @param N - number of elements to allocate
|
||||
* @param S - size of single element (typically sizeof)
|
||||
* @return pointer to allocated memory area
|
||||
*/
|
||||
void *my_alloc(size_t N, size_t S){
|
||||
void *p = calloc(N, S);
|
||||
if(!p) ERR("malloc");
|
||||
//assert(p);
|
||||
return p;
|
||||
}
|
||||
|
||||
/**
|
||||
* Mmap file to a memory area
|
||||
*
|
||||
* @param filename (i) - name of file to mmap
|
||||
* @return stuct with mmap'ed file or die
|
||||
*/
|
||||
mmapbuf *My_mmap(char *filename){
|
||||
int fd;
|
||||
char *ptr;
|
||||
size_t Mlen;
|
||||
struct stat statbuf;
|
||||
/// "îÅ ÚÁÄÁÎÏ ÉÍÑ ÆÁÊÌÁ!"
|
||||
if(!filename){
|
||||
WARNX(_("No filename given!"));
|
||||
return NULL;
|
||||
}
|
||||
if((fd = open(filename, O_RDONLY)) < 0){
|
||||
/// "îÅ ÍÏÇÕ ÏÔËÒÙÔØ %s ÄÌÑ ÞÔÅÎÉÑ"
|
||||
WARN(_("Can't open %s for reading"), filename);
|
||||
return NULL;
|
||||
}
|
||||
if(fstat (fd, &statbuf) < 0){
|
||||
/// "îÅ ÍÏÇÕ ×ÙÐÏÌÎÉÔØ stat %s"
|
||||
WARN(_("Can't stat %s"), filename);
|
||||
close(fd);
|
||||
return NULL;
|
||||
}
|
||||
Mlen = statbuf.st_size;
|
||||
if((ptr = mmap (0, Mlen, PROT_READ, MAP_PRIVATE, fd, 0)) == MAP_FAILED){
|
||||
/// "ïÛÉÂËÁ mmap"
|
||||
WARN(_("Mmap error for input"));
|
||||
close(fd);
|
||||
return NULL;
|
||||
}
|
||||
/// "îÅ ÍÏÇÕ ÚÁËÒÙÔØ mmap'ÎÕÔÙÊ ÆÁÊÌ"
|
||||
if(close(fd)) WARN(_("Can't close mmap'ed file"));
|
||||
mmapbuf *ret = MALLOC(mmapbuf, 1);
|
||||
ret->data = ptr;
|
||||
ret->len = Mlen;
|
||||
return ret;
|
||||
}
|
||||
|
||||
void My_munmap(mmapbuf *b){
|
||||
if(munmap(b->data, b->len)){
|
||||
/// "îÅ ÍÏÇÕ munmap"
|
||||
ERR(_("Can't munmap"));
|
||||
}
|
||||
FREE(b);
|
||||
}
|
||||
|
||||
|
||||
/******************************************************************************\
|
||||
* Terminal in no-echo mode
|
||||
\******************************************************************************/
|
||||
static struct termios oldt, newt; // terminal flags
|
||||
static int console_changed = 0;
|
||||
// run on exit:
|
||||
void restore_console(){
|
||||
if(console_changed)
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // return terminal to previous state
|
||||
console_changed = 0;
|
||||
}
|
||||
|
||||
// initial setup:
|
||||
void setup_con(){
|
||||
if(console_changed) return;
|
||||
tcgetattr(STDIN_FILENO, &oldt);
|
||||
newt = oldt;
|
||||
newt.c_lflag &= ~(ICANON | ECHO);
|
||||
if(tcsetattr(STDIN_FILENO, TCSANOW, &newt) < 0){
|
||||
/// "îÅ ÍÏÇÕ ÎÁÓÔÒÏÉÔØ ËÏÎÓÏÌØ"
|
||||
WARN(_("Can't setup console"));
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
|
||||
signals(0); //quit?
|
||||
}
|
||||
console_changed = 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Read character from console without echo
|
||||
* @return char readed
|
||||
*/
|
||||
int read_console(){
|
||||
int rb;
|
||||
struct timeval tv;
|
||||
int retval;
|
||||
fd_set rfds;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(STDIN_FILENO, &rfds);
|
||||
tv.tv_sec = 0; tv.tv_usec = 10000;
|
||||
retval = select(1, &rfds, NULL, NULL, &tv);
|
||||
if(!retval) rb = 0;
|
||||
else {
|
||||
if(FD_ISSET(STDIN_FILENO, &rfds)) rb = getchar();
|
||||
else rb = 0;
|
||||
}
|
||||
return rb;
|
||||
}
|
||||
|
||||
/**
|
||||
* getchar() without echo
|
||||
* wait until at least one character pressed
|
||||
* @return character readed
|
||||
*/
|
||||
int mygetchar(){ // getchar() without need of pressing ENTER
|
||||
int ret;
|
||||
do ret = read_console();
|
||||
while(ret == 0);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/******************************************************************************\
|
||||
* TTY with select()
|
||||
\******************************************************************************/
|
||||
static struct termio oldtty, tty; // TTY flags
|
||||
static int comfd = -1; // TTY fd
|
||||
|
||||
// run on exit:
|
||||
void restore_tty(){
|
||||
if(comfd == -1) return;
|
||||
ioctl(comfd, TCSANOW, &oldtty ); // return TTY to previous state
|
||||
close(comfd);
|
||||
comfd = -1;
|
||||
}
|
||||
|
||||
#ifndef BAUD_RATE
|
||||
#define BAUD_RATE B4800
|
||||
#endif
|
||||
// init:
|
||||
void tty_init(char *comdev){
|
||||
DBG("\nOpen port %s ...\n", comdev);
|
||||
do{
|
||||
comfd = open(comdev,O_RDWR|O_NOCTTY|O_NONBLOCK);
|
||||
}while (comfd == -1 && errno == EINTR);
|
||||
if(comfd < 0){
|
||||
WARN("Can't use port %s\n",comdev);
|
||||
signals(-1); // quit?
|
||||
}
|
||||
// make exclusive open
|
||||
if(ioctl(comfd, TIOCEXCL)){
|
||||
WARN(_("Can't do exclusive open"));
|
||||
close(comfd);
|
||||
signals(2);
|
||||
}
|
||||
DBG(" OK\nGet current settings... ");
|
||||
if(ioctl(comfd,TCGETA,&oldtty) < 0){ // Get settings
|
||||
/// "îÅ ÍÏÇÕ ÐÏÌÕÞÉÔØ ÎÁÓÔÒÏÊËÉ"
|
||||
WARN(_("Can't get settings"));
|
||||
signals(-1);
|
||||
}
|
||||
tty = oldtty;
|
||||
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
|
||||
tty.c_oflag = 0;
|
||||
tty.c_cflag = BAUD_RATE|CS8|CREAD|CLOCAL; // 9.6k, 8N1, RW, ignore line ctrl
|
||||
tty.c_cc[VMIN] = 0; // non-canonical mode
|
||||
tty.c_cc[VTIME] = 5;
|
||||
if(ioctl(comfd,TCSETA,&tty) < 0){
|
||||
/// "îÅ ÍÏÇÕ ÕÓÔÁÎÏ×ÉÔØ ÎÁÓÔÒÏÊËÉ"
|
||||
WARN(_("Can't set settings"));
|
||||
signals(-1);
|
||||
}
|
||||
DBG(" OK\n");
|
||||
}
|
||||
|
||||
/**
|
||||
* Read data from TTY
|
||||
* @param buff (o) - buffer for data read
|
||||
* @param length - buffer len
|
||||
* @return amount of bytes read
|
||||
*/
|
||||
size_t read_tty(char *buff, size_t length){
|
||||
ssize_t L = 0, l;
|
||||
char *ptr = buff;
|
||||
fd_set rfds;
|
||||
struct timeval tv;
|
||||
int retval;
|
||||
do{
|
||||
l = 0;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(comfd, &rfds);
|
||||
// wait for 100ms
|
||||
tv.tv_sec = 0; tv.tv_usec = 100000;
|
||||
retval = select(comfd + 1, &rfds, NULL, NULL, &tv);
|
||||
if (!retval) break;
|
||||
if(FD_ISSET(comfd, &rfds)){
|
||||
if((l = read(comfd, ptr, length)) < 1){
|
||||
return 0;
|
||||
}
|
||||
ptr += l; L += l;
|
||||
length -= l;
|
||||
}
|
||||
}while(l);
|
||||
return (size_t)L;
|
||||
}
|
||||
|
||||
int write_tty(char *buff, size_t length){
|
||||
ssize_t L = write(comfd, buff, length);
|
||||
if((size_t)L != length){
|
||||
/// "ïÛÉÂËÁ ÚÁÐÉÓÉ!"
|
||||
WARN("Write error");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Safely convert data from string to double
|
||||
*
|
||||
* @param num (o) - double number read from string
|
||||
* @param str (i) - input string
|
||||
* @return 1 if success, 0 if fails
|
||||
*/
|
||||
int str2double(double *num, const char *str){
|
||||
double res;
|
||||
char *endptr;
|
||||
if(!str) return 0;
|
||||
res = strtod(str, &endptr);
|
||||
if(endptr == str || *str == '\0' || *endptr != '\0'){
|
||||
/// "îÅÐÒÁ×ÉÌØÎÙÊ ÆÏÒÍÁÔ ÞÉÓÌÁ double!"
|
||||
WARNX("Wrong double number format!");
|
||||
return FALSE;
|
||||
}
|
||||
if(num) *num = res; // you may run it like myatod(NULL, str) to test wether str is double number
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
FILE *Flog = NULL; // log file descriptor
|
||||
char *logname = NULL;
|
||||
time_t log_open_time = 0;
|
||||
/**
|
||||
* Try to open log file
|
||||
* if failed show warning message
|
||||
*/
|
||||
void openlogfile(char *name){
|
||||
if(!name){
|
||||
WARNX(_("Need filename"));
|
||||
return;
|
||||
}
|
||||
green(_("Try to open log file %s in append mode\n"), name);
|
||||
if(!(Flog = fopen(name, "a"))){
|
||||
WARN(_("Can't open log file"));
|
||||
return;
|
||||
}
|
||||
log_open_time = time(NULL);
|
||||
logname = name;
|
||||
}
|
||||
|
||||
/**
|
||||
* Save message to log file, rotate logs every 24 hours
|
||||
*/
|
||||
int putlog(const char *fmt, ...){
|
||||
if(!Flog) return 0;
|
||||
time_t t_now = time(NULL);
|
||||
if(t_now - log_open_time > 86400){ // rotate log
|
||||
fprintf(Flog, "\n\t\t%sRotate log\n", ctime(&t_now));
|
||||
fclose(Flog);
|
||||
char newname[PATH_MAX];
|
||||
snprintf(newname, PATH_MAX, "%s.old", logname);
|
||||
if(rename(logname, newname)) WARN("rename()");
|
||||
openlogfile(logname);
|
||||
if(!Flog) return 0;
|
||||
}
|
||||
int i = fprintf(Flog, "\n\t\t%s", ctime(&t_now));
|
||||
va_list ar;
|
||||
va_start(ar, fmt);
|
||||
i = vfprintf(Flog, fmt, ar);
|
||||
va_end(ar);
|
||||
fprintf(Flog, "\n");
|
||||
fflush(Flog);
|
||||
return i;
|
||||
}
|
||||
140
Z1000_focus/usefull_macros.h
Normal file
140
Z1000_focus/usefull_macros.h
Normal file
@ -0,0 +1,140 @@
|
||||
/*
|
||||
* usefull_macros.h - a set of usefull macros: memory, color etc
|
||||
*
|
||||
* Copyright 2013 Edward V. Emelianoff <eddy@sao.ru>
|
||||
*
|
||||
* 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 2 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, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301, USA.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef __USEFULL_MACROS_H__
|
||||
#define __USEFULL_MACROS_H__
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/mman.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
#include <errno.h>
|
||||
#include <err.h>
|
||||
#include <locale.h>
|
||||
#if defined GETTEXT_PACKAGE && defined LOCALEDIR
|
||||
/*
|
||||
* GETTEXT
|
||||
*/
|
||||
#include <libintl.h>
|
||||
#define _(String) gettext(String)
|
||||
#define gettext_noop(String) String
|
||||
#define N_(String) gettext_noop(String)
|
||||
#else
|
||||
#define _(String) (String)
|
||||
#define N_(String) (String)
|
||||
#endif
|
||||
#include <stdlib.h>
|
||||
#include <termios.h>
|
||||
#include <termio.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
// unused arguments with -Wall -Werror
|
||||
#define _U_ __attribute__((__unused__))
|
||||
|
||||
/*
|
||||
* Coloured messages output
|
||||
*/
|
||||
#define RED "\033[1;31;40m"
|
||||
#define GREEN "\033[1;32;40m"
|
||||
#define OLDCOLOR "\033[0;0;0m"
|
||||
|
||||
#ifndef FALSE
|
||||
#define FALSE (0)
|
||||
#endif
|
||||
|
||||
#ifndef TRUE
|
||||
#define TRUE (1)
|
||||
#endif
|
||||
|
||||
/*
|
||||
* ERROR/WARNING messages
|
||||
*/
|
||||
extern int globErr;
|
||||
extern void signals(int sig);
|
||||
#define ERR(...) do{globErr=errno; _WARN(__VA_ARGS__); signals(9);}while(0)
|
||||
#define ERRX(...) do{globErr=0; _WARN(__VA_ARGS__); signals(9);}while(0)
|
||||
#define WARN(...) do{globErr=errno; _WARN(__VA_ARGS__);}while(0)
|
||||
#define WARNX(...) do{globErr=0; _WARN(__VA_ARGS__);}while(0)
|
||||
|
||||
/*
|
||||
* print function name, debug messages
|
||||
* debug mode, -DEBUG
|
||||
*/
|
||||
#ifdef EBUG
|
||||
#define FNAME() fprintf(stderr, "\n%s (%s, line %d)\n", __func__, __FILE__, __LINE__)
|
||||
#define DBG(...) do{fprintf(stderr, "%s (%s, line %d): ", __func__, __FILE__, __LINE__); \
|
||||
fprintf(stderr, __VA_ARGS__); \
|
||||
fprintf(stderr, "\n");} while(0)
|
||||
#else
|
||||
#define FNAME() do{}while(0)
|
||||
#define DBG(...) do{}while(0)
|
||||
#endif //EBUG
|
||||
|
||||
/*
|
||||
* Memory allocation
|
||||
*/
|
||||
#define ALLOC(type, var, size) type * var = ((type *)my_alloc(size, sizeof(type)))
|
||||
#define MALLOC(type, size) ((type *)my_alloc(size, sizeof(type)))
|
||||
#define FREE(ptr) do{if(ptr){free(ptr); ptr = NULL;}}while(0)
|
||||
|
||||
#ifndef DBL_EPSILON
|
||||
#define DBL_EPSILON (2.2204460492503131e-16)
|
||||
#endif
|
||||
|
||||
double dtime();
|
||||
|
||||
// functions for color output in tty & no-color in pipes
|
||||
extern int (*red)(const char *fmt, ...);
|
||||
extern int (*_WARN)(const char *fmt, ...);
|
||||
extern int (*green)(const char *fmt, ...);
|
||||
void * my_alloc(size_t N, size_t S);
|
||||
void initial_setup();
|
||||
|
||||
// mmap file
|
||||
typedef struct{
|
||||
char *data;
|
||||
size_t len;
|
||||
} mmapbuf;
|
||||
mmapbuf *My_mmap(char *filename);
|
||||
void My_munmap(mmapbuf *b);
|
||||
|
||||
void restore_console();
|
||||
void setup_con();
|
||||
int read_console();
|
||||
int mygetchar();
|
||||
|
||||
void restore_tty();
|
||||
void tty_init(char *comdev);
|
||||
size_t read_tty(char *buff, size_t length);
|
||||
int write_tty(char *buff, size_t length);
|
||||
|
||||
int str2double(double *num, const char *str);
|
||||
|
||||
void openlogfile(char *name);
|
||||
int putlog(const char *fmt, ...);
|
||||
#endif // __USEFULL_MACROS_H__
|
||||
Loading…
x
Reference in New Issue
Block a user