eddy/zeiss_utils
1
0
Fork 0
mirror of https://github.com/eddyem/zeiss_utils.git synced 2025-12-06 02:35:15 +03:00
Code Issues Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
eddyem 2019-10-22 16:48:47 +03:00
parent 82e167bb74
commit cbec6c1f63
23 changed files with 4267 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
.gitignore vendored
View File

@ -1,52 +1,3 @@
# Prerequisites mk/*
*.d Zphocus.*
can_phocus
# 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

68
Z1000_focus/DS406_canopen.h Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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__