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

fix some bugs, ready for real work

Browse Source
This commit is contained in:
Edward Emelianov 2020-03-17 09:21:37 +03:00
parent 3263d6de4f
commit 8ce37f4151
9 changed files with 471 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
Z1000_focus/HW_dependent.h
View File

@ -22,9 +22,10 @@
#include "motor_cancodes.h" #include "motor_cancodes.h"
// amount of encoder's counts when stop = rawspeed^2/STOPPING_COEFF // On lowest speeds Taccelerated = 0.22s, so wait no more than 0.25s when motor starts
#define STOPPING_COEFF (117500L) #define TACCEL (0.25)
//#define STOPPING_COEFF (1175000L) // max amount of cycles when motor stalled
#define STALL_MAXCTR (50)
// direction of motor rotation positive to encoder (1 - negative) // direction of motor rotation positive to encoder (1 - negative)
#define MOTOR_REVERSE (0) #define MOTOR_REVERSE (0)
@ -45,24 +46,26 @@
#define RAWSPEED(x) (x*5) #define RAWSPEED(x) (x*5)
// max/min speed (rev/min) // max/min speed (rev/min)
#define MAXSPEED 1200 #define MAXSPEED 1200
#define MINSPEED 130 #define MINSPEED 350
// moving timeout (*0.01s) // speed to move from ESW
#define MOVING_TIMEOUT 5000 #define ESWSPEED 350
// moving timeout: 5minutes
#define MOVING_TIMEOUT (300)
// constants for focus conversion: foc_mm = (foc_raw - FOCRAW_0) / FOCSCALE_MM // constants for focus conversion: foc_mm = (foc_raw - FOCRAW_0) / FOCSCALE_MM
#define FOCSCALE_MM 4096. #define FOCSCALE_MM 4096.
#define FOCRAW_0 40960. #define FOCRAW_0 15963187.
#define FOC_RAW2MM(x) (((x)-FOCRAW_0)/FOCSCALE_MM) #define FOC_RAW2MM(x) (((x)-FOCRAW_0)/FOCSCALE_MM)
#define FOC_MM2RAW(x) (FOCRAW_0+(x)*FOCSCALE_MM) #define FOC_MM2RAW(x) (FOCRAW_0+(x)*FOCSCALE_MM)
// raw position precision // raw position precision - 2.5um
#define RAWPOS_TOLERANCE 10 #define RAWPOS_TOLERANCE 10
// raw dF value for accurate focussing // raw dF value for accurate focussing (rough move to F+dF0 and after this slow move to F)
#define dF0 100 #define dF0 500
// minimal & maximal focus positions (should be >min+dF0 & <max-dF0) // minimal & maximal focus positions (should be >min+dF0 & <max-dF0)
#define FOCMIN_MM 0.1 #define FOCMIN_MM 2.75
#define FOCMAX_MM 76.0 #define FOCMAX_MM 76.
// -//- raw values // -//- raw values
#define FOCMIN FOC_MM2RAW(FOCMIN_MM) #define FOCMIN FOC_MM2RAW(FOCMIN_MM)
#define FOCMAX FOC_MM2RAW(FOCMAX_MM) #define FOCMAX FOC_MM2RAW(FOCMAX_MM)

294
Z1000_focus/can_encoder.c
View File

