diff --git a/Z1000_focus/HW_dependent.h b/Z1000_focus/HW_dependent.h
index 02e941f..caf1402 100644
--- a/Z1000_focus/HW_dependent.h
+++ b/Z1000_focus/HW_dependent.h
@@ -22,9 +22,10 @@
#include "motor_cancodes.h"
-// amount of encoder's counts when stop = rawspeed^2/STOPPING_COEFF
-#define STOPPING_COEFF (117500L)
-//#define STOPPING_COEFF (1175000L)
+// On lowest speeds Taccelerated = 0.22s, so wait no more than 0.25s when motor starts
+#define TACCEL (0.25)
+// max amount of cycles when motor stalled
+#define STALL_MAXCTR (50)
// direction of motor rotation positive to encoder (1 - negative)
#define MOTOR_REVERSE (0)
@@ -45,24 +46,26 @@
#define RAWSPEED(x) (x*5)
// max/min speed (rev/min)
#define MAXSPEED 1200
-#define MINSPEED 130
-// moving timeout (*0.01s)
-#define MOVING_TIMEOUT 5000
+#define MINSPEED 350
+// speed to move from ESW
+#define ESWSPEED 350
+// moving timeout: 5minutes
+#define MOVING_TIMEOUT (300)
// constants for focus conversion: foc_mm = (foc_raw - FOCRAW_0) / FOCSCALE_MM
#define FOCSCALE_MM 4096.
-#define FOCRAW_0 40960.
+#define FOCRAW_0 15963187.
#define FOC_RAW2MM(x) (((x)-FOCRAW_0)/FOCSCALE_MM)
#define FOC_MM2RAW(x) (FOCRAW_0+(x)*FOCSCALE_MM)
-// raw position precision
+// raw position precision - 2.5um
#define RAWPOS_TOLERANCE 10
-// raw dF value for accurate focussing
-#define dF0 100
+// raw dF value for accurate focussing (rough move to F+dF0 and after this slow move to F)
+#define dF0 500
// minimal & maximal focus positions (should be >min+dF0 & .
*/
-#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 "can_encoder.h"
+#include "canopen.h"
+#include "motor_cancodes.h"
#include "socket.h"
+#include "usefull_macros.h"
+#include // fabs
+#include // memcpy
+
+// flags: encoder is ready, motor is ready
+static uint8_t encoderRDY = 0, motorRDY = 0;
// printf when -v
extern int verbose(const char *fmt, ...);
@@ -38,7 +41,7 @@ static int encnodenum = 0;
// system status
static sysstatus curstatus = STAT_OK;
// 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_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
// return 0 if all OK
static int chk_eswstates(){
+ if(!motorRDY) return 0;
FNAME();
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_CCW_IDX, &ccw)) goto verybad;
uint32_t parval = DI_ENSTOP;
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(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;
}
return 0;
verybad:
- WARNX("Can't return esw to default state");
curstatus = STAT_ERROR;
return 1;
}
@@ -73,31 +76,38 @@ verybad:
*/
static int chkMove(int spd){
//FNAME();
- if(curstatus == STAT_DAMAGE){
- WARNX("Try to move in damaged state");
+ if(!motorRDY){
+ curstatus = STAT_ESW;
return 1;
}
+ if(curstatus == STAT_DAMAGE){
+ SINGLEWARN(WARN_MOVEDAMAGED);
+ return 1;
+ }else clrwarnsingle(WARN_MOVEDAMAGED);
eswstate e;
if(CAN_NOERR != get_endswitches(&e)){
curstatus = STAT_ERROR;
return 1;
}
if(e == ESW_INACTIVE){
- if(getPos(NULL)) return 1;
- if(curposition <= FOCMIN && spd < 0){
+ double posmm;
+ 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");
return 1;
}
- if(curposition >= FOCMAX && spd > 0){
+ if(posmm >= FOCMAX_MM && spd > 0){
WARNX("Try to move to the right of maximal position");
return 1;
}
return 0;
}
if(e == ESW_BOTH_ACTIVE){
+ SINGLEWARN(WARN_BOTHESW);
curstatus = STAT_DAMAGE;
return 1;
- }
+ }else clrwarnsingle(WARN_BOTHESW);
if(e == ESW_CCW_ACTIVE && spd < 0){
curstatus = STAT_ESW;
WARNX("Try to move over the CCW end-switch");
@@ -117,11 +127,13 @@ static int chkMove(int spd){
* @return 0 if all OK
*/
int init_encoder(int encnode, int reset){
+ FNAME();
unsigned long lval;
encnodenum = encnode;
verbose("cur node: %d\n", 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;
}
if(reset){
@@ -143,16 +155,6 @@ int init_encoder(int encnode, int reset){
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);
@@ -161,7 +163,7 @@ int init_encoder(int encnode, int reset){
if(state == NodeOperational) verbose("Ok!\n");
else{
WARNX("Failed to start node!");
- returnPreOper();
+ returnPreOper(-1);
return 1;
}
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]);
}while(0);
curstatus = STAT_OK;
+ encoderRDY = 1;
return 0;
}
@@ -186,7 +189,8 @@ int init_encoder(int encnode, int reset){
* @return 0 if all OK
*/
int go_out_from_ESW(){
- FNAME();
+ //FNAME();
+ if(!encoderRDY || !motorRDY) return 0;
uint32_t parval;
if(chk_eswstates()) return 1;
// check esw
@@ -196,36 +200,44 @@ int go_out_from_ESW(){
return 1;
}
if(e == ESW_BOTH_ACTIVE){ // error situation!
- WARNX("Error: both end-switches are active!");
- curstatus = STAT_DAMAGE;
+ SINGLEWARN(WARN_BOTHESW);
+ if(curstatus != STAT_DAMAGE){
+ curstatus = STAT_DAMAGE;
+ }
return 1;
- }
+ }else clrwarnsingle(WARN_BOTHESW);
if(e == ESW_INACTIVE){
DBG("Esw inactive");
int r = 0;
- if(curposition < FOCMIN) r = move(FOCMIN, MAXSPEED);
- else if(curposition > FOCMAX) r = move(FOCMAX, -MAXSPEED);
+// TODO: fix trouble with current position if it is over available position
+ // if(curposition < FOCMIN) r = move(FOCMIN, MAXSPEED);
+ // else if(curposition > FOCMAX) r = move(FOCMAX, -MAXSPEED);
curstatus = STAT_OK;
return r;
}
- curstatus = STAT_GOFROMESW;
// check that current position is in available zone
if(e == ESW_CW_ACTIVE && curposition < FOCPOS_CW_ESW){
- // WTF? CW end-switch activated in forbidden zone!
- WARNX("CW end-switch in forbidden zone (to the left of normal position)!");
- curstatus = STAT_DAMAGE;
+ // CW end-switch activated in forbidden zone
+ if(curstatus != STAT_DAMAGE){
+ WARNX("CW end-switch in forbidden zone (to the left of normal position)!");
+ curstatus = STAT_DAMAGE;
+ }
return 1;
- }else if(e == ESW_CCW_ACTIVE && curposition > FOCPOS_CW_ESW){
- // CCW end-switch activated in forbidden zone!
- WARNX("CCW end-switch in forbidden zone (too far)!");
- curstatus = STAT_DAMAGE;
+ }else if(e == ESW_CCW_ACTIVE && curposition > FOCPOS_CCW_ESW){
+ // CCW end-switch activated in forbidden zone
+ if(curstatus != STAT_DAMAGE){
+ WARNX("CCW end-switch in forbidden zone (too far)!");
+ curstatus = STAT_DAMAGE;
+ }
return 1;
}
+ curstatus = STAT_GOFROMESW;
// try to move from esw
parval = DI_NOFUNC;
+ WARNX("Try to move from ESW");
uint16_t idx = (e == ESW_CW_ACTIVE) ? PAR_CW_IDX : PAR_CCW_IDX;
if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, idx, &parval)) goto bad; // turn off motor stopping @esw
- int16_t speed = (e == ESW_CW_ACTIVE) ? -RAWSPEED(MINSPEED) : RAWSPEED(MINSPEED);
+ int16_t speed = (e == ESW_CW_ACTIVE) ? -RAWSPEED(ESWSPEED) : RAWSPEED(ESWSPEED);
for(int i = 0; i < 5; ++i){ // 5 tries to move out
getPos(NULL);
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_p_id = MOTOR_PAR_ID(addr);
DBG("motor POid=%lu, motor_PROCid=%lu", motor_id, motor_p_id);
+ motorRDY = 1;
// check esw roles & end-switches state
if(go_out_from_ESW()) return 1;
return 0;
@@ -271,36 +284,56 @@ int init_motor_ids(int addr){
*/
int getPos(double *pos){
//FNAME();
+ if(!encoderRDY) return 1;
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;
if(CAN_NOERR != get_endswitches(&e)){
curstatus = STAT_ERROR;
}else switch(e){
case ESW_BOTH_ACTIVE:
+ WARNX("Damage state: both end-switches are active");
curstatus = STAT_DAMAGE;
break;
case ESW_CCW_ACTIVE:
- if(curposition > FOCPOS_CCW_ESW) curstatus = STAT_DAMAGE;
- else curstatus = STAT_ESW;
+ if(posmm > FOCMIN_MM + ESW_DIST_ALLOW){
+ WARNX("Damage state: CCW end-switch in forbidden zone");
+ curstatus = STAT_DAMAGE;
+ }else curstatus = STAT_ESW;
break;
case ESW_CW_ACTIVE:
- if(curposition < FOCPOS_CW_ESW) curstatus = STAT_DAMAGE;
- else curstatus = STAT_ESW;
+ if(posmm < FOCMAX_MM - ESW_DIST_ALLOW){
+ WARNX("Damage state: CW end-switch in forbidden zone");
+ curstatus = STAT_DAMAGE;
+ }else curstatus = STAT_ESW;
break;
case ESW_INACTIVE:
default:
curstatus = STAT_OK;
}
+ //DBG("targspd = %d", targspd);
if(targspd){
- if(curposition <= FOCMIN && targspd < 0){ // bad value
- WARNX("Forbidden position < FOCMIN!");
+ if(posmm <= FOCMIN_MM && targspd < 0){ // bad value
+ SINGLEWARN(WARN_LESSMIN);
stop();
curstatus = STAT_FORBIDDEN;
- }else if(curposition >= FOCMAX && targspd > 0){
- WARNX("Forbidden position > FOCMAX!");
+ }else if(posmm >= FOCMAX_MM && targspd > 0){
+ SINGLEWARN(WARN_GRTRMAX);
stop();
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;
@@ -313,16 +346,46 @@ double curPos(){
/**
* @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... ");
setPreOper(encnodenum);
int state=getNodeState(encnodenum);
verbose("State=0x%02x - ", state);
- if(state == NodePreOperational){
- verbose("Ok!\n");
- }else{
+ if(state != NodePreOperational){
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
*/
static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){
+ if(!motorRDY) return CAN_NOANSWER;
const int l = 6; // frame length
/*if(G->verbose){
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");
}*/
if(can_send_frame(motor_id, l, buf) <= 0){
- WARNX("Error sending CAN frame (len %d)", l);
+ SINGLEWARN(WARN_CANSEND);
return CAN_CANTSEND;
- }
+ }else clrwarnsingle(WARN_CANSEND);
int I, rxpnt, idr, dlen;
double rxtime;
unsigned char rdata[8];
@@ -367,9 +431,9 @@ static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){
can_dsleep(0.01);
}
if(I == 50){
- WARNX("can_send_chk(): error getting answer");
+ SINGLEWARN(WARN_CANNOANS);
return CAN_NOANSWER;
- }
+ }else clrwarnsingle(WARN_CANNOANS);
if(obuf) memcpy(obuf, rdata, l);
/*if(G->verbose){
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
*/
static canstatus can_send_param(unsigned char *buf, unsigned char *obuf){
+ if(!motorRDY) return CAN_NOANSWER;
const int l = 8; // frame length
int I, rxpnt, idr, dlen;
double rxtime;
@@ -407,19 +472,19 @@ for(int i=0; i> 8;
@@ -463,6 +529,7 @@ static canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
* @return status
*/
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];
buf[2] = idx >> 8;
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
*/
canstatus get_motor_speed(double *spd){
+ if(!motorRDY) return CAN_NOANSWER;
if(!spd) return CAN_WARNING;
union{
uint32_t u;
@@ -502,13 +570,14 @@ canstatus get_motor_speed(double *spd){
* @return
*/
canstatus get_endswitches(eswstate *Esw){
+ if(!motorRDY) return CAN_NOANSWER;
//FNAME();
uint32_t val = 0;
canstatus s = can_read_par(PAR_DI_SUBIDX, PAR_DIST_IDX, &val);
if(s != CAN_NOERR){
- WARNX("Can't read end-switches state");
+ SINGLEWARN(WARN_ESWSTATE);
return s;
- }
+ }else clrwarnsingle(WARN_ESWSTATE);
if(Esw){
int v = 0;
if(!(val & ESW_CW)){ // + pressed
@@ -527,7 +596,8 @@ canstatus get_endswitches(eswstate *Esw){
* @return 0 if all OK
*/
int stop(){
- FNAME();
+ if(!motorRDY) return 0;
+ //FNAME();
unsigned char buf[6] = {0, CW_STOP,0,};
if(can_send_chk(buf, NULL)){
WARNX("Can't stop motor!");
@@ -539,8 +609,10 @@ int stop(){
/**
* @brief waitTillStop - wait for full stop
+ * @return 0 if all OK
*/
static int waitTillStop(){
+ if(!motorRDY) return 0;
long oldposition = -1;
double spd;
int r = 0;
@@ -563,6 +635,7 @@ static int waitTillStop(){
}else{DBG("can't get motor speed");};
oldposition = curposition;
// now wait for full moving stop
+ if(!encoderRDY) break;
getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition);
//DBG("curpos: %lu, oldpos: %ld", curposition, oldposition);
}while((long)curposition != oldposition);
@@ -579,6 +652,7 @@ static int waitTillStop(){
* @return 0 if all OK
*/
int movewconstspeed(int16_t spd){
+ if(!motorRDY) return 0;
if(chkMove(spd)) return 1;
fix_targspeed(&spd);
targspd = RAWSPEED(spd);
@@ -603,15 +677,17 @@ int movewconstspeed(int16_t spd){
* @return 0 if all OK
*/
static int move(unsigned long targposition, int16_t rawspeed){
+ if(!motorRDY || !encoderRDY) return 1;
//FNAME();
- if(abs(targposition - curposition) < RAWPOS_TOLERANCE){
+ long olddiffr = labs((long)targposition - (long)curposition);
+ if(olddiffr < RAWPOS_TOLERANCE){
verbose("Already at position\n");
DBG("Already at position");
return 0;
}
if(chkMove(rawspeed)) return 1;
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;
targspd = rawspeed;
if(MOTOR_REVERSE) rawspeed = -rawspeed;
@@ -625,17 +701,18 @@ static int move(unsigned long targposition, int16_t rawspeed){
return 1;
}
//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();
-#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, errctr = 0;
- for(i = 0; i < MOVING_TIMEOUT; ++i){
- can_dsleep(0.001);
- //uint16_t motstat;
+ // Steps after stopping = -27.96 + 9.20e-2*v + 3.79e-4*v^2, v in rev/min
+ // in rawspeed = -27.96 + 1.84e-2*v + 1.52e-5*v^2
+ double rs = fabs((double)rawspeed);
+ //double cv = (-27.96 + (1.84e-2 + 1.52e-5*rs)*rs);
+ double cv = (-50. + (1.84e-2 + 1.52e-5*rs)*rs);
+ long corrvalue = (long) cv; // correction due to stopping ramp
+ if(corrvalue < 10) corrvalue = 10;
+ DBG("start-> curpos: %ld, difference: %ld, corrval: %ld",
+ curposition, olddiffr, corrvalue);
+ int errctr = 0, passctr = 0;
+ while(can_dtime() - t0 < MOVING_TIMEOUT){
double speed;
if(emerg_stop){ // emergency stop activated
WARNX("Activated stop while moving");
@@ -649,36 +726,39 @@ static int move(unsigned long targposition, int16_t rawspeed){
curstatus = STAT_ERROR;
return 1;
}
- getPos(NULL);
+ //getPos(NULL);
if(chkMove(targspd)){
WARNX("Can't move further!");
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!");
+ if(fabs(speed) < 0.1){
+ if(can_dtime() - t0 > TACCEL){
+ WARNX("Motor can't moving! Time after start=%.3fs.", can_dtime()-t0);
curstatus = STAT_ERROR;
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){
+ long diffr = labs((long)targposition - (long)curposition);
+ DBG("Speed: %g, curpos: %ld, diff: %ld", speed, curposition, diffr);
+ if(diffr < corrvalue){
DBG("OK! almost reach: olddif=%ld, diff=%ld, corrval=%ld, tm=%g", olddiffr, diffr, corrvalue, can_dtime()-t0);
olddiffr = diffr;
break;
}
- if(abs(diffr) > abs(olddiffr)){
+ if(diffr > olddiffr){ // pass over target -> stop
+ if(++passctr > 2) break;
+ }
+ if(diffr >= olddiffr){ // motor stall -> stop
++errctr;
DBG("errctr: %d", errctr);
- if(errctr > 50) break;
+ if(errctr > STALL_MAXCTR) break;
}
olddiffr = diffr;
}
- if(i == MOVING_TIMEOUT){
+ if(can_dtime() - t0 > MOVING_TIMEOUT){
WARNX("Error: timeout, but motor still not @ position! STOP!");
stop();
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);
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);
DBG("stop-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
curstatus = STAT_OK;
@@ -699,6 +779,7 @@ static int move(unsigned long targposition, int16_t rawspeed){
* @return 0 if all OK
*/
int move2pos(double target){
+ if(!motorRDY || !encoderRDY) return 1;
FNAME();
double cur;
if(getPos(&cur)){
@@ -707,23 +788,24 @@ int move2pos(double target){
}
unsigned long targposition = FOC_MM2RAW(target);
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!");
return 1;
}
- if(abs(targposition - curposition) < RAWPOS_TOLERANCE){
+ if(labs((long)targposition - (long)curposition) < RAWPOS_TOLERANCE){
verbose("Already at position\n");
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;
DBG("absdiff: %ld", absdiff);
- //long spd = (targ0pos - (long)curposition) / 2L;
- if(absdiff > 1000) spd = sign*MAXSPEED;
+ // move not less than for 1s
+ if(absdiff > 1500) spd = sign*MAXSPEED;
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;
int16_t targspd = (int16_t) spd;
+ DBG("TARGSPD: %d", targspd);
/* if(spd > INT16_MAX) targspd = INT16_MAX;
else if(spd < INT16_MIN) targspd = INT16_MIN;
fix_targspeed(&targspd);*/
@@ -748,13 +830,13 @@ int move2pos(double target){
WARNX("Can't get current position");
return 1;
}
- if(abs(targposition - curposition) < RAWPOS_TOLERANCE){
- verbose("Catch the position @ rough movint\n");
- DBG("Catch the position @ rough movint");
+ if(labs((long)targposition - (long)curposition) < RAWPOS_TOLERANCE){
+ verbose("Catch the position @ rough moving\n");
+ DBG("Catch the position @ rough moving");
return 0;
}
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;
}
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");
return 1;
}
- if(abs(targposition - curposition) > RAWPOS_TOLERANCE){
+ if(labs((long)targposition - (long)curposition) > RAWPOS_TOLERANCE){
verbose("Stopped over the accuracy range\n");
return 1;
}
@@ -783,9 +865,14 @@ int get_pos_speed(unsigned long *pos, double *speed){
FNAME();
int ret = 0;
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(!motorRDY){
+ *speed = 0.;
+ return ret;
+ }
if(get_motor_speed(speed) != CAN_NOERR){
*speed = 0.;
ret = 1;
@@ -795,6 +882,7 @@ int get_pos_speed(unsigned long *pos, double *speed){
}
void movewithmon(double spd){
+ if(!motorRDY || !encoderRDY) return;
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);
diff --git a/Z1000_focus/cmdlnopts.c b/Z1000_focus/cmdlnopts.c
index 38f8742..14d9870 100644
--- a/Z1000_focus/cmdlnopts.c
+++ b/Z1000_focus/cmdlnopts.c
@@ -31,7 +31,7 @@
* here are global parameters initialisation
*/
int help;
-glob_pars G;
+glob_pars GP;
#define DEFPIDNAME "/tmp/z1000focus.pid"
@@ -42,7 +42,8 @@ glob_pars const Gdefault = {
.motorID = 12,
.gotopos = NAN,
.port = DEFPORT,
- .pidfilename = DEFPIDNAME
+ .pidfilename = DEFPIDNAME,
+ .chpresetval = -1
};
/*
@@ -52,21 +53,25 @@ glob_pars const Gdefault = {
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"},
- {"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 (default: " DEFPORT ")"},
- {"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"},
- {"pidfile", NEED_ARG, NULL, 'p', arg_string, APTR(&G.pidfilename),"name of PID-file (default: " DEFPIDNAME ")"},
+ {"node", NEED_ARG, NULL, 'n', arg_int, APTR(&GP.nodenum), "encoder node number"},
+ {"reset", NO_ARGS, NULL, 'r', arg_none, APTR(&GP.reset), "reset encoder"},
+ {"verbose", NO_ARGS, NULL, 'v', arg_int, APTR(&GP.verbose), "show more info"},
+ {"motorid", NEED_ARG, NULL, 'i', arg_int, APTR(&GP.motorID), "motor controller address"},
+ {"gotopos", NEED_ARG, NULL, 'g', arg_double, APTR(&GP.gotopos), "target focus position"},
+ {"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(&GP.stop), "stop motor"},
+ {"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(&GP.showesw), "show end-switches state"},
+ {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&GP.logname), "logfile name and path"},
+ {"server", NO_ARGS, NULL, 'S', arg_none, APTR(&GP.server), "work as server"},
+ {"port", NEED_ARG, NULL, 'P', arg_string, APTR(&GP.port), "server port number (default: " DEFPORT ")"},
+ {"host", NEED_ARG, NULL, 'H', arg_string, APTR(&GP.host), "host to connect (default: localhost)"},
+ {"standalone",NO_ARGS, NULL, 'A', arg_none, APTR(&GP.standalone),"run as standalone application"},
+ {"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
};
@@ -78,12 +83,12 @@ myoption cmdlnopts[] = {
* @return allocated structure with global parameters
*/
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"
change_helpstring("Usage: %s [args]\n\n\tWhere args are:\n");
// parse arguments
parseargs(&argc, &argv, cmdlnopts);
if(help) showhelp(-1, cmdlnopts);
- return &G;
+ return &GP;
}
diff --git a/Z1000_focus/cmdlnopts.h b/Z1000_focus/cmdlnopts.h
index b88c26b..8f76a38 100644
--- a/Z1000_focus/cmdlnopts.h
+++ b/Z1000_focus/cmdlnopts.h
@@ -42,6 +42,10 @@ typedef struct{
char *host; // host to connect (in client mode)
int standalone; // run standalone
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;
diff --git a/Z1000_focus/main.c b/Z1000_focus/main.c
index 12d2f01..0b2d189 100644
--- a/Z1000_focus/main.c
+++ b/Z1000_focus/main.c
@@ -28,7 +28,7 @@
#include "socket.h"
#include "usefull_macros.h"
-static glob_pars *G;
+glob_pars *G;
/**
* @brief verbose - printf when parameter `verbose` set
@@ -44,13 +44,14 @@ int verbose(const char *fmt, ...){
return i;
}
+static pid_t childpid;
/**
* @brief signals - signal handler (also called by functions ERR/ERRX)
* @param signo - signal number
*/
void signals(int signo){
WARNX("Received signal %d", signo);
- unlink_pidfile();
+ if(childpid) unlink_pidfile(); // parent process died
exit(signo);
}
@@ -65,7 +66,7 @@ static void cmdparser(){
#undef BUFL
}
-//extern char can_dev[40];
+extern char can_dev[40];
int main (int argc, char *argv[]){
int ret = 0;
@@ -76,12 +77,14 @@ int main (int argc, char *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(G->nomotor) ERRX("Can't set target speed without motor");
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->noencoder || G->nomotor) ERRX("Can't go to target position without encoder or motor");
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;
@@ -92,43 +95,35 @@ int main (int argc, char *argv[]){
signal(SIGKILL, signals);
signal(SIGTSTP, SIG_IGN);
signal(SIGHUP, SIG_IGN);
-//can_dev[8] = '1';
+ can_dev[8] = '1';
if(G->server || G->standalone){ // init hardware
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 !defined EBUG
if(daemon(1, 0)){
ERR("daemon()");
- }
+ };
#endif
while(1){ // guard for dead processes
- pid_t childpid = fork();
+ childpid = fork();
if(childpid){
- putlog("create child with PID %d\n", childpid);
- DBG("Created child with PID %d\n", childpid);
+ DBG("child: %d", childpid);
wait(NULL);
- putlog("child %d died\n", childpid);
- WARNX("Child %d died\n", childpid);
+ DBG("child: %d DIED", childpid);
sleep(1);
}else{
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);
}
}
@@ -137,6 +132,25 @@ int main (int argc, char *argv[]){
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){
movewithmon(G->monitspd);
goto Oldcond;
@@ -188,6 +202,6 @@ Oldcond:
red("ERROR: both end-switches active\n");
}
}
- returnPreOper();
+ returnPreOper(G->chpresetval);
return ret;
}
diff --git a/Z1000_focus/socket.c b/Z1000_focus/socket.c
index d03e3da..b1fd8ab 100644
--- a/Z1000_focus/socket.c
+++ b/Z1000_focus/socket.c
@@ -35,7 +35,7 @@
#include "cmdlnopts.h" // glob_pars
-#define BUFLEN (10240)
+#define BUFLEN (1024)
// Max amount of connections
#define BACKLOG (30)
@@ -137,6 +137,7 @@ static void *move_focus(void *targpos){
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();
+ putlog("Focus value: %.03f", curPos());
pthread_mutex_unlock(&moving_mutex);
pthread_mutex_unlock(&canbus_mutex);
pthread_exit(NULL);
@@ -186,11 +187,18 @@ static void getoutESW(){
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;
+ 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();
while(dtime() - t0 < SOCKET_TIMEOUT){
if(!waittoread(sock)){ // no data incoming
@@ -215,9 +223,13 @@ static void *handle_socket(void *asock){
// here we can process user data
//DBG("user send: %s%s\n", buff, webquery ? ", web" : "");
// empty request == focus request
+// TODO: add requests for min/max values (focus & speed)
if(strlen(found) < 1 || getparam(S_CMD_FOCUS)){
//DBG("position request");
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)){
DBG("Stop request");
emerg_stop = TRUE;
@@ -226,6 +238,7 @@ static void *handle_socket(void *asock){
if(stop()) sprintf(buff, S_ANS_ERR);
else sprintf(buff, S_ANS_OK);
emerg_stop = FALSE;
+ putlog("%s: request to stop @ %.03f", peerIP, curPos());
pthread_mutex_unlock(&moving_mutex);
pthread_mutex_unlock(&canbus_mutex);
}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);
else{
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);
}
pthread_mutex_unlock(&canbus_mutex);
@@ -247,8 +261,11 @@ static void *handle_socket(void *asock){
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));
+ const char *ans = 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)){
const char *msg = S_STATUS_ERROR;
@@ -274,15 +291,14 @@ static void *handle_socket(void *asock){
default:
msg = "Unknown status";
}
- sprintf(buff, msg);
+ sprintf(buff, "%s", msg);
}else sprintf(buff, S_ANS_ERR);
if(!send_data(sock, webquery, buff)){
WARNX("can't send data, some error occured");
}
}
+ FREE(peerIP);
close(sock);
- //DBG("closed");
- //putlog("socket closed, exit");
pthread_exit(NULL);
return NULL;
#undef getparam
@@ -290,11 +306,9 @@ static void *handle_socket(void *asock){
// 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");
+ WARN("listen() failed");
return NULL;
}
while(1){
@@ -304,8 +318,7 @@ static void *server(void *asock){
if(!waittoread(sock)) continue;
newsock = accept(sock, (struct sockaddr*)&their_addr, &size);
if(newsock <= 0){
- putlog("accept() failed");
- WARN("accept()");
+ WARN("accept() failed");
continue;
}
struct sockaddr_in* pV4Addr = (struct sockaddr_in*)&their_addr;
@@ -315,35 +328,51 @@ static void *server(void *asock){
//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()");
+ WARN("pthread_create() failed");
}else{
//DBG("Thread created, detouch");
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
static void daemon_(int sock){
if(sock < 0) return;
pthread_t sock_thread;
+ double oldpos = curPos();
+ subst_file(G->focfilename);
+ DBG("create server() thread");
if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){
- putlog("daemon_(): pthread_create() failed");
- ERR("pthread_create()");
+ ERR("pthread_create() failed");
}
do{
if(pthread_kill(sock_thread, 0) == ESRCH){ // died
- WARNX("Sockets thread died");
- putlog("Sockets thread died");
+ WARNX("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()");
+ ERR("new pthread_create() failed");
}
}
- 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
if(!pthread_mutex_trylock(&canbus_mutex)){
getPos(NULL);
@@ -353,6 +382,10 @@ static void daemon_(int sock){
getoutESW();
}
}
+ if(G->focfilename && (fabs(oldpos - curPos()) > 0.01)){ // position changed -> change it in file
+ oldpos = curPos();
+ subst_file(G->focfilename);
+ }
}while(1);
putlog("daemon_(): UNREACHABLE CODE REACHED!");
}
@@ -368,38 +401,36 @@ void daemonize(const char *port){
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
if(getaddrinfo(NULL, port, &hints, &res) != 0){
- ERR("getaddrinfo");
+ ERR("getaddrinfo failed");
}
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);
+ 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
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");
+ ERR("setsockopt failed");
}
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);
+ DBG("going to run daemon_()");
daemon_(sock);
close(sock);
- putlog("socket closed, exit");
+ putlog("daemonize(): UNREACHABLE CODE REACHED!");
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_flags = AI_PASSIVE;
if(getaddrinfo(host, port, &hints, &res) != 0){
- ERR("getaddrinfo");
+ ERR("getaddrinfo failed");
}
struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr;
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
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;
}
@@ -450,5 +479,6 @@ void sock_send_data(const char *host, const char *port, const char *data){
return;
}
}
- WARN("No answer!");
+ WARN("no answer!");
+ close(sock);
}
diff --git a/Z1000_focus/socket.h b/Z1000_focus/socket.h
index dce38a8..d3f9ada 100644
--- a/Z1000_focus/socket.h
+++ b/Z1000_focus/socket.h
@@ -37,6 +37,7 @@
#define S_CMD_TARGSPEED "targspeed"
#define S_CMD_GOTO "goto"
#define S_CMD_STATUS "status"
+#define S_CMD_LIMITS "limits"
// answers through the socket
#define S_ANS_ERR "error"
@@ -46,11 +47,10 @@
// statuses
#define S_STATUS_OK "OK"
#define S_STATUS_ESW "End-switch active"
-#define S_STATUS_ERROR "Erroneous state"
-#define S_STATUS_ESW "End-switch active"
+#define S_STATUS_ERROR "SEW power if off (or other error)"
#define S_STATUS_GOFROMESW "Moving from end-switch"
#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;
diff --git a/Z1000_focus/usefull_macros.c b/Z1000_focus/usefull_macros.c
index 8e24425..1828a96 100644
--- a/Z1000_focus/usefull_macros.c
+++ b/Z1000_focus/usefull_macros.c
@@ -387,25 +387,42 @@ int str2double(double *num, const char *str){
return TRUE;
}
-FILE *Flog = NULL; // log file descriptor
-char *logname = NULL;
-time_t log_open_time = 0;
+static FILE *Flog = NULL; // log file descriptor
+static char *logname = NULL; // full logfile name (with PID prefix)
+static time_t log_open_time = 0; // time when log file was opened
/**
* Try to open log file
* if failed show warning message
*/
void openlogfile(char *name){
- if(!name){
- WARNX(_("Need filename"));
- return;
+ //char buf[PATH_MAX];
+ if(Flog){
+ 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"))){
WARN(_("Can't open log file"));
return;
}
log_open_time = time(NULL);
logname = name;
+ //fullogname = strdup(buf);
}
/**
@@ -413,22 +430,84 @@ void openlogfile(char *name){
*/
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));
+ char strtm[128];
+ time_t t = time(NULL);
+ 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);
+ Flog = NULL;
char newname[PATH_MAX];
snprintf(newname, PATH_MAX, "%s.old", logname);
if(rename(logname, newname)) WARN("rename()");
- openlogfile(logname);
+ openlogfile(NULL);
if(!Flog) return 0;
}
- int i = fprintf(Flog, "\n\t\t%s", ctime(&t_now));
va_list ar;
+ fprintf(Flog, "%s\t", strtm);
va_start(ar, fmt);
- i = vfprintf(Flog, fmt, ar);
+ i += vfprintf(Flog, fmt, ar);
va_end(ar);
fprintf(Flog, "\n");
+ ++i;
fflush(Flog);
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);
+}
diff --git a/Z1000_focus/usefull_macros.h b/Z1000_focus/usefull_macros.h
index 00b6140..2f7f5db 100644
--- a/Z1000_focus/usefull_macros.h
+++ b/Z1000_focus/usefull_macros.h
@@ -23,16 +23,22 @@
#ifndef __USEFULL_MACROS_H__
#define __USEFULL_MACROS_H__
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
#include
+#include
+#include
#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
#if defined GETTEXT_PACKAGE && defined LOCALEDIR
/*
* GETTEXT
@@ -45,12 +51,7 @@
#define _(String) (String)
#define N_(String) (String)
#endif
-#include
-#include
-#include
-#include
-#include
-#include
+
// unused arguments with -Wall -Werror
@@ -74,13 +75,30 @@
/*
* 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 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)
-
+#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; 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; FNAME(); _WARN(__VA_ARGS__); putlog("WARNING in %s (%s, line %d):", __func__, __FILE__, __LINE__); addtolog(__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
* debug mode, -DEBUG
@@ -137,4 +155,7 @@ int str2double(double *num, const char *str);
void openlogfile(char *name);
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__