mirror of
https://github.com/eddyem/zeiss_utils.git
synced 2025-12-06 02:35:15 +03:00
fix some bugs, ready for real work
This commit is contained in:
parent
3263d6de4f
commit
8ce37f4151
@ -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 & <max-dF0)
|
||||
#define FOCMIN_MM 0.1
|
||||
#define FOCMAX_MM 76.0
|
||||
#define FOCMIN_MM 2.75
|
||||
#define FOCMAX_MM 76.
|
||||
// -//- raw values
|
||||
#define FOCMIN FOC_MM2RAW(FOCMIN_MM)
|
||||
#define FOCMAX FOC_MM2RAW(FOCMAX_MM)
|
||||
|
||||
@ -16,15 +16,18 @@
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <string.h> // memcpy
|
||||
#include <math.h> // fabs
|
||||
#include "canopen.h"
|
||||
#include "can_encoder.h"
|
||||
#include "usefull_macros.h"
|
||||
#include "DS406_canopen.h"
|
||||
#include "motor_cancodes.h"
|
||||
#include "HW_dependent.h"
|
||||
#include "can_encoder.h"
|
||||
#include "canopen.h"
|
||||
#include "motor_cancodes.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
|
||||
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!");
|
||||
SINGLEWARN(WARN_BOTHESW);
|
||||
if(curstatus != STAT_DAMAGE){
|
||||
curstatus = STAT_DAMAGE;
|
||||
return 1;
|
||||
}
|
||||
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!
|
||||
// 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!
|
||||
}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<l; ++i) printf("0x%02x ", buf[i]);
|
||||
printf("\n");
|
||||
*/
|
||||
if(can_send_frame(motor_p_id, l, buf) <= 0){
|
||||
WARNX("Error sending CAN frame (len %d)", l);
|
||||
SINGLEWARN(WARN_CANSEND);
|
||||
return CAN_CANTSEND;
|
||||
}
|
||||
} else clrwarnsingle(WARN_CANSEND);
|
||||
can_clean_recv(&rxpnt, &rxtime);
|
||||
for(I = 0; I < 50; ++I){
|
||||
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);
|
||||
}
|
||||
if(I == 50){
|
||||
WARNX("can_send_param(): error getting answer");
|
||||
SINGLEWARN(WARN_SENDPAR);
|
||||
return CAN_NOANSWER;
|
||||
}
|
||||
}else clrwarnsingle(WARN_SENDPAR);
|
||||
/*
|
||||
green("Received param: ");
|
||||
for(int i=0; i<dlen; ++i) printf("0x%02x ", rdata[i]);
|
||||
@ -445,6 +510,7 @@ printf("\n");
|
||||
* @return status
|
||||
*/
|
||||
static canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
|
||||
if(!motorRDY) return CAN_NOANSWER;
|
||||
if(!parval) return CAN_WARNING;
|
||||
uint8_t buf[8] = {CAN_READPAR_CMD, subidx}, obuf[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
|
||||
*/
|
||||
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;
|
||||
long diffr = labs((long)targposition - (long)curposition);
|
||||
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);
|
||||
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);
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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"));
|
||||
//char buf[PATH_MAX];
|
||||
if(Flog){
|
||||
fclose(Flog);
|
||||
Flog = NULL;
|
||||
}
|
||||
if(!name) return;
|
||||
/*
|
||||
if(!name){ // filename is omit -> try to open log with old name
|
||||
if(!fullogname){
|
||||
WARNX(_("openlogfile(): need filename"));
|
||||
return;
|
||||
}
|
||||
green(_("Try to open log file %s in append mode\n"), name);
|
||||
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);
|
||||
}
|
||||
|
||||
@ -23,16 +23,22 @@
|
||||
#ifndef __USEFULL_MACROS_H__
|
||||
#define __USEFULL_MACROS_H__
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/mman.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
#include <errno.h>
|
||||
#include <err.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.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
|
||||
/*
|
||||
* GETTEXT
|
||||
@ -45,12 +51,7 @@
|
||||
#define _(String) (String)
|
||||
#define N_(String) (String)
|
||||
#endif
|
||||
#include <stdlib.h>
|
||||
#include <termios.h>
|
||||
#include <termio.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
|
||||
// unused arguments with -Wall -Werror
|
||||
@ -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__
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user