@ -16,15 +16,18 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <string.h> // memcpy
#include <math.h> // fabs
#include "canopen.h"
#include "can_encoder.h"
#include "usefull_macros.h"
#include "DS406_canopen.h" #include "DS406_canopen.h"
#include "motor_cancodes.h"
#include "HW_dependent.h" #include "HW_dependent.h"
#include "can_encoder.h"
#include "canopen.h"
#include "motor_cancodes.h"
#include "socket.h" #include "socket.h"
#include "usefull_macros.h"
#include <math.h> // fabs
#include <string.h> // memcpy
// flags: encoder is ready, motor is ready
static uint8_t encoderRDY = 0, motorRDY = 0;
// printf when -v // printf when -v
extern int verbose(const char *fmt, ...); extern int verbose(const char *fmt, ...);
@ -38,7 +41,7 @@ static int encnodenum = 0;
// system status // system status
static sysstatus curstatus = STAT_OK; static sysstatus curstatus = STAT_OK;
// current raw motor speed (without MOTOR_REVERSE) // current raw motor speed (without MOTOR_REVERSE)
int16_t targspd = 0; static int16_t targspd = 0;
static canstatus can_write_par(uint8_t subidx, uint16_t idx, uint32_t *parval); 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 canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval);
@ -48,20 +51,20 @@ static int waitTillStop();
// check if end-switches are in default state // check if end-switches are in default state
// return 0 if all OK // return 0 if all OK
static int chk_eswstates(){ static int chk_eswstates(){
if(!motorRDY) return 0;
FNAME(); FNAME();
uint32_t cw, ccw; uint32_t cw, ccw;
if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CW_IDX, &cw)) goto verybad; if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CW_IDX, &cw)) goto verybad;
if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &ccw)) goto verybad; if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &ccw)) goto verybad;
uint32_t parval = DI_ENSTOP; uint32_t parval = DI_ENSTOP;
if(cw != DI_ENSTOP || ccw != DI_ENSTOP){ // activate enable/stop if(cw != DI_ENSTOP || ccw != DI_ENSTOP){ // activate enable/stop
WARNX("\nThe end-switches state wasn't default!"); WARNX("The end-switches state wasn't default!");
if(waitTillStop()) return 1; // we can change motor parameters only in stopped state if(waitTillStop()) return 1; // we can change motor parameters only in stopped state
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CW_IDX, &parval)) goto verybad; if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CW_IDX, &parval)) goto verybad;
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &parval)) goto verybad; if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &parval)) goto verybad;
} }
return 0; return 0;
verybad: verybad:
WARNX("Can't return esw to default state");
curstatus = STAT_ERROR; curstatus = STAT_ERROR;
return 1; return 1;
} }
@ -73,31 +76,38 @@ verybad:
*/ */
static int chkMove(int spd){ static int chkMove(int spd){
//FNAME(); //FNAME();
if(curstatus == STAT_DAMAGE){ if(!motorRDY){
WARNX("Try to move in damaged state"); curstatus = STAT_ESW;
return 1; return 1;
} }
if(curstatus == STAT_DAMAGE){
SINGLEWARN(WARN_MOVEDAMAGED);
return 1;
}else clrwarnsingle(WARN_MOVEDAMAGED);
eswstate e; eswstate e;
if(CAN_NOERR != get_endswitches(&e)){ if(CAN_NOERR != get_endswitches(&e)){
curstatus = STAT_ERROR; curstatus = STAT_ERROR;
return 1; return 1;
} }
if(e == ESW_INACTIVE){ if(e == ESW_INACTIVE){
if(getPos(NULL)) return 1; double posmm;
if(curposition <= FOCMIN && spd < 0){ if(!encoderRDY) return 0;
if(getPos(&posmm)) return 1;
if(posmm <= FOCMIN_MM && spd < 0){
WARNX("Try to move to the left of minimal position"); WARNX("Try to move to the left of minimal position");
return 1; return 1;
} }
if(curposition >= FOCMAX && spd > 0){ if(posmm >= FOCMAX_MM && spd > 0){
WARNX("Try to move to the right of maximal position"); WARNX("Try to move to the right of maximal position");
return 1; return 1;
} }
return 0; return 0;
} }
if(e == ESW_BOTH_ACTIVE){ if(e == ESW_BOTH_ACTIVE){
SINGLEWARN(WARN_BOTHESW);
curstatus = STAT_DAMAGE; curstatus = STAT_DAMAGE;
return 1; return 1;
} }else clrwarnsingle(WARN_BOTHESW);
if(e == ESW_CCW_ACTIVE && spd < 0){ if(e == ESW_CCW_ACTIVE && spd < 0){
curstatus = STAT_ESW; curstatus = STAT_ESW;
WARNX("Try to move over the CCW end-switch"); WARNX("Try to move over the CCW end-switch");
@ -117,11 +127,13 @@ static int chkMove(int spd){
* @return 0 if all OK * @return 0 if all OK
*/ */
int init_encoder(int encnode, int reset){ int init_encoder(int encnode, int reset){
FNAME();
unsigned long lval; unsigned long lval;
encnodenum = encnode; encnodenum = encnode;
verbose("cur node: %d\n", encnodenum); verbose("cur node: %d\n", encnodenum);
if(!initNode(encnodenum)){ if(!initNode(encnodenum)){
ERRX("Can't find device with Node address %d!", encnodenum); WARNX("Can't find device with Node address %d!", encnodenum);
//signals(9);
return 1; return 1;
} }
if(reset){ if(reset){
@ -143,16 +155,6 @@ int init_encoder(int encnode, int reset){
WARNX("Can't get encoder device type"); WARNX("Can't get encoder device type");
return 1; 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... "); verbose("Set operational... ");
startNode(encnodenum); startNode(encnodenum);
int n, i = recvNextPDO(0.1, &n, &lval); int n, i = recvNextPDO(0.1, &n, &lval);
@ -161,7 +163,7 @@ int init_encoder(int encnode, int reset){
if(state == NodeOperational) verbose("Ok!\n"); if(state == NodeOperational) verbose("Ok!\n");
else{ else{
WARNX("Failed to start node!"); WARNX("Failed to start node!");
returnPreOper(); returnPreOper(-1);
return 1; return 1;
} }
if(i > 0) verbose("Node%d PDO%d %08lx\n",n,i,lval); if(i > 0) verbose("Node%d PDO%d %08lx\n",n,i,lval);
@ -178,6 +180,7 @@ int init_encoder(int encnode, int reset){
verbose("Node%d PDO%d %08lx (%ld)\n",node[i],pdo_n[i],pdo_v[i],pdo_v[n]); verbose("Node%d PDO%d %08lx (%ld)\n",node[i],pdo_n[i],pdo_v[i],pdo_v[n]);
}while(0); }while(0);
curstatus = STAT_OK; curstatus = STAT_OK;
encoderRDY = 1;
return 0; return 0;
} }
@ -186,7 +189,8 @@ int init_encoder(int encnode, int reset){
* @return 0 if all OK * @return 0 if all OK
*/ */
int go_out_from_ESW(){ int go_out_from_ESW(){
FNAME(); //FNAME();
if(!encoderRDY || !motorRDY) return 0;
uint32_t parval; uint32_t parval;
if(chk_eswstates()) return 1; if(chk_eswstates()) return 1;
// check esw // check esw
@ -196,36 +200,44 @@ int go_out_from_ESW(){
return 1; return 1;
} }
if(e == ESW_BOTH_ACTIVE){ // error situation! if(e == ESW_BOTH_ACTIVE){ // error situation!
WARNX("Error: both end-switches are active!"); SINGLEWARN(WARN_BOTHESW);
curstatus = STAT_DAMAGE; if(curstatus != STAT_DAMAGE){
curstatus = STAT_DAMAGE;
}
return 1; return 1;
} }else clrwarnsingle(WARN_BOTHESW);
if(e == ESW_INACTIVE){ if(e == ESW_INACTIVE){
DBG("Esw inactive"); DBG("Esw inactive");
int r = 0; int r = 0;
if(curposition < FOCMIN) r = move(FOCMIN, MAXSPEED); // TODO: fix trouble with current position if it is over available position
else if(curposition > FOCMAX) r = move(FOCMAX, -MAXSPEED); // if(curposition < FOCMIN) r = move(FOCMIN, MAXSPEED);
// else if(curposition > FOCMAX) r = move(FOCMAX, -MAXSPEED);
curstatus = STAT_OK; curstatus = STAT_OK;
return r; return r;
} }
curstatus = STAT_GOFROMESW;
// check that current position is in available zone // check that current position is in available zone
if(e == ESW_CW_ACTIVE && curposition < FOCPOS_CW_ESW){ if(e == ESW_CW_ACTIVE && curposition < FOCPOS_CW_ESW){
// WTF? CW end-switch activated in forbidden zone! // CW end-switch activated in forbidden zone
WARNX("CW end-switch in forbidden zone (to the left of normal position)!"); if(curstatus != STAT_DAMAGE){
curstatus = STAT_DAMAGE; WARNX("CW end-switch in forbidden zone (to the left of normal position)!");
curstatus = STAT_DAMAGE;
}
return 1; return 1;
}else if(e == ESW_CCW_ACTIVE && curposition > FOCPOS_CW_ESW){ }else if(e == ESW_CCW_ACTIVE && curposition > FOCPOS_CCW_ESW){
// CCW end-switch activated in forbidden zone! // CCW end-switch activated in forbidden zone
WARNX("CCW end-switch in forbidden zone (too far)!"); if(curstatus != STAT_DAMAGE){
curstatus = STAT_DAMAGE; WARNX("CCW end-switch in forbidden zone (too far)!");
curstatus = STAT_DAMAGE;
}
return 1; return 1;
} }
curstatus = STAT_GOFROMESW;
// try to move from esw // try to move from esw
parval = DI_NOFUNC; parval = DI_NOFUNC;
WARNX("Try to move from ESW");
uint16_t idx = (e == ESW_CW_ACTIVE) ? PAR_CW_IDX : PAR_CCW_IDX; uint16_t idx = (e == ESW_CW_ACTIVE) ? PAR_CW_IDX : PAR_CCW_IDX;
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, idx, &parval)) goto bad; // turn off motor stopping @esw if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, idx, &parval)) goto bad; // turn off motor stopping @esw
int16_t speed = (e == ESW_CW_ACTIVE) ? -RAWSPEED(MINSPEED) : RAWSPEED(MINSPEED); int16_t speed = (e == ESW_CW_ACTIVE) ? -RAWSPEED(ESWSPEED) : RAWSPEED(ESWSPEED);
for(int i = 0; i < 5; ++i){ // 5 tries to move out for(int i = 0; i < 5; ++i){ // 5 tries to move out
getPos(NULL); getPos(NULL);
DBG("try %d, pos: %lu, E=%d", i, curposition, e); DBG("try %d, pos: %lu, E=%d", i, curposition, e);
@ -259,6 +271,7 @@ int init_motor_ids(int addr){
motor_id = MOTOR_PO_ID(addr); motor_id = MOTOR_PO_ID(addr);
motor_p_id = MOTOR_PAR_ID(addr); motor_p_id = MOTOR_PAR_ID(addr);
DBG("motor POid=%lu, motor_PROCid=%lu", motor_id, motor_p_id); DBG("motor POid=%lu, motor_PROCid=%lu", motor_id, motor_p_id);
motorRDY = 1;
// check esw roles & end-switches state // check esw roles & end-switches state
if(go_out_from_ESW()) return 1; if(go_out_from_ESW()) return 1;
return 0; return 0;
@ -271,36 +284,56 @@ int init_motor_ids(int addr){
*/ */
int getPos(double *pos){ int getPos(double *pos){
//FNAME(); //FNAME();
if(!encoderRDY) return 1;
int r = !(getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition)); int r = !(getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition));
if(pos) *pos = FOC_RAW2MM(curposition); double posmm = FOC_RAW2MM(curposition);
if(pos) *pos = posmm;
verbose("Raw position: %ld\nposition in mm: %.2f\n", curposition, posmm);
eswstate e; eswstate e;
if(CAN_NOERR != get_endswitches(&e)){ if(CAN_NOERR != get_endswitches(&e)){
curstatus = STAT_ERROR; curstatus = STAT_ERROR;
}else switch(e){ }else switch(e){
case ESW_BOTH_ACTIVE: case ESW_BOTH_ACTIVE:
WARNX("Damage state: both end-switches are active");
curstatus = STAT_DAMAGE; curstatus = STAT_DAMAGE;
break; break;
case ESW_CCW_ACTIVE: case ESW_CCW_ACTIVE:
if(curposition > FOCPOS_CCW_ESW) curstatus = STAT_DAMAGE; if(posmm > FOCMIN_MM + ESW_DIST_ALLOW){
else curstatus = STAT_ESW; WARNX("Damage state: CCW end-switch in forbidden zone");
curstatus = STAT_DAMAGE;
}else curstatus = STAT_ESW;
break; break;
case ESW_CW_ACTIVE: case ESW_CW_ACTIVE:
if(curposition < FOCPOS_CW_ESW) curstatus = STAT_DAMAGE; if(posmm < FOCMAX_MM - ESW_DIST_ALLOW){
else curstatus = STAT_ESW; WARNX("Damage state: CW end-switch in forbidden zone");
curstatus = STAT_DAMAGE;
}else curstatus = STAT_ESW;
break; break;
case ESW_INACTIVE: case ESW_INACTIVE:
default: default:
curstatus = STAT_OK; curstatus = STAT_OK;
} }
//DBG("targspd = %d", targspd);
if(targspd){ if(targspd){
if(curposition <= FOCMIN && targspd < 0){ // bad value if(posmm <= FOCMIN_MM && targspd < 0){ // bad value
WARNX("Forbidden position < FOCMIN!"); SINGLEWARN(WARN_LESSMIN);
stop(); stop();
curstatus = STAT_FORBIDDEN; curstatus = STAT_FORBIDDEN;
}else if(curposition >= FOCMAX && targspd > 0){ }else if(posmm >= FOCMAX_MM && targspd > 0){
WARNX("Forbidden position > FOCMAX!"); SINGLEWARN(WARN_GRTRMAX);
stop(); stop();
curstatus = STAT_FORBIDDEN; curstatus = STAT_FORBIDDEN;
}else{
clrwarnsingle(WARN_LESSMIN);
clrwarnsingle(WARN_GRTRMAX);
}
}else{
if(posmm <= FOCMIN_MM){
stop();
//WARNX("Current position <= FOCMIN");
}else if(posmm >= FOCMAX_MM){
stop();
//WARNX("Current position >= FOCMAX");
} }
} }
return r; return r;
@ -313,16 +346,46 @@ double curPos(){
/** /**
* @brief returnPreOper - return encoder into pre-operational state * @brief returnPreOper - return encoder into pre-operational state
* @arg presetval - new preset value (if > -1)
*/ */
void returnPreOper(){ void returnPreOper(long long presetval){
if(!encoderRDY) return;
verbose("Return to Pre-operational... "); verbose("Return to Pre-operational... ");
setPreOper(encnodenum); setPreOper(encnodenum);
int state=getNodeState(encnodenum); int state=getNodeState(encnodenum);
verbose("State=0x%02x - ", state); verbose("State=0x%02x - ", state);
if(state == NodePreOperational){ if(state != NodePreOperational){
verbose("Ok!\n");
}else{
verbose("Failed!\n"); verbose("Failed!\n");
return;
}
verbose("Ok!\n");
encoderRDY = 0;
if(presetval < 0) return;
green("Try to change preset value to %lld", presetval);
printf("\n");
if(presetval > UINT32_MAX){
WARNX("Value %lld too big", presetval);
}
unsigned long val = (uint32_t) presetval, nval = ~val;
// set "configuration valid"
/*
if(!setByte(encnodenum, DS406_CONF_VALID, 0, DS406_CONF_VALID_VALID)){
WARNX("Can't switch to setup mode");
return;
}
*/
// Safety code sequence
if(!setLong(encnodenum, DS406_CONF_PARAMETERS, 2, val)){
WARNX("Can't change safety code sequence value");
return;
}
if(!setByte(encnodenum, DS406_CONF_VALID, 0, DS406_CONF_VALID_VALID)){
WARNX("Can't switch to setup mode");
return;
}
if(!setLong(encnodenum, DS406_CONF_PARAMETERS, 3, nval)){
WARNX("Can't change inverted safety code sequence value");
return;
} }
} }
@ -347,6 +410,7 @@ static void fix_targspeed(int16_t *targspd){
* @return status * @return status
*/ */
static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){ static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){
if(!motorRDY) return CAN_NOANSWER;
const int l = 6; // frame length const int l = 6; // frame length
/*if(G->verbose){ /*if(G->verbose){
printf("Send frame to ID=%lu: ", motor_id); printf("Send frame to ID=%lu: ", motor_id);
@ -355,9 +419,9 @@ static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){
printf("\n"); printf("\n");
}*/ }*/
if(can_send_frame(motor_id, l, buf) <= 0){ if(can_send_frame(motor_id, l, buf) <= 0){
WARNX("Error sending CAN frame (len %d)", l); SINGLEWARN(WARN_CANSEND);
return CAN_CANTSEND; return CAN_CANTSEND;
} }else clrwarnsingle(WARN_CANSEND);
int I, rxpnt, idr, dlen; int I, rxpnt, idr, dlen;
double rxtime; double rxtime;
unsigned char rdata[8]; unsigned char rdata[8];
@ -367,9 +431,9 @@ static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){
can_dsleep(0.01); can_dsleep(0.01);
} }
if(I == 50){ if(I == 50){
WARNX("can_send_chk(): error getting answer"); SINGLEWARN(WARN_CANNOANS);
return CAN_NOANSWER; return CAN_NOANSWER;
} }else clrwarnsingle(WARN_CANNOANS);
if(obuf) memcpy(obuf, rdata, l); if(obuf) memcpy(obuf, rdata, l);
/*if(G->verbose){ /*if(G->verbose){
printf("Got answer with ID=%d: ", idr&0x1fffffff); printf("Got answer with ID=%d: ", idr&0x1fffffff);
@ -397,6 +461,7 @@ static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){
* @return status * @return status
*/ */
static canstatus can_send_param(unsigned char *buf, unsigned char *obuf){ static canstatus can_send_param(unsigned char *buf, unsigned char *obuf){
if(!motorRDY) return CAN_NOANSWER;
const int l = 8; // frame length const int l = 8; // frame length
int I, rxpnt, idr, dlen; int I, rxpnt, idr, dlen;
double rxtime; double rxtime;
@ -407,19 +472,19 @@ for(int i=0; i<l; ++i) printf("0x%02x ", buf[i]);
printf("\n"); printf("\n");
*/ */
if(can_send_frame(motor_p_id, l, buf) <= 0){ if(can_send_frame(motor_p_id, l, buf) <= 0){
WARNX("Error sending CAN frame (len %d)", l); SINGLEWARN(WARN_CANSEND);
return CAN_CANTSEND; return CAN_CANTSEND;
} } else clrwarnsingle(WARN_CANSEND);
can_clean_recv(&rxpnt, &rxtime); can_clean_recv(&rxpnt, &rxtime);
for(I = 0; I < 50; ++I){ for(I = 0; I < 50; ++I){
if(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata) && (idr&0x1fffffff) == motor_p_id+1) break; if(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata) && (idr&0x1fffffff) == motor_p_id+1) break;
//DBG("Got frame from ID 0x%x", idr&0x1fffffff); //DBG("Got frame from ID 0x%x, need: 0x%x", idr&0x1fffffff, motor_p_id+1);
can_dsleep(0.01); can_dsleep(0.01);
} }
if(I == 50){ if(I == 50){
WARNX("can_send_param(): error getting answer"); SINGLEWARN(WARN_SENDPAR);
return CAN_NOANSWER; return CAN_NOANSWER;
} }else clrwarnsingle(WARN_SENDPAR);
/* /*
green("Received param: "); green("Received param: ");
for(int i=0; i<dlen; ++i) printf("0x%02x ", rdata[i]); for(int i=0; i<dlen; ++i) printf("0x%02x ", rdata[i]);
@ -445,6 +510,7 @@ printf("\n");
* @return status * @return status
*/ */
static canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval){ static canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
if(!motorRDY) return CAN_NOANSWER;
if(!parval) return CAN_WARNING; if(!parval) return CAN_WARNING;
uint8_t buf[8] = {CAN_READPAR_CMD, subidx}, obuf[8]; uint8_t buf[8] = {CAN_READPAR_CMD, subidx}, obuf[8];
buf[2] = idx >> 8; buf[2] = idx >> 8;
@ -463,6 +529,7 @@ static canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
* @return status * @return status
*/ */
static canstatus can_write_par(uint8_t subidx, uint16_t idx, uint32_t *parval){ static canstatus can_write_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
if(!motorRDY) return CAN_NOANSWER;
uint8_t buf[8] = {CAN_WRITEPAR_CMD, subidx,0}, obuf[8]; uint8_t buf[8] = {CAN_WRITEPAR_CMD, subidx,0}, obuf[8];
buf[2] = idx >> 8; buf[2] = idx >> 8;
buf[3] = idx & 0xff; buf[3] = idx & 0xff;
@ -483,6 +550,7 @@ static canstatus can_write_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
* @return speed in rev/min * @return speed in rev/min
*/ */
canstatus get_motor_speed(double *spd){ canstatus get_motor_speed(double *spd){
if(!motorRDY) return CAN_NOANSWER;
if(!spd) return CAN_WARNING; if(!spd) return CAN_WARNING;
union{ union{
uint32_t u; uint32_t u;
@ -502,13 +570,14 @@ canstatus get_motor_speed(double *spd){
* @return * @return
*/ */
canstatus get_endswitches(eswstate *Esw){ canstatus get_endswitches(eswstate *Esw){
if(!motorRDY) return CAN_NOANSWER;
//FNAME(); //FNAME();
uint32_t val = 0; uint32_t val = 0;
canstatus s = can_read_par(PAR_DI_SUBIDX, PAR_DIST_IDX, &val); canstatus s = can_read_par(PAR_DI_SUBIDX, PAR_DIST_IDX, &val);
if(s != CAN_NOERR){ if(s != CAN_NOERR){
WARNX("Can't read end-switches state"); SINGLEWARN(WARN_ESWSTATE);
return s; return s;
} }else clrwarnsingle(WARN_ESWSTATE);
if(Esw){ if(Esw){
int v = 0; int v = 0;
if(!(val & ESW_CW)){ // + pressed if(!(val & ESW_CW)){ // + pressed
@ -527,7 +596,8 @@ canstatus get_endswitches(eswstate *Esw){
* @return 0 if all OK * @return 0 if all OK
*/ */
int stop(){ int stop(){
FNAME(); if(!motorRDY) return 0;
//FNAME();
unsigned char buf[6] = {0, CW_STOP,0,}; unsigned char buf[6] = {0, CW_STOP,0,};
if(can_send_chk(buf, NULL)){ if(can_send_chk(buf, NULL)){
WARNX("Can't stop motor!"); WARNX("Can't stop motor!");
@ -539,8 +609,10 @@ int stop(){
/** /**
* @brief waitTillStop - wait for full stop * @brief waitTillStop - wait for full stop
* @return 0 if all OK
*/ */
static int waitTillStop(){ static int waitTillStop(){
if(!motorRDY) return 0;
long oldposition = -1; long oldposition = -1;
double spd; double spd;
int r = 0; int r = 0;
@ -563,6 +635,7 @@ static int waitTillStop(){
}else{DBG("can't get motor speed");}; }else{DBG("can't get motor speed");};
oldposition = curposition; oldposition = curposition;
// now wait for full moving stop // now wait for full moving stop
if(!encoderRDY) break;
getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition); getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition);
//DBG("curpos: %lu, oldpos: %ld", curposition, oldposition); //DBG("curpos: %lu, oldpos: %ld", curposition, oldposition);
}while((long)curposition != oldposition); }while((long)curposition != oldposition);
@ -579,6 +652,7 @@ static int waitTillStop(){
* @return 0 if all OK * @return 0 if all OK
*/ */
int movewconstspeed(int16_t spd){ int movewconstspeed(int16_t spd){
if(!motorRDY) return 0;
if(chkMove(spd)) return 1; if(chkMove(spd)) return 1;
fix_targspeed(&spd); fix_targspeed(&spd);
targspd = RAWSPEED(spd); targspd = RAWSPEED(spd);
@ -603,15 +677,17 @@ int movewconstspeed(int16_t spd){
* @return 0 if all OK * @return 0 if all OK
*/ */
static int move(unsigned long targposition, int16_t rawspeed){ static int move(unsigned long targposition, int16_t rawspeed){
if(!motorRDY || !encoderRDY) return 1;
//FNAME(); //FNAME();
if(abs(targposition - curposition) < RAWPOS_TOLERANCE){ long olddiffr = labs((long)targposition - (long)curposition);
if(olddiffr < RAWPOS_TOLERANCE){
verbose("Already at position\n"); verbose("Already at position\n");
DBG("Already at position"); DBG("Already at position");
return 0; return 0;
} }
if(chkMove(rawspeed)) return 1; if(chkMove(rawspeed)) return 1;
unsigned char buf[6] = {0,}; unsigned char buf[6] = {0,};
DBG("Start moving with speed %d, target position: %lu", rawspeed, targposition); DBG("Start moving with speed %d, target position: %lu", REVMIN(rawspeed), targposition);
buf[1] = CW_ENABLE; buf[1] = CW_ENABLE;
targspd = rawspeed; targspd = rawspeed;
if(MOTOR_REVERSE) rawspeed = -rawspeed; if(MOTOR_REVERSE) rawspeed = -rawspeed;
@ -625,17 +701,18 @@ static int move(unsigned long targposition, int16_t rawspeed){
return 1; return 1;
} }
//DBG("\tOBUF: %d, %d, %d, %d, %d, %d", obuf[0], obuf[1], obuf[2], obuf[3], obuf[4], obuf[5]); //DBG("\tOBUF: %d, %d, %d, %d, %d, %d", obuf[0], obuf[1], obuf[2], obuf[3], obuf[4], obuf[5]);
#ifdef EBUG
double t0 = can_dtime(); double t0 = can_dtime();
#endif // Steps after stopping = -27.96 + 9.20e-2*v + 3.79e-4*v^2, v in rev/min
long olddiffr = targposition - curposition; // in rawspeed = -27.96 + 1.84e-2*v + 1.52e-5*v^2
long corrvalue = (long)rawspeed*(long)rawspeed / STOPPING_COEFF; // correction due to stopping ramp double rs = fabs((double)rawspeed);
DBG("start-> curpos: %ld, difference: %ld, corrval: %ld, tm=%g", //double cv = (-27.96 + (1.84e-2 + 1.52e-5*rs)*rs);
curposition, olddiffr, corrvalue, can_dtime()-t0); double cv = (-50. + (1.84e-2 + 1.52e-5*rs)*rs);
int i, zerctr = 0, errctr = 0; long corrvalue = (long) cv; // correction due to stopping ramp
for(i = 0; i < MOVING_TIMEOUT; ++i){ if(corrvalue < 10) corrvalue = 10;
can_dsleep(0.001); DBG("start-> curpos: %ld, difference: %ld, corrval: %ld",
//uint16_t motstat; curposition, olddiffr, corrvalue);
int errctr = 0, passctr = 0;
while(can_dtime() - t0 < MOVING_TIMEOUT){
double speed; double speed;
if(emerg_stop){ // emergency stop activated if(emerg_stop){ // emergency stop activated
WARNX("Activated stop while moving"); WARNX("Activated stop while moving");
@ -649,36 +726,39 @@ static int move(unsigned long targposition, int16_t rawspeed){
curstatus = STAT_ERROR; curstatus = STAT_ERROR;
return 1; return 1;
} }
getPos(NULL); //getPos(NULL);
if(chkMove(targspd)){ if(chkMove(targspd)){
WARNX("Can't move further!"); WARNX("Can't move further!");
stop(); stop();
return 1; 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(fabs(speed) < 0.1){
if(++zerctr == 10){ if(can_dtime() - t0 > TACCEL){
WARNX("Motor stopped while moving!"); WARNX("Motor can't moving! Time after start=%.3fs.", can_dtime()-t0);
curstatus = STAT_ERROR; curstatus = STAT_ERROR;
stop(); stop();
return 1; return 1;
} }
}else zerctr = 0; }
if(!getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition)) continue; if(!getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition)) continue;
long diffr = targposition - curposition; long diffr = labs((long)targposition - (long)curposition);
DBG("Speed: %g, curpos: %ld, diff: %ld", speed, curposition, diffr); DBG("Speed: %g, curpos: %ld, diff: %ld", speed, curposition, diffr);
if(abs(diffr) < corrvalue || abs(diffr) < RAWPOS_TOLERANCE){ if(diffr < corrvalue){
DBG("OK! almost reach: olddif=%ld, diff=%ld, corrval=%ld, tm=%g", olddiffr, diffr, corrvalue, can_dtime()-t0); DBG("OK! almost reach: olddif=%ld, diff=%ld, corrval=%ld, tm=%g", olddiffr, diffr, corrvalue, can_dtime()-t0);
olddiffr = diffr; olddiffr = diffr;
break; break;
} }
if(abs(diffr) > abs(olddiffr)){ if(diffr > olddiffr){ // pass over target -> stop
if(++passctr > 2) break;
}
if(diffr >= olddiffr){ // motor stall -> stop
++errctr; ++errctr;
DBG("errctr: %d", errctr); DBG("errctr: %d", errctr);
if(errctr > 50) break; if(errctr > STALL_MAXCTR) break;
} }
olddiffr = diffr; olddiffr = diffr;
} }
if(i == MOVING_TIMEOUT){ if(can_dtime() - t0 > MOVING_TIMEOUT){
WARNX("Error: timeout, but motor still not @ position! STOP!"); WARNX("Error: timeout, but motor still not @ position! STOP!");
stop(); stop();
curstatus = STAT_ERROR; curstatus = STAT_ERROR;
@ -686,7 +766,7 @@ static int move(unsigned long targposition, int16_t rawspeed){
} }
DBG("end-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0); DBG("end-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
if(waitTillStop()) return 1; if(waitTillStop()) return 1;
if(abs(targposition - curposition) > RAWPOS_TOLERANCE) if(labs((long)targposition - (long)curposition) > RAWPOS_TOLERANCE)
verbose("Current (%ld) position is too far from target (%ld)\n", curposition, targposition); 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); DBG("stop-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
curstatus = STAT_OK; curstatus = STAT_OK;
@ -699,6 +779,7 @@ static int move(unsigned long targposition, int16_t rawspeed){
* @return 0 if all OK * @return 0 if all OK
*/ */
int move2pos(double target){ int move2pos(double target){
if(!motorRDY || !encoderRDY) return 1;
FNAME(); FNAME();
double cur; double cur;
if(getPos(&cur)){ if(getPos(&cur)){
@ -707,23 +788,24 @@ int move2pos(double target){
} }
unsigned long targposition = FOC_MM2RAW(target); unsigned long targposition = FOC_MM2RAW(target);
DBG("Raw target position: %lu", targposition); DBG("Raw target position: %lu", targposition);
if(target > FOCMAX_MM || target < FOCMIN_MM || targposition < FOCMIN || targposition > FOCMAX){ if(target > FOCMAX_MM || target < FOCMIN_MM){
WARNX("Target focus position over the available range!"); WARNX("Target focus position over the available range!");
return 1; return 1;
} }
if(abs(targposition - curposition) < RAWPOS_TOLERANCE){ if(labs((long)targposition - (long)curposition) < RAWPOS_TOLERANCE){
verbose("Already at position\n"); verbose("Already at position\n");
return 0; return 0;
} }
long spd, targ0pos = (long)targposition + (long)dF0, absdiff = abs(targ0pos - (long)curposition), long spd, targ0pos = (long)targposition + (long)dF0, absdiff = labs(targ0pos - (long)curposition),
sign = (targ0pos > (long)curposition) ? 1 : -1; sign = (targ0pos > (long)curposition) ? 1 : -1;
DBG("absdiff: %ld", absdiff); DBG("absdiff: %ld", absdiff);
//long spd = (targ0pos - (long)curposition) / 2L; // move not less than for 1s
if(absdiff > 1000) spd = sign*MAXSPEED; if(absdiff > 1500) spd = sign*MAXSPEED;
else if(absdiff > 500) spd = sign*MAXSPEED / 2; else if(absdiff > 500) spd = sign*MAXSPEED / 2;
else if(absdiff > 200) spd = sign*MAXSPEED / 4; else if(absdiff > 150) spd = sign*MAXSPEED / 3;
else spd = sign*MINSPEED; else spd = sign*MINSPEED;
int16_t targspd = (int16_t) spd; int16_t targspd = (int16_t) spd;
DBG("TARGSPD: %d", targspd);
/* if(spd > INT16_MAX) targspd = INT16_MAX; /* if(spd > INT16_MAX) targspd = INT16_MAX;
else if(spd < INT16_MIN) targspd = INT16_MIN; else if(spd < INT16_MIN) targspd = INT16_MIN;
fix_targspeed(&targspd);*/ fix_targspeed(&targspd);*/
@ -748,13 +830,13 @@ int move2pos(double target){
WARNX("Can't get current position"); WARNX("Can't get current position");
return 1; return 1;
} }
if(abs(targposition - curposition) < RAWPOS_TOLERANCE){ if(labs((long)targposition - (long)curposition) < RAWPOS_TOLERANCE){
verbose("Catch the position @ rough movint\n"); verbose("Catch the position @ rough moving\n");
DBG("Catch the position @ rough movint"); DBG("Catch the position @ rough moving");
return 0; return 0;
} }
if(curposition < targposition){ if(curposition < targposition){
WARNX("Error in current position!"); WARNX("Error in current position: %.3f instead of %.3f!", FOC_RAW2MM(curposition), FOC_RAW2MM(targposition));
return 1; return 1;
} }
DBG("2) curpos: %ld, difference: %ld\n", curposition, (long)targposition - (long)curposition); DBG("2) curpos: %ld, difference: %ld\n", curposition, (long)targposition - (long)curposition);
@ -771,7 +853,7 @@ int move2pos(double target){
WARNX("Can't get current position"); WARNX("Can't get current position");
return 1; return 1;
} }
if(abs(targposition - curposition) > RAWPOS_TOLERANCE){ if(labs((long)targposition - (long)curposition) > RAWPOS_TOLERANCE){
verbose("Stopped over the accuracy range\n"); verbose("Stopped over the accuracy range\n");
return 1; return 1;
} }
@ -783,9 +865,14 @@ int get_pos_speed(unsigned long *pos, double *speed){
FNAME(); FNAME();
int ret = 0; int ret = 0;
if(pos){ if(pos){
if(!getLong(encnodenum, DS406_POSITION_VAL, 0, pos)) ret = 1; if(!encoderRDY) *pos = FOC_MM2RAW(3.);
else if(!getLong(encnodenum, DS406_POSITION_VAL, 0, pos)) ret = 1;
} }
if(speed){ if(speed){
if(!motorRDY){
*speed = 0.;
return ret;
}
if(get_motor_speed(speed) != CAN_NOERR){ if(get_motor_speed(speed) != CAN_NOERR){
*speed = 0.; *speed = 0.;
ret = 1; ret = 1;
@ -795,6 +882,7 @@ int get_pos_speed(unsigned long *pos, double *speed){
} }
void movewithmon(double spd){ void movewithmon(double spd){
if(!motorRDY || !encoderRDY) return;
unsigned char buf[6] = {0,}; unsigned char buf[6] = {0,};
if(fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED){ if(fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED){
WARNX("Target speed should be be from %d to %d (rev/min)", MINSPEED, MAXSPEED); WARNX("Target speed should be be from %d to %d (rev/min)", MINSPEED, MAXSPEED);

43
Z1000_focus/cmdlnopts.c
View File

@ -31,7 +31,7 @@
* here are global parameters initialisation * here are global parameters initialisation
*/ */
int help; int help;
glob_pars G; glob_pars GP;
#define DEFPIDNAME "/tmp/z1000focus.pid" #define DEFPIDNAME "/tmp/z1000focus.pid"
@ -42,7 +42,8 @@ glob_pars const Gdefault = {
.motorID = 12, .motorID = 12,
.gotopos = NAN, .gotopos = NAN,
.port = DEFPORT, .port = DEFPORT,
.pidfilename = DEFPIDNAME .pidfilename = DEFPIDNAME,
.chpresetval = -1
}; };
/* /*
@ -52,21 +53,25 @@ glob_pars const Gdefault = {
myoption cmdlnopts[] = { myoption cmdlnopts[] = {
// set 1 to param despite of its repeating number: // set 1 to param despite of its repeating number:
{"help", NO_ARGS, NULL, 'h', arg_none, APTR(&help), "show this help"}, {"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"}, {"node", NEED_ARG, NULL, 'n', arg_int, APTR(&GP.nodenum), "encoder node number"},
{"reset", NO_ARGS, NULL, 'r', arg_none, APTR(&G.reset), "reset encoder"}, {"reset", NO_ARGS, NULL, 'r', arg_none, APTR(&GP.reset), "reset encoder"},
{"verbose", NO_ARGS, NULL, 'v', arg_int, APTR(&G.verbose), "show more info"}, {"verbose", NO_ARGS, NULL, 'v', arg_int, APTR(&GP.verbose), "show more info"},
{"motorid", NEED_ARG, NULL, 'i', arg_int, APTR(&G.motorID), "motor controller address"}, {"motorid", NEED_ARG, NULL, 'i', arg_int, APTR(&GP.motorID), "motor controller address"},
{"gotopos", NEED_ARG, NULL, 'g', arg_double, APTR(&G.gotopos), "target focus position"}, {"gotopos", NEED_ARG, NULL, 'g', arg_double, APTR(&GP.gotopos), "target focus position"},
{"targspeed",NEED_ARG, NULL, 't', arg_double, APTR(&G.targspeed), "move motor with constant speed (rev/min)"}, {"targspeed",NEED_ARG, NULL, 't', arg_double, APTR(&GP.targspeed), "move motor with constant speed (rev/min)"},
{"stop", NO_ARGS, NULL, 's', arg_none, APTR(&G.stop), "stop motor"}, {"stop", NO_ARGS, NULL, 's', arg_none, APTR(&GP.stop), "stop motor"},
{"monitor", NEED_ARG, NULL, 'm', arg_double, APTR(&G.monitspd), "move a little with given speed with monitoring"}, {"monitor", NEED_ARG, NULL, 'm', arg_double, APTR(&GP.monitspd), "move a little with given speed with monitoring"},
{"eswstate",NO_ARGS, NULL, 'e', arg_none, APTR(&G.showesw), "show end-switches state"}, {"eswstate",NO_ARGS, NULL, 'e', arg_none, APTR(&GP.showesw), "show end-switches state"},
{"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logname), "logfile name and path"}, {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&GP.logname), "logfile name and path"},
{"server", NO_ARGS, NULL, 'S', arg_none, APTR(&G.server), "work as server"}, {"server", NO_ARGS, NULL, 'S', arg_none, APTR(&GP.server), "work as server"},
{"port", NEED_ARG, NULL, 'P', arg_string, APTR(&G.port), "server port number (default: " DEFPORT ")"}, {"port", NEED_ARG, NULL, 'P', arg_string, APTR(&GP.port), "server port number (default: " DEFPORT ")"},
{"host", NEED_ARG, NULL, 'H', arg_string, APTR(&G.host), "host to connect (default: localhost)"}, {"host", NEED_ARG, NULL, 'H', arg_string, APTR(&GP.host), "host to connect (default: localhost)"},
{"standalone",NO_ARGS, NULL, 'A', arg_none, APTR(&G.standalone),"run as standalone application"}, {"standalone",NO_ARGS, NULL, 'A', arg_none, APTR(&GP.standalone),"run as standalone application"},
{"pidfile", NEED_ARG, NULL, 'p', arg_string, APTR(&G.pidfilename),"name of PID-file (default: " DEFPIDNAME ")"}, {"pidfile", NEED_ARG, NULL, 'p', arg_string, APTR(&GP.pidfilename),"name of PID-file (default: " DEFPIDNAME ")"},
{"preset", NEED_ARG, NULL, '0', arg_longlong,APTR(&GP.chpresetval),"change preset value"},
{"nomotor", NO_ARGS, NULL, 'M', arg_none, APTR(&GP.nomotor), "don't initialize motor"},
{"noencoder",NO_ARGS, NULL, 'E', arg_none, APTR(&GP.noencoder), "don't initialize encoder"},
{"focout", NEED_ARG, NULL, 'f', arg_string, APTR(&GP.focfilename),"filename where to store focus data"},
end_option end_option
}; };
@ -78,12 +83,12 @@ myoption cmdlnopts[] = {
* @return allocated structure with global parameters * @return allocated structure with global parameters
*/ */
glob_pars *parse_args(int argc, char **argv){ glob_pars *parse_args(int argc, char **argv){
void *ptr = memcpy(&G, &Gdefault, sizeof(G)); assert(ptr); void *ptr = memcpy(&GP, &Gdefault, sizeof(GP)); assert(ptr);
// format of help: "Usage: progname [args]\n" // format of help: "Usage: progname [args]\n"
change_helpstring("Usage: %s [args]\n\n\tWhere args are:\n"); change_helpstring("Usage: %s [args]\n\n\tWhere args are:\n");
// parse arguments // parse arguments
parseargs(&argc, &argv, cmdlnopts); parseargs(&argc, &argv, cmdlnopts);
if(help) showhelp(-1, cmdlnopts); if(help) showhelp(-1, cmdlnopts);
return &G; return &GP;
} }

4
Z1000_focus/cmdlnopts.h
View File

@ -42,6 +42,10 @@ typedef struct{
char *host; // host to connect (in client mode) char *host; // host to connect (in client mode)
int standalone; // run standalone int standalone; // run standalone
char *pidfilename; // name of PID-file char *pidfilename; // name of PID-file
long long chpresetval; // change preset value for current position
int nomotor; // don't check and even try to use motor
int noencoder; // don't check and even try to use encoder
char *focfilename; // name of file with focus data
} glob_pars; } glob_pars;

64
Z1000_focus/main.c
View File

@ -28,7 +28,7 @@
#include "socket.h" #include "socket.h"
#include "usefull_macros.h" #include "usefull_macros.h"
static glob_pars *G; glob_pars *G;
/** /**
* @brief verbose - printf when parameter `verbose` set * @brief verbose - printf when parameter `verbose` set
@ -44,13 +44,14 @@ int verbose(const char *fmt, ...){
return i; return i;
} }
static pid_t childpid;
/** /**
* @brief signals - signal handler (also called by functions ERR/ERRX) * @brief signals - signal handler (also called by functions ERR/ERRX)
* @param signo - signal number * @param signo - signal number
*/ */
void signals(int signo){ void signals(int signo){
WARNX("Received signal %d", signo); WARNX("Received signal %d", signo);
unlink_pidfile(); if(childpid) unlink_pidfile(); // parent process died
exit(signo); exit(signo);
} }
@ -65,7 +66,7 @@ static void cmdparser(){
#undef BUFL #undef BUFL
} }
//extern char can_dev[40]; extern char can_dev[40];
int main (int argc, char *argv[]){ int main (int argc, char *argv[]){
int ret = 0; int ret = 0;
@ -76,12 +77,14 @@ int main (int argc, char *argv[]){
if(fabs(G->targspeed) > DBL_EPSILON && !isnan(G->gotopos)) if(fabs(G->targspeed) > DBL_EPSILON && !isnan(G->gotopos))
ERRX("Arguments \"target speed\" and \"target position\" can't meet together!"); ERRX("Arguments \"target speed\" and \"target position\" can't meet together!");
if(fabs(G->targspeed) > DBL_EPSILON){ if(fabs(G->targspeed) > DBL_EPSILON){
if(G->nomotor) ERRX("Can't set target speed without motor");
if(fabs(G->targspeed) < MINSPEED || fabs(G->targspeed) > MAXSPEED){ if(fabs(G->targspeed) < MINSPEED || fabs(G->targspeed) > MAXSPEED){
WARNX("Target speed should be be from %d to %d (rev/min)", MINSPEED, MAXSPEED); WARNX("Target speed should be be from %d to %d (rev/min)", MINSPEED, MAXSPEED);
return 1; return 1;
} }
} }
if(!isnan(G->gotopos)){ if(!isnan(G->gotopos)){
if(G->noencoder || G->nomotor) ERRX("Can't go to target position without encoder or motor");
if(G->gotopos > FOCMAX_MM || G->gotopos < FOCMIN_MM){ if(G->gotopos > FOCMAX_MM || G->gotopos < FOCMIN_MM){
WARNX("Focal distance may be from %g to %g mm", FOCMIN_MM, FOCMAX_MM); WARNX("Focal distance may be from %g to %g mm", FOCMIN_MM, FOCMAX_MM);
return 1; return 1;
@ -92,43 +95,35 @@ int main (int argc, char *argv[]){
signal(SIGKILL, signals); signal(SIGKILL, signals);
signal(SIGTSTP, SIG_IGN); signal(SIGTSTP, SIG_IGN);
signal(SIGHUP, SIG_IGN); signal(SIGHUP, SIG_IGN);
//can_dev[8] = '1'; can_dev[8] = '1';
if(G->server || G->standalone){ // init hardware if(G->server || G->standalone){ // init hardware
check4running(G->pidfilename); check4running(G->pidfilename);
if(G->logname){
openlogfile(G->logname);
}
if(init_encoder(G->nodenum, G->reset)) ERRX("Encoder not found");
if(init_motor_ids(G->motorID)){
WARNX("Error during motor initialization");
ret = 1;
goto Oldcond;
}
if(getPos(&curposition)){
WARNX("Can't read current position");
ret = 1;
goto Oldcond;
}else verbose("Position @ start: %.2fmm\n", curposition);
} }
if(G->server){ // daemonize & run server if(G->server){ // daemonize & run server
#if !defined EBUG #if !defined EBUG
if(daemon(1, 0)){ if(daemon(1, 0)){
ERR("daemon()"); ERR("daemon()");
} };
#endif #endif
while(1){ // guard for dead processes while(1){ // guard for dead processes
pid_t childpid = fork(); childpid = fork();
if(childpid){ if(childpid){
putlog("create child with PID %d\n", childpid); DBG("child: %d", childpid);
DBG("Created child with PID %d\n", childpid);
wait(NULL); wait(NULL);
putlog("child %d died\n", childpid); DBG("child: %d DIED", childpid);
WARNX("Child %d died\n", childpid);
sleep(1); sleep(1);
}else{ }else{
prctl(PR_SET_PDEATHSIG, SIGTERM); // send SIGTERM to child when parent dies prctl(PR_SET_PDEATHSIG, SIGTERM); // send SIGTERM to child when parent dies
if(G->server || G->standalone){ // init hardware
if(!G->noencoder && init_encoder(G->nodenum, G->reset)) ERRX("Encoder not found");
if(!G->nomotor && init_motor_ids(G->motorID)) ERRX("Error during motor initialization");
}
if(G->logname){ // open log file in child
openlogfile(G->logname);
putlog("created child with PID %d", getpid());
}
daemonize(G->port); daemonize(G->port);
} }
} }
@ -137,6 +132,25 @@ int main (int argc, char *argv[]){
return 0; return 0;
} }
if(!G->noencoder && init_encoder(G->nodenum, G->reset)){
WARNX("Encoder not found");
#ifndef EBUG
return 1;
#endif
}
if(!G->nomotor && init_motor_ids(G->motorID)){
WARNX("Error during motor initialization");
ret = 1;
#ifndef EBUG
goto Oldcond;
#endif
}
if(getPos(&curposition)){
WARNX("Can't read current position");
ret = 1;
goto Oldcond;
}else verbose("Position @ start: %.2fmm\n", curposition);
if(fabs(G->monitspd) > DBL_EPSILON){ if(fabs(G->monitspd) > DBL_EPSILON){
movewithmon(G->monitspd); movewithmon(G->monitspd);
goto Oldcond; goto Oldcond;
@ -188,6 +202,6 @@ Oldcond:
red("ERROR: both end-switches active\n"); red("ERROR: both end-switches active\n");
} }
} }
returnPreOper(); returnPreOper(G->chpresetval);
return ret; return ret;
} }

96
Z1000_focus/socket.c
View File

@ -35,7 +35,7 @@
#include "cmdlnopts.h" // glob_pars #include "cmdlnopts.h" // glob_pars
#define BUFLEN (10240) #define BUFLEN (1024)
// Max amount of connections // Max amount of connections
#define BACKLOG (30) #define BACKLOG (30)
@ -137,6 +137,7 @@ static void *move_focus(void *targpos){
pthread_mutex_lock(&canbus_mutex); pthread_mutex_lock(&canbus_mutex);
// in any error case we should check end-switches and move out of them! // in any error case we should check end-switches and move out of them!
if(move2pos(pos)) go_out_from_ESW(); if(move2pos(pos)) go_out_from_ESW();
putlog("Focus value: %.03f", curPos());
pthread_mutex_unlock(&moving_mutex); pthread_mutex_unlock(&moving_mutex);
pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&canbus_mutex);
pthread_exit(NULL); pthread_exit(NULL);
@ -186,11 +187,18 @@ static void getoutESW(){
static void *handle_socket(void *asock){ static void *handle_socket(void *asock){
#define getparam(x) (strncmp(found, x, sizeof(x)-1) == 0) #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 sock = *((int*)asock);
int webquery = 0; // whether query is web or regular int webquery = 0; // whether query is web or regular
char buff[BUFLEN]; char buff[BUFLEN];
ssize_t rd; ssize_t rd;
struct sockaddr_in peer;
socklen_t peerlen = sizeof(peer);
char *peerIP = NULL;
if(getpeername(sock, &peer, &peerlen)){
WARN("getpeername() failed");
}else{
peerIP = strdup(inet_ntoa(peer.sin_addr));
}
double t0 = dtime(); double t0 = dtime();
while(dtime() - t0 < SOCKET_TIMEOUT){ while(dtime() - t0 < SOCKET_TIMEOUT){
if(!waittoread(sock)){ // no data incoming if(!waittoread(sock)){ // no data incoming
@ -215,9 +223,13 @@ static void *handle_socket(void *asock){
// here we can process user data // here we can process user data
//DBG("user send: %s%s\n", buff, webquery ? ", web" : ""); //DBG("user send: %s%s\n", buff, webquery ? ", web" : "");
// empty request == focus request // empty request == focus request
// TODO: add requests for min/max values (focus & speed)
if(strlen(found) < 1 || getparam(S_CMD_FOCUS)){ if(strlen(found) < 1 || getparam(S_CMD_FOCUS)){
//DBG("position request"); //DBG("position request");
snprintf(buff, BUFLEN, "%.03f", curPos()); snprintf(buff, BUFLEN, "%.03f", curPos());
}else if(getparam(S_CMD_LIMITS)){ // send to user limit values
snprintf(buff, BUFLEN, "focmin=%g\nfocmax=%g\nminspeed=%d\nmaxspeed=%d\n",
FOCMIN_MM, FOCMAX_MM, MINSPEED, MAXSPEED);
}else if(getparam(S_CMD_STOP)){ }else if(getparam(S_CMD_STOP)){
DBG("Stop request"); DBG("Stop request");
emerg_stop = TRUE; emerg_stop = TRUE;
@ -226,6 +238,7 @@ static void *handle_socket(void *asock){
if(stop()) sprintf(buff, S_ANS_ERR); if(stop()) sprintf(buff, S_ANS_ERR);
else sprintf(buff, S_ANS_OK); else sprintf(buff, S_ANS_OK);
emerg_stop = FALSE; emerg_stop = FALSE;
putlog("%s: request to stop @ %.03f", peerIP, curPos());
pthread_mutex_unlock(&moving_mutex); pthread_mutex_unlock(&moving_mutex);
pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&canbus_mutex);
}else if(getparam(S_CMD_TARGSPEED)){ }else if(getparam(S_CMD_TARGSPEED)){
@ -237,6 +250,7 @@ static void *handle_socket(void *asock){
if(!ch || !str2double(&spd, ch+1) || fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED || movewconstspeed(spd)) sprintf(buff, S_ANS_ERR); if(!ch || !str2double(&spd, ch+1) || fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED || movewconstspeed(spd)) sprintf(buff, S_ANS_ERR);
else{ else{
DBG("Move with constant speed %g request", spd); DBG("Move with constant speed %g request", spd);
putlog("%s: move with speed %g, current pos.: %.03f", peerIP, spd, curPos());
sprintf(buff, S_ANS_OK); sprintf(buff, S_ANS_OK);
} }
pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&canbus_mutex);
@ -247,8 +261,11 @@ static void *handle_socket(void *asock){
double pos; double pos;
if(!ch || !str2double(&pos, ch+1) || pos < FOCMIN_MM || pos > FOCMAX_MM) sprintf(buff, S_ANS_ERR); if(!ch || !str2double(&pos, ch+1) || pos < FOCMIN_MM || pos > FOCMAX_MM) sprintf(buff, S_ANS_ERR);
else{ else{
DBG("Move to position %g request", pos); const char *ans = startmoving(pos);
sprintf(buff, startmoving(pos)); putlog("%s: move to %.03f, current pos.: %.03f", peerIP, pos, curPos());
addtolog("status: %s", ans);
sprintf(buff, "%s", ans);
DBG("Move to position %g request, status: %s", pos, ans);
} }
}else if(getparam(S_CMD_STATUS)){ }else if(getparam(S_CMD_STATUS)){
const char *msg = S_STATUS_ERROR; const char *msg = S_STATUS_ERROR;
@ -274,15 +291,14 @@ static void *handle_socket(void *asock){
default: default:
msg = "Unknown status"; msg = "Unknown status";
} }
sprintf(buff, msg); sprintf(buff, "%s", msg);
}else sprintf(buff, S_ANS_ERR); }else sprintf(buff, S_ANS_ERR);
if(!send_data(sock, webquery, buff)){ if(!send_data(sock, webquery, buff)){
WARNX("can't send data, some error occured"); WARNX("can't send data, some error occured");
} }
} }
FREE(peerIP);
close(sock); close(sock);
//DBG("closed");
//putlog("socket closed, exit");
pthread_exit(NULL); pthread_exit(NULL);
return NULL; return NULL;
#undef getparam #undef getparam
@ -290,11 +306,9 @@ static void *handle_socket(void *asock){
// main socket server // main socket server
static void *server(void *asock){ static void *server(void *asock){
putlog("server(): getpid: %d, pthread_self: %lu, tid: %lu",getpid(), pthread_self(), syscall(SYS_gettid));
int sock = *((int*)asock); int sock = *((int*)asock);
if(listen(sock, BACKLOG) == -1){ if(listen(sock, BACKLOG) == -1){
putlog("listen() failed"); WARN("listen() failed");
WARN("listen");
return NULL; return NULL;
} }
while(1){ while(1){
@ -304,8 +318,7 @@ static void *server(void *asock){
if(!waittoread(sock)) continue; if(!waittoread(sock)) continue;
newsock = accept(sock, (struct sockaddr*)&their_addr, &size); newsock = accept(sock, (struct sockaddr*)&their_addr, &size);
if(newsock <= 0){ if(newsock <= 0){
putlog("accept() failed"); WARN("accept() failed");
WARN("accept()");
continue; continue;
} }
struct sockaddr_in* pV4Addr = (struct sockaddr_in*)&their_addr; struct sockaddr_in* pV4Addr = (struct sockaddr_in*)&their_addr;
@ -315,35 +328,51 @@ static void *server(void *asock){
//DBG("Got connection from %s", str); //DBG("Got connection from %s", str);
pthread_t handler_thread; pthread_t handler_thread;
if(pthread_create(&handler_thread, NULL, handle_socket, (void*) &newsock)){ if(pthread_create(&handler_thread, NULL, handle_socket, (void*) &newsock)){
putlog("server(): pthread_create() failed"); WARN("pthread_create() failed");
WARN("pthread_create()");
}else{ }else{
//DBG("Thread created, detouch"); //DBG("Thread created, detouch");
pthread_detach(handler_thread); // don't care about thread state pthread_detach(handler_thread); // don't care about thread state
} }
} }
putlog("server(): UNREACHABLE CODE REACHED!"); putlog("UNREACHABLE CODE REACHED!");
}
// refresh file with focus value
static void subst_file(char *name){
if(!name) return;
int l = strlen(name) + 7;
char *aname = MALLOC(char, l);
snprintf(aname, l, "%sXXXXXX", name);
int fd = mkstemp(aname);
if(fd < 0) goto ret;
FILE *f = fdopen(fd, "w");
if(!f) goto ret;
fprintf(f, "FOCUS = %.2f\n", curPos());
fclose(f);
rename(aname, name);
ret:
FREE(aname);
} }
// data gathering & socket management // data gathering & socket management
static void daemon_(int sock){ static void daemon_(int sock){
if(sock < 0) return; if(sock < 0) return;
pthread_t sock_thread; pthread_t sock_thread;
double oldpos = curPos();
subst_file(G->focfilename);
DBG("create server() thread");
if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){
putlog("daemon_(): pthread_create() failed"); ERR("pthread_create() failed");
ERR("pthread_create()");
} }
do{ do{
if(pthread_kill(sock_thread, 0) == ESRCH){ // died if(pthread_kill(sock_thread, 0) == ESRCH){ // died
WARNX("Sockets thread died"); WARNX("sockets thread died");
putlog("Sockets thread died");
pthread_join(sock_thread, NULL); pthread_join(sock_thread, NULL);
if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){
putlog("daemon_(): new pthread_create() failed"); ERR("new pthread_create() failed");
ERR("pthread_create()");
} }
} }
usleep(500000); // sleep a little or thread's won't be able to lock mutex usleep(50000); // sleep a little or thread's won't be able to lock mutex
// get current position // get current position
if(!pthread_mutex_trylock(&canbus_mutex)){ if(!pthread_mutex_trylock(&canbus_mutex)){
getPos(NULL); getPos(NULL);
@ -353,6 +382,10 @@ static void daemon_(int sock){
getoutESW(); getoutESW();
} }
} }
if(G->focfilename && (fabs(oldpos - curPos()) > 0.01)){ // position changed -> change it in file
oldpos = curPos();
subst_file(G->focfilename);
}
}while(1); }while(1);
putlog("daemon_(): UNREACHABLE CODE REACHED!"); putlog("daemon_(): UNREACHABLE CODE REACHED!");
} }
@ -368,38 +401,36 @@ void daemonize(const char *port){
hints.ai_socktype = SOCK_STREAM; hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE; hints.ai_flags = AI_PASSIVE;
if(getaddrinfo(NULL, port, &hints, &res) != 0){ if(getaddrinfo(NULL, port, &hints, &res) != 0){
ERR("getaddrinfo"); ERR("getaddrinfo failed");
} }
struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr; struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr;
char str[INET_ADDRSTRLEN]; char str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(ia->sin_addr), 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); DBG("canonname: %s, port: %u, addr: %s", res->ai_canonname, ntohs(ia->sin_port), str);
// loop through all the results and bind to the first we can // loop through all the results and bind to the first we can
for(p = res; p != NULL; p = p->ai_next){ for(p = res; p != NULL; p = p->ai_next){
if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){ if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){
WARN("socket");
continue; continue;
} }
int reuseaddr = 1; int reuseaddr = 1;
if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) == -1){ if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) == -1){
ERR("setsockopt"); ERR("setsockopt failed");
} }
if(bind(sock, p->ai_addr, p->ai_addrlen) == -1){ if(bind(sock, p->ai_addr, p->ai_addrlen) == -1){
close(sock); close(sock);
WARN("bind");
continue; continue;
} }
break; // if we get here, we have a successfull connection break; // if we get here, we have a successfull connection
} }
if(p == NULL){ if(p == NULL){
putlog("failed to bind socket, exit");
// looped off the end of the list with no successful bind // looped off the end of the list with no successful bind
ERRX("failed to bind socket"); ERRX("failed to bind socket");
} }
freeaddrinfo(res); freeaddrinfo(res);
DBG("going to run daemon_()");
daemon_(sock); daemon_(sock);
close(sock); close(sock);
putlog("socket closed, exit"); putlog("daemonize(): UNREACHABLE CODE REACHED!");
signals(0); signals(0);
} }
@ -416,7 +447,7 @@ void sock_send_data(const char *host, const char *port, const char *data){
hints.ai_socktype = SOCK_STREAM; hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE; hints.ai_flags = AI_PASSIVE;
if(getaddrinfo(host, port, &hints, &res) != 0){ if(getaddrinfo(host, port, &hints, &res) != 0){
ERR("getaddrinfo"); ERR("getaddrinfo failed");
} }
struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr; struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr;
char str[INET_ADDRSTRLEN]; char str[INET_ADDRSTRLEN];
@ -425,11 +456,9 @@ void sock_send_data(const char *host, const char *port, const char *data){
// loop through all the results and bind to the first we can // loop through all the results and bind to the first we can
for(p = res; p != NULL; p = p->ai_next){ for(p = res; p != NULL; p = p->ai_next){
if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){ if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){
WARN("socket");
continue; continue;
} }
if(connect(sock, p->ai_addr, p->ai_addrlen) == -1){ if(connect(sock, p->ai_addr, p->ai_addrlen) == -1){
WARN("connect()");
close(sock); close(sock);
continue; continue;
} }
@ -450,5 +479,6 @@ void sock_send_data(const char *host, const char *port, const char *data){
return; return;
} }
} }
WARN("No answer!"); WARN("no answer!");
close(sock);
} }

6
Z1000_focus/socket.h
View File

@ -37,6 +37,7 @@
#define S_CMD_TARGSPEED "targspeed" #define S_CMD_TARGSPEED "targspeed"
#define S_CMD_GOTO "goto" #define S_CMD_GOTO "goto"
#define S_CMD_STATUS "status" #define S_CMD_STATUS "status"
#define S_CMD_LIMITS "limits"
// answers through the socket // answers through the socket
#define S_ANS_ERR "error" #define S_ANS_ERR "error"
@ -46,11 +47,10 @@
// statuses // statuses
#define S_STATUS_OK "OK" #define S_STATUS_OK "OK"
#define S_STATUS_ESW "End-switch active" #define S_STATUS_ESW "End-switch active"
#define S_STATUS_ERROR "Erroneous state" #define S_STATUS_ERROR "SEW power if off (or other error)"
#define S_STATUS_ESW "End-switch active"
#define S_STATUS_GOFROMESW "Moving from end-switch" #define S_STATUS_GOFROMESW "Moving from end-switch"
#define S_STATUS_FORBIDDEN "Motion in forbidden position" #define S_STATUS_FORBIDDEN "Motion in forbidden position"
#define S_STATUS_DAMAGE "The focuser damaged and can't work further" #define S_STATUS_DAMAGE "Damaged state, call engineer"
bool emerg_stop; bool emerg_stop;

105
Z1000_focus/usefull_macros.c
View File

@ -387,25 +387,42 @@ int str2double(double *num, const char *str){
return TRUE; return TRUE;
} }
FILE *Flog = NULL; // log file descriptor static FILE *Flog = NULL; // log file descriptor
char *logname = NULL; static char *logname = NULL; // full logfile name (with PID prefix)
time_t log_open_time = 0; static time_t log_open_time = 0; // time when log file was opened
/** /**
* Try to open log file * Try to open log file
* if failed show warning message * if failed show warning message
*/ */
void openlogfile(char *name){ void openlogfile(char *name){
if(!name){ //char buf[PATH_MAX];
WARNX(_("Need filename")); if(Flog){
return; fclose(Flog);
Flog = NULL;
} }
green(_("Try to open log file %s in append mode\n"), name); if(!name) return;
/*
if(!name){ // filename is omit -> try to open log with old name
if(!fullogname){
WARNX(_("openlogfile(): need filename"));
return;
}
name = fullogname;
}else{
if(fullogname) FREE(fullogname);
// append PID to name
snprintf(buf, PATH_MAX, "%d_%s", getpid(), name);
name = buf;
green(_("Try to open log file %s in append mode"), name);
printf("\n");
}*/
if(!(Flog = fopen(name, "a"))){ if(!(Flog = fopen(name, "a"))){
WARN(_("Can't open log file")); WARN(_("Can't open log file"));
return; return;
} }
log_open_time = time(NULL); log_open_time = time(NULL);
logname = name; logname = name;
//fullogname = strdup(buf);
} }
/** /**
@ -413,22 +430,84 @@ void openlogfile(char *name){
*/ */
int putlog(const char *fmt, ...){ int putlog(const char *fmt, ...){
if(!Flog) return 0; if(!Flog) return 0;
time_t t_now = time(NULL); char strtm[128];
if(t_now - log_open_time > 86400){ // rotate log time_t t = time(NULL);
fprintf(Flog, "\n\t\t%sRotate log\n", ctime(&t_now)); struct tm *curtm = localtime(&t);
int i = strftime(strtm, 128, "%Y/%m/%d-%H:%M", curtm);
if(t - log_open_time > 86400){ // rotate log
fprintf(Flog, "\n\n%s\tRotate log\n", strtm);
fclose(Flog); fclose(Flog);
Flog = NULL;
char newname[PATH_MAX]; char newname[PATH_MAX];
snprintf(newname, PATH_MAX, "%s.old", logname); snprintf(newname, PATH_MAX, "%s.old", logname);
if(rename(logname, newname)) WARN("rename()"); if(rename(logname, newname)) WARN("rename()");
openlogfile(logname); openlogfile(NULL);
if(!Flog) return 0; if(!Flog) return 0;
} }
int i = fprintf(Flog, "\n\t\t%s", ctime(&t_now));
va_list ar; va_list ar;
fprintf(Flog, "%s\t", strtm);
va_start(ar, fmt); va_start(ar, fmt);
i = vfprintf(Flog, fmt, ar); i += vfprintf(Flog, fmt, ar);
va_end(ar); va_end(ar);
fprintf(Flog, "\n"); fprintf(Flog, "\n");
++i;
fflush(Flog); fflush(Flog);
return i; return i;
} }
// add message to log file without printing time
int addtolog(const char *fmt, ...){
if(!Flog) return 0;
va_list ar;
int i = 1;
fprintf(Flog, "\t\t\t");
va_start(ar, fmt);
i += vfprintf(Flog, fmt, ar);
va_end(ar);
fprintf(Flog, "\n");
++i;
fflush(Flog);
return i;
}
// messages for warning codes
static const char *wmsgs[WARN_LAST] = {
[WARN_NO] = "All OK",
[WARN_ESWSTATE] = "Can't read end-switches state",
[WARN_SENDPAR] = "can_send_param(): error getting answer",
[WARN_MOVEDAMAGED] = "Try to move in damaged state",
[WARN_BOTHESW] = "Both end-switches are active! Damage state.",
[WARN_LESSMIN] = "Forbidden position: < FOCMIN!",
[WARN_GRTRMAX] = "Forbidden position: > FOCMAX!",
[WARN_CANSEND] = "Error sending CAN frame",
[WARN_CANNOANS] = "can_send_chk(): error getting answer",
};
static time_t lasttime[WARN_LAST] = {0};
/**
* @brief clrwarnsingle - reset warning timeout & give log message
* @param errnum - number of warning
*/
void clrwarnsingle(locwarn errnum){
if(errnum >= WARN_LAST) return;
time_t cur = time(NULL);
if(cur - lasttime[errnum] > SINGLEW_TIMEOUT) return;
putlog("Cleared error: %s", wmsgs[errnum]);
lasttime[errnum] = 0;
}
/**
* @brief warnsingle
* @param msg
* @param errnum
*/
void warnsingle(const char *msg, locwarn errnum){
if(errnum >= WARN_LAST) return;
time_t cur = time(NULL);
if(cur - lasttime[errnum] < SINGLEW_TIMEOUT) return;
lasttime[errnum] = cur;
const char *wmsg = wmsgs[errnum];
_WARN("WARNING in %s: %s", msg, wmsg);
putlog("WARNING in %s:", msg);
addtolog(wmsg);
}

59
Z1000_focus/usefull_macros.h
View File

@ -23,16 +23,22 @@
#ifndef __USEFULL_MACROS_H__ #ifndef __USEFULL_MACROS_H__
#define __USEFULL_MACROS_H__ #define __USEFULL_MACROS_H__
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
#include <errno.h>
#include <err.h> #include <err.h>
#include <errno.h>
#include <fcntl.h>
#include <locale.h> #include <locale.h>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <termio.h>
#include <termios.h>
#include <unistd.h>
#if defined GETTEXT_PACKAGE && defined LOCALEDIR #if defined GETTEXT_PACKAGE && defined LOCALEDIR
/* /*
* GETTEXT * GETTEXT
@ -45,12 +51,7 @@
#define _(String) (String) #define _(String) (String)
#define N_(String) (String) #define N_(String) (String)
#endif #endif
#include <stdlib.h>
#include <termios.h>
#include <termio.h>
#include <sys/time.h>
#include <sys/types.h>
#include <stdint.h>
// unused arguments with -Wall -Werror // unused arguments with -Wall -Werror
@ -74,13 +75,30 @@
/* /*
* ERROR/WARNING messages * ERROR/WARNING messages
*/ */
// local warnings
typedef enum{
WARN_NO, // all ok
WARN_ESWSTATE, // can't read esw state
WARN_SENDPAR, // can_send_param(): error getting answer
WARN_MOVEDAMAGED, // try to move in damaged state
WARN_BOTHESW, // both esw are active
WARN_LESSMIN, // curpos < min
WARN_GRTRMAX, // curpos > max
WARN_CANSEND, // can bus: error sending frame
WARN_CANNOANS, // can bus: no answer
WARN_LAST // N of warnings
} locwarn;
// show SINGLEWARN`s not frequently than once per day
#define SINGLEW_TIMEOUT 86400
extern int globErr; extern int globErr;
extern void signals(int sig); extern void signals(int sig);
#define ERR(...) do{globErr=errno; _WARN(__VA_ARGS__); signals(9);}while(0) #define ERR(...) do{globErr=errno; FNAME(); _WARN(__VA_ARGS__); putlog("ERROR \"%s\" in %s (%s, line %d):", strerror(globErr), __func__, __FILE__, __LINE__); addtolog(__VA_ARGS__); signals(9);}while(0)
#define ERRX(...) do{globErr=0; _WARN(__VA_ARGS__); signals(9);}while(0) #define ERRX(...) do{globErr=0; FNAME(); _WARN(__VA_ARGS__); putlog("ERROR \"%s\" in %s (%s, line %d):", strerror(globErr), __func__, __FILE__, __LINE__); addtolog(__VA_ARGS__); signals(9);}while(0)
#define WARN(...) do{globErr=errno; _WARN(__VA_ARGS__);}while(0) #define WARN(...) do{globErr=errno; FNAME(); _WARN(__VA_ARGS__); putlog("WARNING in %s (%s, line %d):", __func__, __FILE__, __LINE__); addtolog(__VA_ARGS__);}while(0)
#define WARNX(...) do{globErr=0; _WARN(__VA_ARGS__);}while(0) #define WARNX(...) do{globErr=0; FNAME(); _WARN(__VA_ARGS__); putlog("WARNING in %s (%s, line %d):", __func__, __FILE__, __LINE__); addtolog(__VA_ARGS__);}while(0)
// warn with no more than once per hour writting to log, n - warning code
#define SINGLEWARN(n) do{globErr=0; warnsingle(__func__, n);}while(0)
/* /*
* print function name, debug messages * print function name, debug messages
* debug mode, -DEBUG * debug mode, -DEBUG
@ -137,4 +155,7 @@ int str2double(double *num, const char *str);
void openlogfile(char *name); void openlogfile(char *name);
int putlog(const char *fmt, ...); int putlog(const char *fmt, ...);
int addtolog(const char *fmt, ...);
void warnsingle(const char *msg, locwarn errnum);
void clrwarnsingle(locwarn errnum);
#endif // __USEFULL_MACROS_H__ #endif // __USEFULL_MACROS_H__