diff --git a/.gitignore b/.gitignore index c6127b3..107292d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,52 +1,3 @@ -# Prerequisites -*.d - -# Object files -*.o -*.ko -*.obj -*.elf - -# Linker output -*.ilk -*.map -*.exp - -# Precompiled Headers -*.gch -*.pch - -# Libraries -*.lib -*.a -*.la -*.lo - -# Shared objects (inc. Windows DLLs) -*.dll -*.so -*.so.* -*.dylib - -# Executables -*.exe -*.out -*.app -*.i*86 -*.x86_64 -*.hex - -# Debug files -*.dSYM/ -*.su -*.idb -*.pdb - -# Kernel Module Compile Results -*.mod* -*.cmd -.tmp_versions/ -modules.order -Module.symvers -Mkfile.old -dkms.conf +mk/* +Zphocus.* +can_phocus diff --git a/Z1000_focus/DS406_canopen.h b/Z1000_focus/DS406_canopen.h new file mode 100644 index 0000000..43a84d4 --- /dev/null +++ b/Z1000_focus/DS406_canopen.h @@ -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__ diff --git a/Z1000_focus/HW_dependent.h b/Z1000_focus/HW_dependent.h new file mode 100644 index 0000000..62b1503 --- /dev/null +++ b/Z1000_focus/HW_dependent.h @@ -0,0 +1,70 @@ +/* + * This file is part of the Zphocus project. + * Copyright 2019 Edward V. Emelianov . + * + * 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 . + */ + +#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 & /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 diff --git a/Z1000_focus/Readme_SEW b/Z1000_focus/Readme_SEW new file mode 100644 index 0000000..fc46dce --- /dev/null +++ b/Z1000_focus/Readme_SEW @@ -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 - индекс, младший байт + +Четыре младших байта - данные. diff --git a/Z1000_focus/can_encoder.c b/Z1000_focus/can_encoder.c new file mode 100644 index 0000000..e33d379 --- /dev/null +++ b/Z1000_focus/can_encoder.c @@ -0,0 +1,716 @@ +/* + * This file is part of the Zphocus project. + * Copyright 2019 Edward V. Emelianov . + * + * 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 . + */ + +#include // memcpy +#include // 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 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> 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 +} diff --git a/Z1000_focus/can_encoder.h b/Z1000_focus/can_encoder.h new file mode 100644 index 0000000..e545695 --- /dev/null +++ b/Z1000_focus/can_encoder.h @@ -0,0 +1,53 @@ +/* + * This file is part of the Zphocus project. + * Copyright 2019 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef CAN_ENCODER_H__ +#define CAN_ENCODER_H__ + +#include + +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__ diff --git a/Z1000_focus/can_io.c b/Z1000_focus/can_io.c new file mode 100644 index 0000000..c2f20ea --- /dev/null +++ b/Z1000_focus/can_io.c @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#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; i0 && 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= 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=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; +} diff --git a/Z1000_focus/can_io.h b/Z1000_focus/can_io.h new file mode 100644 index 0000000..80535cc --- /dev/null +++ b/Z1000_focus/can_io.h @@ -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__ diff --git a/Z1000_focus/canmsg.h b/Z1000_focus/canmsg.h new file mode 100644 index 0000000..779f68c --- /dev/null +++ b/Z1000_focus/canmsg.h @@ -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 +#include + +#else /* __KERNEL__ */ + +#include +#include + +#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< 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); +} diff --git a/Z1000_focus/canopen.h b/Z1000_focus/canopen.h new file mode 100644 index 0000000..d11e03f --- /dev/null +++ b/Z1000_focus/canopen.h @@ -0,0 +1,47 @@ +// (c) vsher@sao.ru +#pragma once +#ifndef CANOPEN_H__ +#define CANOPEN_H__ + +#include +#include +#include +#include +#include +#include +#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__ diff --git a/Z1000_focus/cmdlnopts.c b/Z1000_focus/cmdlnopts.c new file mode 100644 index 0000000..baa5228 --- /dev/null +++ b/Z1000_focus/cmdlnopts.c @@ -0,0 +1,86 @@ +/* + * cmdlnopts.c - the only function that parse cmdln args and returns glob parameters + * + * Copyright 2013 Edward V. Emelianoff + * + * 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 +#include +#include +#include +#include +#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; +} + diff --git a/Z1000_focus/cmdlnopts.h b/Z1000_focus/cmdlnopts.h new file mode 100644 index 0000000..6ce5b6b --- /dev/null +++ b/Z1000_focus/cmdlnopts.h @@ -0,0 +1,50 @@ +/* + * cmdlnopts.h - comand line options for parceargs + * + * Copyright 2013 Edward V. Emelianoff + * + * 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__ + diff --git a/Z1000_focus/main.c b/Z1000_focus/main.c new file mode 100644 index 0000000..db42203 --- /dev/null +++ b/Z1000_focus/main.c @@ -0,0 +1,191 @@ +/* + * This file is part of the Zphocus project. + * Copyright 2019 Edward V. Emelianov . + * + * 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 . + */ + +#include // fabs +#include +#include +#include +#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; +} diff --git a/Z1000_focus/motor_cancodes.h b/Z1000_focus/motor_cancodes.h new file mode 100644 index 0000000..1a26963 --- /dev/null +++ b/Z1000_focus/motor_cancodes.h @@ -0,0 +1,98 @@ +/* + * This file is part of the Zphocus project. + * Copyright 2019 Edward V. Emelianov . + * + * 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 . + */ +#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__ diff --git a/Z1000_focus/parseargs.c b/Z1000_focus/parseargs.c new file mode 100644 index 0000000..b235752 --- /dev/null +++ b/Z1000_focus/parseargs.c @@ -0,0 +1,497 @@ +/* geany_encoding=koi8-r + * parseargs.c - parsing command line arguments & print help + * + * Copyright 2013 Edward V. Emelianoff + * + * 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 // printf +#include // getopt_long +#include // calloc, exit, strtoll +#include // assert +#include // strdup, strchr, strlen +#include // strcasecmp +#include // INT_MAX & so on +#include // gettext +#include // 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; +} diff --git a/Z1000_focus/parseargs.h b/Z1000_focus/parseargs.h new file mode 100644 index 0000000..a0ac099 --- /dev/null +++ b/Z1000_focus/parseargs.h @@ -0,0 +1,124 @@ +/* + * parseargs.h - headers for parsing command line arguments + * + * Copyright 2013 Edward V. Emelianoff + * + * 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 // bool +#include + +#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__ diff --git a/Z1000_focus/sdo_abort_codes.h b/Z1000_focus/sdo_abort_codes.h new file mode 100644 index 0000000..e5345e7 --- /dev/null +++ b/Z1000_focus/sdo_abort_codes.h @@ -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 + * + * 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 // addrinfo +#include // inet_ntop +#include +#include // INT_xxx +#include // fabs +#include // pthread_kill +#include // daemon +#include // 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!"); +} diff --git a/Z1000_focus/socket.h b/Z1000_focus/socket.h new file mode 100644 index 0000000..c1ee33a --- /dev/null +++ b/Z1000_focus/socket.h @@ -0,0 +1,50 @@ +/* + * geany_encoding=koi8-r + * socket.h + * + * Copyright 2017 Edward V. Emelianov + * + * 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__ diff --git a/Z1000_focus/usefull_macros.c b/Z1000_focus/usefull_macros.c new file mode 100644 index 0000000..8e24425 --- /dev/null +++ b/Z1000_focus/usefull_macros.c @@ -0,0 +1,434 @@ +/* + * usefull_macros.h - a set of usefull functions: memory, color etc + * + * Copyright 2013 Edward V. Emelianoff + * + * 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 +#include // 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; +} diff --git a/Z1000_focus/usefull_macros.h b/Z1000_focus/usefull_macros.h new file mode 100644 index 0000000..00b6140 --- /dev/null +++ b/Z1000_focus/usefull_macros.h @@ -0,0 +1,140 @@ +/* + * usefull_macros.h - a set of usefull macros: memory, color etc + * + * Copyright 2013 Edward V. Emelianoff + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if defined GETTEXT_PACKAGE && defined LOCALEDIR +/* + * GETTEXT + */ +#include +#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 +#include +#include +#include +#include +#include + + +// 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__